PCL之kd-tree详解

kd，即k-dimension，kd树就是k维树，由于点云所在空间几乎都是三维的，所以最常见的也是3d树。

考虑到演示方便，所以用二维空间中的数据来做一点说明，假设现有六个数据点{（2,3），（5,4），（9,6），（4,7），（8,1），（7,2）}，如何能够通过这六个点将空间分割，就如下图一样。

当然上面这个图只是一种方案，或许还有其他的划分方式。

对此例来说，其分割步骤为

1. 计算x，y方向上数据的标准差，可得$sx\approx 2.4,sy\approx 2.1$，即$sx>xy$，所以先分割x方向的数据。
2. x轴方向的值为2,5,9,4,8,7，排序后为2,4,5,7,8,9，中值为7，所以先在$x=7$的位置画一条与y轴平行的线，这条线经过点(7,2)，将平面分成左子空间和右子空间。
3. $x⩽7$的左子空间，包含3个节点{(2,3), (5,4), (4,7)}；$x>7$的右子空间，则包含2个节点{(9,6)，(8,1)}。
4. 对左子空间和右子空间重复1过程，直到所有的点都画了线，最终就得到上图了。

而这个生成分割图的过程，也是生成kd树的过程，其树图为

7,2

2,3

5,4

4,7

9,6

8,1

在算法导论中，树往往是和查找挂钩的，kd树亦然，可用于查找与目标点最近的点。

例如要查询点$(3,3)$，则其步骤为

1. 和点

(7,2)比较$x$坐标大小，由于$3<7$，故在其左子节点；如果在平面图中看，则在$x=7$的左子区间。

• 进入左子节点后，和$(5,4)$比较$y$值大小，由于$3<4$，故继续比较左子节点；如果在平面图中比较，则在$y=4$的下子区间。
• 由于$(5,4)$的左子节点只有一个，所以距离

• (3,3)最近的点就是

  (2,3)。

  在PCL中生成kd-tree，只需通过setInputCloud建立搜索空间，便能借助kd树的速度来进行临近点索引，接下来仍然以兔子为例，试用一下PCL中的两个搜索函数nearestKSearch和radiusSearch，前者指明最近的K个点；后者搜索出距离目标小于r的点。

  #include<iostream>
  #include<pcl/io/pcd_io.h>
  #include<pcl/common/common.h>         //getMinMax3D函数需要用到
  #include <pcl/kdtree/kdtree_flann.h> 	//kdtree类定义头文件
  

  using namespace pcl;
  using namespace std;

  void randomPoint(PointXYZ pSearch, PointXYZ pMin, PointXYZ pMax) {
  pSearch->x = rand()/RAND_MAX (pMax.x - pMin.x) + pMin.x;
  pSearch->y = rand()/ RAND_MAX (pMax.x - pMin.x) + pMin.y;
  pSearch->z = rand()/ RAND_MAX (pMax.x - pMin.x) + pMin.z;
  cout << “the point (”
  << pSearch->x << " “ << pSearch->y << ” “ << pSearch->z
  << ”) will be searched" <<endl;
  }

  void kdTest() {
  PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
  char strfilepath[256] = “rabbit.pcd”;
  io::loadPCDFile(strfilepath, *cloud);
  KdTreeFLANN<PointXYZ> kdtree; //建立kd tree
  kdtree.setInputCloud(cloud); //设置搜索空间

  PointXYZ pSearch, pMin, pMax; //搜索点，三个轴的最大值和最小值
  getMinMax3D(*cloud, pMin, pMax); //需要include<pcl/common/common.h>
  randomPoint(&pSearch, pMin, pMax); //随机生成搜索点
  PointXYZ tmp; //用于存储临时点

  int K = 2;
  std::vector<int> ptIdxByKNN(K); //存储查询点近邻索引
  std::vector<float> ptKNN(K); //存储近邻点对应距离平方
  kdtree.nearestKSearch(pSearch, K, ptIdxByKNN, ptKNN); //执行K近邻搜索

  cout << “KNN search with K=” << K << endl;
  for (size_t i = 0; i < ptIdxByKNN.size(); ++i) {
  tmp = cloud->points[ptIdxByKNN[i]];
  cout << tmp.x << " “ << tmp.y << ” “ << tmp.z //打印搜索到的点
  << ” (squared distance: “ << ptKNN[i] << ”)" << endl;
  }

  // 半径 R内近邻搜索方法
  float r = 2.5;
  std::vector<int> ptIdxByRadius; //存储近邻索引
  std::vector<float> ptRadius; //存储近邻对应距离的平方
  kdtree.radiusSearch(pSearch, r, ptIdxByRadius, ptRadius);
  std::cout << “search with radius=” << r << endl;
  for (size_t i = 0; i < ptIdxByRadius.size(); ++i) {
  tmp = cloud->points[ptIdxByRadius[i]];
  cout << tmp.x << " “ << tmp.y << ” “ << tmp.z
  << ” (squared distance: “ << ptRadius[i] << ”)" << endl;
  }
  }

  

  其输出结果为

  the point (-6.793 -6.22287 -7.08207) will be searched
  KNN search with K=2
  -3.55957 -6.14628 -1.86421 (squared distance: 37.687)
  -3.99903 -6.1438 -1.59019 (squared distance: 37.9733)
  search with radius=6.15
  -3.55957 -6.14628 -1.86421 (squared distance: 37.687)