PCL 基于FPFH特征描述子获取点云对应关系

目录

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

[2.1.1 FPFH特征计算函数](#2.1.1 FPFH特征计算函数)

[2.1.2 获取点云之间的对应点对函数](#2.1.2 获取点云之间的对应点对函数)

[2.1.3 可视化函数](#2.1.3 可视化函数)

2.2完整代码

三、实现效果

---

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

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

---

一、概述

在三维点云配准中，特征描述子是用来表示点云中局部几何特性的数学特征。通过比较源点云和目标点云中的特征描述子，可以找到点云之间的对应关系。FPFH（Fast Point Feature Histograms）是点云配准中常用的一种局部特征描述子。本文将介绍如何使用PCL（Point Cloud Library）中的FPFH描述子计算点云的特征，并基于这些特征获取点云之间的匹配点对。

1.1原理

特征描述子（Descriptor）是一种表示局部几何特征的矢量，通常用于匹配两组点云中的特征点。**FPFH是一种加速计算的几何特征，它可以捕捉点云的局部几何信息。**通过在源点云和目标点云上计算FPFH描述子，我们可以通过比较这些描述子来找到两组点云的匹配点对。

1.2实现步骤

1. 加载源点云和目标点云。
2. 计算源点云和目标点云的法向量。
3. 使用FPFH计算每个点的特征描述子。
4. 根据FPFH特征描述子，找到源点云和目标点云之间的对应点对。
5. 可视化源点云、目标点云以及它们的对应关系。

1.3应用场景

1. **点云配准：**通过特征描述子获取点云之间的初步对应关系，为后续的精确配准提供基础。
2. **多视角点云拼接：**在不同视角下获取的点云之间找到对应关系，进行拼接和对齐。
3. **物体识别：**在复杂的三维场景中，通过特征描述子匹配物体模型和场景中的对象，实现物体识别。

二、代码实现

2.1关键函数

2.1.1 FPFH特征计算函数

该函数计算点云的FPFH特征描述子，首先通过法向量估计，然后计算FPFH特征。

fpfhFeature::Ptr compute_fpfh_feature(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
{
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

    //-------------------------法向量估计-----------------------
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
    n.setInputCloud(input_cloud);
    n.setNumberOfThreads(8); // 设置使用8个线程
    n.setSearchMethod(tree); // 使用KdTree进行近邻搜索
    n.setKSearch(10); // 设置K近邻的点数
    n.compute(*normals); // 计算法向量

    //------------------FPFH估计-------------------------------
    fpfhFeature::Ptr fpfh(new fpfhFeature);
    pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> f;
    f.setNumberOfThreads(8); // 指定8核计算
    f.setInputCloud(input_cloud); // 输入点云
    f.setInputNormals(normals); // 输入点云的法向量
    f.setSearchMethod(tree); // 使用KdTree进行近邻搜索
    f.setKSearch(10); // 设置K近邻的点数
    f.compute(*fpfh); // 计算FPFH特征

    return fpfh;
}

2.1.2 获取点云之间的对应点对函数

该函数根据FPFH特征描述子，计算源点云和目标点云之间的对应关系。

void get_correspondences_by_features(const fpfhFeature::Ptr& source_fpfh,
                                     const fpfhFeature::Ptr& target_fpfh,
                                     pcl::CorrespondencesPtr& correspondences)
{
    pcl::registration::CorrespondenceEstimation<pcl::FPFHSignature33, pcl::FPFHSignature33> crude_cor_est;
    crude_cor_est.setInputSource(source_fpfh); // 输入源点云的FPFH特征
    crude_cor_est.setInputTarget(target_fpfh); // 输入目标点云的FPFH特征
    crude_cor_est.determineReciprocalCorrespondences(*correspondences); // 计算匹配点对
}

2.1.3 可视化函数

该函数用于可视化源点云、目标点云以及它们之间的对应关系。

void visualize_registration(const pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
                            const pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
                            const pcl::CorrespondencesPtr& correspondences)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));
    viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色

    // 源点云着色为绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZ>(source, source_color, "source cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source cloud");

    // 目标点云着色为红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");

    // 添加对应点对的可视化
    viewer->addCorrespondences<pcl::PointXYZ>(source, target, *correspondences, "correspondence");

    // 开启渲染循环，直到窗口关闭
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
}

2.2完整代码

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h> 
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

using namespace std;

typedef pcl::PointCloud<pcl::FPFHSignature33> fpfhFeature;

// 函数：计算点云的FPFH特征
fpfhFeature::Ptr compute_fpfh_feature(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
{
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

    // 计算法向量
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
    n.setInputCloud(input_cloud);
    n.setNumberOfThreads(8);
    n.setSearchMethod(tree);
    n.setKSearch(10);
    n.compute(*normals);

    // 计算FPFH特征
    fpfhFeature::Ptr fpfh(new fpfhFeature);
    pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> f;
    f.setNumberOfThreads(8);
    f.setInputCloud(input_cloud);
    f.setInputNormals(normals);
    f.setSearchMethod(tree);
    f.setKSearch(10);
    f.compute(*fpfh);

    return fpfh;
}

// 函数：根据FPFH特征获取对应点对
void get_correspondences_by_features(const fpfhFeature::Ptr& source_fpfh,
    const fpfhFeature::Ptr& target_fpfh,
    pcl::CorrespondencesPtr& correspondences)
{
    pcl::registration::CorrespondenceEstimation<pcl::FPFHSignature33, pcl::FPFHSignature33> crude_cor_est;
    crude_cor_est.setInputSource(source_fpfh);
    crude_cor_est.setInputTarget(target_fpfh);
    crude_cor_est.determineReciprocalCorrespondences(*correspondences);
}

// 函数：可视化点云及对应关系
void visualize_registration(const pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    const pcl::CorrespondencesPtr& correspondences)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));
    viewer->setBackgroundColor(0, 0, 0);

    // 源点云为绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZ>(source, source_color, "source cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source cloud");

    // 目标点云为红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");

    // 可视化对应关系
    viewer->addCorrespondences<pcl::PointXYZ>(source, target, *correspondences, "correspondence");

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

int main(int argc, char** argv)
{
    // 加载源点云和目标点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *source_cloud);
    pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *target_cloud);

    // 计算FPFH特征
    fpfhFeature::Ptr source_fpfh = compute_fpfh_feature(source_cloud);
    fpfhFeature::Ptr target_fpfh = compute_fpfh_feature(target_cloud);

    // 获取对应关系
    pcl::CorrespondencesPtr correspondences(new pcl::Correspondences);
    get_correspondences_by_features(source_fpfh, target_fpfh, correspondences);

    cout << "源点云点数: " << source_cloud->size() << endl;
    cout << "目标点云点数: " << target_cloud->size() << endl;
    cout << "匹配点对数量: " << correspondences->size() << endl;

    // 可视化
    visualize_registration(source_cloud, target_cloud, correspondences);

    return 0;
}

三、实现效果