本文介绍了ICP算法及其MATLAB实现

经典的ICP（iterative closet point）算法是由McKay和Besl1提出的，它的核心思想是迭代调姿使距离偏差最小化。原始ICP算法的基本描述是：对于点云P的每个点，在另一个点云Q中求取距离它最近的点（理想状态下本应重合的点），计算对应点的欧式距离平方的平均值，然后通过迭代算法，最小化平均值，这样不断更新点云片间的相对位置，达到点云片之间配准对齐的效果，如图6所示。

扫描点云与CAD模型最佳拟合对齐是利用奇异值分解法、四元组法找出扫描点云与模型表面对应点的变换矩阵，多次迭代直至目标函数满足一定的精度为止。其目标函数为：

式中：pi(i=0,1,…,n) 为待配准的扫描点； qi(i=0,1,…,n)为pi在CAD模型表面上的匹配点； R,T分别为待求的旋转矩阵与平移向量。 具体计算步骤如下： Step1. 计算两个待配准点集的质心：

Step2. 计算两个待配准点集相对于各自质心的位移：

Step3. 计算协方差矩阵：

Step4. 构造对称矩阵：

Step5. 计算旋转矩阵

Step6. 计算平移变换矢量

ICP有以下局限性： （1）初始位置的依赖性 初始位置对它的影响很大，ICP算法的收敛速度及收敛的最终位置直接受制于求取的匹配点（最近点）是否精确，而只有在扫面点云与CAD模型有较好的初始位置关系时，匹配点才能精准地获取到。 （2）局部最优性 ICP算法得到的是局部最优解，从本质上来讲，该算法每次迭代总是选择距离差的最小值，并不能得到全局最优解。 （3）不区分正负性 ICP算法是以最小二乘的方式最小化距离差，其收敛结果趋向于使得各配对点的差值均匀化，并且该距离差不具有正负性和方向性。 （4）计算匹配点复杂度较高 ICP算法的时间复杂度非常高：计算扫描点的匹配点（求取最近点）时，对于点云片P中的每个点，需要遍历CAD模型的曲面集合 中每个面，计算匹配点集 时间复杂度为 ，并且点到曲面最近点的计算也是较为耗时的。为了提高最佳拟合对齐的效率，点云数据需要精简，计算最近点的方式需要改进。

代码如下：