PCL 基于中值距离的点云对应关系(永久免费版)

一、概述

  在点云配准过程中，初始对应点对可能包含错误匹配的点对，因此在执行进一步的精确配准（如ICP算法）之前，通常需要进行初步的筛选。通过基于中值距离的方法，可以有效剔除错误匹配的点对。此方法通过计算所有匹配点对的距离中值，然后剔除距离大于中值乘以系数的点对，从而保留正确匹配的点对

1.1 原理

  中值距离剔除算法 是通过以下步骤进行的：
1.获取初始对应点对：通过最近邻搜索等方法，找到源点云和目标点云之间的初始匹配点对。
2.计算距离：对于每对匹配点，计算两者之间的欧几里得距离。
3.计算距离中值：对所有匹配点对的距离进行排序，找到中值距离 𝑑。
4.剔除错误匹配：将距离大于 𝑑×k 的匹配点对视为错误点对剔除，𝑘 为用户指定的系数，控制筛选的严格程度。通常，较小的 𝑘 值会剔除更多的点对，较大的 𝑘 值会保留更多的点对。
5.输出筛选结果：保留满足筛选条件的匹配点对，作为进一步配准或分析的基础。

1.2 实现步骤

加载点云数据：读取源点云和目标点云。
获取对应关系：通过最近邻搜索获取初始的匹配点对。
基于中值距离进行筛选：计算匹配点对的距离中值，剔除大于中值乘以系数的匹配点对。
可视化结果：展示源点云、目标点云及其筛选后的匹配点对。

1.3应用场景

点云配准的初步过滤：基于中值距离剔除错误的对应点对，可以用于提高后续精配准算法（如ICP）的效率和准确性。
物体识别与姿态估计：通过剔除错误的对应关系，提高点云识别与定位的准确性。
多视角点云拼接：在多视角点云拼接中，基于中值距离可以有效剔除多余或错误的匹配点对。

二、关键函数

2.1 获取初始点对

// 获取初始对应点对
void getCorrespondences(const pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::CorrespondencesPtr& correspondences)
{
    pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> corr_est;
    corr_est.setInputSource(source);  // 设置源点云
    corr_est.setInputTarget(target);  // 设置目标点云
    corr_est.determineCorrespondences(*correspondences); // 获取初始匹配点对
}

2.2 获取中值距离筛选后的对应点对

// 基于中值距离剔除错误对应关系并返回中值距离
float rejectCorrespondencesByMedian(const pcl::CorrespondencesPtr& correspondences,
    pcl::CorrespondencesPtr& correspondences_result_rej_media,
    float median_factor = 0.2)
{
    pcl::registration::CorrespondenceRejectorMedianDistance rejector;
    rejector.setInputCorrespondences(correspondences); // 输入初始匹配点对
    rejector.setMedianFactor(median_factor);  // 设置中值系数，控制筛选严格程度
    rejector.getCorrespondences(*correspondences_result_rej_media); // 获取剔除后的匹配点对
    return rejector.getMedianDistance();  // 返回中值距离
}

2.3 可视化

// 可视化点云及其对应关系
void visualizeCorrespondencesResult(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(u8"基于中值距离的对应关系"));

    viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色
    // 为目标点云设置红色
    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");
    // 为源点云设置绿色
    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");
    // 可视化对应关系
    viewer->addCorrespondences<pcl::PointXYZ>(source, target, *correspondences, "correspondence");
    // 开启渲染循环，直到窗口关闭
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
}

三、完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/correspondence_estimation.h>  // 获取对应关系的基类
#include <pcl/registration/correspondence_rejection_median_distance.h>  // 使用中值距离剔除的类

using namespace std;

// 获取初始对应点对
void getCorrespondences(const pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& target,
    pcl::CorrespondencesPtr& correspondences)
{
    pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ> corr_est;
    corr_est.setInputSource(source);  // 设置源点云
    corr_est.setInputTarget(target);  // 设置目标点云
    corr_est.determineCorrespondences(*correspondences); // 获取初始匹配点对
}

// 基于中值距离剔除错误对应关系并返回中值距离
float rejectCorrespondencesByMedian(const pcl::CorrespondencesPtr& correspondences,
    pcl::CorrespondencesPtr& correspondences_result_rej_media,
    float median_factor = 0.2)
{
    pcl::registration::CorrespondenceRejectorMedianDistance rejector;
    rejector.setInputCorrespondences(correspondences); // 输入初始匹配点对
    rejector.setMedianFactor(median_factor);  // 设置中值系数，控制筛选严格程度
    rejector.getCorrespondences(*correspondences_result_rej_media); // 获取剔除后的匹配点对
    return rejector.getMedianDistance();  // 返回中值距离
}

// 可视化点云及其对应关系
void visualizeCorrespondencesResult(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(u8"基于中值距离的对应关系"));

    viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色
    // 为目标点云设置红色
    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");
    // 为源点云设置绿色
    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");
    // 可视化对应关系
    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(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *source) == -1)
    {
        PCL_ERROR("读取源标点云失败 \n");
        return (-1);
    }
    cout << "从源点云中读取 " << source->size() << " 个点" << endl;
    // ---------------------------------加载目标云---------------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *target) == -1)
    {
        PCL_ERROR("读取目标点云失败 \n");
        return (-1);
    }
    cout << "从目标点云中读取 " << target->size() << " 个点" << endl;

    // 2. 获取初始对应点对
    pcl::CorrespondencesPtr correspondences(new pcl::Correspondences);
    getCorrespondences(source, target, correspondences);

    // 3. 基于中值距离剔除错误对应点对，并获取中值距离
    pcl::CorrespondencesPtr correspondences_result_rej_media(new pcl::Correspondences);
    float median_distance = rejectCorrespondencesByMedian(correspondences, correspondences_result_rej_media);

    cout << "初始对应点对数量: " << correspondences->size() << endl;
    cout << "中值距离为: " << median_distance << endl;
    cout << "基于中值距离剔除后剩余: " << correspondences_result_rej_media->size() << endl;

    // 4. 可视化结果
    visualizeCorrespondencesResult(source, target, correspondences_result_rej_media);

    return 0;
}

四、结果展示

从源点云中读取 40256 个点
从目标点云中读取 40097 个点
初始对应点对数量: 40256
中值距离为: 0.000229776
基于中值距离剔除后剩余: 11528