PCL 之 ICP 算法实现

ICP是什么？

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

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

PCL+ICP

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

1. 对PCL中ICP算法进行一些注解
2. 创建可视化窗口，通过设置键盘回调函数，控制迭代过程，观察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