点云拓扑结构kd-tree

3D划分k-d tree

代码示例：
以下案例通过两种方式进行邻域搜索

方式一：指定搜索最近的K个邻居
方式二：通过指定半径搜索邻居

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

#include <iostream>
#include <vector>
#include <ctime>
//#include <pcl/search/kdtree.h>
//#include <pcl/search/impl/search.hpp>
#include <pcl/visualization/cloud_viewer.h>

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

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

    // 生成点云数据1000个
    cloud->width = 1000;
    cloud->height = 1;  // 1 表示点云为无序点云
    cloud->points.resize(cloud->width * cloud->height);

    // 给点云填充数据 0 - 1023
    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的实现类KdTreeFLANN (Fast Library for Approximate Nearest Neighbor)
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    // pcl::search::KdTree<pcl::PointXYZ> kdtree;
    // 设置搜索空间，把cloud作为输入
    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 nearest neighbor search
    // 方式一：搜索K个最近邻居

    // 创建K和两个向量来保存搜索到的数据
    // K = 10 表示搜索10个临近点
    // pointIdxNKNSearch        保存搜索到的临近点的索引
    // pointNKNSquaredDistance  保存对应临近点的距离的平方
    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
                      << " (距离平方: " << pointNKNSquaredDistance[i] << ")" << std::endl;
    }

    // Neighbors within radius search
    // 方式二：通过指定半径搜索
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    // 创建一个随机[0,256)的半径值
    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;


    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;
    }

    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor(0.0, 0.0, 0.5);
    viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud");

    pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
    // 添加从原点到搜索点的线段
    viewer.addLine(originPoint, searchPoint);
    // 添加一个以搜索点为圆心，搜索半径为半径的球体
    viewer.addSphere(searchPoint, radius, "sphere", 0);
    // 添加一个放到200倍后的坐标系
    viewer.addCoordinateSystem(200);

    while (!viewer.wasStopped()) {
        viewer.spinOnce();
    }

    return 0;
}

实现效果

输出结果
K nearest neighbor search at (834.26 469.506 42.1248) with K=10
811.675 503.8 11.0198 (距离平方: 2653.71)
831.143 528.575 49.1108 (距离平方: 3547.73)
895.526 474.046 15.4881 (距离平方: 4483.69)
800.533 404.722 17.5067 (距离平方: 5940.43)
883.627 521.055 80.4733 (距离平方: 6565.14)
812.125 504.955 118.511 (距离平方: 7581.4)
945.496 505.443 62.8462 (距离平方: 14094.3)
768.686 368.386 8.52981 (距离平方: 15653.6)
946.231 536.488 38.1303 (距离平方: 17040.2)
716.405 516.283 75.6063 (距离平方: 17198.9)
Neighbors within radius search at (834.26 469.506 42.1248) with radius=89.2612
811.675 503.8 11.0198 (距离平方:: 2653.71)
831.143 528.575 49.1108 (距离平方:: 3547.73)
895.526 474.046 15.4881 (距离平方:: 4483.69)
800.533 404.722 17.5067 (距离平方:: 5940.43)
883.627 521.055 80.4733 (距离平方:: 6565.14)
812.125 504.955 118.511 (距离平方:: 7581.4)