PCL 点云配准-4PCS算法（粗配准）

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 加载点云数据

2.1.2 执行4PCS粗配准

2.1.3 可视化源点云、目标点云和配准结果

2.2完整代码

三、实现效果

3.1原始点云

3.2配准后点云

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

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

一、概述

        4PCS（四点一致集）算法是一种用于点云配准的粗配准方法。该算法通过寻找目标点云和源点云之间具有几何约束的四点集合进行匹配，继而估计出变换矩阵。4PCS 算法具有较好的抗噪性和计算效率，适用于较大尺度的点云配准场景。

1.1原理

4PCS 算法通过以下步骤进行粗配准：

1. 点云采样：从源点云和目标点云中采样若干点，形成四点集合。
2. 几何一致性验证：计算这四个点在两个点云中的相对距离，通过几何一致性约束找到符合要求的四点集合。
3. 估计变换矩阵：使用一致的四点集合，计算源点云到目标点云的变换矩阵。
4. 应用变换矩阵：将计算得到的变换矩阵应用到源点云上，使其与目标点云对齐。

配准结果的质量依赖于：

• 重叠率：设置源点云和目标点云的近似重叠率。
• 采样点数量：设置参与匹配的采样点数量。
• 精度参数 Delta：控制配准的精度，通过对配准点云的稀疏化进行加速。

1.2实现步骤

1. 加载源点云和目标点云。
2. 设置4PCS配准参数：包括近似重叠率、采样点数量、精度参数等。
3. 执行4PCS粗配准：通过设置参数执行粗配准，得到变换矩阵。
4. 应用变换矩阵：将源点云应用变换矩阵对齐至目标点云。
5. 可视化结果：将源点云、目标点云以及对齐后的点云进行可视化对比。

1.3应用场景

1. 粗配准阶段：4PCS 可以用于点云配准的初步阶段，提供较为快速的粗略对齐结果，后续可以使用更精细的算法（如ICP）进行精配准。
2. 多场景拼接：在多视角点云场景下，4PCS 可以帮助快速匹配不同视角的点云数据。
3. 点云地图生成：在SLAM（同步定位与地图构建）中，4PCS 可以用于不同帧之间的点云匹配与对齐。

二、代码实现

2.1关键函数

2.1.1 加载点云数据

void loadPointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud)
{
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("hand_trans.pcd", *target_cloud) == -1) {
        PCL_ERROR("读取目标点云失败 \n");
    }

    if (pcl::io::loadPCDFile<pcl::PointXYZ>("hand.pcd", *source_cloud) == -1) {
        PCL_ERROR("读取源点云失败 \n");
    }
}

2.1.2 执行4PCS粗配准

void perform4PCSRegistration(pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud, Eigen::Matrix4f& transformation_matrix)
{
    pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> fpcs;
    fpcs.setInputSource(source_cloud);
    fpcs.setInputTarget(target_cloud);
    fpcs.setApproxOverlap(0.7);         // 设置近似重叠率
    fpcs.setDelta(0.01);                // 精度参数
    fpcs.setNumberOfSamples(100);       // 采样点数量

    fpcs.align(*aligned_cloud);         // 执行配准
    transformation_matrix = fpcs.getFinalTransformation(); // 获取变换矩阵
}

2.1.3 可视化源点云、目标点云和配准结果

// 可视化源点云、目标点云和配准结果
void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Point Cloud Registration Viewer"));

    viewer->setBackgroundColor(1.0, 1.0, 1.0);  // 设置背景颜色为黑色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 0, 0);
    viewer->addPointCloud(target_cloud, target_color, "target cloud"); // 目标点云（红色）

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source_cloud, 0, 0, 255);
    viewer->addPointCloud(source_cloud, source_color, "source cloud"); // 源点云（蓝色）

    

    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source cloud");


    while (!viewer->wasStopped()) {
        viewer->spinOnce();
    }
}

2.2完整代码

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

// 加载点云数据
void loadPointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud)
{
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("hand_trans.pcd", *target_cloud) == -1) {
        PCL_ERROR("读取目标点云失败 \n");
    }

    if (pcl::io::loadPCDFile<pcl::PointXYZ>("hand.pcd", *source_cloud) == -1) {
        PCL_ERROR("读取源点云失败 \n");
    }
}

// 执行4PCS粗配准
void perform4PCSRegistration(pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud, Eigen::Matrix4f& transformation_matrix)
{
    pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> fpcs;
    fpcs.setInputSource(source_cloud);
    fpcs.setInputTarget(target_cloud);
    fpcs.setApproxOverlap(0.7);         // 设置近似重叠率
    fpcs.setDelta(0.01);                // 精度参数
    fpcs.setNumberOfSamples(1000);       // 采样点数量

    fpcs.align(*aligned_cloud);         // 执行配准
    transformation_matrix = fpcs.getFinalTransformation(); // 获取变换矩阵
}

// 可视化源点云、目标点云和配准结果
// 可视化源点云、目标点云和配准结果
void visualizePointClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Point Cloud Registration Viewer"));

    viewer->setBackgroundColor(1.0, 1.0, 1.0);  // 设置背景颜色为黑色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target_cloud, 255, 0, 0);
    viewer->addPointCloud(target_cloud, target_color, "target cloud"); // 目标点云（红色）

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source_cloud, 0, 0, 255);
    viewer->addPointCloud(source_cloud, source_color, "source cloud"); // 源点云（蓝色）

    

    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source cloud");


    while (!viewer->wasStopped()) {
        viewer->spinOnce();
    }
}




int main(int argc, char** argv)
{
    pcl::console::TicToc time;
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    loadPointClouds(source_cloud, target_cloud);

    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    Eigen::Matrix4f transformation_matrix;

    time.tic();
    perform4PCSRegistration(source_cloud, target_cloud, aligned_cloud, transformation_matrix);
    cout << "FPCS配准用时： " << time.toc() << " ms" << endl;
    cout << "变换矩阵：" << transformation_matrix << endl;

    //显示原始点云
    visualizePointClouds(source_cloud, target_cloud);
    //显示配准后点云
    visualizePointClouds(target_cloud, aligned_cloud);


    return 0;
}

三、实现效果

3.1原始点云

3.2配准后点云