ICP算法详细推导过程--SVD方法(包含SVD的分解过程)

文字简述ICP(迭代最近点算法)：

输入两组点云X(目标点云)与P(源点云)，使点云P经过刚性变换(旋转R、平移t)与点云X重合，即使得X=R.P+t；并经过不断迭代使得均方差E(R,t)达到最小。

ICP的配准一般可分为两种：(1)已知初始点云之间的位置关系(2)未知初始点云之间的位置关系。
两者之间的区别在于，后者需要确定初始位姿即确定两组点云之间的匹配关系，且点云数量不等。实际情况也更倾向于后者，故在此先给出未知初始点云位置关系的ICP求解算法。
1.未知两组点云位置关系
1.1 计算两组点云之间的最近距离，确定两组点云的初始位姿，形成匹配点云：对于P中的一个点pi,选择X中与其欧式距离最近的点作为匹配点，使用KD-tree进行加速搜寻，直至P中的每个点都有在X中与之匹配的点。(实际情况中，匹配结果会存在一定的误差,应选择合适的算法进行剔除，此处不展开)
此时，两组点云的位置关系已确定。
1.2 基于两组点云之间的初始位置关系，求解出旋转矩阵R与平移t,详细步骤如下：
1.2.1 去掉两组点云的质心
对于给定的两组点云集合X，P：

求两组点云的质心：

则，两组点云中以质心为坐标原点的点的坐标为:

1.2.2 求解使得均方差E(R,t)最小时的R，t：

又：

所以：

E(R,t)要为最小，则

，即


则最优旋转矩阵为：

所以要使R取得最优，则：





1.3 设定迭代误差限e,若E(R,t)>e,则将经本次计算所得的R，t旋转后的点云P’设为初值，重新进行迭代计算，直到E(R,t)小于设定的迭代阈值e为止。
2.未知两组点云位置关系
将确定点云之间的关系即1中的1.1部分去掉即可

参考资料
https://zhuanlan.zhihu.com/p/63964953
https://blog.csdn.net/u014709760/article/details/99241393