PCL之K维树--KD-tree

原理:KD-tree 又称 K 维树是计算机科学中使用的一种数据结构，用来组织表示 K 维空间中点集合。它是一种带有其他约束条件的二分查找树。KD-tree对于区间和近邻搜索十分有用。我们为了达到目的，通常只在三个维度中进行处理，因此所有的 KD-tree 都将是三维 KD-tree。 如下图所示（动图，慢慢看）， KD-tree 的每一级在指定维度上分开所有的子节点。在树的根部所有子节点是以第一个指定的维度上被分开（即如果第一维坐标小于根节点的点它将被分在左边的子树中，如果大于根节点的点它将分在右边的子树中）。树的每一级都在下一个维度上分开，所有其他的维度用完之后就回到第一个维度，建立 KD-tree 最高效的方法是像快速分类一样使用分割法，把指定维度的值放在根上，在该维度上包含较小数值的在左子树，较大的在右子树。然后分别在左边和右边的子树上重复这个过程，直到用户准备分类的最后一个树仅仅由一个元素组成。

代码展示:

#include<pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/cloud_viewer.h>

using PointT = pcl::PointXYZRGB;

int main() {
    // 1.读取点云
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    if (pcl::io::loadPCDFile<PointT>("/home/pi/Desktop/pcl/Open3D/examples/test_data/fragment.pcd",*cloud)==-1)
    {
        PCL_ERROR("cloud not load file!");
        return -1;
    }
    // 2.原始点云着色
    for (auto &pt: cloud->points)
    {
        pt.r = 255;
        pt.g = 255;
        pt.b = 255;
    }
    // 3.建立kd-tree
    pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
    kdtree.setInputCloud(cloud);

    // 4.K近邻搜索
    PointT searchPoint = cloud->points[0]; //设置查找点
    searchPoint.r = 0;
	searchPoint.g = 0;
	searchPoint.b = 255;

    int K = 300;  //设置需要查找的近邻点个数
    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){
            cloud->points[pointIdxNKNSearch[i]].r = 0;
            cloud->points[pointIdxNKNSearch[i]].g = 255;
            cloud->points[pointIdxNKNSearch[i]].b = 0;
        }
    }
    std::cout << "K = 300近邻点个数：" << pointIdxNKNSearch.size() << endl;

    // 5.radius半径搜索
    PointT searchPoint1 = cloud->points[3000];  //设置查找点
    std::vector<int> pointIdxRadiusSearch;  //保存每个近邻点的索引
    std::vector<float> pointRadiusSquaredDistance;  //保存每个近邻点与查找点之间的欧式距离平方
    float radius = 0.3;  //设置查找半径范围

    std::cout << "Neighbors within radius search at (" << searchPoint.x
              << " " << searchPoint.y
              << " " << searchPoint.z
              << ") with radius=" << radius << std::endl;

    if (kdtree.radiusSearch(searchPoint1, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
    {
        for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i){
            cloud->points[pointIdxRadiusSearch[i]].r = 255;
            cloud->points[pointIdxRadiusSearch[i]].g = 0;
            cloud->points[pointIdxRadiusSearch[i]].b = 0;
        }
    }
    std::cout << "半径0.03近邻点个数： " << pointIdxRadiusSearch.size() << endl;

    // 6.显示点云
    pcl::visualization::CloudViewer viewer("cloud viewer");
    viewer.showCloud(cloud);
    while (!viewer.wasStopped())
    {
    }

    return 0;
}

效果展示: