基本介绍

ICP匹配，中文应该叫临近点迭代吧，是计算机图形学中的一个非常有用的算法，尤其是在三维重建点云配准中有着极其重要的地位，此外在SLAM等移动机器人导航等领域也有着很大的用武之地。经过了十多年的发展ICP也有着很多的变种，今天我们首先熟悉下最基本的ICP匹配算法，PCL中的实现与参考文献中的一致，最终的变换矩阵都是基于SVD（奇异值分解）的。

在具体的实践中，一共有3个约束来终止迭代：

迭代次数，默认值为10；
上次转换与当前转换的差值；
前后两次迭代方差的差值。

当然我们还可以使用kdtree加速算法。

还有一点需要重点提示下，输入的点云需要经过预处理，过于复杂和过多噪声的点与将会出现“Invalid (NaN, Inf) point coordinates given to nearestKSearch!”的错.不同版本的PCL函数命名可能不同，函数个数可能也不一样。

ICP算法基于SVD，其大致思路如下:

(1) 将初始配准后的两片点云P′(经过坐标变换后的源点云)和Q，作为精配准的初始点集；
(2) 对源点云P’中的每一点pi，在目标点云Q中寻找距离最近的对应点qi，作为该点在目标点云中的对应点，组成初始对应点对；
(3) 初始对应点集中的对应关系并不都是正确的，错误的对应关系会影响最终的配准结果，采用方向向量阈值剔除错误的对应点对；
(4) 计算旋转矩阵R和平移向量T，使最小，即对应点集之间的均方误差最小；
(5) 设定某一阈值ε=dk-dk-1和最大迭代次数Nmax, 将上一步得到的刚体变换作用于源点云P′，得到新点云P”，计算P”和Q的距离误差,，如果两次迭代的误差小于阈值ε或者当前迭代次数大于Nmax，则迭代结束，否则将初始配准的点集更新为P”和Q，继续重复上述步骤，直至满足收敛条件。

详细介绍

参数输入输出

此类由基类Registration派生( #include <pcl/registration/icp.h> )，生成对象方式也很简单，如下：

Pcl:: IterativeClosestPoint icp

成员函数

这里我就不一一介绍所有的成员函数了，只是把几个非常重要的成员函数给列出来，并给出其的使用方法：

最基本的函数:
- inline void setInputSource (constPointCloudSourceConstPtr &cloud) 需要匹配的点云。
- inline void setInputTarget (constPointCloudTargetConstPtr &cloud) 基准点云，也就是从Source到Target的匹配。
- inline void setMaxCorrespondenceDistance (doubledistance_threshold)
  - 设置最大对应点的欧式距离，只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对。默认值为：1.7976931348623157e+308，基本上对所有点都计算了匹配点。
  - 忽略在此距离之外的点，如果两个点云距离较大，这个值要设的大一些（PCL默认距离单位是m）。
  - icp setMaxCorrespondenceDistance 宁小勿大: 小了影响时间大了就影响精度了
三个迭代终止条件设置:
- inline void setMaximumIterations (intnr_iterations)
  - 设置最大的迭代次数。迭代停止条件之一，几十上百都可能出现。
- inline void setTransformationEpsilon (doubleepsilon)
  - 设置前后两次迭代的转换矩阵的最大容差（epsilion），一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，迭代停止。迭代停止条件之二，默认值为：0
  - 这个值一般设为1e-6或者更小。
- inline void setEuclideanFitnessEpsilon (doubleepsilon)
  - 前后两次迭代误差的差值。收敛条件是均方误差和小于阈值，停止迭代。
  - 设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max ()
inline void inline void setSearchMethodTarget(const KdTreePtr &tree) kdtree加速搜索，还有一个Target的函数，用法与之一致。
inline void setRANSACOutlierRejectionThreshold(double inlier_threshold) ;
- //剔除错误估计，可用 RANSAC 算法，或减少数量
- 如果目标数target据索引和转换后的源source索引之间的距离小于给定的内部距离阈值inlier_threshold，则该方法将一个点视为一个内点
- 该值默认设置为0.05m。
inline typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr getConvergeCriteria()
- 这允许在align（）方法之后检查收敛状态，以及配置在调用align（）方法之前无法通过ICP API使用的DefaultConvergenceCriteria参数。请注意，align方法设置max_iterations_，euclidean_fitness_epsilon_和Transformation_epsilon_，因此将覆盖DefaultConvergenceCriteria实例的默认值/设置值。
inline void align (PointCloudSource &output)输出配准后点云。
inline Matrix4 getFinalTransformation () 获取最终的转换矩阵 4x4。

一个例子

目的：给定原始点云当前的RGB图像帧+深度图+相机位姿，计算出当前点云，在叠加到原始点云上。在叠加前使用ICP进行精配准。

PointCloud::Ptr Tracking::JoinPointCloudToOriginal(PointCloud::Ptr &original, const cv::Mat &imRGB, const cv::Mat &imD, cv::Mat &currentPose)
{//Eigen::Isometry3d T;PointCloud::Ptr currentCloud = Image2PointCloud( imRGB, imD);PointCloud::Ptr newCloud( new PointCloud );// 合并点云PointCloud::Ptr output (new PointCloud());Eigen::Matrix<float, 4, 4> T;cv::cv2eigen(currentPose.inv(), T);cout << "-----------   currentPose =    -----------" << endl;cout << T.matrix() << endl;pcl::transformPointCloud(*currentCloud, *output, T.matrix());PointCloud::Ptr targetCloud (new PointCloud());if(mpLastCloudforICP->size() != 0) *targetCloud = *mpLastCloudforICP;else *targetCloud = *original;std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();//ICP配准static pcl::IterativeClosestPoint<PointT, PointT> icp;PointCloud::Ptr final(new PointCloud);pcl::search::KdTree<PointT>::Ptr treeSrc(new pcl::search::KdTree<PointT>);pcl::search::KdTree<PointT>::Ptr treeTarget(new pcl::search::KdTree<PointT>);treeSrc->setInputCloud(output);treeTarget->setInputCloud(targetCloud);icp.setSearchMethodSource(treeSrc);icp.setSearchMethodTarget(treeTarget);icp.setInputSource(output);icp.setInputTarget(targetCloud);icp.setMaxCorrespondenceDistance(30); //Be careful of this value. It has a large influence to the result of ICP.icp.setMaximumIterations (50);icp.setTransformationEpsilon(1e-10);icp.setEuclideanFitnessEpsilon(0.01);icp.align(*final);std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();cout << "JoinPointCloudToOriginal:icp " << ttrack << endl;//pcl::registration::DefaultConvergenceCriteria<float>::Ptr convergenceCriteria = icp.getConvergeCriteria();std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;std::cout << icp.getFinalTransformation() << std::endl;  *mpLastCloudforICP = *final;   *newCloud = *original + *final;//*newCloud = *original + *output;t1 = std::chrono::steady_clock::now();// Voxel grid 滤波降采样static pcl::VoxelGrid<PointT> voxel;PointCloud::Ptr tmp( new PointCloud() );    // only reserve one point in every 2 cm^3 areadouble gridsize = 0.02;voxel.setLeafSize( gridsize, gridsize, gridsize );voxel.setInputCloud( newCloud );voxel.filter( *tmp );t2 = std::chrono::steady_clock::now();ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();cout << "JoinPointCloudToOriginal:VoxelGrid " << ttrack << endl;return tmp;
}

参考:

PCL入门笔记-人脸配准
PCL中的点云配准（Registration）ICP算法

PCL中的ICP算法(Registration模块之IterativeClosestPoint点云配准)相关推荐

在医学图像分析中使用ICP算法进行点云配准
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达本文转载自「计算机视觉工坊」,该公众号重点在于介绍深度学习.智能驾驶等领域,一个小众的公众号. 论文标 ...
第七周PCL学习--点云配准（七）
目录引言一.点云配准 1.1.定义 1.2.含义 1.3.配准过程 1.4.算法原理 1.5.实验二.总结三.参考引言随着计算机辅助设计技术的发展,通过实物模型产生数字模型的逆向工程技术, ...
PCL学习笔记二：Registration (ICP算法)
ICP in PCL Registration 点云配准是什么,维基百科上这样介绍: Point cloud registration, is the process of finding a spa ...
斯坦福的著名小兔子模型的点云数据_传统方法的点云分割以及PCL中分割模块
之前在微信公众号中更新了以下几个章节 1,如何学习PCL以及一些基础的知识 2,PCL中IO口以及common模块的介绍 3, PCL中常用的两种数据结构KDtree以及Octree树的介绍有兴趣 ...
ICP算法进行点云匹配
[原文:http://www.cnblogs.com/yhlx125/p/5234156.html] 上一篇:http://www.cnblogs.com/yhlx125/p/4924283.html ...
安装使用python-pcl调用ICP算法|debug
安装使用python-pcl调用ICP算法|debug 最近需要使用迭代最近点算法计算两帧二维点云数据的转换矩阵TTT,PCL库中自带ICP算法,由于当程序都是用python编写,所以安装python ...
点云配准5：4pcs算法在pcl上的实现
目录配准结果点云配准系列准备完整项目文件参数设定及说明数据参数代码结果 Bunny hippo 算法缺点参考及感谢完配准结果偶尔效果比较好,白色是目标点云0°的Bunny,紫 ...
点云配准2：icp算法在PCL1.10.0上的实现+源码解析
目录本文最后实现的配准实例点云配准系列准备程序结构主程序 1.为什么要降采样 2.体素降采样原理 3.点云更新 icp 配准前的参数设置 icp配准算法内部对应点对确定(determine ...
点云配准的传统算法ICP与NDT概述
公众号致力于分享点云处理,SLAM,三维视觉,高精地图相关的文章与技术,欢迎各位加入我们,一起交流一起进步,有兴趣的可联系微信:920177957.本文来自点云PCL博主的分享,未经作者允许请勿转载, ...
微型计算机中的pcl是指,PCL中的类
1. PCLBase pcl_base.h中定义了PCL中的基类PCLBase,PCL中的大部分算法都使用了其中的方法. PCLBase实现了点云数据及其索引的定义和访问. 两个主要的变量input_ ...

PCL中的ICP算法(Registration模块之IterativeClosestPoint点云配准)