0. 简介

在众多机器人应用中，通过最近邻搜索建立新采集点与历史累积数据（即地图）之间的对应关系至关重要。然而，静态树数据结构不足以实时处理大型且动态增长的地图。为了解决这个问题，我们在文中《i-Octree: A Fast, Lightweight, and Dynamic Octree for Proximity Search》提出了i-Octree，一个动态八叉树数据结构，它既支持快速最近邻搜索，也支持实时动态更新，如点插入、删除和树上下采样。i-Octree基于叶节点的八叉树构建，并具有两个关键特性：一种局部空间连续的存储策略，允许快速访问点同时最小化内存使用；以及局部树上更新，与现有的静态或动态树结构相比，显著减少了计算时间。实验表明，i-Octree通过平均在真实世界开放数据集上实现19%的运行时间减少，超越了当今最先进的方法。相关代码已经在Github上开源了

1. 主要贡献

1. 在本文中，我们提出了一种称为i-Octree的动态八叉树结构，它能够用新点增量更新八叉树，并实现快速的最近邻搜索(NNS)。
2. 此外，我们的i-Octree在时间和内存效率上均表现出色，适应各种类型的点，并允许进行树上下采样和按盒删除。
3. 我们在随机数据和真实世界开放数据集上进行验证实验，以评估i-Octree的有效性。在随机数据实验中，与最新提出的增量k-d树（即ikd-Tree[9]）相比，我们的i-Octree在运行时间上展现了显著改进。具体来说，它在构建树的运行时间上减少了64%，在点插入上减少了66%，在KNN搜索上减少了30%，在半径邻居搜索上减少了56%。此外，当应用于基于LiDAR的SLAM的真实世界数据时，i-Octree展示了显著的时间性能提升。它的速度是原始方法的两倍多，同时通常保持更高的精度水平。

2. i-Octree的设计与实现

i-Octree以序列化点云作为输入，目标有两个：动态维护一个全局地图并在该地图上执行快速的最近邻搜索（即，KNN搜索和半径邻居搜索）。图1展示了i-Octree的典型应用场景。范围传感器持续感知周围环境，并定期生成序列化的3D范围数据。利用范围数据的初始扫描来构建i-Octree并定义全局坐标框架。随后，i-Octree便能通过KNN搜索或半径邻居搜索，建立新到达数据与历史数据之间的对应关系。基于这些对应关系，可以估算新数据的姿态，并将带有姿态的3D点添加到i-Octree中。为了防止i-Octree中的地图大小无限制地增长，只保持以当前位置为中心的大型本地区域（即，与轴对齐的盒子）内的地图点。接下来，我们首先描述i-Octree的数据结构和构造方法，然后重点讨论动态更新和最近邻搜索。

图1. 在测距中使用i-Octree的示例。i-Octree和测距技术协同工作，以估算从范围传感器获得的3D数据的姿态。i-Octree提供了一个稳健且高效的数据结构，用于存储和查询3D数据，而测距技术则使得数据点的姿态估算成为可能。

3. 数据结构和构建

一个i-Octree节点最多有八个子节点，每个对应于覆盖其上的轴对齐立方体的一个八分区。一共八分区，以一个中心为c∈\mathbb{R}^3且边长相等c∈\mathbb{R}的轴对齐边界盒开始，会递归地细分为边长为\frac{1}{2}e的更小八分区，直到它包含的点数少于给定的数量——桶大小b，或者其边长小于最小边长e_{min}。为了内存效率，没有点的八分区不会被创建。此外，我们只在每个叶八分区中保留点的索引和坐标，并且非叶八分区中没有点。为了实现对每个叶八分区中每个点的极快访问，我们提出了一种局部空间连续存储策略（如图2所示），该策略在细分后为叶八分区中存储点信息的连续内存段重新分配空间。此外，重新分配便于按盒子删除和增量更新，因为它允许操作一段内存而不影响其他部分。

图2. 八分区点在内存中的位置示意图。（a）分散的位置；（b）连续的位置。

基于上述考虑，我们i-Octree的一个八分区C_o包含中心c_o∈\mathbb{R}^3，边长e_o，存储坐标和索引的点P_o，以及指向第一个子八分区地址的指针。下标“o”用于区分不同的八分区。特别地，设C_r为根八分区，P_r、e_r和c_r分别为点、边长和中心。

在构建增量八叉树时，我们首先去除无效点，计算所有有效点的轴对齐边界盒，并只保留有效点的索引和坐标。然后，从根开始，i-Octree递归地在中心处将轴对齐的边界盒分割成由Morton码[18]索引的八个立方体，并根据计算出的立方体索引将当前八分区中的所有点细分到每个立方体中。当满足停止条件时，将创建一个叶八分区，并分配一段连续内存以存储叶节点中点的信息。

4. 动态更新

动态更新包括插入一个或多个点（即增量更新）和删除轴对齐盒内的所有点（即盒式删除）。插入与下采样集成在一起，这维持了八叉树在预定分辨率下。

4.1 增量更新

插入新点时，我们必须考虑一些点可能超出原始树的轴对齐边界盒范围的情况。一旦有点超出八叉树的范围，我们就必须通过创建新的根八分区来扩展边界盒，其子八分区包含当前的根八分区。这个过程可能需要执行多次，以确保所有新点都在树的范围内。然后，将新点添加到扩展的八叉树中（见图3）。
考虑到机器人应用中高效的点查询，i-Octree支持与点插入同时执行的下采样。下采样关注新点，并删除满足一定条件的点：它们被细分到一个叶子八分区，其范围小于2e_{min}且大小大于b/8。将新点添加到八分区C_o的过程与八分区的构建类似。如果C_o是叶节点且满足细分标准，C_o中的所有点（即旧点和新添加的点）将被递归细分到子八分区。如果启用了下采样，e_o ≤ 2e_{min}且|P_o| > b/8，新点将稍后被删除而不是被添加到C_o。否则，将为更新的点分配一段连续的内存。如果C_o有子八分区，问题就变成了将新添加的点分配给各个子八分区，且只有新点需要被细分。这个过程类似于上面提到的，除了递归更新八分区。

图3. 图（a）和（b）说明了向 i-Octree 插入超出范围的新点（红色）的过程。在（a）中，左侧的黄色立方体是最初的根八分之一，同时也是具有初始点（黑色）的叶子八分之一。插入后，根八分之一变为浅紫色立方体。在（b）中，紫色节点是根八分之一，插入新八分之一后进行了更新（虚线黑色矩形）。

4.2 盒式删除

在某些机器人应用中，如SLAM，只需要估计靠近代理的点的状态。因此，在i-Octree中远离代理的点并不是必需的，可以出于效率考虑被移除。
在移除轴对齐立方体内不必要的点时，i-Octree首先检查八分区是否在给定盒子内，而不是直接搜索给定空间内的点并删除它们。所有在给定盒子内的八分区将被直接删除，无需搜索其中的点，这显著减少了删除时间。由于局部空间连续存储策略，八分区的删除不会影响其他八分区。对于与给定盒子重叠的叶子八分区，我们删除盒子内的点，并为剩余的点分配新的内存段。如果一个叶子八分区在删除后不包含任何点，它将被删除，如图4所示。

图4. 盒子式删除的示意图，蓝色盒子是给定的盒子，数字0到7（Morton编码）是八分之一的索引。具有索引0、1、4和5的八分之一直接被删除，由于没有点，八分之七也被删除。

5. K最近邻搜索

使用i-Octree，我们可以为任意查询点q∈\mathbb{R}^3 检索k个最近邻。i-Octree上的最近搜索是一种精确的最近搜索[19]，而不是[20]中的近似搜索。我们维护一个优先级队列h，其最大长度为k，用于存储迄今为止遇到的k个最近邻及其与查询点q的距离。无论是推入还是弹出，h的最后一个元素总是拥有最大的距离。每个八分区的轴对齐盒被有效利用，以加速最近邻搜索，使用“边界重叠球”测试[21]和根据子八分区的固定索引预先计算的优先级搜索顺序。

首先，我们从其根节点递归搜索i-Octree，直到到达最接近q的叶节点。然后，将q到叶节点中所有点的距离及相应索引推入优先级队列h。在h填满之前，迄今为止遇到的所有叶节点将被搜索。如果h已满，且由q和h中最大距离dmax定义的搜索球S(q, d_{max})位于当前八分区的轴对齐盒内，则搜索结束。如果一个八分区C_k不包含搜索球S(q, d_{max})，则必须满足以下三个条件之一：

其中q = (q_x, q_y, q_z)^T，c_k = (c_{k,x}, c_{k,y}, c_{k,z})^T。如果上述条件均不成立，则搜索球位于八分区内。我们通过调查与搜索球(q, d_{max})重叠的八分区来更新h，因为只有这些八分区可能包含也在期望邻域内的点。我们定义q和C_k之间的距离d如下：

其中1 = (1, 1, 1)^T，且σ(x) = x 如果 x > 0，否则 σ(x) = 0。d < d_{max} 表示C_k与S(q, d_{max})重叠。为了加速这一过程，我们根据C_o的候选子八分区到C_k的距离进行排序，并得到8个不同的序列在I_{order}中。这样，更靠近的八分区会更早被搜索到，搜索也会更早结束。

6. 半径邻居搜索

对于任意查询点q∈\mathbb{R}^3和半径r，半径邻居搜索方法找到满足∥p − q∥2 < r的每个点p。这个过程与KNN搜索类似，除了固定半径和不限制k。我们采用了Behley等人[16]提出的剪枝策略，并进行了改进以减少计算成本。在测试S(q, r)是否完全包含一个八分区C_k之前，我们进行一个简单的测试，是否r^2大于3e^2_k。简单测试是S(q, r)包含C_k的必要条件，成本小。此外，我们尝试避免在算法中提取根。这两个技巧与我们的局部空间连续存储策略一起，在加速搜索中发挥了关键作用。

7. 总结

本文提出了一种新颖的动态八叉树数据结构，即 i-Octree，它支持树上逐点插入与降采样、盒子式删除和快速最近邻搜索。此外对于随机数据集和开放数据集的大量实验表明i-Octree 在所有最先进的树形数据结构中都能实现最佳的整体性能。