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

举报
华章计算机 发表于 2019/06/02 22:01:54 2019/06/02
【摘要】 本书摘自《 无人驾驶原理与实践》一书中的第二章,第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算法的主要应用代码。

image.png

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

image.png

image.png

image.png


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

image.png

image.png

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

image.png

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

image.png

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

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


【版权声明】本文为华为云社区用户转载文章,如果您发现本社区中有涉嫌抄袭的内容,欢迎发送邮件进行举报,并提供相关证据,一经查实,本社区将立刻删除涉嫌侵权内容,举报邮箱: cloudbbs@huaweicloud.com
  • 点赞
  • 收藏
  • 关注作者

评论(0

0/1000
抱歉,系统识别当前为高风险访问,暂不支持该操作

全部回复

上滑加载中

设置昵称

在此一键设置昵称,即可参与社区互动!

*长度不超过10个汉字或20个英文字符,设置后3个月内不可修改。

*长度不超过10个汉字或20个英文字符,设置后3个月内不可修改。