kd-tree && ikd-tree解读

一、kd-tree

1. 定义
全称Multidimensional Binary Search Tree，用k来指代数据空间的维度，是一种高维索引树形数据结构，常用于在大规模的高维数据空间进行最近邻查找，其本质是二分查找树，类似于k维二叉树，存储k维数据。注意：是二叉树的一种。

2.特点
（1）树形结构最上层的顶点称为根节点，最下层的节点称为叶子节点，中间的称为中间节点；
（2）下层节点称为上层节点的孩子，又称子节点；孩子区分左右，左边为左孩子，右边为右孩子；反之，上层节点称下层节点的父节点；
（3）根节点没有父节点，叶子节点没有孩子节点；
（4）父节点左边称为左子树，右边称为右子树；
（5）在每一层中，必须指定一个维度，并在该维度上取一个值用于划分左右子树，比如第一层取X轴为划分维度，取7为划分值，则左子树节点X坐标全部小于7，右子树节点X坐标全部大于7；每到下一层，需要重新选择新的划分维度和划分值；
（6）树形结构中的每一个节点代表着数据所在空间的一个有界区域，比如左子树的右孩子节点（4,7），从根节点往下捋，该节点代表着X小于4，Y大于7的边界约束。

3.缺点

1. 数据为多维结构，不同树节点根据不同维度进行数据划分；
2. 父节点将所有的数据划分到左右子树，叶子节点包含所有数据；
3. 使用于动态程度不高的数据，插入删除不方便；
4. 非动态增量式结构，每次都要重新构建kd-tree；
5. 深度影响搜索速度，图的结构可能不平衡。

理解：kd-tree的搜索速度与其深度有关，很多SLAM方案都需要kd-tree来维护一个动态的局部地图，因此，必然需要不断地插入新点，删除过远过旧的点，导致kd-tree的平衡性遭到破坏。换言之，在kd-tree总点数不变的情况下，由于一些位置的点被删除、其他位置的点有新增，导致kd-tree不平衡，进而导致搜索效率下降。

4.构建
kd-tree的构建是一个不断寻找划分轴和划分值，并划分左子树和右子树的递归过程。

划分值的选择通常是中值点法，即选择某维坐标中位数作为划分值，完美体现二分法思想，能最有效划分数据。

划分轴的选择有两种策略，一种是各维坐标轴轮流划分，比如第一层选X轴，第二层选Y轴，第三层选X轴，…；另一种是始终选择分布最分散的轴，无论位于哪一层（第二种策略不太懂）。也有说以K维中的最大包围边划分，即每一次分割空间以按内部元素的包围盒最大的那一边的中点位置进行分割，直至分割到一个区域内只有一个对象。

5.搜索
kd树搜索包含k近邻搜索（KNN）和ranged-k近邻搜索（ranged-KNN）。从根节点开始搜索，在每一个节点处，计算节点和查询点的距离，并放入一个排序队列中（该队列中只保留距离最近的 k 个节点），然后根据节点的划分轴和划分值判断下一步是往左子树走还是右子树走，并在相应子节点重复上述过程；同时，判断另一个子树与解空间是否有交叉，若有，则同样在对应子节点重复上述过程；所有的递归搜索直至到达叶子节点才结束，此时从排序队列获取前 k 个近邻即可。

6.增加
从根节点开始，在每一个节点处根据划分轴和划分值决定往左还是往右走，直至到达某个叶子节点，然后将新点插入到该叶子节点下方。此时，原来的叶子节点变成了中间节点。

7.删除
删除某个节点时，首先对该节点下方子树上的所有节点打散并重新构建新的子树，然后用新子树的根节点替换被删除的节点，并释放被删除节点的内存，完成删除。

二、ikd-tree

ikd-tree是由香港大学的MaRS实验室提出的最新开源增量式kd-tree数据结构（incremental kd-tree），也是Fast-lio2的主要贡献之一。

为什么提出ikd-tree？因为传统的静态kd-tree结构存在大量数据增删的情况而变得不平衡，导致深度不均衡和搜索效率下降的问题。该问题导致静态的kd-tree不适用于SLAM中。为解决静态kd-tree不均衡的问题，需要频繁构建kd-tree，造成很大的时间成本，无法满足Fast-lio2对实时性的要求。因此，若能构建一个增量式的动态kd-tree结构，支持增量式的插入和删除，能够自行调整树结构、始终保持动态平衡，那问题就很好解决了。所以，提出了ikd-tree。

1.SLAM需要的kd树
（1）支持局部地图随位姿移动，即支持高效地动态增删操作；
（2）支持平衡的高效kNN和ranged-kNN搜索，即能够始终保持树的平衡，做到动态自平衡；
（3）点的数量不要太大，不需要太稠密，即最好自带体素降采样功能；

2.ikd-tree数据结构
treesize：当前树/子树中所有节点的数量，含被标记为删除的节点；
invalidnum：当前树/子树中被标记为删除的节点的数量；
deleted：当前节点本身是否被标记为删除；
treedeleted：当前树/子树是否整个被标记为删除；
range：当前树/子树包含节点张成的cuboid空间，包含了两组{x, y, z}坐标，正好是最小包络盒的min point和max point，并且仅包括子树没有被删除的点，那些被标记为删除的点不在统计范围内。

3.搜索
基本与kd树的搜索一致，需要额外补充的是：ikd树充分利用了数据结构提到的range信息来进行剪枝加速，即在每个节点处，除了计算节点本身与查询点的距离之外，也会分别判断左右子树的range是否与目标解空间有重叠，有重叠的子树才会继续递归搜索，没有重叠的子树将直接剪枝，实现搜索加速。

4.平衡性判断依据
实现动态再平衡需要先知道哪些树或子树是不平衡的，有两个准则进行判断，一个是α-balanced，另一个是α-deleted。

α-balanced准则描述最大能够容忍一个子树左右两侧有多不平衡，否则就会被执行重构；任意一个子节点对应子树的规模不能大于特定比例，αbal越接近0.5，则子树越接近完美平衡，同时也意味着一个子树更容易被判定为不平衡。

α-deleted准则描述最多能够容忍一个子树中有多大比例被标记为删除但还没有物理删除的点，否则就会被执行重构；任意一个子树中被标记删除点的规模不能大于特定比例，αdel越小，则能够容忍标记删除点的比例越低。

5.ikd-tree再平衡算法
当ikd-tree判断发现某个子树不平衡时，就会触发对该子树的重建，实现再平衡。如果子树规模很小，重建时间成本可以忽略，在主线程上直接重建，但如果规模很大，在主线程中执行重建会导致ikd-tree一直被占用，外部的SLAM或LIO算法无法方法ikd-tree进行增删查而导致算法阻塞，因此需要辅助并行线程。

主线程重建过程：先把子树所有的节点打乱重排，在这一过程中会拿掉标记删除的点，即Flatten过程；然后，对剩下的点按中值法执行常规的kd-tree构建过程，即Build过程；构建完成后，把新的子树替换到原来的位置，即可完成。

并行线程重建过程：重建过程在并行线程中执行，主线程仍旧对外提供查询和增删服务，其中涉及到的“正在重建的子树”的增删操作会被额外缓存到OperationLogger容器中，待并行线程完成重建后，从OperationLogger把增删操作“补作业”到新的子树上，这样确保新的子树没有遗漏任何过程；最后，把新子树替换到相应位置，即可完成。

6.ikd-tree删除节点
删除操作称为box-wise delete，即输入一个立方体区域给ikd-tree，ikd-tree帮你删除这个区域内的所有点。

box-wise delete结合了range信息和lazy delete策略，其中假设各种颜色的框代表range范围，灰色框代表期望删除的区域，则其删除过程如下：从根几点开始，在每个节点处先判断节点自身是否处于灰色区域，若是则标记删除；然后，判断两个子节点的range是否与灰色区域有交叉，若没有则直接剪枝，否则重复上述过程，直至搜索完所有的子节点，完成所有被删除节点的标记删除。

7.ikd-tree插入节点与同步降采样
首先，将整个空间体素化，并明确新点落入哪个体素（目标体素）。然后向ikd-tree查询目标体素内是否已经有点以及有哪些点；如果已有点，则将已有点和新点一起排序，找出离体素中心最近的那个点，然后做判断。如果最近的点是已有点，则新点无需插入，结束处理；如果最近的点是新点，则把已有点全部标记删除，并插入新点。如果体素内不存在点，则直接插入新点。