AMCL代码详解（七）amcl中的kd-Tree

1、kd-Tree基础

kd树（k-dimensional树的简称），是一种分割k维数据空间的数据结构，主要应用于多维空间关键数据的近邻查找(Nearest Neighbor)和近似最近邻查找(Approximate Nearest Neighbor)。

如果我们要处理的对象集合是一个K维空间中的数据集，我们首先需要确定是：怎样将一个K维数据划分到左子树或右子树？

在构造1维BST树类似，只不过对于Kd树，在当前节点的比较并不是通过对K维数据进行整体的比较，而是选择某一个维度d，然后比较两个K维数据在该维度 d上的大小关系，即每次选择一个维度d来对K维数据进行划分，相当于用一个垂直于该维度d的超平面将K维数据空间一分为二，平面一边的所有K维数据 在d维度上的值小于平面另一边的所有K维数据对应维度上的值。也就是说，我们每选择一个维度进行如上的划分，就会将K维数据空间划分为两个部分，如果我 们继续分别对这两个子K维空间进行如上的划分，又会得到新的子空间，对新的子空间又继续划分，重复以上过程直到每个子空间都不能再划分为止。以上就是构造 Kd-Tree的过程，上述过程中涉及到两个重要的问题：

每次对子空间的划分时，怎样确定在哪个维度上进行划分；
在某个维度上进行划分时，怎样确保建立的树尽量地平衡，树越平衡代表着分割得越平均，搜索的时间也就是越少。

1、在哪个维度上进行划分？
一种选取轴点的策略是median of the most spread dimension pivoting strategy，统计样本在每个维度上的数据方差，挑选出对应方差最大值的那个维度。数据方差大说明沿该坐标轴方向上数据点分散的比较开。这个方向上，进行数据分割可以获得最好的平衡。

2、怎样确保建立的树尽量地平衡？
给定一个数组，怎样才能得到两个子数组，这两个数组包含的元素 个数差不多且其中一个子数组中的元素值都小于另一个子数组呢？方法很简单，找到数组中的中值（即中位数，median），然后将数组中所有元素与中值进行 比较，就可以得到上述两个子数组。同样，在维度d上进行划分时，划分点（pivot）就选择该维度d上所有数据的中值，这样得到的两个子集合数据个数就基本相同了。

因此，kdtree的构建如下：

1）、在K维数据集合中选择具有最大方差的维度k，然后在该维度上选择中值m为pivot对该数据集合进行划分，得到两个子集合；同时创建一个树结点node，用于存储；
2）、对两个子集合重复（1）步骤的过程，直至所有子集合都不能再划分为止；

2、amcl与kd-Tree

了解了kd树，那么kd树与amcl之间有什么关系呢？

在amcl中，对于每个粒子滤波器对象都有两个粒子集合，该粒子集合是为后续Resampling时，能将其中一个粒子集合作为缓存集合使用。但是由于每个粒子集合中包含的粒子数量很大（比如5000个粒子，可自定义），因此每个粒子的权重即使在多次Resampling之后都还是很小，并且这也将导致位置上相邻的粒子的权重差值不大，从而使粒子滤波器在最后输出预测位姿时不准确。

为了解决这个问题，在Ros的代码中便引入了一个新的结构体（Cluster）。Cluster是相邻粒子的一个集合，其具有权重与位姿等属性，与粒子相似。而其位姿是通过该Cluster中包含的粒子的位姿加权平均得到。由于初始化时粒子在地图上的位置是通过均匀分布得到，所以此时Cluster的数量与粒子数量相等，即每一个Cluster中只有一个粒子。 而当粒子在地图上产生聚合后，Cluster的数量就会急剧下降，此时选取权重最大的Cluster作为粒子滤波器的输出结果。

为了能高效存储粒子信息，并且能将相邻的粒子进行聚类分析，因此需要引入KD-Tree数据结构。

3、amcl中kd-Tree的插入流程

插入数据的基本流程：

1）新数据(pose, weight)加进来，计算key值，这里的key值其实就是pose的放大版;

2）如果是第一个数据，此时根节点为空，所以生成第一个节点，并初始化，设置leaf=1，为叶子节点，也是root节点，相应的count增加;第一个数据插入完成

3）第二个数据进来，重复1

4）此时根节点存在，且为叶子节点，判断第二个数据的key值是否等于根节点的key值，若等于，则更新根节点的value值，即同一个key值，权重累加。若不等于，则要根据key差值最大的维度划分左右子节点。

　　4.1）计算key[i]的差值，确定是在(x,y,theta)中的一个维度继续进行划分，注意，这个划分就相当于直方图中划分不同组距。

　　4.2）如当前key的某个维度的值大于根节点的key的值，则为右节点，根变成左节点;反之。（二叉排序树）

　　4.3）左右子节点执行递归调用，对新生成的两个子节点初始化（执行2）

　　4.4）将根节点的叶子节点的标识设置为0，因为此时已经不再是叶子节点了。第二个数据插入完成，此时树的状态是两个左右子节点，同时，第一个插入的节点根节点又是子节点，第二个节点是叶子节点。

5）第三个数据进来，重复1

6）此时根节点存在，且存在子节点，判断根节点的划分维度的key值与当前插入数据的key值比较，然后递归调用。第三个数据插入按照前面的流程进行，此时第一个或第二个节点成为root，与第三个节点比较插入。

7）所有数插入完成。

参考：

https://blog.csdn.net/qq_34107425/article/details/106321837

https://www.cnblogs.com/havain/p/15010138.html

https://www.jianshu.com/p/ba79ebddd93c