PCL KD树的使用

一、概述

1.1原理

1.1.1 数据拆分过程

1.1.2 树的构建示例

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1KD树构建与查询：

2.1.2 k近邻搜索

2.1.3半径搜索

2.2完整代码

三、实现效果

3.1处理后点云

3.2数据显示

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

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

一、概述

KD树（K-D Tree）是一种用于组织k维空间中的点的数据结构，常用于高效的最近邻搜索、范围查询等操作。KD树通过递归地将空间划分成k维的超矩形，使得在高维空间中的搜索变得更加高效。本文将详细介绍KD树的构建原理及其在PCL中的应用。

1.1原理

KD树的全称是k维树（k-Dimensional Tree），它是一种二叉树，用于对k维空间中的点进行组织，以便于快速地进行点的查询操作（如最近邻搜索）。KD树的基本思想是通过递归地将数据空间划分为两个部分，从而形成一棵二叉树。每个节点代表一个k维超矩形，划分过程直到节点包含的数据点数目低于某个阈值或不能再划分为止。

1.1.1 数据拆分过程

1.选择维度：
   - 在构建KD树时，选择一个维度来划分数据空间。通常，使用循环选择的方法，即在每一层递归中，依次选择数据的不同维度。例如，在三维空间（k=3）中，第一层选择x维度，第二层选择y维度，第三层选择z维度，第四层再回到x维度，以此类推。
2.选择分割点：
   - 在选定的维度上，通过找到数据在该维度上的中位数，将数据空间划分为两个部分。中位数的选择保证了数据点被尽量均匀地分割开。
3.递归构建：
   - 将数据空间划分后，左子树包含小于或等于中位数的数据点，右子树包含大于中位数的数据点。递归地对每个子空间构建KD树，直到满足终止条件（如节点数据点数目小于某个阈值）。

1.1.2 树的构建示例

假设我们有一组二维点 (x, y)，要将其构建为一棵KD树：

第一层（根节点）：选择x维度进行划分，找到x维度的中位数，作为根节点的分割点。左子树包含所有x值小于或等于中位数的点，右子树包含所有x值大于中位数的点。
第二层：对左子树和右子树，选择y维度进行划分，找到y维度的中位数，分别作为左右子树的分割点。继续递归划分。
第三层：再回到x维度进行划分，依次类推，直到不能再划分为止。

1.2实现步骤

        KD树的搜索过程通常是以递归的方式进行的，主要分为最近邻搜索和范围搜索两种常见的操作。
1.最近邻搜索：
   - 从根节点开始，沿树向下递归搜索，比较查询点与节点的分割点，决定搜索左子树还是右子树。
   - 在找到一个叶节点后，记录下该叶节点的点，并计算它与查询点的距离。
   - 回溯检查其他可能包含更近点的子树。
2.范围搜索：
   - 同样从根节点开始，根据分割维度的值，判断是否需要搜索左右子树。
   - 判断当前节点的分割点是否在查询范围内，如果是，则将其纳入结果集。
   - 继续递归地检查子树，直到所有符合条件的节点都被搜索到。
通过这种分割和搜索方法，KD树能够有效地减少高维空间中的搜索范围，从而加快查询速度。

1.3应用场景

KD树广泛应用于以下场景：

最近邻搜索：用于查找点云中距离某点最近的邻居点，常用于点云配准。
k近邻搜索：用于查找点云中某点的k个最近邻点，适用于点云平滑、特征计算等。
范围搜索：用于查找在一定范围内的所有邻居点，适用于聚类、边界检测等。

二、代码实现

2.1关键函数

2.1.1KD树构建与查询：

pcl::KdTreeFLANN：用于创建KD树对象，支持最近邻、k近邻、范围搜索。
setInputCloud：将点云数据输入到KD树中进行构建。
nearestKSearch：执行k近邻搜索，找到指定点的k个最近邻点。
radiusSearch：执行范围搜索，找到指定点在给定半径内的所有邻居点。

2.1.2 k近邻搜索

int pcl::KdTreeFLANN< PointT, Dist >::nearestKSearch  ( const PointT &  point,  unsigned int  k,  Indices &  k_indices,  std::vector< float > &  k_sqr_distances  )  const

2.1.3半径搜索

int pcl::KdTreeFLANN< PointT, Dist >::radiusSearch  ( const PointT &  point,  double  radius,  Indices &  k_indices,  std::vector< float > &  k_sqr_distances,  unsigned int  max_nn = 0  )  const

2.2完整代码

#include <pcl/io/pcd_io.h>  // 用于加载PCD文件
#include <pcl/point_types.h>  // PCL点类型定义
#include <pcl/kdtree/kdtree_flann.h>  // PCL中的KD树实现
#include <pcl/visualization/pcl_visualizer.h>  // 用于可视化点云
#include <vector>
#include <iostream>// 将搜索到的结果渲染为红色，并在可视化窗口中显示
void visualizeResult(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const std::vector<int>& indices)
{// 创建一个带颜色信息的点云对象，用于存储带有颜色信息的点云pcl::PointCloud<pcl::PointXYZRGB>::Ptr coloredCloud(new pcl::PointCloud<pcl::PointXYZRGB>);// 遍历原始点云中的每个点for (size_t i = 0; i < cloud->points.size(); ++i){// 创建一个带有RGB颜色信息的点对象pcl::PointXYZRGB point;point.x = cloud->points[i].x;  // 复制点的X坐标point.y = cloud->points[i].y;  // 复制点的Y坐标point.z = cloud->points[i].z;  // 复制点的Z坐标// 判断当前点是否在搜索到的索引列表中if (std::find(indices.begin(), indices.end(), i) != indices.end()){// 如果是搜索到的点，将颜色设置为红色point.r = 255;point.g = 0;point.b = 0;}else{// 如果不是搜索到的点，将颜色设置为白色point.r = 255;point.g = 255;point.b = 255;}// 将带颜色的点添加到新的点云对象中coloredCloud->points.push_back(point);}// 设置点云的基本属性coloredCloud->width = coloredCloud->points.size();  // 设置点云宽度为点的数量coloredCloud->height = 1;  // 设置点云高度为1（无序点云）coloredCloud->is_dense = true;  // 设置点云为稠密点云// 创建一个PCLVisualizer对象，用于显示带颜色的点云pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));viewer->setBackgroundColor(0, 0, 0);  // 设置背景颜色为黑色viewer->addPointCloud<pcl::PointXYZRGB>(coloredCloud, "result");  // 将带颜色的点云添加到可视化窗口viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "result");  // 设置点的大小// 启动可视化主循环，保持窗口打开直到用户关闭while (!viewer->wasStopped()){viewer->spinOnce(100);  // 刷新窗口，间隔100ms}
}int main(int argc, char** argv)
{// 1. 创建PointCloud对象，用于存储点云数据pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);// 2. 读取点云数据，加载PCD文件if (pcl::io::loadPCDFile("bunny.pcd", *cloud) == -1){PCL_ERROR("Couldn't read file input.pcd \n");  // 如果文件加载失败，输出错误信息return -1;  // 返回错误代码并退出程序}// 3. 创建KD树对象，并将点云数据输入到KD树中进行构建pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;  // 创建KD树对象kdtree.setInputCloud(cloud);  // 将点云数据设置为KD树的输入// 4. 定义一个查询点，用于进行搜索pcl::PointXYZ searchPoint(-0.0208042, 0.125974, 0.0209606);  // 查询点的坐标// 5. k近邻搜索（K Nearest Neighbor Search）int k = 1000;  // 设置k值，表示要查找的最近邻点的数量std::vector<int> pointIdxNKNSearch(k);  // 用于存储最近邻点的索引std::vector<float> pointNKNSquaredDistance(k);  // 用于存储最近邻点的距离平方值std::cout << "K nearest neighbor search at (" << searchPoint.x<< " " << searchPoint.y<< " " << searchPoint.z << ") with K=" << k << std::endl;// 使用KD树进行k近邻搜索，找到距离查询点最近的k个点if (kdtree.nearestKSearch(searchPoint, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0){// 如果搜索成功，将搜索结果进行可视化visualizeResult(cloud, pointIdxNKNSearch);}// 6. 半径搜索（Radius Search）float radius = 0.03;  // 设置搜索半径std::vector<int> pointIdxRadiusSearch;  // 用于存储在指定半径范围内的点的索引std::vector<float> pointRadiusSquaredDistance;  // 用于存储在指定半径范围内的点的距离平方值std::cout << "Neighbors within radius " << radius << " around point ("<< searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ")" << std::endl;// 使用KD树进行半径搜索，找到查询点在给定半径范围内的所有点if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0){// 如果搜索成功，将搜索结果进行可视化visualizeResult(cloud, pointIdxRadiusSearch);}// 7. 混合搜索（k近邻搜索 + 半径搜索）std::vector<int> hybridSearchIndices;  // 存储混合搜索结果的索引for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i){// 判断k近邻搜索的点是否也在指定半径范围内if (pointNKNSquaredDistance[i] <= radius * radius){hybridSearchIndices.push_back(pointIdxNKNSearch[i]);  // 如果满足条件，将索引添加到结果中}}std::cout << "Hybrid search results within K=" << k << " and radius " << radius << std::endl;// 如果混合搜索结果不为空，将结果进行可视化if (!hybridSearchIndices.empty()){visualizeResult(cloud, hybridSearchIndices);}return 0;
}

三、实现效果

3.1处理后点云

3.2数据显示

K nearest neighbor search at (-0.0208042 0.125974 0.0209606) with K=1000
Neighbors within radius 0.03 around point (-0.0208042 0.125974 0.0209606)
Hybrid search results within K=1000 and radius 0.03