迭代最近点（ICP）进行点云配准

最新推荐文章于 2024-05-18 00:25:42 发布

lCOODE

最新推荐文章于 2024-05-18 00:25:42 发布

阅读量7.9k

点赞数 8

分类专栏： PCL 文章标签：迭代算法库

本文链接：https://blog.csdn.net/j_shui/article/details/73356802

版权

ICP算法原理

这部分在很多博客中有说明，这里不加以复述，提供几个博客链接，有兴趣者请自行研究，本文主要介绍如何使用PCL中的ICP算法。不了解PCL库的请看链接。
参考资料1：关于配准问题
 参考资料2：ICP基本原理

PCL中ICP算法使用详解

pcl中实现了ICP算法类 IterativeClosestPoint，也提供了关于该类的使用教程DEMO，但不够具体，下面对其做写补充：
几个主要的设置：
1. setInputCloud (cloud_source) 设置原始点云
2. setInputTarget (cloud_target) 设置目标点云
3. setMaxCorrespondenceDistance（）设置最大对应点的欧式距离，只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对。默认值为：1.7976931348623157e+308，基本上对所有点都计算了匹配点。
4. 三个迭代终止条件设置：
1) setMaximumIterations（）设置最大的迭代次数。迭代停止条件之一
2) setTransformationEpsilon（）设置前后两次迭代的转换矩阵的最大容差（epsilion），一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，迭代停止。迭代停止条件之二，默认值为：0
3) setEuclideanFitnessEpsilon（）设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max ()
迭代满足上述任一条件，终止迭代。原文如下：

      * ConvergenceCriteria, and implements the following criteria for registration loop
      * evaluation:
      *
      *  * a maximum number of iterations has been reached
      *  * the transformation (R, t) cannot be further updated (the difference between current and previous is smaller than a threshold)
      *  * the Mean Squared Error (MSE) between the current set of correspondences and the previous one is smaller than some threshold (both relative and absolute tests)
      *
      * \note Convergence is considered reached if ANY of the above criteria are met.

5.getFinalTransformation () 获取最终的配准的转化矩阵，即原始点云到目标点云的刚体变换，返回Matrix4数据类型，该数据类型采用了另一个专门用于矩阵计算的开源c++库eigen。
6. align (PointCloudSource &output) 进行ICP配准，输出变换后点云
7. hasConverged（）获取收敛状态，注意，只要迭代过程符合上述三个终止条件之一，该函数返回true。
8. min_number_correspondences_ 最小匹配点对数量，默认值为3，即由空间中的非共线的三点就能确定刚体变换，建议将该值设置大点，否则容易出现错误匹配。
9. 最终迭代终止的原因可从convergence_criteria_变量中获取，

getFitnessScore（）用于获取迭代结束后目标点云和配准后的点云的最近点之间距离的均值。
值得注意的是，在PCL中，通常以两点之间的欧式距离的平方作为两点之间的距离，之所以这样设置，是为了省掉开根号计算，开根号计算是很费时的操作。

使用案例

main.cpp

#include "PointCloudProcess.hpp"
#include <pcl/console/time.h>   
int main(int argc, char *argv[])
{
    //icp匹配实验
#if 1
    pcl::console::TicToc time;
    resolution = 0.0009338;
    cv::Scalar color = cv::Scalar(0, 255, 0);
    pcl::visualization::PCLVisualizer viewer("ICP demo");
    pcl::PointCloud<PointType>::Ptr pcloud_ori(new pcl::PointCloud<PointType>()); 
    pcl::PointCloud<PointType>::Ptr pcloud_source(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr pcloud_targ