C++ PCL kdtree

2024-08-09

本文作者原创，首发在本博客；CSDN 上的同名文章已标注为转载并回链到本页：https://blog.csdn.net/qq_30220519/article/details/141070650

kdtree测试


#include <pcl/point_cloud.h>        //点类型定义头文件
#include <pcl/kdtree/kdtree_flann.h> //kdtree类定义头文件

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

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对象，并把创建的点云设置为输入,创建一个searchPoint变量作为查询点
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; // pcl::KdTreeFLANN<PointT, Dist>::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices)
  //设置搜索空间
  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 临近搜索
  //创建一个整数（设置为10）和两个向量来存储搜索到的K近邻，两个向量中，一个存储搜索到查询点近邻的索引，另一个存储对应近邻的距离平方
  int K = 10;

  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;

  if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )  //执行K近邻搜索
  {
     //打印所有近邻坐标
    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 
                << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
  }
  /**********************************************************************************
   下面的代码展示查找到给定的searchPoint的某一半径（随机产生）内所有近邻，重新定义两个向量
   pointIdxRadiusSearch  pointRadiusSquaredDistance来存储关于近邻的信息
   ********************************************************************************/
  // 半径 R内近邻搜索方法

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

  float radius = 256.0f * rand () / (RAND_MAX + 1.0f);   //随机的生成某一半径
  //打印输出
  std::cout << "Neighbors within radius search at (" << searchPoint.x 
            << " " << searchPoint.y 
            << " " << searchPoint.z
            << ") with radius=" << radius << std::endl;

  // 假设我们的kdtree返回了大于0个近邻。那么它将打印出在我们"searchPoint"附近的10个最近的邻居并把它们存到先前创立的向量中。
  if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )  //执行半径R内近邻搜索方法
  {
    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 
                << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
  }


  return 0;
}

输出

K nearest neighbor search at (384.369 386.372 300.143) with K=10
    451.114 363.553 229.213 (squared distance: 10006.7)
    452.721 398.977 207.076 (squared distance: 13492.4)
    276.971 343.368 327.111 (squared distance: 14111)
    447.772 491.576 334 (squared distance: 16234.2)
    410.276 386.502 175.288 (squared distance: 16259.9)
    445.201 488.637 253.626 (squared distance: 16322.6)
    257.128 435.577 321.633 (squared distance: 19073.3)
    312.382 389.858 178.369 (squared distance: 20023)
    439.166 412.965 167.177 (squared distance: 21390)
    482.03 493.711 266.617 (squared distance: 22183.4)
Neighbors within radius search at (384.369 386.372 300.143) with radius=193.159
    451.114 363.553 229.213 (squared distance: 10006.7)
    452.721 398.977 207.076 (squared distance: 13492.4)
    276.971 343.368 327.111 (squared distance: 14111)
    447.772 491.576 334 (squared distance: 16234.2)
    410.276 386.502 175.288 (squared distance: 16259.9)
    445.201 488.637 253.626 (squared distance: 16322.6)
    257.128 435.577 321.633 (squared distance: 19073.3)
    312.382 389.858 178.369 (squared distance: 20023)
    439.166 412.965 167.177 (squared distance: 21390)
    482.03 493.711 266.617 (squared distance: 22183.4)
    521.246 429.693 256.285 (squared distance: 22535.5)
    512.669 307.615 320.109 (squared distance: 23062.3)
    529.352 399.11 354.953 (squared distance: 24186.5)
    292.235 430.095 178.472 (squared distance: 25204.2)
    468.447 428.21 169.938 (squared distance: 25772.9)
    486.896 429.648 184.04 (squared distance: 25864.7)
    484.24 305.494 397.702 (squared distance: 26033.2)
    285.843 501.384 233.359 (squared distance: 27395.3)
    314.749 252.534 227.244 (squared distance: 28073.7)
    370.492 316.152 146.118 (squared distance: 28846.8)
    272.179 257.047 296.281 (squared distance: 29326.1)
    424.155 426.806 135.94 (squared distance: 30180.5)
    535.554 370.488 215.772 (squared distance: 30227.8)
    237.352 309.898 353.828 (squared distance: 30344.3)
    222.959 424.764 356.473 (squared distance: 30700.3)
    505.552 501.396 241.199 (squared distance: 31390.5)
    519.049 502.714 303.867 (squared distance: 31688.1)
    374.487 562.587 337.03 (squared distance: 32510.2)
    471.023 434.838 140.425 (squared distance: 35367.6)
    204.779 353.992 361.033 (squared distance: 37008.4)
    219.731 482.102 272.668 (squared distance: 37024.6)

kdtree扩展

支持百万级别的图层快速搜索和光线追踪

内存管理：确保高效地利用内存。
数据结构优化：使用合适的数据结构来存储节点和图层信息。
算法优化：确保搜索算法能够快速找到最近的图层。
并行处理：利用多线程或多进程来加速搜索过程。

扩展到图层搜索


struct Layer {
  Vec3 position; // 图层的位置
  float radius;  // 图层的半径
  // ... 其他图层属性
};

struct KDTreeNode {
  int axis;        // 当前节点分割轴
  float split;     // 分割值
  std::vector<Layer> layers; // 存储在此节点内的图层
  KDTreeNode* left; // 左子树
  KDTreeNode* right; // 右子树
};

KDTreeNode* buildKDTree(std::vector<Layer>& layers, int depth = 0) {
  if (layers.empty()) return nullptr;

  int axis = depth % 3; // 选择 x, y, z 轴
  std::sort(layers.begin(), layers.end(), [axis](const Layer& a, const Layer& b) {
    return a.position[axis] < b.position[axis];
  });

  int mid = layers.size() / 2;
  KDTreeNode* node = new KDTreeNode();
  node->axis = axis;
  node->split = layers[mid].position[axis];
  node->left = buildKDTree(std::vector<Layer>(layers.begin(), layers.begin() + mid), depth + 1);
  node->right = buildKDTree(std::vector<Layer>(layers.begin() + mid + 1, layers.end()), depth + 1);
  node->layers = std::vector<Layer>(layers.begin() + mid, layers.begin() + mid + 1);

  return node;
}

加入光线追踪


struct Ray {
  Vec3 origin; // 光线起点
  Vec3 direction; // 光线方向
};
bool intersectRayLayer(const Ray& ray, const Layer& layer, float& t) {
  // 实现光线与图层相交的计算
  // 返回是否相交，t 为交点距离
  // ...
}

std::vector<Layer> searchKDTree(KDTreeNode* node, const Ray& ray, float t_min, float t_max) {
  std::vector<Layer> result;

  if (!node) return result;

  // 检查光线是否与当前节点的图层相交
  for (const auto& layer : node->layers) {
    float t;
    if (intersectRayLayer(ray, layer, t) && t >= t_min && t <= t_max) {
      result.push_back(layer);
    }
  }

  // 根据光线与分割平面的关系决定搜索哪一侧
  if (ray.origin[node->axis] < node->split) {
    result.insert(result.end(), searchKDTree(node->left, ray, t_min, t_max).begin(), searchKDTree(node->left, ray, t_min, t_max).end());
    result.insert(result.end(), searchKDTree(node->right, ray, t_min, t_max).begin(), searchKDTree(node->right, ray, t_min, t_max).end());
  } else {
    result.insert(result.end(), searchKDTree(node->right, ray, t_min, t_max).begin(), searchKDTree(node->right, ray, t_min, t_max).end());
    result.insert(result.end(), searchKDTree(node->left, ray, t_min, t_max).begin(), searchKDTree(node->left, ray, t_min, t_max).end());
  }

  return result;
}

扩展测试


int main() {
  // 准备图层数据
  std::vector<Layer> layers;
  // 填充 layers...

  // 构建 KDTree
  KDTreeNode* root = buildKDTree(layers);

  // 定义光线
  Ray ray;
  ray.origin = {0, 0, 0};
  ray.direction = {1, 0, 0};

  // 搜索 KDTree
  std::vector<Layer> intersections = searchKDTree(root, ray, 0.01, 1000.0);

  // 打印结果
  for (const auto& layer : intersections) {
    // 打印相交的图层信息
    // ...
  }

  // 清理内存
  // ...

  return 0;
}