一、简介

ICP算法总共分为6个阶段，如下图所示：

（1）挑选发生重叠的点云子集，这一步如果原始点云数据量比较巨大，一般会对原始点云进行下采样操作。
（2）匹配特征点。通常是距离最近的两个点，当然这需要视评判的准则而定。
（3） 加权。根据点的匹配程度对找到的对应点进行加权。
（4）抑制匹配点。根据匹配点的匹配程度来对一些质量较差的点对进行抑制（剔除）。
（5）误差最小化。通过最小化距离的平方和来估计变换参数。
（6）点云变换。通过评估出的变换矩阵来转换源点云。

整个过程除了最后一步，剩余的步骤已有大量的文献进行过探索和研究，因此也出现了诸多变种方法，其中点到面的ICP就是其中一种，即使用一个点到另一个点切平面的垂直距离(“点到平面”误差度量，如下图所示)来衡量配准结果的质量，这种误差度量的特点是收敛速度快，因为平坦区域可