ICP算法 朱珠

迭代最近点算法（ICP） 在20世纪80年代中期，很多学者开始对点集数据的配准进行了大量研究。1987年，Horn、Arun等人用四元数法提出点集对点集配准方法。这种点集与点集坐标系匹配算法通过实践证明是一个解决复杂配准问题的关键方法。1992年，计算计视觉研究者Besl和Mckay介绍了一种高层次的基于自由形态曲面的配准方法，也称为迭代最近点法ICP（Iterative Closest Point）。以点集对点集（PSTPS）配准方法为基础，他们阐述了一种曲面拟合算法，该算法是基于四元数的点集到点集配准方法。从测量点集中确定其对应的最近点点集后，运用Faugera和Hebert提出的方法计算新的最近点点集。用该方法进行迭代计算，直到残差平方和所构成的目标函数值不变，结束迭代过程。ICP配准法主要用于解决基于自由形态曲面的配准问题。它直接对深度图像进行无关表面的三维数据处理，而且不需要对物体的特征进行假设和分割，所以很快就成为深度图像配准的一种主流算法，取得了非常广泛的应用。

深度图像配准 问题描述： 为了获得被扫描物体的多幅深度图像数据，一般做法是固定扫描物体，改变激光扫描仪的位置或者固定扫描仪改变目标物体的位置。不管采用哪种方式，从逻辑上讲，都可以认为是目标物体不动，而扫描仪位置发生改变，也就是深度图像的成像坐标系（即局部坐标系）发生了旋转和平移变换。视点之间的这种旋转和平移变换关系参数台球之为视点或深度图像的运动参数。一般而言，运动参数的获得主要是利用重叠区域的数据来进行估计，准确地计算运动参数就是深度图像配准的核心任务。

ICP算法的发展 迭代最近点法ICP最近点法经过十几年的发展，不断地得到了完善和补充。Chen和Medioni及Bergevin等人提出了point-to-plane搜索最近点的精确配准方法。Rusinkiewicz和Levoy提出了point-to-p rojection搜索最近点的快速配准方法。Soon-Yong和Murali提出了Contractive-projection-point搜索最近点的配准方法。此外，Andrew和Sing提取了基于彩色三维扫描数据点纹理信息的数据配准方法，主要在ICP算法中考虑三维扫描点的纹理色彩信息进行搜索最近点。Natasha等人分析了ICP算法中的点云数据配准质量问题。

基本原理 三维空间R3存在两组含有n个坐标点的点集PL和PR，分别为： 上式中，R为三维旋转矩阵，t为平移向量。 在ICP配准方法中，空间变换参数向量X可表示为： 参数向量中四元数参数满足约束条件为： 根据迭代的初值X0，由式(0-1)计算新点集Pi为： 式中，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简述

ICP算法的局限性 首先，该算法假设其中一个表面是另一个的子集，也就是说，只有一个表面含在第二个表面中，这一要求很多时候难以满足。 其实，该算法在寻找对应点的过程中，其计算代价是非常大的。最坏情况下为O（NpNx） 第三，ICP算法在对应点寻找的时候，使用的一个基本假设是，欧氏距离最近的点就是对应点，从某种意义上说，这个判断是武断的，它会产生一定量的错误以点。 此外，ICP要取得精确的配准结果，需要好的初始运动参数假设。

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。

Point to Projection最近点搜索算法 Point to Projection最近点搜索法是一种快速的配准方法。如图1-d所示，图中Oq是扫描目标曲面的透视点的位置。Point to Projection法是根据源曲面上的一个点p和透视点Oq，在目标曲面上找出q点作为对应于p点的最近点。通过确定Oq点向p点方向的投影线与目标曲面的交点q，作为搜索的最近点。

　图1 ICP算法最近点搜索图