《 无人驾驶原理与实践》一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的实例代码
将上述代码组合,添加相应头文件。
根据不同模型设置合适的参数,便可实现最初目的。实例展示如图33所示。
其中,左边图像用来显示初始位置,右边图像用以显示配准过程。设置源点云的颜色为红色代码生成彩图无法展示,见本书中彩插,有需要的读者也可通过华章网站(wwwhzbookcom)下载图片。本书后同。,目标点云为绿色,将点云添加到初始窗口,设置配准结果为白色。通过图33可以看到ICP算法实现逐渐配准的计算过程。
注意 ICP有很多变种,包括pointtopoint、pointtoplain,一般情况下基于法向量的pointtoplain计算速度快一些,它需要输入数据有较好的法向量,具体使用时可根据自己的需要及可用的输入数据选择最合适的方法。
- 点赞
- 收藏
- 关注作者
评论(0)