《 无人驾驶原理与实践》一3.2迭代最近点算法

3.2迭代最近点算法

       目前三维配准中最常用的迭代算法就是迭代最近点（ICP）算法，这种算法需要提供一个较好的初值，同时由于算法本身缺陷，最终迭代结果可能会陷入局部最优，而不是全局最优。
       ICP算法原理：给定参考点集P和数据点集Q（在给定初始估计的旋转矩阵R和平移向量T的条件下），对Q中的每一个点寻找P中的对应最近点，构成匹配点对，全部匹配点对的欧氏距离之和作为误差目标函数error的值，利用SVD（Singular Value Decomposition，奇异值分解）求出新的R和T，使得error最小，将Q按照R和T做旋转变化，并以此为基准再次计算，重新寻找对应点对。
       缺点：要剔除噪声点（点对距离过大或者包含边界点的点对）；基于点对的配准没有包含局部形状的信息；每次迭代都要搜索最近点，耗时大；可能陷入局部最优。
       具体代码实现：首先看一下点云库（PCL）中ICP算法的主要应用代码。

    需要说明的是：
    1）PCL中的ICP算法是基于SVD实现的。
    2）使用PCL的ICP之前需要设置几个参数：
     ●   setMaximumIterations，最大迭代次数（若结合可视化并逐次显示，可将次数设置为1）。
     ●   setEuclideanFitnessEpsilon，设置收敛条件是均方误差和小于阈值，则停止迭代。
     ●   setTransformtionEpsilon，设置两次变化矩阵之间的差值（一般设置为1e-10）。
     ●   setMaxCorrespondenaceDistance，设置对应点对之间的最大距离（此值对配准结果影响较大）。
       如果仅仅运行上述代码并设置合理的预估计参数，便可实现利用ICP对点云数据进行配准计算。为了更深入地了解ICP的计算过程，我们继续添加如下代码。

                                                                                 代码清单3-3添加ICP的实例代码

                                                                       将上述代码组合，添加相应头文件。

         根据不同模型设置合适的参数，便可实现最初目的。实例展示如图33所示。

       其中，左边图像用来显示初始位置，右边图像用以显示配准过程。设置源点云的颜色为红色代码生成彩图无法展示，见本书中彩插，有需要的读者也可通过华章网站（wwwhzbookcom）下载图片。本书后同。，目标点云为绿色，将点云添加到初始窗口，设置配准结果为白色。通过图33可以看到ICP算法实现逐渐配准的计算过程。

       注意       ICP有很多变种，包括pointtopoint、pointtoplain，一般情况下基于法向量的pointtoplain计算速度快一些，它需要输入数据有较好的法向量，具体使用时可根据自己的需要及可用的输入数据选择最合适的方法。