ICP点云配准算法_icp点云配准算法实现

ICP算法介绍

ICP（Iterative Closest Point），即最近点迭代算法，是最为经典的数据配准算法。其特征在于，通过求取源点云和目标点云之间的对应点对，基于对应点对构造旋转平移矩阵，并利用所求矩阵，将源点云变换到目标点云的坐标系下，估计变换后源点云与目标点云的误差函数，若误差函数值大于阀值，则迭代进行上述运算直到满足给定的误差要求.

建立对应点对的点云集：

最小二乘估计最优变换矩阵：


ICP算法采用最小二乘估计计算最优变换矩阵，目标函数E(R,t)的优化是ICP算法的最后一个阶段。在求得目标函数后，采用什么样的方法来使其收敛到最小，也是一个比较重要的问题。求解方法有基于奇异值分解的方法、牛顿法等。ICP算法重复进行“确定对应关系的点集→计算最优刚体变换”的过程，直到某个表示正确匹配的收敛准则得到满足。

ICP算法流程图

ICP算法优缺点与改进

ICP算法的优点

1. 如果初始值合适， 可以获得不错的配准效果
2. 不必对处理的点云集进行分割和特征提取
3. 在较好的变换矩阵初值情况下，可以得到很好的算法收敛性

ICP算法的不足

1. 在搜索对应点的过程中，计算量非常大，速度慢，这是传统ICP算法的瓶颈
2. 对待配准点云的初始位置有一定要求，若所选初始位置不合理，则会导致算法陷入局部最优
3. 标准ICP算法中寻找对应点时，认为欧氏距离最近的点就是对应点。这种假设有不合理之处，会产生一定数量的错误对应点

ICP算法的改进

基于ICP的不足，许多研究者提出了各种改进版本，主要有如下几个方面：点云集中点的过滤（采样）、对应点云的匹配方式、匹配点对的加权、匹配点对的筛选、点云转换后目标函数的选择和收敛。

• 点云采样：
  标准的ICP算法选用所有的点来计算对应点，但通常用于配准的点集元素数量非常大，为了缩减，选用最少的点来表征原始点集的全部特征信息，点集选取时，可以采用如下方式：1. 均匀采样（Uniform sub-sampling ）；2.随机采样（Random sampling）；3.基于特征采样（Feature based Sampling ）；4.法向空间均匀采样（Normal-space sampling）
  法向采样保证了法向上的连续性，如图：
  
  基于特征的采样使用一些具有明显特征的点集来进行配准，大量减少了对应点的数目，提高了效率和精确度，但要求点云预处理：
  

  • 点集匹配方式：
    最近邻点（Closest Point），稳定，但速度慢；
    
    法方向最近邻点（Normal Shooting），平滑曲面效果好，但对噪音敏感
    
    投影法（Projection），搜索对应点速度快
    

  • 加快搜索效率
    用K-D树或Voronoi图加快最近点搜索效率。

  • 损失（目标）函数
    可以采用不同距离量测方式来定义损失函数，有点到点（point-to-point）、点到线（point-to-line）、点到面(point-to-plane)和面到面(plane-to-plane)的方式。

ICP算法PCL库实现

PCL点云库已经实现了多种点云配准算法，ICP算法是基于SVD（Singular Value Decomposition）实现的，使用PCL的ICP算法之前要设置的参数：
1. setMaximumIterations，最大迭代次数
2. setEuclideanFitnessEpsilon，设置收敛条件是均方误差和小于阈值，停止迭代
3. setTransformtionEpsilon, 设置两次变化矩阵之间的差值（一般设置为1e-10即可）
4. setMaxCorrespondenaceDistance, 设置对应点对之间的最大距离（此值对配准结果影响较大）
PCL库的ICP实现链接：
http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp