PCL 基于kd树快速删除点云中重叠的点
目录
一、概述
1.1原理
1.2实现步骤
1.3应用场景
二、代码实现
2.1关键函数
2.1.1 KD树构建
2.1.2半径搜索
2.1.3 删除重叠点
2.2完整代码
PCL点云算法汇总及实战案例汇总的目录地址链接：
PCL点云算法与项目实战案例汇总（长期更新）
一、概述
本文将介绍如何使用 KD树 进行快速查找和删除点云中的重叠点。KD树是一种空间划分的数据结构，适合在高维空间中进行高效的邻域查找。通过使用KD树对点云进行最近邻查找，我们可以快速检测到重复或重叠的点，并将它们从点云中删除。
1.1原理
KD树能够通过快速查找相邻的点来加速重叠点的检测。具体步骤如下：
- 构建KD树：将点云数据输入到KD树中，构建一棵KD树结构。
- 邻域查找：对于每个点，使用KD树进行邻域搜索，找到距离该点很近的点（如相距小于一个指定的阈值）。
- 删除重叠点：根据查找结果，删除距离小于阈值的点（即重叠点）。
1.2实现步骤
- 读取点云数据。
- 使用KD树对点云进行构建，并加速查找最近邻点。
- 对每个点执行邻域搜索，检测重叠点。
- 删除距离小于某个阈值的重叠点。
- 输出处理后的点云数据。
1.3应用场景
- 高效去重：快速删除大规模点云数据中的重叠点。
- 点云预处理：为后续的点云处理步骤（如特征提取、点云配准）提供优化数据。
- 数据清理：去除采集过程中的冗余点、重复点。
二、代码实现
2.1关键函数
2.1.1 KD树构建
KD树用于高效地执行点云数据的最近邻查找。pcl::KdTreeFLANN 是PCL中KD树的实现，用于快速进行近邻搜索。首先需要将点云数据输入到KD树中以构建索引。
#include <pcl/kdtree/kdtree_flann.h> // 包含KD树相关的头文件
// 1. 创建KD树对象
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
// 2. 将点云数据输入到KD树中
kdtree.setInputCloud(cloud); // cloud 是已经加载的点云数据
// 这样就成功构建了KD树，可以用来进行后续的最近邻搜索和重叠点查找。
2.1.2半径搜索
通过 radiusSearch 函数，我们可以为每个点查找其在指定半径范围内的所有邻居点。该函数返回邻域点的索引及其到查询点的距离
std::vector<int> pointIdxRadiusSearch; // 用于存储邻域内的点的索引
std::vector<float> pointRadiusSquaredDistance; // 用于存储邻域内的点到查询点的距离平方值
float radius = 0.001f; // 设置删除重叠点的距离阈值（如1毫米）
// 对点云中的某个点（如第i个点）执行半径搜索，查找在指定半径内的邻域点
int numNeighbors = kdtree.radiusSearch(cloud->points[i], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
// 如果该点在指定半径内找到了其他重叠点，则可以进一步进行处理
if (numNeighbors > 0)
{
// pointIdxRadiusSearch 中保存了找到的邻域点的索引
// pointRadiusSquaredDistance 中保存了对应邻域点的距离
}
2.1.3 删除重叠点
通过邻域搜索，可以检测到距离查询点非常近的点，即重叠点。我们只保留那些没有邻居点（即不与其他点重叠）的点。
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>); // 创建一个新的点云对象，用于存储去重后的点云
// 遍历每个点，并检查它是否有重叠点
for (size_t i = 0; i < cloud->points.size(); ++i)
{
// 对当前点执行半径搜索，找到其邻域点
int numNeighbors = kdtree.radiusSearch(cloud->points[i], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
// 如果该点在指定半径内没有找到其他重叠点（即邻域内只有它自己），则保留该点
if (numNeighbors == 1) // 只有1个点（即查询点本身），说明该点没有重叠
{
filtered_cloud->points.push_back(cloud->points[i]); // 将该点保存到过滤后的点云中
}
}
// 此时，filtered_cloud 包含了所有没有重叠的点。
2.2完整代码
#include <iostream>
#include <vector>
#include <pcl/io/pcd_io.h> // 用于加载和保存PCD文件
#include <pcl/point_types.h> // PCL点类型定义
#include <pcl/kdtree/kdtree_flann.h> // KD树实现
int main(int argc, char** argv)
{
// --------------------------------读取点云------------------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 加载PCD文件中的点云数据
if (pcl::io::loadPCDFile<pcl::PointXYZ>("example.pcd", *cloud) == -1)
{
PCL_ERROR("Couldn't read file!"); // 如果文件加载失败，输出错误信息
return -1; // 返回错误代码并退出程序
}
std::cout << "Original cloud size: " << cloud->points.size() << " points." << std::endl;
// -----------------------------KD树构建---------------------------------
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; // 创建KD树对象
kdtree.setInputCloud(cloud); // 设置KD树的输入点云
// -----------------------------删除重叠点---------------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>); // 用于存储过滤后的点云
std::vector<int> pointIdxRadiusSearch; // 用于存储邻域点的索引
std::vector<float> pointRadiusSquaredDistance; // 用于存储邻域点的距离平方值
float radius = 0.001f; // 设置删除重叠点的距离阈值（如1毫米）
// 遍历点云中的每个点
for (size_t i = 0; i < cloud->points.size(); ++i)
{
// 对当前点执行半径搜索，查找在指定半径内的点
if (kdtree.radiusSearch(cloud->points[i], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
// 如果该点没有找到与其重叠的点，则保留
if (pointIdxRadiusSearch.size() == 1) // 保留没有重叠的点
{
filtered_cloud->points.push_back(cloud->points[i]);
}
}
}
std::cout << "Filtered cloud size: " << filtered_cloud->points.size() << " points." << std::endl;
// --------------------------------保存结果------------------------------------
pcl::io::savePCDFileASCII("E://data//filtered_kd_example.pcd", *filtered_cloud);
std::cout << "Filtered point cloud saved." << std::endl;
return 0; // 程序结束
}