PCL从理解到应用【03】KDTree 原理分析 | 案例分析

前言

本文分析KDTree的原理，集合案例深入理解，同时提供源代码。

三个案例：K近邻搜索、半径内近邻搜索、近似最近邻搜索。方法对比，如下表所示：

特性	K近邻搜索	半径内近邻搜索	近似最近邻搜索
描述	查找K个最近邻点	查找指定半径内的所有点	查找近似最近邻点
返回结果数量	固定K个	不固定，取决于半径内点的数量	不固定，取决于近似效果
适用场景	需要固定数量最近邻点的应用	需要查找固定范围内点的应用	需要快速查询的应用
精度	高	高	低
速度	较慢（点云数据量大时）	较慢（半径大时）	中
代码复杂度	中	中	低

看一下示例效果：

白色的是随机生成的原始点云，红色是查询点，绿色是找到的10个最近点。

一、KDTree 原理分析

KDTree（K-Dimensional Tree，K维树）是一种用于多维空间中数据点的快速点查找的数据结构。它是计算几何领域中的一种二叉树。

构建过程:
- 将数据点递归地划分成两个子集，直到每个子集中的点数目小于等于一个。
- 每次划分时，选择某个维度，将数据点按照该维度的中位数进行分割，这样一半的数据点在分割超平面的左侧，另一半在右侧。
- 每个节点保存一个数据点及一个用于分割的维度。
搜索过程:
- 从根节点开始，递归地向下遍历树，根据查询点在当前分割维度上的值决定向左子树或右子树移动，直到达到叶节点。
- 回溯过程中，检查是否需要跨越分割超平面搜索另一子树。
- 使用一个优先级队列维护当前最优的 K 个最近邻点。

二、KDTree常用方法

KDTree常用的方法，汇总如下所示：

1. setInputCloud 方法
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr())
设置输入点云数据。

2. nearestKSearch 方法
int nearestKSearch(const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
查找查询点的K近邻。

3. radiusSearch 方法
int radiusSearch(const PointT &point, double radius, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
查找查询点在指定半径内的所有近邻。

4. getPointCloud 方法
const PointCloudConstPtr& getPointCloud() const
获取输入点云。

5. getIndices 方法
const IndicesConstPtr& getIndices() const
获取索引。

6. approxNearestSearch 方法
int approxNearestSearch(const PointT &point, int &index, float &sqr_distance) const
查找查询点的近似最近邻。近似最近邻搜索相对于精确最近邻搜索速度更快，但结果不是完全准确的。

官网：Introduction — Point Cloud Library 0.0 documentation

对应函数：Point Cloud Library (PCL): Module kdtree

Point Cloud Library (PCL): pcl::KdTree< PointT > Class Template Reference

三、KDTree优缺点分析

优点:

高效邻近搜索: 在低维数据中，KDTree 提供了一种高效的 K 近邻和范围搜索方法。
动态更新: KDTree 可以动态地插入和删除数据点，保持数据结构的有效性。
适用多种距离度量: KDTree 可以使用多种距离度量，如欧氏距离、曼哈顿距离等，适应不同应用需求。

缺点:

高维数据性能下降: 随着维度增加，KDTree 的性能会急剧下降，这是因为高维空间中的数据分布变得稀疏，导致分割效率降低。这种现象被称为“维度灾难”。
构建和维护成本: 构建和维护 KDTree 的成本较高，尤其是在数据频繁变化的场景中。
不适用于动态变化的场景: 如果数据频繁更新，KDTree 需要频繁重建，维护成本较高。

四、KDTree案例——K近邻搜索

K近邻搜索（K-Nearest Neighbors Search）是一种用于查找给定点的K个最近邻点的搜索方法，KDTree提供了一种高效的实现方式。

看一个示例深入理解，在这个示例中：

随机生成一个包含1000个点的点云。
随机选择一个查询点。
使用 kdtree.nearestKSearch 进行近似最近邻搜索，查找10个距离最近的点。
使用 pcl::visualization::PCLVisualizer 可视化原始点云、查询点和近似最近邻点。

代码示例：

#include <pcl/point_cloud.h>        // 点类型定义头文件
#include <pcl/kdtree/kdtree_flann.h> // kdtree类定义头文件
#include <pcl/visualization/pcl_visualizer.h> // PCL可视化类定义头文件

#include <iostream>
#include <vector>
#include <ctime>
#include <chrono>
#include <thread>

/*
函数功能：
K近邻搜索 (K-Nearest Neighbors, KNN)：找到距离查询点最近的K个点。
*/

int main (int argc, char** argv)
{
  srand(time(NULL)); // 用系统时间初始化随机种子

  // 创建一个PointCloud<pcl::PointXYZ>对象
  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 (size_t i = 0; i < cloud->points.size(); ++i) // 循环填充点云数据
  {
    cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f); // 产生数值为0-1024的浮点数
    cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
    cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
  }

  // 创建KdTreeFLANN对象，并把创建的点云设置为输入
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  kdtree.setInputCloud(cloud);

  // 设置查询点并赋随机值
  pcl::PointXYZ searchPoint;
  searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
  searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
  searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

  // K近邻搜索
  int K = 10; // 设置K值为10，表示查找10个最近邻
  std::vector<int> pointIdxNKNSearch(K);         // 存储查询点近邻的索引
  std::vector<float> pointNKNSquaredDistance(K); // 存储近邻点对应的距离平方

  // 打印相关信息
  std::cout << "K近邻搜索，查询点为 (" << searchPoint.x 
            << " " << searchPoint.y 
            << " " << searchPoint.z
            << ")，K=" << K << std::endl;

  // 执行K近邻搜索
  if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
  {
    // 打印所有近邻坐标
    for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
      std::cout << "    " << cloud->points[pointIdxNKNSearch[i]].x 
                << " " << cloud->points[pointIdxNKNSearch[i]].y 
                << " " << cloud->points[pointIdxNKNSearch[i]].z 
                << " (距离平方: " << pointNKNSquaredDistance[i] << ")" << std::endl;
  }

  // 创建PCLVisualizer对象
  pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("nearestKSearch"));
  viewer->setBackgroundColor(0, 0, 0); // 设置背景色为黑色

  // 添加原始点云
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> originalColor(cloud, 255, 255, 255); // 白色
  viewer->addPointCloud<pcl::PointXYZ>(cloud, originalColor, "original cloud");

  // 添加查询点
  pcl::PointCloud<pcl::PointXYZ>::Ptr searchPointCloud(new pcl::PointCloud<pcl::PointXYZ>());
  searchPointCloud->push_back(searchPoint);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> searchPointColor(searchPointCloud, 255, 0, 0); // 红色
  viewer->addPointCloud<pcl::PointXYZ>(searchPointCloud, searchPointColor, "search point");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "search point");

  // 添加K近邻点
  pcl::PointCloud<pcl::PointXYZ>::Ptr kNearestPoints(new pcl::PointCloud<pcl::PointXYZ>());
  for (size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
  {
    kNearestPoints->push_back(cloud->points[pointIdxNKNSearch[i]]);
  }
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> kNearestColor(kNearestPoints, 0, 255, 0); // 绿色
  viewer->addPointCloud<pcl::PointXYZ>(kNearestPoints, kNearestColor, "k nearest points");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "k nearest points");

  // 启动可视化
  viewer->addCoordinateSystem(1.0);
  viewer->initCameraParameters();
  while (!viewer->wasStopped())
  {
    viewer->spinOnce(100);
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }

  return 0;
}

可视化K近邻搜索的效果，如下图所示：

白色的是随机生成的原始点云，红色是查询点，绿色是找到的10个最近点。

K近邻搜索的思路流程：

初始化：
- 从根节点开始，根据查询点的坐标，决定向左子树还是右子树移动。
递归搜索：
- 递归地向下遍历树，直至达到叶节点。
- 在叶节点处，计算该叶节点数据点与查询点之间的距离，将其加入优先级队列（最大堆），用于存储当前最近的K个点。
回溯：
- 回溯到父节点，检查当前节点的数据点与查询点之间的距离，并更新优先级队列。
- 判断是否需要跨越分割超平面搜索另一子树：如果查询点到分割超平面的距离小于优先级队列中最远点的距离，则跨越分割超平面，进入另一子树进行搜索。
重复搜索和回溯：
- 重复上述搜索和回溯过程，直至回溯到根节点，最终优先级队列中存储的就是查询点的K个最近邻点。

         构建KDTree
           /    \
          /      \
         /        \
       节点      节点
       /  \      /  \
      /    \    /    \
  节点    节点  节点  节点

五、KDTree案例——近似最近邻搜索搜索

近似最近邻搜索的目标是找到查询点的近似K个最近邻点，允许一定的误差以提高搜索速度。

常见的做法是通过多次随机采样、设置较大的搜索半径或者在其他库中使用误差参数来实现近似搜索。

特点	K近邻搜索	近似最近邻搜索
精度	高	中等
性能	速度较慢，计算量大	速度快，计算量小
应用场景	需要精确结果的场景，如分类、回归等	允许一定误差的快速检索，如大规模数据处理、实时应用等
优点	结果精确，找到的是最邻近的K个点	搜索速度快，适用于大规模数据集
缺点	在高维数据中性能急剧下降，计算量大	结果不够精确，存在一定误差

示例代码：

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <vector>
#include <ctime>
#include <algorithm>
#include <chrono>
#include <thread>

int main (int argc, char** argv)
{
  srand(time(NULL));

  // 创建一个PointCloud<pcl::PointXYZ>对象
  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 (size_t i = 0; i < cloud->points.size(); ++i)
  {
    cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
    cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
    cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
  }

  // 创建KdTreeFLANN对象，并把创建的点云设置为输入
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  kdtree.setInputCloud(cloud);

  // 设置查询点并赋随机值
  pcl::PointXYZ searchPoint;
  searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
  searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
  searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

  // 近似最近邻搜索
  int K = 10;
  int trials = 5; // 设置尝试次数，通过增加随机性来实现近似搜索
  std::vector<int> pointIdxNKNSearch(K);
  std::vector<float> pointNKNSquaredDistance(K);

  std::vector<int> all_neighbors;
  std::vector<float> all_distances;

  // 多次随机采样
  for (int t = 0; t < trials; ++t)
  {
    pcl::PointXYZ randomSearchPoint = searchPoint;
    randomSearchPoint.x += static_cast<float>(rand()) / RAND_MAX * 2.0 - 1.0;
    randomSearchPoint.y += static_cast<float>(rand()) / RAND_MAX * 2.0 - 1.0;
    randomSearchPoint.z += static_cast<float>(rand()) / RAND_MAX * 2.0 - 1.0;

    if (kdtree.nearestKSearch(randomSearchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    {
      all_neighbors.insert(all_neighbors.end(), pointIdxNKNSearch.begin(), pointIdxNKNSearch.end());
      all_distances.insert(all_distances.end(), pointNKNSquaredDistance.begin(), pointNKNSquaredDistance.end());
    }
  }

  // 排序并去重
  std::vector<std::pair<float, int>> distance_index_pairs;
  for (size_t i = 0; i < all_neighbors.size(); ++i)
  {
    distance_index_pairs.emplace_back(all_distances[i], all_neighbors[i]);
  }

  std::sort(distance_index_pairs.begin(), distance_index_pairs.end());
  distance_index_pairs.erase(std::unique(distance_index_pairs.begin(), distance_index_pairs.end()), distance_index_pairs.end());

  // 选择前K个近似最近邻
  std::vector<int> final_neighbors;
  for (size_t i = 0; i < std::min(size_t(K), distance_index_pairs.size()); ++i)
  {
    final_neighbors.push_back(distance_index_pairs[i].second);
  }

  // 创建PCLVisualizer对象
  pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Vis"));
  viewer->setBackgroundColor(0, 0, 0); // 设置背景色为黑色

  // 添加原始点云
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> originalColor(cloud, 255, 255, 255); // 白色
  viewer->addPointCloud<pcl::PointXYZ>(cloud, originalColor, "original cloud");

  // 添加查询点
  pcl::PointCloud<pcl::PointXYZ>::Ptr searchPointCloud(new pcl::PointCloud<pcl::PointXYZ>());
  searchPointCloud->push_back(searchPoint);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> searchPointColor(searchPointCloud, 255, 0, 0); // 红色
  viewer->addPointCloud<pcl::PointXYZ>(searchPointCloud, searchPointColor, "search point");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "search point");

  // 添加近似最近邻点
  pcl::PointCloud<pcl::PointXYZ>::Ptr approxNearestPoints(new pcl::PointCloud<pcl::PointXYZ>());
  for (size_t i = 0; i < final_neighbors.size(); ++i)
  {
    approxNearestPoints->push_back(cloud->points[final_neighbors[i]]);
  }
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> approxNearestColor(approxNearestPoints, 0, 255, 0); // 绿色
  viewer->addPointCloud<pcl::PointXYZ>(approxNearestPoints, approxNearestColor, "approx nearest points");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "approx nearest points");

  // 启动可视化
  viewer->addCoordinateSystem(1.0);
  viewer->initCameraParameters();
  while (!viewer->wasStopped())
  {
    viewer->spinOnce(100);
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }

  return 0;
}

结果可视化：

结果分析：

白色的是随机生成的原始点云，红色是查询点，绿色是找到的2个最近点（本文需要找到10个点的）。
允许一定误差，以提高搜索速度。
在示例代码中，通过增加随机性和多次采样来实现近似搜索，最终合并和去重结果。

六、KDTree案例——半径内近邻搜索

径内近邻搜索 (Radius Search)，找到指定半径内的所有点。

思路流程：

构建 KDTree：首先，构建包含所有数据点的KDTree。这一步骤将数据点按空间位置递归地分割成子区域。
查询节点搜索：从根节点开始，检查当前节点是否在查询点的半径内。如果是，则将其加入结果集中。
递归搜索：递归地检查当前节点的子节点：
- 如果查询球体与子节点对应的空间区域相交，则继续搜索该子节点。
- 如果查询球体与子节点对应的空间区域不相交，则跳过该子节点。
合并结果：合并所有符合条件的节点，得到最终的近邻点集合。

看一个示例深入理解，在这个示例中：

随机生成一个包含1000个点的点云。
随机选择一个查询点。
使用 kdtree.radiusSearch 进行半径内近邻搜索，半径为116。
使用 pcl::visualization::PCLVisualizer 可视化原始点云、查询点和近似最近邻点。

代码示例：

#include <pcl/point_cloud.h>        // 点类型定义头文件
#include <pcl/kdtree/kdtree_flann.h> // kdtree类定义头文件
#include <pcl/visualization/pcl_visualizer.h> // PCL可视化类定义头文件

#include <iostream>
#include <vector>
#include <ctime>
#include <chrono>
#include <thread>

/*
函数功能：
半径内近邻搜索 (Radius Search)：找到指定半径内的所有点。
*/
int main (int argc, char** argv)
{
  srand(time(NULL)); // 用系统时间初始化随机种子

  // 创建一个PointCloud<pcl::PointXYZ>对象
  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 (size_t i = 0; i < cloud->points.size(); ++i) // 循环填充点云数据
  {
    cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f); // 产生数值为0-1024的浮点数
    cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
    cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
  }

  // 创建KdTreeFLANN对象，并把创建的点云设置为输入
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
  kdtree.setInputCloud(cloud);

  // 设置查询点并赋随机值
  pcl::PointXYZ searchPoint;
  searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
  searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
  searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

  // 半径内近邻搜索方法
  std::vector<int> pointIdxRadiusSearch;           // 存储近邻索引
  std::vector<float> pointRadiusSquaredDistance;   // 存储近邻对应距离的平方

  float radius = 256.0f * rand() / (RAND_MAX + 1.0f); // 随机生成某一半径

  // 打印输出
  std::cout << "半径内近邻搜索，查询点为 (" << searchPoint.x 
            << " " << searchPoint.y 
            << " " << searchPoint.z
            << ")，半径=" << radius << std::endl;

  // 执行半径内近邻搜索方法
  if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
  {
    // 打印所有近邻坐标
    for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
      std::cout << "    " << cloud->points[pointIdxRadiusSearch[i]].x 
                << " " << cloud->points[pointIdxRadiusSearch[i]].y 
                << " " << cloud->points[pointIdxRadiusSearch[i]].z 
                << " (距离平方: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
  }

  // 创建PCLVisualizer对象
  pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("radiusSearch"));
  viewer->setBackgroundColor(0, 0, 0); // 设置背景色为黑色

  // 添加原始点云
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> originalColor(cloud, 255, 255, 255); // 白色
  viewer->addPointCloud<pcl::PointXYZ>(cloud, originalColor, "original cloud");

  // 添加查询点
  pcl::PointCloud<pcl::PointXYZ>::Ptr searchPointCloud(new pcl::PointCloud<pcl::PointXYZ>());
  searchPointCloud->push_back(searchPoint);
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> searchPointColor(searchPointCloud, 255, 0, 0); // 红色
  viewer->addPointCloud<pcl::PointXYZ>(searchPointCloud, searchPointColor, "search point");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "search point");

  // 添加半径内近邻点
  pcl::PointCloud<pcl::PointXYZ>::Ptr radiusNearestPoints(new pcl::PointCloud<pcl::PointXYZ>());
  for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
  {
    radiusNearestPoints->push_back(cloud->points[pointIdxRadiusSearch[i]]);
  }
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> radiusNearestColor(radiusNearestPoints, 0, 255, 0); // 绿色
  viewer->addPointCloud<pcl::PointXYZ>(radiusNearestPoints, radiusNearestColor, "radius nearest points");
  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "radius nearest points");

  // 启动可视化
  viewer->addCoordinateSystem(1.0);
  viewer->initCameraParameters();
  while (!viewer->wasStopped())
  {
    viewer->spinOnce(100);
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }

  return 0;
}

可视化半径内近邻搜索的效果，如下图所示：

白色的是随机生成的原始点云，红色是查询点，绿色是找到的3个最近点（半径范围内）。

半径内近邻搜索，查询点为 (200.242 73.3622 785.961)，半径=116.108
166.217 33.6048 783.911 (距离平方: 2742.57)
164.154 125.101 776.535 (距离平方: 4068.13)
239.646 7.50222 856.443 (距离平方: 10857.9)

分享完成～

PCL从理解到应用【03】KDTree 原理分析 | 案例分析 | 代码实现

前言

一、KDTree 原理分析

二、KDTree常用方法

三、KDTree优缺点分析

四、KDTree案例——K近邻搜索

五、KDTree案例——近似最近邻搜索搜索

六、KDTree案例——半径内近邻搜索

网站公告

今日签到

热门文章

最新发布