PCL 中 kdtree

PCL 中 kdtree 模块

pcl 中 kd-tree 库提供了 kd-tree 数据结构，基于 FLANN 进行快速最近林检索，最近邻检索在匹配、特征描述子计算、临近特征提取中是非常基础的核心操作.

k 近邻搜索、邻域点、半径搜索、

//
// Created by xuming on 21-5-19.
//

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

#include <pcl-1.11/pcl/point_types.h>
#include <pcl-1.11/pcl/kdtree/kdtree_flann.h>

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

    // 下面代码创建 kdTreeFLANN 对象

    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;     // 创建 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);

    // 现在创建一个整数 (10) 和两个向量来存储搜索到 K 邻近

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

    // 假设e kd-tree 对象反悔了多于 0 个近邻，搜索结果已经存在之前创建的两个向量
    // pointIdxNKNSearch 和 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;
        }
    }

    // 下面代码查找到给定 searchPoint 的某一半径(随机产生) 内的所欲邻近
    // pointIdxRadiusSearch 和　pointRadiusSquaredDistance
    // 半径　r 内　近邻搜索方式

    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;

    // 像之前一样，如果 kd Tree 对象在指定半径内返回多于 0 个邻近，它将打印出

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