PCL点云库学习（三）—— KdTree

k-d树，或称k维树，是计算机科学中用于在k维空间中组织若干点的一种数据结构。它是一种带有额外约束的二叉搜索树。k-d树在范围搜索和最近邻搜索中非常有用。就我们的目的而言，我们通常只处理三维点云，因此我们所有的k-d树都是三维的。k-d树的每一层都使用垂直于相应轴的超平面，沿特定维度划分所有子节点。在树的根节点，所有子节点将根据第一个维度进行划分（即，如果第一个维度的坐标小于根节点，则它将位于左子树中；如果大于根节点，则显然位于右子树中）。树中每下降一层，就沿下一个维度进行划分，在所有其他维度都用完后返回到第一个维度。构建k-d树最有效的方法是使用一种分区方法，类似于快速排序使用的方法，将中值点放在根节点，一维值较小的放在左侧，较大的放在右侧。然后，您在左子树和右子树上重复此过程，直到要分区的最后树只包含一个元素为止。

1. 构建过程（二维示例）

假设有这些点：(2,3), (5,4), (9,6), (4,7), (8,1), (7,2)

步骤1：按X坐标排序，取中位数作为根节点
点：2, 4, 5, 7, 8, 9
中位数：7 → (7,2) 是根节点

左子树（X < 7）：(2,3), (4,7), (5,4)
右子树（X > 7）：(8,1), (9,6)

步骤2：对左右子树按Y坐标排序
左子树：(4,7) 是中位数 → 左子节点
右子树：(8,1) 是中位数 → 右子节点

步骤3：继续递归…

2. 可视化结构

text

(7,2) ← 根节点（按X分割）
/ \\
(5,4) (9,6) ← 第二层（按Y分割）
/ \\ \\
(2,3) (4,7) (8,1) ← 第三层（按X分割）

3. 分割效果图

text

Y轴
↑
8 | (4,7)
7 |
6 | (9,6)
5 |
4 | (5,4)
3 | (2,3)
2 | (7,2)
1 | (8,1)
0 └─────────────────────────────→ X轴
0 1 2 3 4 5 6 7 8 9

4.代码实现

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <iostream>
#include <vector>
#include <ctime>

int main()
{
srand(time(NULL));

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

cloud->width = 1000;
cloud->height = 1;
cloud->points.resize(cloud->width * cloud->height);

for (std::size_t i = 0; i < cloud->size(); ++i)
{
(*cloud)[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
(*cloud)[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
(*cloud)[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
}

pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;// 创建KD树对象（使用FLANN库实现）
kdtree.setInputCloud(cloud);// 将点云数据设置到KD树中，建立索引结构

pcl::PointXYZ searchPoint;// 创建一个查询点

searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);// 为查询点生成随机坐标（范围0-1024）
searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

int k = 10;// 设置K近邻搜索中K的值为10（找10个最近邻）

std::vector<int> pointIdxKNNSearch(k);// 创建向量存储K近邻搜索的结果
std::vector<float> pointKNNSquareDistance(k);// 存储对应点到查询点的距离平方

std::cout << "k nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with k=" << k << std::endl;

if (kdtree.nearestKSearch(searchPoint, k, pointIdxKNNSearch,// 输出：邻居的索引
pointKNNSquareDistance// 输出：到邻居的距离平方
) > 0)// 执行K近邻搜索 返回值：实际找到的邻居数量（>0表示找到了）
{
for (std::size_t i = 0; i < pointIdxKNNSearch.size(); ++i)// 遍历所有找到的邻居点
{
std::cout << " " << (*cloud)[pointIdxKNNSearch[i]].x
<< " " << (*cloud)[pointIdxKNNSearch[i]].y
<< " " << (*cloud)[pointIdxKNNSearch[i]].z
<< "(squared distance: " << pointKNNSquareDistance[i] << ")" << std::endl;// 输出邻居点的坐标和距离平方
}
}

std::vector<int> pointIdxRadiusSearch;// 准备半径搜索的容器（大小动态调整）
std::vector<float> pointRadiusSquareDistance;// 存储半径内点的距离平方

float radius = 256.0f * rand() / (RAND_MAX + 1.0f);// 生成随机半径（0-256范围内）

std::cout << "Neighbors within radius search at (" << searchPoint.x// 输出半径搜索的信息
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with radius= " << radius << std::endl;

if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquareDistance) > 0)// 执行半径搜索：查找所有在radius范围内的点
{
for (std::size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)// 遍历所有在半径内的点
{
std::cout << " " << (*cloud)[pointIdxRadiusSearch[i]].x
<< " " << (*cloud)[pointIdxRadiusSearch[i]].y
<< " " << (*cloud)[pointIdxRadiusSearch[i]].z
<< " (squared distance: " << pointRadiusSquareDistance[i] << ")" << std::endl;// 输出点的坐标和距离平方
}
}

return 0;
}

运行结果如下，代码分别使用kdtree进行了k近邻搜索，和半径领域搜索，运行结果可以看出能搜索筛选出了下述这些点。