PCL 中 KdTree 的使用心得

最新推荐文章于 2025-05-08 09:19:58 发布

Neverland_LY

最新推荐文章于 2025-05-08 09:19:58 发布

阅读量4.4k

点赞数 7

CC 4.0 BY-SA版权

分类专栏：点云处理 - PCL 文章标签： kdtree优化 kdtree对比

本文链接：https://blog.csdn.net/qq_34719188/article/details/102565436

点云处理 - PCL 专栏收录该内容

38 篇文章

订阅专栏

文章目录

Notes

为了促进同行业人员（特指 LiDAR 点云处理人员或相近行业）的技术交流，解决平时开发过程中遇到的技术性问题，博主建立一个QQ群，欢迎大家积极加入，共同引领点云行业的快速发展 ~

群名：LiDAR点云部落
群号：190162198

1. 背景

今天在优化自己的代码时，发现影响程序运行时间的罪魁祸首在 KdTree 的使用上，经过优化，将 30s 的程序优化到 7s，故记录下 KdTree 的优化心得

链接：PCL KdTree 官方教程

2. nearestKSearch() 和 radiusSearch() 谁快

首先从搜索方式来讲，一个是基于最近点的个数来搜索，一个是基于半径搜索；

从结果来讲，两者得到的都是搜索到的点及这些点到搜索点距离的额平方；

两者的具体原理请自行查找；

那么在单线程的情况下，到底谁快？

其实这个没有绝对的好坏，并且也很难通过一定的方法对比好坏；

可以明确的是，这两个方法在大多数场景下是没法替换的，因为选择使用哪一种搜索方式肯定与你的用途或目的有关，比如说，你要求邻域的协方差，你只能用 radiusSearch()，你是无法通过 nearestKSearch() 来控制邻域点的个数的；反之同理；

既然你根据你的想法确定了搜索方式，那么你会问，怎么最快？

3. nearestKSearch() 的加速

方法一：并行加速(如 omp ，或者用 GPU 加速)

这里推荐一篇 xuyiqiang87 写的非常好的 OMP 文章

首先我们以 nearestKSearch() 为例，讲解怎么加速：

#pragma omp parallel for
for(int i = 0; i < cloud->points.size(); ++i) {
    pcl::PointXYZ pSearch = cloud->points[i];
    int K = 10;
    std::vector<int> vecIdx(K);
    std::vector<float> vecDistance(K);
    if( kdtree.nearestKSearch(pSearch, K, vecIdx, vecDistance) > 0 ) {
        std::cout << vecIdx.size() << std::endl;
    }
}

值得注意的是，我把 std::vector vecIdx(K); 和 std::vector vecDistance(K); 写在了 for 循环里！！！否则在并行计算中程序会 crash！！！

方法二：预先设定 K 值

预先设定 K ，并且直接初始化 vecIdx(K) 和 vecDistance(K);，从理论上来讲，该操作一定程度上防止了 std::vector 在 capacity 不够的情况下导致的内存申请与释放；

但是在现实大部分场景中，你的 capacity 足够你用。也就是说，这并不会很明显的加快你的程序，或者说，在千万级别的点云处理中，基本无速度变化…上亿级别的估计会有吧…

所以这个方法无实质性作用

方法三：在保证正确率的情况下，减少或者抽稀要构建 KdTree 的点云中点的个数

为什么要这样做？假设你的点非常密集，那么 KdTree 在做 K 临近搜索时，要进行一定的排序比较操作，点非常密集，那么差别就小，比较起来比较耗时。所以从某种程度上讲，nearestKSearch() 更适合于点数适中的情况下。

但是记得前提：在保证正确率的情况下！！！

方法四：构建多个相同的 KdTree ，分别搜索？？？

放弃吧 … 这不是明智的选择

方法五：待挖掘

…

4. radiusSearch() 的加速

方法一：并行加速(如 omp )

原理同上

方法二：点云质量较好的情况下，将大半径搜索改为小半径搜索加聚类

这个详细说一下什么意思；

在点云质量较好的情况下，假设你原来是用的 float radius = 0.5f; 进行搜索，那么的话，速度会比较慢，半径越大，需要判断的点越多，需要计算的距离的平方也越多；

那么的话，换一种想法，我们改成 float radius = 0.1f; 进行搜索，在搜索得到的点中，依次以这些点为搜索点，继续向外扩散，最终达到 radius = 0.5f 的效果；

这个思想非常类似于聚类中的 DBSCAN(网上有很多介绍)；

这种思想非常适用于区域增长，或者是基于一定条件聚类的算法中，配合上 OMP 的加速，可以达到相当好的效果；

伪代码大概为：

#pragma omp parallel for
for(int i = 0; i < cloud->points.size(); ++i) {
    pcl::PointXYZ pSearch = cloud->points[i];

    // 使用 std::queue
    std::queue<pcl::PointXYZ> q;
    std::vector<int> vecIdx;
    std::vector<float> vecDistance;
    float radius = 0.1f;
    if( kdtree.radiusSearch(pSearch, radius, vecIdx, vecDistance) > 0 ) {
        for(int j = 0; j < vecIdx.size(); ++j) {
            q.push(cloud->points[vecIdx[j]]);
        }
    }
    // 开始新的遍历
    while(q.size() > 0) {
        // TODO ...
    }
    // ...
}