当前位置：首页 > article >正文

PCL 基于中值距离的点云对应关系

article 2024/10/22 8:12:12

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

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

2.1.2获取初始点对

2.1.3可视化

2.2完整代码

三、实现效果

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

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

一、概述

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

1.1原理

中值距离剔除算法是通过以下步骤进行的：
1.获取初始对应点对：通过最近邻搜索等方法，找到源点云和目标点云之间的初始匹配点对。
2.计算距离：对于每对匹配点，计算两者之间的欧几里得距离。其计算公式为：

3.计算距离中值：对所有匹配点对的距离进行排序，找到中值距离 𝑑。
4.剔除错误匹配：将距离大于 𝑑×k 的匹配点对视为错误点对剔除，𝑘 为用户指定的系数，控制筛选的严格程度。通常，较小的 𝑘 值会剔除更多的点对，较大的 𝑘 值会保留更多的点对。
5.输出筛选结果：保留满足筛选条件的匹配点对，作为进一步配准或分析的基础。

1.2实现步骤

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

1.3应用场景

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

二、代码实现

2.1关键函数

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

float reject_correspondences_by_median(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(*corres
    
    
    
    pondences_result_rej_media); // 获取剔除后的匹配点对
    return rejector.getMedianDistance();  // 返回中值距离
}

2.1.2获取初始点对

// 函数：获取初始对应点对
void get_correspondences(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.1.3可视化

void visualize_correspondences(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("media_distance"));

    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));
    }
}

2.2完整代码

#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 get_correspondences(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 reject_correspondences_by_median(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 visualize_correspondences(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> 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)
{
    // 1. 加载点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *source);
    pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *target);

    cout << "源点云: " << source->size() << " 个点" << endl;
    cout << "目标点云: " << target->size() << " 个点" << endl;

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

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

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

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

    return 0;
}