PCL中利用KD-Tree搜索空间点云最临近点源码

0 关于KD-Tree

关于kd-tree的基本原理和构造实现情况可以参考博文：《KD-Tree结构与算法原理》

1 PCL中使用kd-tree进行最近邻点搜索

先分部解释，完整代码见第二部分。

1.1 最近邻搜索关键函数

int pcl::search::KdTree< PointT, Tree >::nearestKSearch (
const PointT & point,
int k,
std::vector< int > & k_indices,
std::vector< float > & k_sqr_distances
) const

其中参数对应分别为：

• point：输入的目标点
• k: 选取的最近点个数
• k_indices：最近点的下标集合
• k_sqr_distances： 对应点到目标点的距离平方集合

返回的int返回值为找到的点的个数。

1.2 半径邻域搜索

int pcl::search::KdTree< PointT, Tree >::radiusSearch(
const PointT & point,
int radius,
std::vector< int > & k_indices,
std::vector< float > & k_sqr_distances
) const

其中参数对应分别为：

• point：输入的目标点
• radius： 搜索半径
• k_indices：最近点的下标集合
• k_sqr_distances： 对应点到目标点的距离平方集合

返回的int返回值为找到的点的个数。
具体使用方法见下方代码。

2 例程

//
// Created by greg on 5/5/21.
//

#include <pcl/pcl_base.h>
#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->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);
    }
	//初始化输入点集与目标点
    pcl::KdTreeFLANN<pcl::PointXYZ> 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);

    // K nearest neighbor 搜索，指定大小为10个点

    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->points[ pointIdxNKNSearch[i] ].x
                      << " " << cloud->points[ pointIdxNKNSearch[i] ].y
                      << " " << cloud->points[ 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->points[ pointIdxRadiusSearch[i] ].x
                      << " " << cloud->points[ pointIdxRadiusSearch[i] ].y
                      << " " << cloud->points[ pointIdxRadiusSearch[i] ].z
                      << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
    }

    return 0;
}

输出结果：

其中第一部分为k近邻搜索结果，输出找到的点以及距离平方。第二部分为半径邻域搜索结果，因为给出的半径r为随机数，所以搜索结果数量会变化。