迭代最近点算法（ICP）
在20世纪80年代中期，很多学者开始对点集数据的配准进行了大量研究。1987年，Horn[1]、Arun[2]等人用四元数法提出点集对点集配准方法。这种点集与点集坐标系匹配算法通过实践证明是一个解决复杂配准问题的关键方法。1992年，计算机视觉研究者Besl和Mckay[3]介绍了一种高层次的基于自由形态曲面的配准方法，也称为迭代最近点法ICP（Iterative Closest Point）。以点集对点集（PSTPS）配准方法为基础，他们阐述了一种曲面拟合算法，该算法是基于四元数的点集到点集配准方法。从测量点集中确定其对应的最近点点集后，运用Faugera和Hebert提出的方法计算新的最近点点集。用该方法进行迭代计算，直到残差平方和所构成的目标函数值不变，结束迭代过程。ICP配准法主要用于解决基于自由形态曲面的配准问题。
迭代最近点法ICP最近点法经过十几年的发展，不断地得到了完善和补充。Chen和Medioni[4]及Bergevin等人[5]提出了point-to-plane搜索最近点的精确配准方法。Rusinkiewicz和Levoy提出了point-to-p rojection搜索最近点的快速配准方法。Soon-Yong和Murali提出了Contractive-projection-point搜索最近点的配准方法。此外，Andrew和Sing[6]提取了基于彩色三维扫描数据点纹理信息的数据配准方法，主要在ICP算法中考虑三维扫描点的纹理色彩信息进行搜索最近点。Natasha等人[7]分析了ICP算法中的点云数据配准质量问题。[8]
一、基本原理[3]
三维空间R3存在两组含有n个坐标点的点集PL和PR，分别为： 和
。三维空间点集PL中各点经过三维空间变换后与点集PR中点一一对应，其单点变换关系式为：
(0-1)
上式中，R为三维旋转矩阵，t为平移向量。
在ICP配准方法中，空间变换参数向量X可表示为[9] 。参数向量中四元数参数满足约束条件为：
(0-2)
根据迭代的初值X0，由式(0-1)计算新点集Pi为：
(0-3)
式中，P表示原始未修改过的点集，Pi的下标i表示迭代次数，参数向量X的初始值X0为 。
根据以上数据处理方法，ICP配准算法可以概括为以下七个步骤：
1） 根据点集Plk中的点坐标，在曲面S上搜索相应最近点点集Prk；
2） 计算两个点集的重心位置坐标，并进行点集中心化生成新的点集；
3） 由新的点集计算正定矩阵N，并计算N的最大特征值及其最大特征向量；
4） 由于最大特征向量等价于残差平方和最小时的旋转四元数，将四元数转换为旋转矩阵R；
5） 在旋转矩阵R被确定后，由平移向量t仅仅是两个点集的重心差异，可以通过两个坐标系中的重心点和旋转矩阵确定；
6） 根据式(0-3)，由点集Plk计算旋转后的点集P’lk。通过Plk与P’lk计算距离平方和值为fk+1。以连续两次距离平方和之差绝对值 作为迭代判断数值；
7） 当 时，ICP配准算法就停止迭代，否则重复1至6步，直到满足条件 后停止迭代。
二、ICP搜索最近点的主要方法
1. Point to Point最近点搜索法
Point to Point最近点搜索法是ICP算法中最经典的一种方法。如图1a所示, Point to Point法根据源曲面上的一个点p，在目标曲面上找出对应于p点距离最近的q点。在这个方法中通常运用kd-tree的方法实现最近点搜索。如图1b所示，pi是源曲面点云数据中的一个点，Vi是生成目标曲面点云数据中距Pi最近的点。根据Vi点搜索出在曲面上与Vi点相邻的点构成的三角形格网，计算pi点投影到每个三角形平面上的投影点qi的坐标。对于每个三角形来说，当投影点qi位于三角形内部，则距离最近点是搜索的最近点，当投影点qi位于三角形外部，搜索的最近点应位于三角形的两条边界上，Vi是该三角形到pi点的最近距离点。将每个三角形确定的最近距离点进行比较可获得一个最近点。
2. Point to Plane最近点搜索算法
如图1c所示，Point to Plane法是根据源曲面上的一个点p，在目标曲面上找出对应于p点一个最近的q点。搜索算法是根据源曲面上p点的切平面的法线，确定发现于目标曲面的交点q’。根据目标曲面上q’点求出的过q’点切平面，然后求源曲面上p点到过q’点切平面的垂线的交点q。
3. Point to Projection最近点搜索算法
Point to Projection最近点搜索法是一种快速的配准方法。如图1-d所示，图中Oq是扫描目标曲面的透视点的位置。Point to Projection法是根据源曲面上的一个点p和透视点Oq，在目标曲面上找出q点作为对应于p点的最近点。通过确定Oq点向p点方向的投影线与目标曲面的交点q，作为搜索的最近点。
ICP最近点搜索图
图1 ICP算法最近点搜索图
[1] B. K. P. Horn, B. G. Schunk. Determing optical flow[J]. Artificial Intelligence, 1987. 17: 185-203.
[2] K. S. Arun, T. S Huang, S. D. Blostein. Least-squares fitting of two 3-D point sets[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1987. 9(5): 698-700.
[3] Paul J. Besl, Neil D. McKay. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992. 14(2): 239-256.
[4] Y. Chen, G. Medioni. Object Modeling by Registration of Multiple Range Images[J]. Image and Vision Computing, 1992. 10: 145-155.
[5] R. Bergevin, M Soucy, H. Gagnon, et al. Towards a general multi-view registration technique[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1996. 18(5): 540-547.
[6] Andrew Edie Johnson, Sing Bing Kang. Registration and Integration of Textured 3D Data[J]. Image and Vision Computing, 1999. 17: 135-147.
[7] Natasha Gelfand, Leslie Ikemoto, Szymon Rusinkiewicz, et al. Geometrically Stable Sampling for the ICP Algorithm[A]//. Proceedings of Fourth International Conference on 3-D Digital Imaging and Modeling[C]. Stanford University, CA, USA, 2003: 260-267.
[8] 郑德华. ICP算法及其在建筑物扫描点云数据配准中的应用[J]. 测绘科学, 2007. 32(2).
