PCL 点云配准基于目标对称的ICP算法（精配准）

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1计算点云的法线

2.1.2基于对称误差估计的ICP配准

2.1.3可视化

2.2完整代码

三、实现效果

PCL点云算法汇总及实战案例汇总的目录地址链接：

PCL点云算法与项目实战案例汇总（长期更新）

一、概述

基于目标对称的ICP算法（Symmetric ICP）是一种改进的迭代最近点算法（Iterative Closest Point, ICP）。在传统ICP中，配准过程依赖于最小化源点云和目标点云之间的欧氏距离。然而，传统ICP对有对称性特征的场景配准时容易陷入局部最优解，无法充分利用目标的对称性特征进行精确配准。

1.1原理

基于目标对称的ICP算法通过对称点到平面的误差估计方法（Symmetric Point-to-Plane）优化变换矩阵的估计。该算法同时最小化源点到目标点云表面的距离以及目标点云到源点云表面的距离，从而对对称性场景有更好的适应性。它能够有效地处理含有对称结构的点云，提升配准的精度和稳定性。

1.2实现步骤

加载源点云和目标点云：读取待配准的源点云和目标点云数据。
计算法线信息：为源点云和目标点云计算法线，并将点云与法线信息合并。
基于对称误差估计的ICP配准：利用对称的点到平面距离误差估计，进行点云配准。
结果输出与可视化：输出配准后的变换矩阵，并可视化源点云、目标点云和配准后的结果点云。

1.3应用场景

对称物体的3D点云配准，如汽车、飞机等结构体的点云匹配。
自动驾驶、机器人等场景中，基于对称目标进行定位与姿态估计。
在含有对称结构的复杂场景中，提高点云配准的精度与效率。

二、代码实现

2.1关键函数

2.1.1计算点云的法线

// 计算点云的法线并与点云数据拼接，生成带法线的点云
void cloud_with_normal(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, 
                       pcl::PointCloud<pcl::PointNormal>::Ptr& cloud_normals) 
{
    // 使用OMP加速法线计算
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

    // 设置KD树搜索方法
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    ne.setNumberOfThreads(8);  // 设置使用的线程数
    ne.setInputCloud(cloud);   // 输入点云
    ne.setSearchMethod(tree);  // KD树搜索
    ne.setKSearch(10);         // 设置K近邻点个数
    ne.compute(*normals);      // 计算法线

    // 拼接点云数据和法线信息，生成带法线的点云
    pcl::concatenateFields(*cloud, *normals, *cloud_normals);
}

2.1.2基于对称误差估计的ICP配准

// 执行基于对称点到平面的ICP配准
void perform_symmetric_icp(pcl::PointCloud<pcl::PointNormal>::Ptr& source_with_normals,
    pcl::PointCloud<pcl::PointNormal>::Ptr& target_with_normals,
    pcl::PointCloud<pcl::PointNormal>::Ptr& aligned_cloud,
    Eigen::Matrix4f& final_transformation)
{
    // 创建对称点到平面ICP对象
    pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> symm_icp;

    // 使用对称点到平面的变换估计方法
    pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr symm_point_to_plane
    (new pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);

    // 设置ICP算法参数
    symm_icp.setTransformationEstimation(symm_point_to_plane);  // 设置对称点到平面距离
    symm_icp.setInputSource(source_with_normals);               // 设置源点云
    symm_icp.setInputTarget(target_with_normals);               // 设置目标点云
    symm_icp.setMaxCorrespondenceDistance(10);                  // 设置最大对应点对之间的距离
    symm_icp.setTransformationEpsilon(1e-10);                   // 设置终止条件：最小转换差异
    symm_icp.setEuclideanFitnessEpsilon(0.001);                 // 设置收敛条件：均方误差
    symm_icp.setMaximumIterations(50);                          // 设置最大迭代次数

    // 执行配准
    symm_icp.align(*aligned_cloud);

    // 输出配准结果
    if (symm_icp.hasConverged()) {
        std::cout << "Symmetric ICP has converged, score is " << symm_icp.getFitnessScore() << std::endl;
        final_transformation = symm_icp.getFinalTransformation();
        std::cout << "变换矩阵：\n" << final_transformation << std::endl;
    }
    else {
        PCL_ERROR("Symmetric ICP未收敛。\n");
        exit(-1);
    }
}

2.1.3可视化

// 可视化配准结果
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));
    viewer->setBackgroundColor(0, 0, 0);

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud(target, target_color, "target cloud");

    /*pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
    viewer->addPointCloud(source, source_color, "source cloud");*/

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 0, 0, 255);
    viewer->addPointCloud(aligned, aligned_color, "aligned cloud");

    viewer->spin();
}

2.2完整代码

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h> // icp算法
#include <pcl/features/normal_3d_omp.h> // 法线计算头文件
#include <pcl/visualization/pcl_visualizer.h> // 可视化

using namespace std;

// 计算点云法线并生成带法线的点云
void cloud_with_normal(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
    pcl::PointCloud<pcl::PointNormal>::Ptr& cloud_normals)
{
    // 使用OMP加速法线计算
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);

    // KD树用于近邻搜索
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);

    ne.setNumberOfThreads(8);   // 使用多线程加速法线计算
    ne.setInputCloud(cloud);    // 输入点云
    ne.setSearchMethod(tree);   // KD树搜索
    ne.setKSearch(10);          // 设置K近邻点的个数
    ne.compute(*normals);       // 计算法线

    // 合并点云与法线信息，生成带法线的点云
    pcl::concatenateFields(*cloud, *normals, *cloud_normals);
}

// 执行基于对称点到平面的ICP配准
void perform_symmetric_icp(pcl::PointCloud<pcl::PointNormal>::Ptr& source_with_normals,
    pcl::PointCloud<pcl::PointNormal>::Ptr& target_with_normals,
    pcl::PointCloud<pcl::PointNormal>::Ptr& aligned_cloud,
    Eigen::Matrix4f& final_transformation)
{
    // 创建对称点到平面ICP对象
    pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> symm_icp;

    // 使用对称点到平面的变换估计方法
    pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr symm_point_to_plane
    (new pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);

    // 设置ICP算法参数
    symm_icp.setTransformationEstimation(symm_point_to_plane);  // 设置对称点到平面距离
    symm_icp.setInputSource(source_with_normals);               // 设置源点云
    symm_icp.setInputTarget(target_with_normals);               // 设置目标点云
    symm_icp.setMaxCorrespondenceDistance(10);                  // 设置最大对应点对之间的距离
    symm_icp.setTransformationEpsilon(1e-10);                   // 设置终止条件：最小转换差异
    symm_icp.setEuclideanFitnessEpsilon(0.001);                 // 设置收敛条件：均方误差
    symm_icp.setMaximumIterations(50);                          // 设置最大迭代次数

    // 执行配准
    symm_icp.align(*aligned_cloud);

    // 输出配准结果
    if (symm_icp.hasConverged()) {
        std::cout << "Symmetric ICP has converged, score is " << symm_icp.getFitnessScore() << std::endl;
        final_transformation = symm_icp.getFinalTransformation();
        std::cout << "变换矩阵：\n" << final_transformation << std::endl;
    }
    else {
        PCL_ERROR("Symmetric ICP未收敛。\n");
        exit(-1);
    }
}

// 可视化配准结果
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));
    viewer->setBackgroundColor(0, 0, 0);

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud(target, target_color, "target cloud");

    /*pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
    viewer->addPointCloud(source, source_color, "source cloud");*/

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 0, 0, 255);
    viewer->addPointCloud(aligned, aligned_color, "aligned cloud");

    viewer->spin();
}

int main()
{
    // --------------------加载源点云-----------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *source);
    cout << "从源点云中读取 " << source->size() << " 个点" << endl;

    // -------------------加载目标点云----------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *target);
    cout << "从目标点云中读取 " << target->size() << " 个点" << endl;

    // 计算源点云的法线
    pcl::PointCloud<pcl::PointNormal>::Ptr source_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    cloud_with_normal(source, source_with_normals);

    // 计算目标点云的法线
    pcl::PointCloud<pcl::PointNormal>::Ptr target_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    cloud_with_normal(target, target_with_normals);

    // 创建对齐后的点云
    pcl::PointCloud<pcl::PointNormal>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointNormal>);
    Eigen::Matrix4f final_transformation = Eigen::Matrix4f::Identity();

    // 执行对称ICP配准
    perform_symmetric_icp(source_with_normals, target_with_normals, aligned_cloud, final_transformation);

    // 使用创建的变换对输入点云进行变换
    pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*source, *final_cloud, final_transformation);

    // 可视化配准结果
    visualize_registration(source, target, final_cloud);

    return 0;
}