i-Octree：一种用于最近邻搜索的快速、轻量级和动态的八叉树

文章：i-Octree: A Fast, Lightweight, and Dynamic Octree for Proximity Search

作者：Jun Zhu , Hongyi Li , Zhepeng Wang , Shengjie Wang, and Tao Zhang

编辑：点云PCL

代码：https://github.com/zhujun3753/i-octree.git

摘要

建立新获取点云与历史累积数据（即地图）之间的对应关系，通过最近邻搜索在许多机器人应用中至关重要。然而，静态树数据结构无法实时处理大型且动态增长的地图。为了解决这个问题，作者提出了 i-Octree，一种动态八叉树数据结构，支持快速最近邻搜索和实时动态更新，例如点插入、删除和树上的降采样，i-Octree 基于基于叶子的八叉树构建，并具有两个关键特征：局部连续的空间存储策略，允许快速访问点同时最小化内存使用；以及局部树上的更新，与现有的静态或动态树结构相比，显著减少计算时间。实验证明i-Octree 在实际开放数据集上平均运行时间减少了 19%，表现优于当代最先进的方法。

主要贡献

本文提出了一种名为 i-Octree 的动态八叉树结构，它可以逐步更新八叉树以纳入新的点，并实现快速最近邻搜索。此外i-Octree 在时间和内存效率上表现出色，适用于各种类型的点，并允许在树上进行降采样和基于盒子的删除。我们对随机数据和实际开放数据集进行了验证实验，以评估 i-Octree 的有效性。在随机数据实验中i-Octree 在运行时与最先进的增量 k-d 树相比表现出显著的改进。具体而言，它将树的构建时间减少了 64%，点插入时间减少了 66%，KNN 搜索时间减少了 30%，半径邻居搜索时间减少了 56%。此外在基于 LiDAR 的 SLAM 中应用于实际数据时，i-Octree 展示出显著的时间性能提升，它的速度是原始方法的两倍以上，而通常保持更高的准确性水平，此外i-Octree 实现已在 Github 上开源。

内容概述

i-Octree 接受顺序点云作为输入，具有两个目标：动态维护全局地图并在地图上执行快速最近邻搜索（即 KNN 搜索和半径邻居搜索）。图1展示了 i-Octree 的典型应用场景，深度传感器持续感知其周围环境，并定期生成顺序的3D距离数据，深度数据的初始扫描用于构建 i-Octree 并定义全局坐标框架，然后i-Octree 通过KNN搜索或半径邻居搜索促进了新数据与历史数据之间的对应关系的建立。基于这些对应关系，可以估计新数据的姿态，并将带有位姿的3D点云添加到 i-Octree 中，为了防止 i-Octree 中的地图大小不受控制地增长，仅维护围绕当前位置居中的大型局部区域（即轴对齐盒子）内的地图点。

图1. 在里程计中使用 i-Octree 的示例，i-Octree 和里程计协同工作，估计来自深度传感器获取的3D数据的位姿，i-Octree 提供了一个稳健高效的数据结构，用于存储和查询3D数据，而里程计则能够估计数据点的位姿。

A. 数据结构和构建

i-Octree 是一种动态八叉树数据结构，用于存储和处理三维点云数据。i-Octree每个节点最多有八个子节点，对应八叉树的八个方向（或八个八分之一）。作者提到了八分之一的概念，即从一个以中心和相等范围的轴对齐边界框开始，逐步将其递归地细分为更小的八分之一，直到满足停止条件。这个停止条件可以是包含的点数小于一个给定的阈值（桶大小），或者八分之一的范围小于最小范围。为了提高内存利用效率，作者提出了一种本地连续空间存储策略（如图2所示）。

图2. 八分之一中点在内存中的位置示意图。(a) 分散的位置；(b) 连续的位置

具体来说，在叶子节点中，作者重新分配一段连续的内存来存储点的信息，这样可以实现对每个点的快速访问，并且便于进行基于盒子的删除和增量更新操作。在构建增量八叉树的过程中，作者首先消除无效点，并计算所有有效点的轴对齐边界框。然后，从根节点开始，递归地将边界框在中心处分成八个立方体，并根据计算的立方体索引将当前节点中的所有点细分到每个立方体中。当满足停止条件时，将创建一个叶子节点，并分配一段连续的内存来存储叶子节点中的点的信息。

B. 动态更新

动态更新包括插入一个或多个点（即增量更新）和删除轴对齐盒子中的所有点（即盒子式删除）。插入操作与降采样集成在一起，它使得八叉树保持在预定的分辨率上。

增量更新：在插入新点时，必须考虑到一些点可能超出了原始树的轴对齐边界框的情况。一旦有点超出八叉树的范围，我们必须通过创建新的根八分之一来扩展边界框，其子节点包含当前根八分之一。这个过程可能需要执行多次，以确保所有新点都在树的范围内。然后，新点被添加到扩展的八叉树中（参见图3）。 考虑到在机器人应用中高效进行点查询，i-Octree 支持降采样，它与点插入同时执行。降采样专注于新点，并删除满足某种条件的点。

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

盒子式删除：删除范围内的点，在某些机器人应用中，例如 SLAM，只需要与代理相邻的点来估计状态。因此，远离代理的点在 i-Octree 中并不重要，可以为了效率考虑而删除。删除操作，i-Octree 通过检查八分之一是否在给定的盒子内来删除不必要的点。所有在给定盒子内的八分之一将被直接删除，而不需要搜索其中的点，这显著减少了删除时间，删除操作不会影响其他八分之一，这得益于局部连续的存储策略。对于与给定盒子重叠的叶子八分之一，我们删除盒子内的点，并为剩余的点分配一个新的内存段。如果叶子八分之一删除后不再包含点，则会被删除。

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

C. K-最近邻搜索

首先，通过维护一个优先队列来存储查询点 q 到 k 个最近邻点的距离，并利用轴对齐边界框和预先计算的优先搜索顺序来加速搜索过程。搜索从根节点开始递归向下进行，直到到达最接近查询点 q 的叶子节点为止。然后，将叶子节点中所有点与查询点的距离及其索引推入优先队列。在搜索过程中，通过检查搜索球是否与八分之一的边界框重叠，来决定是否继续搜索。如果不重叠，则进一步检查三个条件来确定搜索球的位置。为了加速搜索过程，通过对候选子八分之一按照与当前八分之一的距离进行排序，确保越近的八分之一越早被搜索。这样，可以有效地找到查询点 q 的 k 个最近邻点。

D.半径搜索

在半径邻居搜索中，针对任意查询点 q ∈ R^3 和半径 r，该方法旨在找到满足 ∥p − q∥2 < r 的每个点 p。与 KNN 搜索相似，但不同之处在于半径邻居搜索采用固定的半径和无限制的 k。作者采用了 Behley 等人提出的修剪策略，并进行了改进以降低计算成本。在判断搜索球 S(q, r) 是否完全包含八分之一 Ck 之前，进行了简单测试以检查 r^2 是否大于 3e^2k。这个简单测试是判断搜索球是否以较小成本包含八分之一的必要条件。此外，作者还试图避免在算法中提取根节点，这些技巧与局部连续空间存储策略一起，发挥了关键作用来加速搜索过程。

实验

将i-Octree 与公开可用的静态 k-d 树实现（即 Point Cloud Library (PCL) 中使用的 k-d 树）、增量 k-d 树（即 ikd-Tree）以及 PCL 八叉树进行比较。我们在随机数据和各种开放的真实世界数据集上进行实验。首先评估了我们的 i-Octree 在不同大小的随机三维点数据上与 PCL 八叉树和最先进的增量 k-d 树（即 ikd-Tree）进行树构建、点插入、KNN 搜索、半径邻居搜索以及盒子式删除的性能比较。然后通过在真实世界数据集上将 i-Octree 替换静态 k-d 树进行实际机器人应用的验证，而不进行任何改进，并评估时间性能和准确性。所有实验均在一台搭载 Intel i7-13700K CPU 、主频为 3.40GHz 的电脑上进行。

A. 随机数据实验

性能比较：在这个实验中，研究了三种动态数据结构（即 i-Octree、ikd-Tree 和 PCL 八叉树）的时间性能。ikd-Tree 和 PCL 八叉树都是高效的先进实现，支持点插入，这是我们选择它们与我们的 i-Octree 进行公平比较的关键考虑。图5展示了不同树大小下的动态数据结构比较。

图5，不同树大小的动态数据结构比较

表格 I 显示了平均时间消耗和峰值内存使用量的比较。当树大小从 200,000 增加到 400,000 时，i-Octree 和 PCL 八叉树的点插入时间（不包括下采样）保持稳定在 0.8ms，而ikd-Tree的时间是后者的 3 倍，并且随着树大小的增加呈线性增长。对于 KNN 搜索，我们的方法表现出了超过 PCL 八叉树两倍的性能，并比 ikd-Tree 提高了 20% 的运行时间。对于半径邻居搜索的执行时间，i-Octree 表现出了超过 ikd-Tree 和 PCL 八叉树两倍的性能。此外，与 ikd-Tree 相比，i-Octree 的构建时间减少了不到 36%。此外，i-Octree 的峰值内存使用量最低，不到 ikd-Tree 的 35%。

盒子式删除：盒子式删除的时间和树大小显示在表 II 中,i-Octree 和 ikd-Tree 在 KNN 搜索和半径邻居搜索上的运行时性能显示在图 6 中。在这个实验中，似乎 ikd-Tree 上的盒子式删除操作并没有将给定盒子内的所有点都删除，如表 II 所示。

结果显示 i-Octree 在盒子式删除上运行速度比 ikd-Tree 快了超过 10 倍，并且在半径邻居搜索上大约快了 3 倍。此外i-Octree 将 KNN 搜索的运行时间比 ikd-Tree 减少了 58%。

图6.时间消耗的比较

B. 真实数据实验

在实际的机器人应用中测试了我们开发的 i-Octree，例如 LiDAR 惯性 SLAM 和纯 LiDAR SLAM。在这些实验中直接用我们提出的 i-Octree 替换了 LiDAR SLAM 中的静态 k-d 树，没有进行任何改进，并评估了时间性能和准确性。为了确保公平和完整的比较，我们在三个数据集上进行了测试：M2DGR、Newer College Dataset 和 NCLT，它们具有不同的传感器设置。为了简化起见，将重点关注三种代表性的 LiDAR-based SLAM 算法：FAST-LIO2 和 LIO-SAM（结合 LiDAR 和惯性测量的 LiDAR 惯性 SLAM）以及 FLOAM（仅依赖 LiDAR 数据的纯 LiDAR SLAM）。每个数据集中的每个序列的参数设置如表 III 所示。这两种算法都建立了两个独立的地图：表面地图和边缘地图，其中将建立两个 k-d 树或 i-Octree。

第一个数据集是 M2DGR，由一辆装备有完整传感器套件的无人地面车辆（UGV）收集而来，包括采样频率为 10 Hz 的 Velodyne VLP-32C LiDAR、采样频率为 150 Hz 的 9 轴 Handsfree A9 惯性测量单元（IMU）以及其他传感器。对于在室外环境运行的 FLOAM，地图的大小逐渐增长，使用静态 k-d 树时会使计算变得昂贵。当地图大小较大时，重建过程可能会成本高昂，这会显著降低实时性能。

图 7. 数据集街道 02 序列上时间消耗的比较。

如图 7 所示，重建整个 k-d 树的时间占每次扫描总时间的很大比例。在将 k-d 树替换为 i-Octree 后，增量时间减少至不到 10ms，实时性能几乎得到保证。使用 i-Octree 的 FLOAM 运行速度比原始方法快 5 倍以上，尽管有时会出现轻微的精度损失，如表 V 所示。使用 i-Octree 的 LIO SAM 6AXIS 在几乎所有序列上的运行速度比原始方法快 2 倍以上，并且通常可以提高准确性。

第二个数据集是 Newer College Dataset，这是一个多相机 LiDAR 惯性数据集，步行距离为 4.5 公里。该数据集由一个手持设备收集，配备有 Ouster OS0-128 LiDAR，采样频率为 10 Hz，并带有嵌入式 IMU，采样频率为 100 Hz。在此测试中，记录了每次扫描的平均总时间消耗，并计算了绝对平移误差的 RMSE，如表 V 所示。我们发现，i-Octree 与 LIO SAM 6AXIS 具有良好的兼容性，因为它不仅大大减少了时间消耗，还提高了准确性。此外，我们还使用这个数据集对 i-Octree 和 ikd-Tree 集成到 FAST-LIO2 中但没有并行化的性能进行了比较，如表 VI 所示。

第三个数据集为NCLT 的大规模数据集，由 Velodyne HDL-32E LiDAR 以 10 Hz 的采样率、Microstrain 3DM-GX3-45 IMU 以 100 Hz 的采样率和其他传感器捕获。序列的长度为 6.4 公里，持续时间为 5633 秒。每次扫描的平均运行时间和绝对平移误差已添加到表 V 中。在大规模序列上，FLOAM 的漂移严重，而 LIO SAM 6AXIS（i-Octree）表现出惊人的性能，即使没有启用回环闭合，仅有在 z 轴上存在轻微的漂移。LIO SAM 6AXIS（静态 k-d 树）只能使用最近的关键帧来估算姿态，而 LIO SAM 6AXIS（i-Octree）可以从一开始就利用点信息，这有助于其更好的性能。

总结

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