PCL ICP配准【点到面】（Registration_ICP）

PCL专栏目录及须知

点到点的ICP算法同点到面的ICP算法基本原理相同。只是点对点是计算源点云的点到目标点云相应点对之间的距离，点对面是计算源点云的点到目标点云相应点对所在平面之间的距离。

ICP算法解释

1.解释

ICP算法的基本思想是通过迭代的方式寻找两个点云之间的最佳刚体变换，使得它们之间的对应点之间的距离最小化。点到面的ICP算法在计算对应点之间的距离时，计算目标点云的每个点到参考点云的所在局部平面的距离，从而提高配准的准确性。

区别如下图所示：

使用点到平面（point-plane）误差度量的迭代最近点ICP算法已被证明比使用点到点（point-point）误差度量的算法收敛得更快。

2.关键函数

（1）为终止条件设置最小转换差异，即最优点对距离的阈值（本值一般都设很小）

registration.setTransformationEpsilon(1e-10);							// 为终止条件设置最小转换差异

（2）设置对应点对之间的最大距离，即pcl::registration::CorrespondenceEstimation调用determineCorrespondences(*correspondences, dis)时的dis的值。

registration.setMaxCorrespondenceDistance(1.5);							// 设置对应点对之间的最大距离

（3）设置收敛条件是均方误差和小于阈值， 停止迭代；

registration.setEuclideanFitnessEpsilon(0.01);

（4）最大迭代次数

registration.setMaximumIterations(500);

3.完整代码

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

void cloudNormal(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointNormal>::Ptr& cloud_normals)
{
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;//OMP加速
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    n.setNumberOfThreads(8);
    n.setInputCloud(cloud);
    n.setSearchMethod(tree);
    n.setKSearch(10);
    n.compute(*normals);
    pcl::concatenateFields(*cloud, *normals, *cloud_normals);       // 拼接点云及法向量
}

int main()
{
    /****************ICP配准********************/
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);			// 源点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);			// 目标点云
    pcl::io::loadPCDFile("D:/code/csdn/data/person2.pcd", *source);
    pcl::io::loadPCDFile("D:/code/csdn/data/person3.pcd", *target);
    pcl::PointCloud<pcl::PointNormal>::Ptr result(new pcl::PointCloud<pcl::PointNormal>);			// 目标点云转换后的结果

    // ICP配准
    // 计算点云法向量
    pcl::PointCloud<pcl::PointNormal>::Ptr sourceNormals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::PointCloud<pcl::PointNormal>::Ptr targetNormals(new pcl::PointCloud<pcl::PointNormal>);
    cloudNormal(source, sourceNormals);
    cloudNormal(target, targetNormals);

    pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal> registration;
    registration.setInputSource(sourceNormals);
    registration.setInputTarget(targetNormals);
    registration.setTransformationEpsilon(1e-10);                       // 为终止条件设置最小转换差异
    registration.setMaxCorrespondenceDistance(1.5);                     // 设置对应点对之间的最大距离（此值对配准结果影响较大）。
    registration.setEuclideanFitnessEpsilon(0.01);                      // 设置收敛条件是均方误差和小于阈值， 停止迭代；
    //registration.setUseSymmetricObjective(false);                       // 设置为true则变为另一个算法
    registration.setMaximumIterations(500);                             // 最大迭代次数
    registration.align(*result);

    std::cout << "点对匹配分数：" << registration.getFitnessScore() << std::endl;
    std::cout << "刚体变换矩阵：" << registration.getFinalTransformation() << std::endl;

    /****************展示********************/
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("ICP"));
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(target, 255, 0, 0);		// 目标点云
    viewer->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal>result_color(result, 0, 255, 0);		// 配准结果点云
    viewer->addPointCloud<pcl::PointNormal>(result, result_color, "result cloud");

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    return (0);
}

4.结果展示

（1）待配准点云

（2）配准点云结果

二者完全合并到一起了，所以看起来是一个点云。

​​​​​​​