版权声明：本文为博主原创文章，欢迎转载。转载请注明出处http://blog.csdn.net/jsgaobiao https://blog.csdn.net/jsgaobiao/article/details/78873718
ICP迭代最近点算法
代码下载：http://download.csdn.net/download/jsgaobiao/10169426
一、实验原理
点云配准指将两个不同视点的点群三维数据整合到一个统一的座标系的过程。
迭代最近点法（ICP, Iterative Closest Points）的思路为：根据某种几何特性对数据进行匹配，并设这些匹配点为假想的对应点，然后根据这种对应关系求解运动参数。再利用这些运动参数对数据进行变换。并利用同一几何特征，确定新的对应关系，重复上述过程进行迭代，使得数据中的重叠部分充分吻合。ICP方法中使用的几何特性即点在空间中的距离。
（下面一段公式太多直接贴图了。。好懒）
二、具体实现
本算法基于Python语言实现，使用Geomagic Studio或者matplotlib作为可视化工具，具体实现的步骤如下：
- 使用Geomagic Studio将点云文件进行粗配准；
- 在点集中随机选取指定数量（用户可自定义）的点作为控制点，用于调节ICP算法的计算效率和配准精度；
- 在另一个点集中根据欧几里得距离找到每个点的对应点；
- 利用控制点和其对应点计算旋转矩阵和位移向量；
- 将计算出来的旋转位移量应用到点集上，并迭代进行2、3、4步骤。
优化方法：
ICP进行匹配的过程中有一些可以优化的细节：
删除点云数据中在采集时产生的噪声，例如明显偏离物体很远的点，减少异常值产生的影响；
使用KD-Tree这一数据结构对查找最近点对的过程进行优化，可以节省很多时间。
三、实验结果
图 1和图 3分别为点云的粗配准和ICP配准后的结果，可以看到，图 1原始的点云两种颜色分别占据了大块的面积，尤其从图 2的视角可以看出，粗配准的结果下两个点集依然有较大的距离。
图 3两种颜色互相融合，说明配准取得了一定效果。
图 1 点云粗配准结果
图 2 点云粗配准（误差较大）
图 3 配准结果
四、总结
ICP算法作为点云配准的经典方法，实现简单，但有诸多细节需要考虑，如控制点的选取，异常值的处理，距离的度量方法等。而且ICP方法需要有较好的初值，否则也难以得到好的结果。