【泡泡点云时空-PCL源码解读】ICP点云精配准算法

泡泡点云时空，带你剖析PCL点云库 

来源：PCL库

编辑：黄玉玺

审核：徐二帅，吕佳俊

欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权

介绍

PCL（Point Cloud Library  http://pointclouds.org/）是点云开发开源库。PCL是在吸收了前人点云相关研究基础上建立起来的大型跨平台开源C++编程库，它实现了大量点云相关的通用算法和高效数据结构，涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。支持多种操作系统平台，可在Windows、Linux、Android、Mac OS X、部分嵌入式实时系统上运行。如果说OpenCV是2D信息获取与处理的结晶，那么PCL就在3D信息获取与处理上具有同等地位，PCL是BSD授权方式，可以免费进行商业和学术应用。

ICP（Iterative Closest Point）算法是点云配准中最常用到的算法之一，PCL点云库集成了该算法，本文将对PCL中ICP算法的源码实现进行解析。

ICP算法原理

ICP算法是求解两个3D点云集之间刚体变换关系的一种算法。如图1所示，黑色为点云A，绿色为点云B，ICP算法即为求出刚体变换T，使点云A经过T变换后与点云B重合。该问题最核心的部分在于如何定义两个点云集“重合”。ICP将“重合”定义为：点集A中每一个点距离点集B中最近点的距离之和最小。通常，不同的定义方式产生了不同的ICP方法，如标准ICP，Normal-ICP，GICP等。

图1 ICP匹配示意图

ICP算法步骤：

(1)对点集A中每一个点Pai施加初始变换T0，得到Pai’；

(2)从点集B中寻找距离点Pai’最近的点Pbi，形成对应的点对；

(3)求解最优变换：

其中，

(4)根据前后两次的迭代误差以及迭代次数等条件判断是否收敛。如果收敛，则输出最终结果:T=△T*T0，否则T0=△T*T0,重复步骤(1)；

图2 ICP 匹配过程示意图 红色为源点云，绿色为目标点云；最左边的图为匹配前的初始状态；中间为寻找近邻的过程；最右边为匹配完成的状态；

ICP源码解析

I. PCL中常用ICP接口

pcl::IterativeClosestPoint<PointT, PointT> icp;

//设定最大迭代次数，超过此次数ICP终止
icp.setMaximumIterations (iterations);

//输入源点云，即被转换的点云
icp.setInputSource (cloud_icp);

//输入目标点云，即参考点云
icp.setInputTarget (cloud_in);

//设置查找近邻时是否双向搜索

icp.setUseReciprocalCorrespondences(false);

//设置最大近邻点距离，超过此距离的点对将不参与计算

icp.setMaxCorrespondenceDistance(0.5);

//设置匹配对剔除器

icp.addCorrespondenceRejector(rejector);

//设置迭代误差收敛阈值

icp.setEuclideanFitnessEpsilon(0.0001);

//设置ICP计算方法，可以为SVD、LM…

icp.setTransformationEstimation(te);

//执行运算，ICP的主要运算都在align函数中
icp.align (*cloud_icp);
//获取ICP是否收敛
icp.hasConverged ();
//获取ICP匹配得到的变换矩阵T，
Eigen::Matrix4f transformation_matrix = icp.getFinalTransformation ();

//获取ICP匹配得分

icp.getFitnessScore();

上面是使用PCL ICP时常用的接口，标粗的部分为大家可能不常用但是会比较有用的接口；

II. align函数实现分析

align()函数是ICP算法的父类Registration类定义的一个函数，来实现配准算法的统一接口调用。align()函数中又调用了computeTransformation()函数，来具体实现变换矩阵的解算方法。由于函数实现太长，为节省篇幅，拿出computeTransformation()函数中最重要的几步来分析。

1.transformCloud(*input_,*input_transformed,guess);

使用该函数将源点云input_根据初始变换估计矩阵guess进行坐标转换得到坐标转换后的点云 “input_transformed”。这个初始的估计矩阵guess默认为单位矩阵，可以由用户自己指定，因为align函数有一个重载的原型为：

void align(PointCloudSource &output, const Matrix4& guess);

(2)correspondence_estimation_->

determineCorrespondences(*correspondences_, corr_dist_threshold_);

利用kdtree寻找近邻点建立对应关系。即从target点云中搜索距离source点云中每个点最近的点，形成一个对应的点集。

这里PCL还给出了另一种实现方法，即：

correspondence_estimation_->

determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);

即双向搜索方法，假设Ps是source点云中的一点，Pt是target点云距离Ps最近的点,那么同时会在source点云中搜索距离Pt最近的点，如果也为Ps那么该对应关系确立，否则不成立。

可以通过调用void

setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)函数确定是否启用，true为启用；

(3)rej->getCorrespondences (*correspondences_);

若用户自己设置了对应关系剔除器，则根据用户设置的剔除器，剔除不满足条件的对应点对，常用的剔除方法是根据对应点对的法向量夹角剔除，当法向量夹角超过某个阈值时，则剔除此对应关系；

(4)transformation_estimation_->

estimateRigidTransformation(*input_transformed, *target_, *correspondences_, transformation_);

根据对应关系计算变换矩阵T。PCL种提供了很多种变换矩阵的计算方式，比如SVD分解，LM优化，还有对于Normal-ICP的Point-to-Plane方法。通过翻阅源码发现标准ICP默认是使用SVD分解求解得到的，因为构造函数中的初始化如下：

transformation_estimation_.reset(new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar> ());

当然用户可以根据自己的需要使用不同的ICP解算方法，只要设置函数void 

setTransformationEstimation(const TransformationEstimationPtr &te)即可。

例如要使用LM方法求解ICP，则需要使用以下语句：

pcl::registration::TransformationEstimationLM<Pt,Pt,float>::Ptr te_lm_（new pcl::registration::

TransformationEstimationLM<Pt,Pt,float>）;

icp.setTransformationEstimation(te_lm_);

其中，Pt代表点类型。

（关于SVD求解ICP的推导会在后续的文章中介绍。）

(5)final_transformation_=

transformation_ * final_transformation_;

将计算得到的变换矩阵transformation叠加到上次结果中，这样不断迭代，不断更新结果。

(6)converged_ = 

static_cast<bool> ((*convergence_criteria_));

判断是否收敛。pcl提供判断收敛的几个条件以及先后顺序为:是否超过最大迭代次数，判断前后两次迭代的旋转以及平移差是否小于某个阈值、最后是判断前后两次迭代“平均距离”之差是否小于某个阈值（这里的“平均距离”是指：对应点对之间距离的平均值）。

（为了方便同学们把玩测试，将PCL ICP的部分源码摘抄出来编成一个工程，放到了git上:

https://github.com/hyx007/paopao_ws/tree/master/icp_example，感兴趣的同学可以下载测试。）

III. 划重点

(1)PCL ICP是支持输入初始估计矩阵的，只需要在align()函数中添加即可；

(2)用户可以根据自己的需要选择不同的ICP求解方法，只需要设置void 

setTransformationEstimation(const TransformationEstimationPtr &te)函数即可。

(3)通过getFitnessScore()得到的ICP得分实际是算出的点云对之间的“平均距离”，即得分越小越好，得分越大说明匹配的越不好；

(4)可以通过void 

addCorrespondenceRejector(const CorrespondenceRejectorPtr &rejector);函数设置外点剔除方法；以及通过void

setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)；函数确定是否需要双向查找近邻，以增强ICP鲁棒性；

(5) ICP计算的是把source点云转换到target点云所在坐标系的变换矩阵，即默认target点云所在坐标系为参考坐标系；

匹配结果示例

图3 ICP匹配结果示例1

（左图中，红色为由随机产生的1000个点组成的源点云，绿色为对源点云进行刚体变换生成的目标点云；右图中为源点云ICP变换后的点云与目标点云）

图4 ICP匹配结果示例2

（左图中，红色激光雷达对某个房间扫描得到源点云，绿色为对源点云进行刚体变换生成的目标点云；右图中为源点云ICP变换后的点云与目标点云）

图5 ICP匹配结果示例3

（左图中，红色激光雷达对某个房间扫描得到源点云，绿色的点云为隔一段时间后，激光雷达对再次同一个房间扫描得到的点云；右图中为源点云ICP变换后的点云与目标点云）

由图3、图4可以看到当源点云与目标点云相似度比较高时ICP匹配可以得到很好的效果；图5由于扫描得到目标点云时房间内物品的位置发生了变化，导致ICP没有收敛到真值附近。可见ICP算法也有它相应的不足之处，即当环境变化比较大时，容易导致匹配误差较大（当然，图5实验时并未使用外点剔除器，设置相应的外点剔除器，可能会得到更优的结果），因此也产生了其它变种的ICP，如Normal-ICP,GICP,以及NDT等算法。

如果你对本文感兴趣，想要持续关注该领域文章，可以关注【泡泡机器人SLAM】公众号。

欢迎来到泡泡论坛，这里有大牛为你解答关于SLAM的任何疑惑。

有想问的问题，或者想刷帖回答问题，泡泡论坛欢迎你！

泡泡网站：www.paopaorobot.org

泡泡论坛：http://paopaorobot.org/forums/

泡泡机器人SLAM的原创内容均由泡泡机器人的成员花费大量心血制作而成，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！

商业合作及转载请联系liufuqiang_robot@hotmail.com