C++ 点云数据 KdTree 处理：PCL 库代码示例

以下是使用 PCL 库对点云数据进行 KdTree 处理的 C++ 代码示例：

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

int main()
{
    // 创建点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->width = 5;
    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 * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
    }

    // 创建 KdTree 对象并设置输入点云数据
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud);

    // 设置查询点
    pcl::PointXYZ searchPoint;
    searchPoint.x = 0;
    searchPoint.y = 0;
    searchPoint.z = 0;

    // 设置最近邻点的个数
    int K = 3;

    // 执行 KdTree 搜索
    std::vector<int> pointIdxNKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(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
                      << ' (squared distance: ' << pointNKNSquaredDistance[i] << ')' << std::endl;
    }

    return 0;
}

以上代码创建了一个包含 5 个点的点云数据，并使用 KdTreeFLANN 对象对其进行处理。在代码中，首先创建了一个 pcl::PointCloud<pcl::PointXYZ>::Ptr 类型的指针 cloud，并设置其宽度和高度。然后使用随机数生成了每个点的 x、y 和 z 坐标。接着创建了一个 pcl::KdTreeFLANN<pcl::PointXYZ> 类型的对象 kdtree，并调用 setInputCloud 方法将点云数据设置为输入。然后设置了查询点 searchPoint 的坐标。接下来设置了最近邻点的个数 K，并调用 nearestKSearch 方法执行 KdTree 搜索。最后，输出了最近邻点的索引和距离。

请注意，此示例使用的是 PointXYZ 点类型，如果您的点云数据类型不同，请相应地更改代码中的类型。此外，您需要确保已正确安装和配置了 PCL 库。