迭代最近点算法（ICP）学习

1.问题引出

1）当知道两个点云确切的对应关系时，即xi和pi是同一个点在不同坐标系下的表达，可以用最小二乘法获得R和t的闭式解；

2）当不知道两个点云的对应关系时，只能认为最接近的两个点时同一个点，然后迭代计算出R和t。

2.算法流程

1）设定R1，t1初值；

2）旋转测量点云X，用最邻近法获得两组点云的匹配关系（一般用kd-tree加速）；

3）计算两组点云的去质心坐标：

4）用SVD分解计算旋转矩阵：