KD树邻域搜索原理，一看就懂，不懂请扇我

最近在学习pcl库，对其中的KD树邻域搜索不是很了解，然后查阅了很多资料都讲的不是很清楚，遂作此文章。

KD树构建

以二维平面为例，构建步骤如下：

1. 按照$j\%k+1$公式，计算要将点云数据划分的坐标轴。其中，j代表点云所处树的深度（处在树的第几行），k代表维度数（二维平面的维度数为2，三维空间的维度数为3）

   • 取$j\in [0,1,2,3,...]$，则二维平面划分坐标轴的顺序应为1，2，1，2，…；三维空间划分坐标轴的顺序应为1，2，3，1，2，3，…

   • 二维平面的1坐标轴代表x轴，2坐标轴代表y轴；三维同理

2. 依据待划分坐标轴将点云进行排序，取中位数所在的点云为根节点

   1. 假设现有点云集合<(2,3)，(4,7)，(3,2)，(7,4)，(5.3)，(6.2)>且带划分坐标轴为x轴，那么应将点云集合按x坐标进行排序，得到<(2,3)，(3,2)，(4,7)，(5,3)，(6,2)，(7,4)>

   2. 取排序后点云集合的中位数(4+5)/2=4.5，但由于KD树属于二叉查找树，即左子树值<根节点值<右子数值，故应选择较大的（5,3）点云作为根节点

   3. 此时KD树的形式如下所示，其中53代表（5，3）

3. 将树的左右子树继续按照划分坐标轴进行划分，循环往复，直到划分到叶子节点

   1. 将步骤2中KD树的左子树按照划分坐标轴（1%2）+1=2进行划分

      1. 将左子树按照y轴进行排序，得到<(3,2)，(2,3)，(4,7)>，取（2,3）点云为左子树根节点
      2. 将右子树按照y轴进行排序，得到<(6,2)，(7,4)>，取（7,4）点云为右子树根节点

   2. 此时KD树如下所示

KD树邻域搜索

搜索最邻近点

1. 原理

   1. 将待搜索最邻近点的点云$P$与KD树上的某点云$P_{j,i}$（第j行的第i个树节点）按照划分坐标轴进行大小比较
      1. 如果$P>P_{j,i}$,则向$P_{j,i}$的右子树移动，并记录右子树节点
      2. 如果$P<P_{j,i}$,则向$P_{j,i}$的左子树移动，并记录左子树节点
   2. 依据步骤1进行不断比较遍历，直到叶子节点，在该过程中记录遍历的节点。
   3. 进行回溯操作，获得最邻近点
      1. 计算到达叶子节点与待搜索最邻近点的点云$P$距离$L_P$,并将$L_P$作为最短距离$L_{min}$，$P$作为最邻近点
      2. 回溯节点
         1. 回溯节点主要包含两个操作
            • 判断回溯的节点在划分坐标轴上与待搜索最邻近点的点云$P$距离是否小于$L_{min}$.如果小于，则将回溯节点中另一未记录的节点进行记录，之后进行回溯
            • 计算回溯的节点到待搜索最邻近点的点云$P$距离，之后判断是否需要更新最小距离$L_{min}$.
         2. 不断回溯，直到根节点。此时记录的最短距离对应的点云即是最临近点。

2. 举例说明

   1. 假设待搜索最邻近点的点云坐标为（4,5）,构造的KD树如下所示。

   2. 将（4,5）与（5,3)在划分坐标轴x轴进行比较，4<5，则此时应向左子树移动，记录<(5,3)，(2,3)>

   3. 将（4,5）与（2,3)在划分坐标轴y轴进行比较，5>3，则此时应向右子树移动，记录<(5,3)，(2,3)，(4,7)>，此时到达叶子节点，深度遍历结束。

   4. 进行回溯

      1. 计算待搜索最邻近点(4,5)与叶子节点(4,7)的距离，得到2，此时设置最短距离为2，最邻近点为(4,7)
      2. 回溯到(2,3)节点
         • 计算（2,3）节点在划分坐标轴y轴上与（4,5)的距离，即5-3=2，与最短距离相同，此时还应回溯的点云集合为<(5,3)>
         • 计算（2,3）节点与（4,5)的距离，为$2\sqrt{2}>2$.则最短距离和最邻近点不变
      3. 回溯到(5,3)节点
         • 计算（5,3）节点在划分坐标轴x轴上与（4,5)的距离，即5-4=1<2，小于最短距离，此时还应回溯的点云集合增加（5,3）节点未记录的节点，变为<(7,6)>
         • 计算（5,3）节点与（4,5)的距离，为$\sqrt{5}>2$.则最短距离和最邻近点不变
      4. 由于新增了(7,6)节点，并且(7,6)节点的左子树不为空。则应该重复步骤2，3，得到更新后的还应回溯的点云集合<(7,6)，(6,2)>
      5. 之后对<(7,6)，(6,2)>进行回溯，不断更新最短距离以及增加回溯点云，得到最终最短距离为2，最邻近点为(4,7)

多个最邻近点搜索

1. 原理：基本与一个最邻近点搜索一致，下面只列出不同

   1. 假设寻找待搜索点云的m个最邻近点，则应设最短距离及对应点云集合为长度为m的列表
   2. 在回溯过程中：
      1. 填充列表。不论回溯节点与待搜索点云的距离长短，首先将最短距离和对应点云列表填满，并进行排序，找到列表中最远距离及其对应点云
      2. 不断更新最远距离。回溯过程中，将列表中的最远距离与（待搜索节点与回溯节点的距离）进行比较。若小于等于最远距离，则将（待搜索节点与回溯节点的距离）及其对应点云填入最短距离和点云列表，并更新最远距离，继续回溯，直到回溯完毕，输出点云列表即可完成多个邻近点搜索。

2. 举例说明

   1. 假设待搜索最邻近点的点云坐标为（4,5）,构造的KD树如下所示。寻找点云（4，5）的两个最邻近点。

   2. 按照之前记录，经过深度遍历后，需要回溯的点云集合为：<(5,3)，(2,3)，(4,7)>。设置最短距离列表L1，对应点云列表L2。

   3. 进行回溯

      1. 回溯到节点（4,7）,对应的距离计算为2，则此时L1={2}，L2={(4,7)}。
      2. 回溯到节点（2,3），对应距离计算为$2\sqrt{2}$。由于列表的长度为1<2，则应在列表中继续添加节点，则此时L1={2，$2\sqrt{2}$.}，L2={(4,7)，(2,3)}，最远距离为$2\sqrt{2}$.
         • 此时回溯节点与待搜索节点在划分坐标轴y轴的距离为5-3=2，不需遍历（2,3)未记录的点
      3. 回溯到节点（5,3)，对应距离计算为$\sqrt{5}<2\sqrt{2}$小于最远距离，则列表需应更新，此时L1={2，$\sqrt{5}$.}，L2={(4,7)，(5,3)}，最远距离更新为$\sqrt{5}$
         • 此时回溯节点与待搜索节点在划分坐标轴x轴的距离为5-4=1<2小于最短距离，则还需遍历（5,3）未记录的节点<(7,6)，(6,2)>
      4. 回溯到节点（7,6），对应距离计算为$\sqrt{5}=\sqrt{5}$等于最远距离，则列表需应更新，此时L1={2，$\sqrt{5}$.}，L2={(4,7)，(5,3)}，最远距离更新为$\sqrt{5}$.
      5. 回溯到点（6,2），对应距离计算为$\sqrt{13}>\sqrt{5}$大于最远距离，则列表无需应更新，此时L1={2，$\sqrt{5}$.}，L2={(4,7)，(5,3)}，最远距离仍为$\sqrt{5}$.

   4. 回溯完成，输出L2列表内的点云，即完成了对点（4,5）的多个最邻近点的搜索，结果为（4,7），(5,3)

半径R内的邻域搜索

1. 原理：与最邻近点搜索相似，只是将最短距离改成了固定距离，其他步骤不变，下面只列出不同

   1. 回溯过程中，将最邻近点搜索中的最短距离转换为固定距离（搜索半径）
      • 回溯过程中，计算待搜索点与回溯节点的距离，若该距离小于搜索半径，则记录该点云；若该距离大于搜索半径，则不记录，继续向上回溯
      • 回溯完成，即完成了半径R内的邻域搜索

2. 举例说明：

   1. 假设待搜索最邻近点的点云坐标为（4,5）,构造的KD树如下所示。进行点云（4，5）在半径为3的邻域搜索。

   2. 按照之前记录，经过深度遍历后，需要回溯的点云集合为：<(5,3)，(2,3)，(4,7)>。设置半径2内的点云列表L。

   3. 进行回溯

      1. 回溯到节点（4,7）,对应的距离计算为2<3小于搜索半径，则应记录该节点，此时L={(4,7)}
      2. 回溯到节点（2,3），对应距离计算为$2\sqrt{2}<3$小于搜索半径，则应记录该节点，此时L={(4,7)，(2,3)}
         • 此时回溯节点与待搜索节点在划分坐标轴y轴的距离为5-3=2，不需遍历（2,3)未记录的点
      3. 回溯到节点（5,3)，对应距离计算为$\sqrt{5}<3$小于搜索半径，则应记录该节点，此时L={(4,7)，(2,3)，(5,3)}
         • 此时回溯节点与待搜索节点在划分坐标轴x轴的距离为5-4=1<2小于最短距离，则还需遍历（5,3）未记录的节点<(7,6)，(6,2)>
      4. 回溯到节点（7,6），对应距离计算为$\sqrt{5}<3$小于搜素半径，则应记录该节点，此时L={(4,7)，(2,3)，(5,3)，(7,6)}
      5. 回溯到点（6,2），对应距离计算为$\sqrt{13}>3$大于搜素半径，则列表无需更新，此时L={(4,7)，(2,3)，(5,3)，(7,6)}

   4. 回溯完成，输出L列表内的点云，即完成了对点（4,5）在半径为3的邻域搜索，结果为（4,7），(2,3)，(5,3)，(7,6)

   总结

   1. KD树的构建、最邻近点搜素最重要
   2. 多个邻近点搜素和半径R搜素均是在最邻近点搜素的基础上进行