amcl之kd_tree

一、Kd-tree

 Kd-Tree，即K-dimensional tree，是一棵二叉树，树中存储的是一些K维数据。在一个K维数据集合上构建一棵Kd-Tree代表了对该K维数据集合构成的K维空间的一个划分，即树中的每个结点就对应了一个K维的超矩形区域（Hyperrectangle）。

k-d树算法可以分为两大部分，一部分是有关k-d树本身这种数据结构建立的算法，另一部分是在建立的k-d树上如何进行最邻近查找的算法。

在介绍Kd-tree的相关算法前，我们先回顾一下二叉查找树（Binary Search Tree）的相关概念和算法。

二叉查找树（Binary Search Tree，BST），是具有如下性质的二叉树（来自wiki）：

1）若它的左子树不为空，则左子树上所有结点的值均小于它的根结点的值；

2）若它的右子树不为空，则右子树上所有结点的值均大于它的根结点的值；

3）它的左、右子树也分别为二叉排序树；

例如，图1中是一棵二叉查找树，其满足BST的性质。

图1 二叉查找树（来源：Wiki）

                                                              怎样构造一棵Kd-tree？

对于Kd-tree这样一棵二叉树，我们首先需要确定怎样划分左子树和右子树，即一个K维数据是依据什么被划分到左子树或右子树的。

在构造1维BST树时，一个1维数据根据其与树的根结点和中间结点进行大小比较的结果来决定是划分到左子树还是右子树，同理，我们也可以按照这样的方式， 将一个K维数据与Kd-tree的根结点和中间结点进行比较，只不过不是对K维数据进行整体的比较，而是选择某一个维度Di，然后比较两个K维数在该维度 Di上的大小关系，即每次选择一个维度Di来对K维数据进行划分，相当于用一个垂直于该维度Di的超平面将K维数据空间一分为二，平面一边的所有K维数据 在Di维度上的值小于平面另一边的所有K维数据对应维度上的值。也就是说，我们每选择一个维度进行如上的划分，就会将K维数据空间划分为两个部分，如果我 们继续分别对这两个子K维空间进行如上的划分，又会得到新的子空间，对新的子空间又继续划分，重复以上过程直到每个子空间都不能再划分为止。以上就是构造 Kd-Tree的过程，上述过程中涉及到两个重要的问题：1）每次对子空间的划分时，怎样确定在哪个维度上进行划分；2）在某个维度上进行划分时，怎样确 保在这一维度上的划分得到的两个子集合的数量尽量相等，即左子树和右子树中的结点个数尽量相等。

问题1： 每次对子空间的划分时，怎样确定在哪个维度上进行划分？

最简单的方法就是轮着来，即如果这次选择了在第i维上进行数据划分，那下一次就在第j(j≠i)维上进行划分，例如：j = (i mod k) + 1。想象一下我们切豆腐时，先是竖着切一刀，切成两半后，再横着来一刀，就得到了很小的方块豆腐。

可是“轮着来”的方法是否可以很好地解决问题呢？再次想象一下，我们现在要切的是一根木条，按照“轮着来”的方法先是竖着切一刀，木条一分为二，干净利 落，接下来就是再横着切一刀，这个时候就有点考验刀法了，如果木条的直径（横截面）较大，还可以下手，如果直径较小，就没法往下切了。因此，如果K维数据 的分布像上面的豆腐一样，“轮着来”的切分方法是可以奏效，但是如果K维度上数据的分布像木条一样，“轮着来”就不好用了。因此，还需要想想其他的切法。

如果一个K维数据集合的分布像木条一样，那就是说明这K维数据在木条较长方向代表的维度上，这些数据的分布散得比较开，数学上来说，就是这些数据在该维度 上的方差（invariance）比较大，换句话说，正因为这些数据在该维度上分散的比较开，我们就更容易在这个维度上将它们划分开，因此，这就引出了我 们选择维度的另一种方法：最大方差法（max invarince），即每次我们选择维度进行划分时，都选择具有最大方差维度。

备注：

   1. 方差是在概率论和统计方差衡量随机变量或一组数据时离散程度的度量。概率论中方差用来度量随机变量和其数学期望（即均值）之间的偏离程度。在统计描述中，方差用来计算每一个变量（观察值）与总体均数之间的差异。为避免出现离均差总和为零，离均差平方和受样本含量的影响，统计学采用平均离均差平方和来描述变量的变异程度。总体方差计算公式：

当数据分布比较分散（即数据在平均数附近波动较大）时，各个数据与平均数的差的平方和较大，方差就较大；当数据分布比较集中时，各个数据与平均数的差的平方和较小。因此方差越大，数据的波动越大；方差越小，数据的波动就越小。

  2.。我们研究Kd-Tree是为了优化在一堆数据中高频查找的速度，用树的形式，也是为了尽快的缩小检索范围，所以这个“比对维”就很关键，通常来说，更为分散的维度，我们就更容易的将其分开，是以这里我们通过求方差，用方差最大的维度来进行划分——这也就是最大方差法（max invarince）。
 

问题2：在某个维度上进行划分时，怎样确保在这一维度上的划分得到的两个子集合的数量尽量相等，即左子树和右子树中的结点个数尽量相等？

假设当前我们按照最大方差法选择了在维度i上进行K维数据集S的划分，此时我们需要在维度i上将K维数据集合S划分为两个子集合A和B，子集合A中的数据 在维度i上的值都小于子集合B中的数据。首先考虑最简单的划分法，即选择第一个数作为比较对象（即划分轴，pivot），S中剩余的其他所有K维数据都跟该 pivot在维度i上进行比较，如果小于pivot则划A集合，大于则划入B集合。把A集合和B集合分别看做是左子树和右子树，那么我们在构造一个二叉树 的时候，当然是希望它是一棵尽量平衡的树，即左右子树中的结点个数相差不大。而A集合和B集合中数据的个数显然跟pivot值有关，因为它们是跟pivot比较后才被划分到相应的集合中去的。好了，现在的问题就是确定pivot了。给定一个数组，怎样才能得到两个子数组，这两个数组包含的元素 个数差不多且其中一个子数组中的元素值都小于另一个子数组呢？方法很简单，找到数组中的中值（即中位数，median），然后将数组中所有元素与中值进行 比较，就可以得到上述两个子数组。同样，在维度i上进行划分时，pivot就选择该维度i上所有数据的中值，这样得到的两个子集合数据个数就基本相同了。

备注：

     选择何值未比对值，目的也是为了要加快检索速度。一般来说我们在构造一个二叉树的时候，当然是希望它是一棵尽量平衡的树，即左右子树中的结点个数相差不大。所以这里用当前维度的中值是比较合理的。

 解决了上面两个重要的问题后，就得到了Kd-Tree的构造算法了。

Kd-Tree的构建算法：

（1） 在K维数据集合中选择具有最大方差的维度k，然后在该维度上选择中值m为pivot对该数据集合进行划分，得到两个子集合；同时创建一个树结点node，用于存储；

（2）对两个子集合重复（1）步骤的过程，直至所有子集合都不能再划分为止；如果某个子集合不能再划分时，则将该子集合中的数据保存到叶子结点（leaf node）。　

 

                                                  表1  k-d树中每个节点的数据类型

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

　　从上面对k-d树节点的数据类型的描述可以看出构建k-d树是一个逐级展开的递归过程。表2给出的是构建k-d树的伪码。

                                                                             表2  构建k-d树的伪码

算法：构建k-d树（createKDTree）

输入：数据点集Data-set和其所在的空间Range

输出：Kd，类型为k-d tree

1.If Data-set为空，则返回空的k-d tree

2.调用节点生成程序： 　　（1）确定split域：对于所有描述子数据（特征矢量），统计它们在每个维上的数据方差。以SURF特征为例，描述子为64维，可计算64个方差。挑选出最大值，对应的维就是split域的值。数据方差大表明沿该坐标轴方向上的数据分散得比较开，在这个方向上进行数据分割有较好的分辨率； 　　（2）确定Node-data域：数据点集Data-set按其第split域的值排序。位于正中间的那个数据点被选为Node-data。此时新的Data-set' = Data-set\Node-data（除去其中Node-data这一点）。

3.dataleft = {d属于Data-set' && d[split] ≤ Node-data[split]}    Left_Range = {Range && dataleft}    dataright = {d属于Data-set' && d[split] > Node-data[split]}    Right_Range = {Range && dataright}

4.left = 由（dataleft，Left_Range）建立的k-d tree，即递归调用createKDTree（dataleft，Left_    Range）。并设置left的parent域为Kd；    right = 由（dataright，Right_Range）建立的k-d tree，即调用createKDTree（dataleft，Left_    Range）。并设置right的parent域为Kd。

　　先以一个简单直观的实例来介绍k-d树算法。假设有6个二维数据点{（2,3），（5,4），（9,6），（4,7），（8,1），（7,2）}，数据点位于二维空间内（如图1中黑点所示）。k-d树算法就是要确定图1中这些分割空间的分割线（多维空间即为分割平面，一般为超平面）。下面就要通过一步步展示k-d树是如何确定这些分割线的。

                                                            

                                                         图1  二维数据k-d树空间划分示意图

　　由于此例简单，数据维度只有2维，所以可以简单地给x，y两个方向轴编号为0,1，也即split={0,1}。

　　（1）确定split域的首先该取的值。分别计算x，y方向上数据的方差得知x方向上的方差最大（方差计算公式见前文），所以split域值首先取0，也就是x轴方向；

　　（2）确定Node-data的域值。根据x轴方向的值2,5,9,4,8,7排序选出中值为7，所以Node-data = （7,2）。这样，该节点的分割超平面就是通过（7,2）并垂直于split = 0（x轴）的直线x = 7；

　　（3）确定左子空间和右子空间。分割超平面x = 7将整个空间分为两部分，如图2所示。x < =  7的部分为左子空间，包含3个节点{（2,3），（5,4），（4,7）}；另一部分为右子空间，包含2个节点{（9,6），（8,1）}。

                                                         

                                                           图2  x=7将整个空间分为两部分

　　如算法所述，k-d树的构建是一个递归的过程。然后对左子空间和右子空间内的数据重复根节点的过程就可以得到下一级子节点（5,4）和（9,6）（也就是左右子空间的'根'节点），同时将空间和数据集进一步细分。如此反复直到空间中只包含一个数据点，如图1所示。最后生成的k-d树如图3所示。

                                                       

                                                                 图3  上述实例生成的k-d树

　注意：每一级节点旁边的'x'和'y'表示以该节点分割左右子空间时split所取的值。

                                                                              k-d树上的最邻近查找算法

　　在k-d树中进行数据的查找也是特征匹配的重要环节，其目的是检索在k-d树中与查询点距离最近的数据点。，以下给出利用Kd-Tree进行近期邻查找的算法，若需要查出树中与查询点Q的最临近点：

（1）从树的根结点开始，将Q与各个结点在具体的节点的划分维度上进行比较，向下访问Kd-Tree，直至达到叶子结点。

        当中Q与结点m的比較指的是将Q相应于结点中的k维度上的值与结点进行比較，若Q(k) < m(k)，则訪问左子树。否则訪问右子树。达到叶子结点时，计算Q与叶子结点上保存的数据之间的距离。记录下最小距离相应的数据点。记为当前“近邻点”Pcur和最小距离Dcur。

（2）进行回溯（Backtracking）操作，该操作是为了找到离Q更近的“近邻点”。

       即推断未被訪问过的分支里是否还有离Q更近的点。它们之间的距离小于Dcur。

假设Q与其父结点下的未被訪问过的分支之间的距离小于Dcur，则觉得该分支中有可能存在离P更近的数据，进入该结点，进行（1）步骤一样的查找过程，假设找到更近的数据点，则更新为当前的“近期邻点”Pcur，并更新Dcur。回溯的推断过程是从下往上进行的，直到回溯到根结点时已经不存在与P更近的分支为止。

备注：可以看出这样的搜索方式，会有很多的节点不会被访问到，进而节省了搜索时间，但如何证明这种方式找到的点一定会离查询点Q最近呢？

这里先以一个简单的实例来描述最邻近查找的基本思路。

　　星号表示要查询的点（2.1,3.1）。通过二叉搜索，顺着搜索路径很快就能找到最邻近的近似点，也就是叶子节点（2,3）。

这个搜索过程为：

 从根结点（7，2）开始，因这个节点的以x=7为维度划分，因为2.1<7,所以进入左子树，即进入节点（5，4），因为节点（5，4）以维度y=4划分，切因为目标点3.1<4，所以进入下一个左子树（2，3）,因为（2，3）已经为叶子结点，搜素结束。

而找到的叶子节点并不一定就是最邻近的，因为从本质上来讲，最邻近点肯定距离查询点最近，应该位于以查询点为圆心且通过叶子节点的圆域内。

         为了找到真正的最近邻，还需要进行'回溯'操作：算法沿搜索路径反向查找是否有距离查询点更近的数据点。此例中先从（7,2）点开始进行二叉查找，然后到达（5,4），最后到达（2,3），此时搜索路径中的节点为<（7,2），（5,4），（2,3）>，首先以（2,3）作为当前最近邻点，计算其到查询点（2.1,3.1）的距离为0.1414，然后回溯到其父节点（5,4），并判断在该父节点的其他子节点空间中是否有距离查询点更近的数据点。以（2.1,3.1）为圆心，以0.1414为半径画圆，如图4所示。发现该圆并不和超平面y = 4交割，因此不用进入（5,4）节点右子空间中去搜索。

图4  查找（2.1，3.1）点的两次回溯判断

　　再回溯到（7,2），以（2.1,3.1）为圆心，以0.1414为半径的圆更不会与x = 7超平面交割，因此不用进入（7,2）右子空间进行查找。至此，搜索路径中的节点已经全部回溯完，结束整个搜索，返回最近邻点（2,3），最近距离为0.1414。

　　一个复杂点了例子如查找点为（2，4.5）。同样先进行二叉查找，先从（7,2）查找到（5,4）节点，在进行查找时是由y = 4为分割超平面的，由于查找点为y值为4.5，因此进入右子空间查找到（4,7），形成搜索路径<（7,2），（5,4），（4,7）>，取（4,7）为当前最近邻点，计算其与目标查找点的距离为3.202。然后回溯到（5,4），计算其与查找点之间的距离为3.041。以（2，4.5）为圆心，以3.041为半径作圆，如图5所示。可见该圆和y = 4超平面交割，所以需要进入（5,4）左子空间进行查找。此时需将（2,3）节点加入搜索路径中得<（7,2），（2,3）>。回溯至（2,3）叶子节点，（2,3）距离（2,4.5）比（5,4）要近，所以最近邻点更新为（2，3），最近距离更新为1.5。回溯至（7,2），以（2,4.5）为圆心1.5为半径作圆，并不和x = 7分割超平面交割，如图6所示。至此，搜索路径回溯完。返回最近邻点（2,3），最近距离1.5。k-d树查询算法的伪代码如表3所示。

图5  查找（2，4.5）点的第一次回溯判断

图6  查找（2，4.5）点的第二次回溯判断

 

表3  标准k-d树查询算法

算法：k-d树最邻近查找

输入：Kd，    //k-d tree类型      target  //查询数据点

输出：nearest， //最邻近数据点      dist      //最邻近数据点和查询点间的距离

1. If Kd为NULL，则设dist为infinite并返回

2. //进行二叉查找，生成搜索路径    Kd_point = &Kd；                   //Kd-point中保存k-d tree根节点地址    nearest = Kd_point -> Node-data；  //初始化最近邻点    while（Kd_point）    　　push（Kd_point）到search_path中； //search_path是一个堆栈结构，存储着搜索路径节点指针  /*** If Dist（nearest，target） > Dist（Kd_point -> Node-data，target）    　　　　nearest  = Kd_point -> Node-data；    //更新最近邻点    　　　　Max_dist = Dist(Kd_point，target）；  //更新最近邻点与查询点间的距离  ***/    　　s = Kd_point -> split；                       //确定待分割的方向    　　If target[s] <= Kd_point -> Node-data[s]     //进行二叉查找    　　　　Kd_point = Kd_point -> left；    　　else    　　　　Kd_point = Kd_point ->right；    nearest = search_path中最后一个叶子节点； //注意：二叉搜索时不比计算选择搜索路径中的最邻近点，这部分已被注释    Max_dist = Dist（nearest，target）；    //直接取最后叶子节点作为回溯前的初始最近邻点

3. //回溯查找    while（search_path != NULL）    　　back_point = 从search_path取出一个节点指针；   //从search_path堆栈弹栈    　　s = back_point -> split；                   //确定分割方向    　　If Dist（target[s]，back_point -> Node-data[s]） < Max_dist   //判断还需进入的子空间    　　　　If target[s] <= back_point -> Node-data[s]    　　　　　　Kd_point = back_point -> right；  //如果target位于左子空间，就应进入右子空间    　　　　else    　　　　　　Kd_point = back_point -> left;    //如果target位于右子空间，就应进入左子空间    　　　　将Kd_point压入search_path堆栈；    　　If Dist（nearest，target） > Dist（Kd_Point -> Node-data，target）    　　　　nearest  = Kd_point -> Node-data；                 //更新最近邻点    　　　　Min_dist = Dist（Kd_point -> Node-data,target）；  //更新最近邻点与查询点间的距离