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）第二个数据进来，重复14）此时根节点存在，且为叶子节点，判断第二个数据的key值是否等于根节点的key值，若等于，则更新根节点的value值，即同一个key值，权重累加。若不等于，则要根据key差值最大的维度划分左右子节点。4.1）计算key[i]的差值，确定是在(x,y,theta)中的一个维度继续进行划分，注意，这个划分就相当于直方图中划分不同组距。4.2）如当前key的某个维度的值大于根节点的key的值，则为右节点，根变成左节点;反之。（二叉排序树）4.3）左右子节点执行递归调用，对新生成的两个子节点初始化（执行2）4.4）将根节点的叶子节点的标识设置为0，因为此时已经不再是叶子节点了。第二个数据插入完成，此时树的状态是两个左右子节点，同时，第一个插入的节点根节点又是子节点，第二个节点是叶子节点。5）第三个数据进来，重复16）此时根节点存在，且存在子节点，判断根节点的划分维度的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