（八）k-dTree库教程一--k-d树在PCL中的应用

k-d树在PCL中的应用

上一讲中，我们介绍了k-d树在二维上的原理，k-d树构建算法和k-d树是如何应用于最近邻查找算法的。对于PCL中的k-d树来说，只不过是将二维提升到了三维而已。下面我们就看看PCL是如何将k-d树应用于范围搜索和最近邻搜索的。

PCL中使用的k-d树的算法来自FLANN（Fast Library for Approximate Nearest Neighbors），它是目前最完整的（近似）最近邻开源库。不但实现了一系列查找算法，还包含了一种自动选取最快算法的机制。

K近邻搜索

K近邻搜索是给定查询点及正整数K，从数据集中找到距离查询点最近的K个数据。

头文件

#include <pcl/kdtree/kdtree_flann.h>

步骤

1. 初始化k-d树

// 初始化kdTreepcl::KdTreeFLANN<pcl::PointXYZ> kdtree;// 设置要搜索的点云kdtree.setInputCloud(cloud);

1. 输入查询点和正整数K进行搜索

kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance)

关键函数：

int       nearestKSearch (const PointT &point, int k,                       std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const;

• 输入输出参数：
  
  • point[in]：查询点
  • k[in]：正整数K，即要搜索的最近邻点的数目
  • k_indices[out]：搜索到的最近邻点在点云中的下标
  • k_sqr_distances[out]：搜索到的最近邻点距离查询点的距离的平方值
  • 返回值：搜索到的最近邻点的数目

范围搜索

范围搜索就是给定查询点和查询距离的阈值，从数据集中找出所有与查询点距离小于阈值的数据。

头文件

#include <pcl/kdtree/kdtree_flann.h>

步骤

1. 初始化k-d树

// 初始化kdTreepcl::KdTreeFLANN<pcl::PointXYZ> kdtree;// 设置要搜索的点云kdtree.setInputCloud(cloud);

1. 输入查询点和搜索半径进行搜索

kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)

关键函数：

int       radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,                    std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;

• 输入输出参数：
  
  • point[in]：查询点
  • radius[in]：搜索半径
  • k_indices[out]：搜索到的最近邻点在点云中的下标
  • k_sqr_distances[out]：搜索到的最近邻点距离查询点的距离的平方值
  • max_nn[in]：默认为0，搜索到的点的上限，如果搜索到的点数目多于它，那么丢弃超出的部分；如果max_nn设为0或者大于搜索点云的大小，则返回所有找到的点。
  • 返回值：搜索到的最近邻点的数目

程序

/* * 功能：kdTree的应用，包括k最近邻搜索和范围搜索*/#include <pcl/point_cloud.h>#include <pcl/kdtree/kdtree_flann.h>#include <iostream>#include <vector>#include <ctime>using namespace std;intmain(int argc, char** argv){    // 种下随机数种子    srand(time(NULL));    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);    // 用随机数，随机生成1000个点的无序点云    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);          // c++中rand()函数生成的范围:0~RAND_MAX，        cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);        cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);    }    // 初始化kdTree    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近邻搜索    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 (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;    }    // 以radius为半径的范围搜索    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 (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;    }    system("pause");    return 0;}