ICP迭代最近点算法

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

ICP算法可使不同坐标下的点云数据整合至同一坐标系统中。首先需要确定一个适用的变换矩阵，在此过程中将不同基准下的空间点进行几何配准。配准过程本质上就是要建立从原坐标系至目标坐标系的刚性变换模型。

ICP算法主要基于最小二乘法的最优配准技术。该算法反复执行选择对应关系点对的过程，并计算出最适宜的刚体变换参数，在达到预期配准精度时停止运算。

ICP算法的目标在于确定待配准点云与参考点云之间的旋转和平移参数R和T以达到最优匹配

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

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

第二步为计算使上述对应点对平均距离最小的刚体变换，并确定其平移参数和平移参数

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

当两个新变换点集合与原始参考