点云数据下的KD-tree

KD-tree简称K维树，是一种空间划分的数据结构。常被用于高维空间中的搜索，比如范围搜索和最近邻搜索。kd-tree是二进制空间划分树的一种特殊情况。
在激光雷达SLAM中，一般使用的是三维点云。所以，kd-tree的维度是3。
由于三维点云的数目一般都比较大，所以，使用kd-tree来进行检索，可以减少很多的时间消耗，可以确保点云的关联点寻找和配准处于实时的状态

数据结构

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

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

上面的数据在进行算法解析中，并不是全部都会用到。一般情况下，会用到的数据是{数据矢量，切割轴号，左支节点，右支节点}。这些数据就已经满足kd-tree的构建和检索了。

构建KD-tree

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

构建算法：

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

这样的化，就可以构建出一颗完整的kd-tree了。

例如一个python实现的二维样例：

from collections import namedtuple
from operator import itemgetter
from pprint import pformat

class Node(namedtuple("Node", "location left_child right_child")):
    def __repr__(self):
        return pformat(tuple(self))

def kdtree(point_list, depth: int = 0):
    if not point_list:
        return None

    k = len(point_list[0])  # assumes all points have the same dimension
    # Select axis based on depth so that axis cycles through all valid values
    axis = depth % k

    # Sort point list by axis and choose median as pivot element
    point_list.sort(key=itemgetter(axis))
    median = len(point_list) // 2

    # Create node and construct subtrees
    return Node(
        location=point_list[median],
        left_child=kdtree(point_list[:median], depth + 1),
        right_child=kdtree(point_list[median + 1 :], depth + 1),
    )

def main():
    """Example usage"""
    point_list = [(7, 2), (5, 4), (9, 6), (4, 7), (8, 1), (2, 3)]
    tree = kdtree(point_list)
    print(tree)

if __name__ == "__main__":
    main()

输出结果为

((7, 2),
 ((5, 4), ((2, 3), None, None), ((4, 7), None, None)),
 ((9, 6), ((8, 1), None, None), None))

生成的kd-tree检索图示：

将实验中的7个点展示在坐标轴上：(7, 2), (5, 4), (9, 6), (4, 7), (8, 1), (2, 3)
x方向维度方差较大，取x轴坐标中位数2 4 5 7 8 9，取5或者7都可以，这里取7；
取 x=7 为分割轴，则 x<7 的点为左分支， x>7 的点为右分支；
左分支：(5, 4), (4, 7), (2, 3)
右分支：(9, 6),(8, 1)

更新分割轴：下一个维度为y轴。
在左支中找到y轴的中位数(5,4)，左支数据更新为{(2,3)}，右支数据更新为{(4,7)}
在右支中找到y轴的中位数(9,6)，左支数据更新为{(8,1)}，右支数据为null。

更新分割轴：下一个维度为x轴。
确定(5,4)的子节点：
由于只有一个数据，所以，左节点为(2,3)
由于只有一个数据，所以，右节点为(4,7)
确定(9,6)的子节点：
由于只有一个数据，所以，左节点为(8,1)
右节点为空。

最终，就可以构建整个的kd-tree了。
在点云数据下，为三维环境，在三维空间x y z 轴进行空间分割；

相关资料可参考：https://en.wikipedia.org/wiki/K-d_tree

最近邻检索

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

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

由于kd-tree已经按照维度进行划分了，所以，我们在进行比较的时候，也可以通过维度进行比较，来快速定位到与其最接近的点。由于可能会进行多个最近邻点的检索，所以，算法也可能会发生变化。因此，我将从最简单的一个最近邻开始说起。

一个最近邻
一个最近邻其实很简单，我们只需要定位到对应的分支上，找到最接近的点就可以了。

举个例子：查找(2.1,3.1)的最近邻。

计算当前节点(7,2)的距离，为6.23，并且暂定为(7,2)，根据当前分割轴的维度（2.1 < 7），选取左支。
计算当前节点(5,4)的距离，为3.03，由于3.03 < 6.23，暂定为(5,4)，根据当前分割轴维度（3.1 < 4），选取左支。
计算当前节点(2,3)的距离，为0.14，由于0.14 < 3.03，暂定为(2,3)，根据当前分割轴维度（2.1 > 2），选取右支，而右支为空，回溯上一个节点。
计算(2.1,3.1)与(5,4)的分割轴{y = 4}的距离，如果0.14小于距离值，说明就是最近值。如果大于距离值，说明，还有可能存在值与(2.1,3.1)最近，需要往右支检索。
由于0.14 < 0.9，我们找到了最近邻的值为(2,3)，最近距离为0.14。