【PCL】Kdtree (K-dimensional tree) 模块介绍和代码实现

最新推荐文章于 2025-05-02 08:17:46 发布

原创最新推荐文章于 2025-05-02 08:17:46 发布 · 565 阅读

5 ·

CC 4.0 BY-SA版权

文章标签：

#计算机视觉 #c++ #3d

PCL 专栏收录该内容

26 篇文章

订阅专栏

1、Kd树（K-dimensional tree）简介

Kd树是一种用于组织k维空间中点的数据结构，主要用于高效地进行范围搜索和最近邻搜索。Kd树是二叉树的一种，每个节点代表一个k维空间中的点，并且通过递归地将空间划分为两个子空间来构建。

2、Kd树的构建过程

选择分割维度：在构建Kd树时，通常选择方差最大的维度作为分割维度，或者简单地轮流选择各个维度。
选择分割点：在选定的维度上，选择中位数作为分割点，这样可以保证树的平衡。
递归构建：将数据集分为两部分，分别递归地构建左子树和右子树。

3、Kd树的搜索

最近邻搜索：从根节点开始，递归地向下搜索，直到找到叶子节点。然后回溯，检查是否有更近的邻居。
范围搜索：从根节点开始，递归地检查每个节点是否在搜索范围内，如果在范围内则加入结果集。

4、PCL（Point Cloud Library）中的Kd树

PCL是一个强大的点云处理库，提供了Kd树的实现，主要用于点云数据的最近邻搜索、范围搜索等操作。

5、PCL中Kd树的代码实现

以下是一个使用PCL库实现Kd树并进行最近邻搜索的C++示例代码kdtree_demo.cpp：

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

int main()
{
    // 创建一个点云对象
    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);
    }

    // 创建KdTree对象
    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;
    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)
    {
        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;
    }

    // 半径搜索
    float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    std::cout << "Neighbors within radius search at (" << searchPoint.x
              << " " << searchPoint.y
              << " " << searchPoint.z
              << ") with radius=" << 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
                      << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    }

    return 0;
}

CMakeLists.txt:

cmake_minimum_required(VERSION 3.10 FATAL_ERROR)

project(kdtree_demo)

find_package(PCL 1.14 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (kdtree_demo kdtree_demo.cpp)
target_link_libraries (kdtree_demo ${PCL_LIBRARIES})

运行结果

$ mkdir build && cd build
$ cmake ..
$ make
[ 50%] Building CXX object CMakeFiles/kdtree_demo.dir/kdtree_demo.cpp.o
[100%] Linking CXX executable kdtree_demo
[100%] Built target kdtree_demo
$ ./kdtree_demo 
K nearest neighbor search at (60.1689 14.2143 637.145) with K=10
    60.1326 76.0427 656.145 (squared distance: 4183.76)
    99.8589 2.16376 580.404 (squared distance: 4939.98)
    33.6821 65.0861 702.179 (squared distance: 7518.98)
    38.5616 46.545 729.416 (squared distance: 10026.1)
    125.016 39.1832 527.294 (squared distance: 16895.7)
    24.4359 5.19803 498.948 (squared distance: 20456.4)
    51.4696 21.3647 490.962 (squared distance: 21496.2)
    32.3649 114.848 744.73 (squared distance: 22475)
    3.54107 65.8858 507.298 (squared distance: 22736.9)
    182.712 110.098 671.656 (squared distance: 25401.5)
Neighbors within radius search at (60.1689 14.2143 637.145) with radius=10.0186