kd-tree学习

发表于 分类于
我爱kd-tree，你呢？

kd-tree数据结构学习

kd-tree简介

kd-tree简称k维树，是一种空间划分的数据结构。常被用于高维空间中的搜索，比如范围搜索和最近邻搜索。kd-tree是二进制空间划分树的一种特殊情况

在激光雷达SLAM中，一般使用的是三维点云，所以kd-tree的维度是3

由于三维点云的数目一般都比较大，使用kd-tree来进行检索，可以减少很多的时间消耗，可以确保点云的关联点寻找和配准处于实时的状态

原理

数据结构

kd-tree，是k维的二叉树。其中的每一个节点都是k维的数据，数据结构如下所示

1
2
3
4
5
6
7
8
struct kdtree {
    Node-data 	- 数据矢量   数据集中某个数据点，是n维矢量（这里也就是k维）
    Range     		 - 空间矢量   该节点所代表的空间范围
    split     			 - 整数       垂直于分割超平面的方向轴序号
    Left      			 - kd树       由位于该节点分割超平面左子空间内所有数据点所构成的k-d树
    Right     		   - kd树       由位于该节点分割超平面右子空间内所有数据点所构成的k-d树
    parent    		 - kd树       父节点  
}

上面的数据在进行算法解析中，并不是全部都会用到。一般情况下，会用到的数据是

  • 数据矢量Node-data
  • 切割轴号split
  • 左支节点Left
  • 右支节点Right

这些数据就已经满足kd-tree的构建和检索了

构建方式

kd-tree的构建就是按照某种顺序将无序化的点云进行有序化排列，方便进行快捷高效的检索

1
2
3
4
5
6
7
8
9
10
11
12
Input:		无序化的点云, 维度k
Output:	 点云对应的kd-tree
Algorithm：
1、初始化分割轴：对每个维度的数据进行方差的计算，取最大方差的维度作为分割轴，标记为r；
2、确定节点：对当前数据按分割轴维度进行检索，找到中位数数据，并将其放入到当前节点上；
3、划分双支：
    划分左支：在当前分割轴维度，所有小于中位数的值划分到左支中；
    划分右支：在当前分割轴维度，所有大于等于中位数的值划分到右支中。
4、更新分割轴：r = (r + 1) % k;
5、确定子节点：
    确定左节点：在左支的数据中进行步骤2；
    确定右节点：在右支的数据中进行步骤2；

举例说明

二维样例：{（2,3），（5,4），（9,6），（4,7），（8,1），（7,2）}

这样的点分布在一个平面的不同位置上，给他画一个图

然后开始构建kd-tree

  • 初始化分割轴：按照x和y两个维度上的数据分别计算方差得到
image-20211010203732375

​ 看来还是x上的方差大，因此选择分割轴在x轴上，此时得到r = 0(x轴对应0，y轴对应1)

  • 确定当前节点：按照中位数来找，由于分割轴在x轴上，因此需要在{2，5，9，4，8，7}找中位数，发现{5,7}都可以，这里我们选择7，也就是**(7,2)**

  • 划分左右分支：从x轴对数据进行二分，以中位数7为界线，在x轴维度上，比较和7的大小，进行划分：

    • 左支：{(2,3)，(5,4)，(4,7)}

    • 右支：{(9,6)，(8,1)}

  • 更新分割轴：r = (0+1)%2 = 1(这个2是指维度数)，所以下一次分割轴在y

  • 先切左分支：得到节点为4，即**(4,7)**，此时r = 1

  • 更新分割轴，r = (1+1)%2 = 0

  • 再切右支：得到节点为9，即**(9,6)**(这里是因为上一次我取中位数就选的是靠大的那边)

  • 再重复，重复。。。

最后就分割完成啦！

image-20211010210407498

看吧！kd-tree一点也不难！

应用模式：最近邻

在构建了完整的kd-tree之后，我们想要使用他来进行高维空间的检索。所以，这里需要讲解一下比较常用的最近邻检索。

最近邻搜索，其实和之前我们曾经学习过的KNN很像。不过，在激光点云章，如果使用常规的KNN算法的话，时间复杂度会空前高涨。因此，为了减少时间消耗，在工程上，一般使用kd-tree进行最近邻检索。

举例子

由于kd-tree已经按照维度进行划分了，所以，我们在进行比较的时候，选择通过维度进行比较，来快速定位到与其最接近的点。

当然，由于可能会进行多个最近邻点的检索，所以，算法也可能会发生变化。这里我们将从最简单的一个最近邻开始说起，就以上面那个分好的kd-tree为了例子。

刚才的分割结果如下

preview
  • 比如现在我们想要检查一个点**(2.1, 3.1)**在刚才的kd-tree中的最近邻，步骤如下
    1. 先看(7, 2)，计算距离为6.23，存储最近邻点为(7, 2)。此时分割轴r=0，对应x轴，因为2.1 < 7，因此直接到左分支
    2. 看(5, 4)，计算距离为3.03，距离变小，更新最近邻点为(5, 4)。此时分割轴r= 1，对应y轴，因为3.1 > 4，因此去往左分支
    3. 看(2, 3)，计算距离为0.14，距离变小，更新最近邻点为(2, 3)。此时分割轴为x轴，因为2.1 > 2，因此去往右分支，但是往下无分支，因此直接回溯到上一节点(2, 3)处
    4. 计算0.14和(2.1, 3.1)到(5, 4)节点对应分割轴y=4的距离，0.14 < 0.9，说明右侧无可能近邻的点，否则还需要往右侧分支继续去找

How to code？

在PCL中是有现成的函数的！

kd-tree在日常使用中，一般会在两个方面使用：

  • 最近邻搜索
1
2
3
4
5
6
7
8
// 头文件
#include <pcl/kdtree/kdtree_flann.h>
// 设定kd-tree的智能指针
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
// 输入三维点云，构建kd-tree
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
// 在点云中寻找点searchPoint的k近邻的值，返回下标pointSearchInd和距离pointSearchSqDis
kdtreeCornerLast->nearestKSearch (searchPoint, k, pointIdxNKNSearch, pointNKNSquaredDistance);

注意：其中，当k为1的时候，就是最近邻搜索。当k大于1的时候，就是多个最近邻搜索

  • 距离范围搜索
1
2
3
// 在点云中寻找和点searchPoint满足radius距离的点和距离
// 返回下标pointIdxRadiusSearch和距离pointRadiusSquaredDistance
kdtreeCornerLast->radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)

一个关于kd-tree的小项目，拿去练练手吧！(官方给的)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
#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>);

  // Generate pointcloud data
  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 search

  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;
  }

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


  return 0;
}