PCL 基于距离阈值去除错误对应关系(永久免费版)

即日起，付费专栏所有内容将以永久免费形式陆续进行发表！！！

一、概述

  在3D点云的配准过程中，我们通常会找到源点云和目标点云之间的对应点对。然而，这些对应点对可能包含一些错误的匹配关系。为了提升配准的精度，必须通过某些策略来剔除这些错误的对应点。本文将介绍一种基于距离阈值的错误对应关系剔除方法，实现对应点对的距离进行筛选，并移除那些不符合阈值要求的点对。

1.1 原理

  在点云配准中，通常通过某种策略找到两个点云之间的初始对应点对。这些对应点对可能包含一些错误的匹配，比如距离过远的点对。通过设定一个合理的距离阈值，剔除那些不符合要求的点对，可以大大提高配准的精度。这里我们使用PCL中的
CorrespondenceRejectorDistance类对初始的对应关系进行筛选，保留距离在指定阈值内的点对。

1.2 实现步骤

加载源点云和目标点云。
使用PCL的CorrespondenceEstimation类获取初始的点对匹配关系。
使用CorrespondenceRejectorDistance类基于距离阈值筛选对应点对。
可视化筛选前后的点对匹配关系，展示最终的结果

1.3应用场景

1. 多视角点云的精确配准。
2. 机器人SLAM中的环境建模。
3. 自动驾驶中点云数据的对齐与去噪。

二、关键函数

2.1 获取初始点对

该函数使用CorrespondenceEstimation类来计算源点云与目标点云之间的初始匹配点对。

// 获取初始对应点对
void getCorrespondencesByDistance(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 基于距离的对应关系筛选函数

该函数使用CorrespondenceRejectorDistance类来剔除距离过大的对应点对。

// 基于距离的对应关系筛选
void rejCorrespondencesByDistance(const pcl::CorrespondencesPtr& correspondences,pcl::CorrespondencesPtr& correspondences_result_rej_dist,double max_distance)
{// 使用CorrespondenceRejectorDistance类剔除距离过大的点对pcl::registration::CorrespondenceRejectorDistance corr_rej_dist;corr_rej_dist.setInputCorrespondences(correspondences); // 设置输入的对应关系corr_rej_dist.setMaximumDistance(max_distance);        // 设置最大距离阈值corr_rej_dist.getCorrespondences(*correspondences_result_rej_dist); // 获取剔除后的对应关系
}

2.3 可视化

// 可视化点云及对应点对
void visualizeRegistrationResult(const pcl::PointCloud<pcl::PointXYZ>::Ptr& source,const pcl::PointCloud<pcl::PointXYZ>::Ptr& target,const pcl::CorrespondencesPtr& correspondences_result_rej_dist)
{// 创建PCLVisualizer对象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_result_rej_dist, "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_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); // 获取初始匹配点对
}// 基于距离的对应关系筛选
void rejCorrespondencesByDistance(const pcl::CorrespondencesPtr& correspondences,pcl::CorrespondencesPtr& correspondences_result_rej_dist,double max_distance)
{// 使用CorrespondenceRejectorDistance类剔除距离过大的点对pcl::registration::CorrespondenceRejectorDistance corr_rej_dist;corr_rej_dist.setInputCorrespondences(correspondences); // 设置输入的对应关系corr_rej_dist.setMaximumDistance(max_distance);        // 设置最大距离阈值corr_rej_dist.getCorrespondences(*correspondences_result_rej_dist); // 获取剔除后的对应关系
}// 可视化点云及其对应关系
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>("bun000.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>("bun045.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(new pcl::Correspondences);rejCorrespondencesByDistance(correspondences, correspondences_result_rej, 0.2);cout << "初始对应点对数量: " << correspondences->size() << endl;cout << "基于中值距离剔除后剩余: " << correspondences_result_rej->size() << endl;// 4. 可视化结果visualizeCorrespondencesResult(source, target, correspondences_result_rej);return 0;
}

四、结果展示

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