PCL 点云配准 GICP算法（精配准）

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 执行GICP配准

2.1.2 可视化

2.2完整代码

三、实现效果

---

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

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

---

一、概述

        广义迭代最近点（Generalized Iterative Closest Point，GICP）算法是一种点云精配准算法，基于ICP（迭代最近点）算法的扩展。GICP不同于传统的ICP，它在进行点云匹配时，不仅考虑了每个点的位置，还将其周围局部结构的协方差矩阵引入到匹配过程中，使得配准过程更具鲁棒性。

1.1原理

        GICP的目标是通过迭代的方式，最小化源点云和目标点云之间的误差。该误差以广义点到点（Point-to-Point）和点到平面（Point-to-Plane）距离的组合形式存在，并通过使用局部结构的协方差矩阵进行加权，以实现更精确的配准。

1.2实现步骤

1. 加载目标点云和源点云。
2. 初始化GICP算法，并设置其参数，包括KD树加速方法。
3. 执行GICP配准，计算刚体变换矩阵。
4. 使用变换矩阵对源点云进行刚性变换。
5. 可视化配准前后点云结果。

1.3应用场景

1. 精细化的3D点云配准，适用于复杂几何结构的场景。
2. 机器人和自动驾驶中的环境感知和物体检测。
3. 在噪声较大或局部区域扭曲较明显时，相较于ICP更具鲁棒性。

二、代码实现

2.1关键函数

2.1.1 执行GICP配准

// 执行GICP配准，并返回最终的配准结果和变换矩阵
void perform_gicp(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, 
                  pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, 
                  pcl::PointCloud<pcl::PointXYZ>::Ptr& icp_cloud, 
                  Eigen::Matrix4f& transformation_matrix, int iterations)
{
    // 创建GICP对象
    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;

    // 设置KD树加速源点云与目标点云的匹配搜索
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>);
    tree1->setInputCloud(source_cloud);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>);
    tree2->setInputCloud(target_cloud);
    gicp.setSearchMethodSource(tree1);
    gicp.setSearchMethodTarget(tree2);

    // 设置GICP的输入源和目标点云
    gicp.setInputSource(source_cloud);
    gicp.setInputTarget(target_cloud);

    // 设置GICP的配准参数
    gicp.setMaxCorrespondenceDistance(100);      // 设置最大匹配距离
    gicp.setTransformationEpsilon(1e-10);        // 设置最小变换差异作为终止条件
    gicp.setEuclideanFitnessEpsilon(0.001);      // 设置收敛条件：均方误差和
    gicp.setMaximumIterations(iterations);       // 设置最大迭代次数

    // 执行GICP配准
    gicp.align(*icp_cloud);
    
    // 判断配准是否收敛并输出相关信息
    if (gicp.hasConverged()) {
        std::cout << "\nGICP has converged, score is " << gicp.getFitnessScore() << std::endl;
        transformation_matrix = gicp.getFinalTransformation();
        std::cout << "变换矩阵：\n" << transformation_matrix << std::endl;
    } else {
        PCL_ERROR("GICP配准未能收敛。\n");
        exit(-1); // 如果未收敛，退出程序
    }
}

2.1.2 可视化

// 可视化配准前后的点云结果，使用不同颜色显示源点云、目标点云和配准后的点云
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, 
                            pcl::PointCloud<pcl::PointXYZ>::Ptr& target, 
                            pcl::PointCloud<pcl::PointXYZ>::Ptr& result)
{
    // 创建可视化窗口
    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> result_color(result, 0, 0, 255);
    viewer->addPointCloud(result, result_color, "result cloud");
    
    // 启动可视化
    viewer->spin();
}

2.2完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/gicp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>

using namespace std;

// 执行GICP配准，并返回最终的配准结果和变换矩阵
void perform_gicp(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& icp_cloud,
    Eigen::Matrix4f& transformation_matrix, int iterations)
{
    // 创建GICP对象
    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;

    // 设置KD树加速源点云与目标点云的匹配搜索
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>);
    tree1->setInputCloud(source_cloud);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>);
    tree2->setInputCloud(target_cloud);
    gicp.setSearchMethodSource(tree1);
    gicp.setSearchMethodTarget(tree2);

    // 设置GICP的输入源和目标点云
    gicp.setInputSource(source_cloud);
    gicp.setInputTarget(target_cloud);

    // 设置GICP的配准参数
    gicp.setMaxCorrespondenceDistance(100);      // 设置最大匹配距离
    gicp.setTransformationEpsilon(1e-10);        // 设置最小变换差异作为终止条件
    gicp.setEuclideanFitnessEpsilon(0.001);      // 设置收敛条件：均方误差和
    gicp.setMaximumIterations(iterations);       // 设置最大迭代次数

    // 执行GICP配准
    gicp.align(*icp_cloud);

    // 判断配准是否收敛并输出相关信息
    if (gicp.hasConverged()) {
        std::cout << "\nGICP has converged, score is " << gicp.getFitnessScore() << std::endl;
        transformation_matrix = gicp.getFinalTransformation();
        std::cout << "变换矩阵：\n" << transformation_matrix << std::endl;
    }
    else {
        PCL_ERROR("GICP配准未能收敛。\n");
        exit(-1); // 如果未收敛，退出程序
    }
}

// 可视化配准前后的点云结果
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& result)
{
    // 创建可视化窗口
    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> result_color(result, 0, 0, 255);
    viewer->addPointCloud(result, result_color, "result cloud");

    // 启动可视化
    viewer->spin();
}

int main()
{
    pcl::console::TicToc time;

    // 加载源点云和目标点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *target_cloud);

    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *source_cloud);

    // 打印读取的点云个数
    std::cout << "读取源点云个数：" << source_cloud->points.size() << std::endl;
    std::cout << "读取目标点云个数：" << target_cloud->points.size() << std::endl;

    // 初始化GICP参数和结果变量
    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();

    // 执行GICP配准
    perform_gicp(source_cloud, target_cloud, icp_cloud, transformation_matrix, 35);

    // 可视化配准结果
    visualize_registration(source_cloud, target_cloud, icp_cloud);

    return 0;
}

三、实现效果

红色及绿色点云为初始点云，红色与蓝色表示配准后的点云