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

向右看齐 2022-12-21 06:14 936阅读 0赞

**目录**

基本介绍

详细介绍

参数输入输出

成员函数

一个例子

--------------------

# 基本介绍 #

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 area
        double 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_-]
 *  [PCL中的点云配准（Registration）ICP算法][PCL_Registration_ICP]

[PCL_-]: https://zhuanlan.zhihu.com/p/30327449
[PCL_Registration_ICP]: https://blog.csdn.net/wokaowokaowokao12345/article/details/73741957