PCL中使用KdTree搜索

PCL中使用KdTree搜索 摘要

本文介绍了如何使用pcl中的KdTree模块实现最近邻搜索和radius搜索。

理论

kd tree，也就是k维树，是计算机科学中用来组织k维空间中点数据的一种数据结构。它是二叉树的一种变种，主要是在二叉树上添加了一些其他条件限制。对于范围和最近邻搜索来说，K-d树是非常有用的数据结构。在点云中，通常只需要三维树，因此K-d树对于我们的目标来说就是3D的。k-d 树的每个级别都使用垂直于相应轴的超平面沿特定维度拆分所有子级。在树的根部，所有子级都将根据第一个维度进行拆分（即，如果第一个维度坐标小于根，它将位于左子树中，如果它大于根，则它显然将位于右侧子树中）。树中的每个级别都在下一个维度上划分，一旦所有其他维度都用尽，就会返回到第一个维度。构建 k-d 树的最有效方法是使用分区方法，就像 Quick Sort 用于将中位数点放在根目录下的方法一样，将一维值较小的内容放在左侧，将较大的一维值放置在右侧。然后，在左侧和右侧子树上重复此过程，直到要分区的最后一个树仅由一个元素组成。

上图是一个二维k-d树。

最近邻搜索原理解析：

在PCL中实现K-d树以及KNN和Radius search

// pcl
#include 
#include  //for KdTreeFLANN
// std
#include 
#include 
#include 
#include 
int
main ()
{
  srand (time (NULL)); // 随机数发生器

  pcl::PointCloud::Ptr cloud (new pcl::PointCloud);

  // Generate pointcloud data
  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 kdtree; // 使用KDTREE数据结构的3D空间locator

  kdtree.setInputCloud (cloud);

  pcl::PointXYZ queryPoint;

  queryPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
  queryPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
  queryPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
  // -------------------------------
  // K nearest neighbor search
  // -------------------------------
  int K = 10;

  std::vector pointIdxKNNSearch(K);
  std::vector pointKNNSquaredDistance(K);

  std::cout << "K nearest neighbor search at (" << queryPoint.x 
            << " " << queryPoint.y 
            << " " << queryPoint.z
            << ") with K=" << K << std::endl;

  if ( kdtree.nearestKSearch (queryPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) > 0 )
  {
    for (std::size_t i = 0; i < pointIdxKNNSearch.size (); ++i)
      std::cout << "    "  <<   (*cloud)[ pointIdxKNNSearch[i] ].x 
                << " " << (*cloud)[ pointIdxKNNSearch[i] ].y 
                << " " << (*cloud)[ pointIdxKNNSearch[i] ].z 
                << " (squared distance: " << pointKNNSquaredDistance[i] << ")" << std::endl;
  }
  // -------------------------------
  // Neighbors within radius search
  // -------------------------------
  std::vector pointIdxRadiusSearch;
  std::vector pointRadiusSquaredDistance;

  float radius = 256.0f * rand () / (RAND_MAX + 1.0f);

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


  if ( kdtree.radiusSearch (queryPoint, 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;
}