PCL 之 ICP 算法实现

ICP是什么?

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

ICP算法采用最小二乘估计计算变换矩阵,原理简单且具有较好的精度,但是由于采用了迭代计算,导致算法计算速度较慢,而且采用ICP进行配准计算时,其对待配准点云的初始位置有一定要求,若所选初始位置不合理,则会导致算法陷入局部最优。

PCL+ICP

PCL点云库已经实现了多种点云配准算法,结合pcl,本次配准的主要目的是:

  1. 对PCL中ICP算法进行一些注解

  2. 创建可视化窗口,通过设置键盘回调函数,控制迭代过程,观察ICP算法的计算过程

PCL中ICP的官方参考文档 http://pointclouds.org/documentation/tutorials/interactive_icp.php

具体代码实现

首先看一下pcl中ICP的主要代码:

pcl::IterativeClosestPoint icp;  //创建ICP的实例类 icp.setInputSource(cloud_sources); icp.setInputTarget(cloud_target); icp.setMaxCorrespondenceDistance(100);   icp.setTransformationEpsilon(1e-10);  icp.setEuclideanFitnessEpsilon(0.001);  icp.setMaximumIterations(100);    icc.align(final);

需要说明的是:

其一:PCL中的ICP算法是基于SVD(Singular Value Decomposition)实现的.

其二:使用pcl的ICP之前要set几个参数:

  1. setMaximumIterations, 最大迭代次数,icp是一个迭代的方法,最多迭代这些次(若结合可视化并逐次显示,可将次数设置为1);

  2. setEuclideanFitnessEpsilon, 设置收敛条件是均方误差和小于阈值, 停止迭代;

  3. setTransformtionEpsilon, 设置两次变化矩阵之间的差值(一般设置为1e-10即可);

  4. setMaxCorrespondenaceDistance,设置对应点对之间的最大距离(此值对配准结果影响较大)。

如果仅仅运行上述代码,并设置合理的的预估计参数,便可实现利用ICP对点云数据进行配准计算.

其次为了更深入的了解ICP的计算过程,即本试验的第二个目的,继续添加以下代码:

boost::shared_ptr view(new pcl::visualization::PCLVisualizer("icp test"));  //定义窗口共享指针int v1 ; //定义两个窗口v1,v2,窗口v1用来显示初始位置,v2用以显示配准过程int v2 ;view->createViewPort(0.0,0.0,0.5,1.0,v1);  //四个窗口参数分别对应x_min,y_min,x_max.y_max.view->createViewPort(0.5,0.0,1.0,1.0,v2);pcl::visualization::PointCloudColorHandlerCustom sources_cloud_color(cloud_in,250,0,0); //设置源点云的颜色为红色view->addPointCloud(cloud_in,sources_cloud_color,"sources_cloud_v1",v1);pcl::visualization::PointCloudColorHandlerCustom target_cloud_color (cloud_target,0,250,0);  //目标点云为绿色view->addPointCloud(cloud_target,target_cloud_color,"target_cloud_v1",v1); //将点云添加到v1窗口view->setBackgroundColor(0.0,0.05,0.05,v1); //设着两个窗口的背景色view->setBackgroundColor(0.05,0.05,0.05,v2);view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2,"sources_cloud_v1");  //设置显示点的大小view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2,"target_cloud_v1");pcl::visualization::PointCloudColorHandlerCustomaligend_cloud_color(Final,255,255,255);  //设置配准结果为白色view->addPointCloud(Final,aligend_cloud_color,"aligend_cloud_v2",v2);view->addPointCloud(cloud_target,target_cloud_color,"target_cloud_v2",v2);view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2,"aligend_cloud_v2");view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2,"target_cloud_v2");view->registerKeyboardCallback(&keyboardEvent,(void*)NULL);  //设置键盘回调函数int iterations = 0; //迭代次数while(!view->wasStopped()){        view->spinOnce();  //运行视图        if (next_iteration)        {                icp.align(*Final);  //icp计算                cout updatePointCloud(Final,aligend_cloud_color,"aligend_cloud_v2");        }        next_iteration = false;  //本次迭代结束,等待触发}

最后还需要设置以下键盘回调函数,用以控制迭代进程:

bool next_iteration = false;//设置键盘交互函数void keyboardEvent(const pcl::visualization::KeyboardEvent &event,void *nothing)    {            if(event.getKeySym() == "space" && event.keyDown())                    next_iteration = true;    }    /*... 上述函数表示当键盘空格键按下时,才可执行ICP计算 ... */

实例练习

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

# include # include # include # include # include # include # include   //pcl控制台解析

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

ICP之变种

ICP有很多变种,有point-to-point的,也有point-to-plain的,一般后者的计算速度快一些,是基于法向量的,需要输入数据有较好的法向量,具体使用时建议根据自己的需要及可用的输入数据选择具体方法。

  1. pcl::GeneralizedIterativeClosestPoint Class Template Reference

  2. pcl::IterativeClosestPoint Class Template Reference

  3. pcl::IterativeClosestPointWithNormals Class Template Reference

  4. pcl::IterativeClosestPointNonLinear Class Template Reference

  5. pcl::JointIterativeClosestPoint Class Template Reference

  6. pcl::registration::IncrementalICP Class Template Reference

c++# linux, ubuntu, emacs

版权声明

本文来自互联网用户投稿,文章观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处。如若内容有涉嫌抄袭侵权/违法违规/事实不符,请点击 举报 进行投诉反馈!

相关文章

立即
投稿

微信公众账号

微信扫一扫加关注

返回
顶部