迭代最近点（Iterative Closest Point, ICP）算法

1. 定义

ICP（Iterative Closest Point，迭代最近点）算法是一种迭代计算方法，主要用于计算机视觉中深度图像的精确拼合，通过不断迭代最小化源数据与目标数据对应点来实现精确地拼合。已经有很多变种，主要热点是怎样高效、鲁棒的获得较好地拼合效果。

2. 问题引入

ICP算法常被用于点云匹配（刚性配准）。给定两组点云（源点云$P$与目标点云$Q$）：

{ P = { p 1 , p 2 , . . . , p n } Q = { q 1 , q 2 , . . . , q n } \begin{cases} P=\{p_1,p_2,...,p_n\} Q=\{q_1,q_2,...,q_n\} \end{cases} {P={p1​,p2​,...,pn​}Q={q1​,q2​,...,qn​}​

点云配准的过程即为寻找一个空间的最优转换矩阵$T_{norm}$（由旋转$R$和平移$t$组成），使得点集P和Q之间的距离最小。

可通过最小化下式来求$T_{norm}$：

$$E(R,t)=\frac{1}{n}\sum _{i=1}^n\Vert q_i-\widehat{p_i}{\Vert }^2$$

3. 算法步骤

具体步骤为：

步骤	内容	说明
①	计算Q中的每一个点在P点集中的对应近点	可利用kd-tree算法来提高查找效率
②	求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数	SVD等，见下一节
③	对P使用上一步求得的平移和旋转参数，得到新的变换点集	/
④	如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新的P继续迭代，直到达到目标函数的要求	/

4. SVD求解R、t

（1）将点集P、Q中的点均减去自身的质心
两组点集的质心分别为：

两组点集去掉质心后的坐标为：

（2）求去中心后两点集的协方差矩阵：

（3）计算旋转矩阵R
对公式(3.4-5)中的W进行SVD分解，有：

当W满秩的时候，有唯一解：

（4）计算平移矢量t
通过上述公式，可以求得平移矢量为：

参考

[1] https://zhuanlan.zhihu.com/p/63964953
[2] https://zhuanlan.zhihu.com/p/35893884
[3] https://blog.csdn.net/hongbin_xu/article/details/80537100
[4] https://www.cnblogs.com/haoyul/p/7979289.html