PCL 之 ICP 算法实现

浏览: 203 发布日期: 2016-08-17 分类: emacs

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#interactive-icp

具体代码实现

首先看一下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-plain的,一般后者的计算速度快一些,是基于法向量的,需要输入数据有较好的法向量,具体使用时建议根据自己的需要及可用的输入数据选择具体方法。

返回顶部