【PCL自学：kdTree】PCL中kd-Tree的原理及使用(持续更新)

一、Kd-Tree结构体及其使用

  在本章中，我们将学习如何使用KdTree来找到一个特定的点或位置的K个最近的邻点，然后我们还将学习如何在用户指定的某个半径内(在本例中是随机的)找到所有的邻点。

1）原理:什么是kd-Tree？

  k-dTree，称为k维树，是计算机科学中用于组织k维空间中的一些点的一种数据结构。它是一棵二叉搜索树，K-d树对于距离和最近邻搜索非常有用。在进行特征点计算或者点云配准等工作时常用，可以用来减小搜索时间，我们通常只处理三维的点云，所以我们所有的k-d树都是三维的。k-d树的每一层都使用垂直于相应轴的超平面，沿着特定的维度分割所有子节点。在树的根，所有的子树将基于第一个维度进行分割(例如，如果第一个维度的坐标小于根，它将在左子树中，如果它大于根，它将明显在右子树中)。树中的每一层向下划分到下一个维度，当所有其他维度都耗尽时，返回到第一个维度。构建k-d树的最有效方法是使用一种排序划分方法，比如【快速排序】将中值放在根上，将一维值较小的值放在左边，较大的值放在右边。然后在左右子树上重复这个过程，直到要划分的最后一个树仅由一个元素组成。在二维数据上建立KD-tree结构如下图所示。

2）实践：如何使用PCL？

  如果不是专业搞计算优化的同学，对于kd-Tree的原理我们只需要做到略懂即可，找工作面试能说出其所以然就行了。在实际使用中，我们只需要知道通过把点云变成KD-Tree的排列方式可以加快搜索的速度即可。
  那么如何使用KD-Tree结构呢，或者说如何把点云排列成KD-Tree形式呢，请看如下代码：

#include 
#include 

#include 
#include 
#include 

int
main ()
{
  srand (time (NULL)); // 随机种子 
// [1] 创建点云指针
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
// [2] 生成一千个无序点云数据
  // 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);
  }

// [3]创建KD-Tre对象
  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
// [4]向KDTREE中传入数据，即将点云数据设置成KD-Tree结构
  kdtree.setInputCloud (cloud);
// [5]随机生成一个点
  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);

  
  int K = 10;
 // [6]K近邻搜索，即搜索该点周围的10个点
  std::vector<int> pointIdxKNNSearch(K);
  // [7]设置搜索距离为10
  std::vector<float> pointKNNSquaredDistance(K);

  std::cout << "K nearest neighbor search at (" << searchPoint.x 
            << " " << searchPoint.y 
            << " " << searchPoint.z
            << ") with K=" << K << std::endl;
// 开始K近邻搜索
  if ( kdtree.nearestKSearch (searchPoint, 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<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;
}

---

【博主简介】
  斯坦福的兔子，男，天津大学机械工程工学硕士。毕业至今从事光学三维成像及点云处理相关工作。因工作中使用的三维处理库为公司内部库，不具有普遍适用性，遂自学开源PCL库及其相关数学知识以备使用。谨此将自学过程与君共享。
博主才疏学浅，尚不具有指导能力，如有问题还请各位在评论处留言供大家共同讨论。
若前辈们有工作机会介绍欢迎私信。