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

最近在做点云匹配，需要用c++实现ICP算法，下面是简单理解，期待高手指正。

ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中，首先是找到一个可用的变换，配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换。

ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对， 计算最优刚体变换，直到满足正确配准的收敛精度要求。

ICP 算法的目的是要找到待配准点云数据与参考云数据之间的旋转参数R和平移参数 T，使得两点数据之间满足某种度量准则下的最优匹配。

假设给两个三维点集 X1 和 X2，ICP方法的配准步骤如下：

第一步，计算X2中的每一个点在X1 点集中的对应近点；

第二步，求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数；

第三步，对X2使用上一步求得的平移和旋转参数，得到新的变换点集；

第四步， 如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新