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

版权声明

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