点云搜索--KdTree近邻点搜索

1.版本要求

版本: >1.0

2.简介

Kdtree全称K dimension tree，在点云处理中用于搜索指定点附近的n个最近的点或指定点半径范围内的所有点。它是一种数据结构，在计算机科学中用于组织K维空间的点，实现了快速的近邻点搜索。在点云处理中，它就是三维的，K等于三。它是一种二分搜索方法，也叫二叉树。
下面网址的视频将Kdtree介绍得很清楚，大家有兴趣可以去学习。
https://www.bilibili.com/video/BV1d5411w7f5

3.代码

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

#include <iostream>
#include <vector>
#include <ctime>

int main(int argc, char** argv)
{
    srand(time(NULL));

    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 (std::size_t i = 0; i < cloud->size(); ++i)
    {
        (*cloud)[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
        (*cloud)[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
        (*cloud)[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
    }

    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;  //创建kdtree
    kdtree.setInputCloud(cloud);  //kdtree输入点云

    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 (std::size_t i = 0; i < pointIdxNKNSearch.size(); ++i)
            std::cout << "    " << (*cloud)[pointIdxNKNSearch[i]].x
            << " " << (*cloud)[pointIdxNKNSearch[i]].y
            << " " << (*cloud)[pointIdxNKNSearch[i]].z
            << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
    }

    // 第二种方式:搜索指定点设定半径范围内的所有点
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    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 (std::size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
            std::cout << "    " << (*cloud)[pointIdxRadiusSearch[i]].x
            << " " << (*cloud)[pointIdxRadiusSearch[i]].y
            << " " << (*cloud)[pointIdxRadiusSearch[i]].z
            << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    }


    return 0;
}

4.效果