ICP算法实现

—————————-【转自：SimpleTriangle 】————————————-

ICP是什么？

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

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

PCL+ICP

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

对PCL中ICP算法进行一些注解
创建可视化窗口，通过设置键盘回调函数，控制迭代过程，观察ICP算法的计算过程
PCL中ICP的官方参考文档 http://pointclouds.org/docume…

具体代码实现

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

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> 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<pcl::visualization::PCLVisualizer> 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<pcl::PointXYZ> sources_cloud_color(cloud_in,250,0,0); //设置源点云的颜色为红色view->addPointCloud(cloud_in,sources_cloud_color,"sources_cloud_v1",v1);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> 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::PointCloudColorHandlerCustom<pcl::PointXYZ>aligend_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 <<"has conveged:"<<icp.hasConverged()<<"score:"<<icp.getFitnessScore()<<endl;                cout<<"matrix:\n"<<icp.getFinalTransformation()<<endl;                cout<<"iteration = "<<++iterations;                /*... 如果icp.hasConverged=1,则说明本次配准成功，icp.getFinalTransformation()可输出变换矩阵   ...*/                if (iterations == 1000)  //设置最大迭代次数                        return 0;                view->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 <iostream>#include <pcl/io/pcd_io.h>#include <pcl/point_types.h>#include <pcl/registration/icp.h>#include <pcl/visualization/pcl_visualizer.h>#include <boost/thread/thread.hpp>#include <pcl/console/parse.h>  //pcl控制台解析

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

ICP之变种

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

pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget > Class Template Reference
pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference
pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar > Class Template Reference
pcl::IterativeClosestPointNonLinear< PointSource, PointTarget, Scalar > Class Template Reference
pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference
pcl::registration::IncrementalICP< PointT, Scalar > Class Template Reference