ICP迭代最近点优化

点云之间的局部配准问题采样迭代最近点算法（IteratedClosest Points, ICP），ICP算法因为其思想简单，精度高等特点成为了局部配准的主流算法。

迭代最近点算法（ICP）顾名思义，就是采用迭代优化的思想以空间距离作为匹配点的选择依据，通过不断调整点云的位姿使得匹配点之间距离累计最小。假设有两组点云，其中一个目标点云A另一个为参考点云B，ICP算法的目的是为了算出一个最优的旋转矩阵R和平移向量t使得变换后的点云A能与B达到最精确的匹配。如公式（1）所示，求得匹配点误差平方和达到极小的R，t：

ICP需要反复执行以下两步直至收敛：第一步，计算A点集与B点集之间的匹配点对；第二步，根据前一步得到的匹配点对计算出A点集与B点集之间的转换矩阵，并对A点集进行相应的转换。在取得了点集A与B的匹配点基础上，点集之间的转换矩阵求解也分为两种方式：利用线性代数的求解（主要是SVD），以及利用非线性优化方式的求解（类似于Bundle Adjustment）。其中奇异值分解法（SVD）因为准确、稳定以及高效等特点被广泛运用，接下来介绍利用SVD方法的转换矩阵计算方法。

首先，定义两组点的质心：

随后，在误差函数中做如下处理：

仔细观察左右两项，可以发现左边只和旋转矩阵R相关，而右边既有R也有t，但只和质心相关。只要获得了R，令第二项为零就能得到t。那么最下化问题就转化为如下公式：

    解得R后，即可求解t。