点击下方 卡片 ，关注 「3D视觉工坊」 公众号
选择 星标 ，干货第一时间送达

来源：智驾机器人技术前线，作者：zhujun

原文链接：https://zhuanlan.zhihu.com/p/685201963#/

扫描下方二维码，加入 3D视觉知识星球 ，星球内凝聚了众多3D视觉实战问题，以及各个模块的学习资料： 近20门视频课程（星球成员免费学习） 、 最新顶会论文 、 计算机视觉书籍 、 优质3D视觉算法源码 等。 想要 入门3D视觉、做项目、搞科研，欢迎扫码加入！

本文介绍我们在点云近邻搜索领域的新工作： i-Octree 。 i-Octree ，支持快速最近邻搜索和实时动态更新，如点插入、删除和树上下采样。实验表明， i-Octree 在real-world开放数据集上的性能优于 ikd-Tree ， 平均每帧的运行时间减少了 19% 。目前的代码实现仅仅采用单线程，如果加入多线程的话速度会更快。

我们的工作已被 ICRA 2024 接收，论文和代码已公开：

• 论文：

https://arxiv.org/pdf/2309.08315.pdf

• 代码：

GitHub - zhujun3753/i-octree: [ICRA2024] Implementation of A Fast, Lightweight, and Dynamic Octree for Proximity Search

最近邻搜索（NNS）在许多机器人应用中是很必要的。例如，在激光雷达SLAM中，NNS对于计算特征、估计法线以及将新点与地图匹配至关重要。目前的激光雷达传感器可以在每秒数百米的测量范围内产生大量厘米级精度的3D点。大量的序列数据需要实时处理，这对机载计算资源有限的机器人来说是一项极具挑战性的任务。为了保证NNS的效率，维护一个支持高效查询和实时动态更新的地图至关重要。静态kd树或八叉树在实际机器人中应用时常常需要从头开始构建整个树，造成了大量的时间消耗。为此，我们提出了一种称为 i-Octree 的八叉树结构，它用新的点增量更新八叉树，并实现快速NNS。此外，我们的 i-Octree 在时间和内存方面都具有较高的效率，可适应各种类型的点，并允许树上 下采样 和 逐框删除 。我们在随机数据和real-world开放数据集上进行验证实验，以评估 i-Octree 的有效性。在随机数据实验中，与最新的增量 ikd-Tree 树相比，我们的 i-Octree 在运行时间有了显著的改进。具体来说，它将构建树的运行时间减少了 64 %，点插入减少了 66 %，KNN搜索减少了 30 %，半径邻居搜索减少了 56 %。此外，在real-world开放数据集上，当应用于激光雷达SLAM时， i-Octree 显示出显著的时间性能增强。它的速度是原始方法的两倍多，同时通常保持更高的精度水平。和 ikd-Tree 相比，在real-world开放数据集上的运行时间平均减少了 19%。

设计与实现

01

数据结构与构建

i-Octree 的一个父节点有八个子节点，每个子节点对应一个 octant，一个 octant 即一个轴对齐立方体。除非一个节点包含的点数小于给定值 或者extent 小于最小 extent ，否则这个节点对应的 octant 都会被均匀划分为八个更小的 octant。为了节省内存，不包含点的节点不会被创建。此外，每个叶子节点中仅仅保留点的索引和坐标，非叶子节点不保留点。

为了快速访问叶子节点中的每个点，我们提出了一种局部空间连续的存储策略，如上图所示，对于每一个叶子节点中的点数据，我们都重新分配一段连续内存来储存这段数据。这种存储策略也便利了 box-wise 删除和增量更新，使得两种操作对于其他叶子节点的数据没有影响。

构建 i-Octree 时，从根节点开始， i-Octree 会递归地将轴对齐立方体在中心处分成八个以Morton码索引的立方体，并根据计算的立方体索引将当前八分之一内的所有点细分到每个立方体中。当满足停止条件时，将分配一段连续内存来存储叶子节点中的点的信息。

动态更新包括插入一个或多个点（即增量更新）和删除轴对齐盒中的所有点（即 box-wise 删除）。下采样伴随插入操作，并将八叉树维持在预先确定的分辨率上。

1）增量更新

在插入新点时，我们必须考虑到一些点可能超出了原始树的轴对齐边界框的情况。一旦有点超出八叉树的范围，我们就必须通过创建新的根节点来扩展边界框，其子节点包含当前根节点。这个过程可能会执行多次，以确保所有新点都在树的范围内，如下图所示。

为了在机器人应用中实现高效的点查询， i-Octree 支持与点插入同时进行的下采样。下采样主要针对新点，并删除满足一定条件的点：它们被细分为一个叶子octant，其范围小于 且大小大于 。添加新点到一个octant 的过程类似于构建一个octant。如果 是一个叶子节点，并且满足细分条件， 中的所有点（即旧点和新添加的点）将被递归地细分为子octants。如果启用了下采样， ，且 ，新点将被删除，而不是被添加到 中。否则，将为更新后的点分配一段连续内存。如果 有子octants，则问题变成将新添加的点分配给不同的子节点，并且只有新点需要细分。

2）box-wise 删除

在某些机器人应用中，例如SLAM，估计机器人的状态仅需要知道周围的点。因此， i-Octree 中远离机器人的点并不重要，可以出于效率考虑而移除。在删除轴对齐立方体中的不必要点时， i-Octree 首先检查节点是否位于给定的空间内。所有位于给定空间内的节点直接被删除，而无需在其中搜索点，这显著减少了删除时间。由于采用了本地连续存储策略，给定空间内的节点的删除不会影响其他节点。对于与给定框重叠的叶子节点，我们删除框内的点，并为剩余的点分配一段新的内存。如果叶子节点不包含点，则会被删除。这个过程如下图所示。

03

KNN搜索

使用 i-Octree ，我们可以为任意查询点 检索k个最近邻点。我们维护一个优先队列 ，其最大长度为k，用于存储到目前为止遇到的最近的k个邻居及其到查询点 的距离。 的最后一个元素始终具有最大的距离，无论是入队还是出队。每个octant的轴对齐框都被有效地利用，以加速最近邻搜索，使用“bounds-overlap-ball”测试和根据子octant的固定索引预先计算的优先搜索顺序。首先，我们从 i-Octree 的根节点开始递归搜索，直到到达最接近 的叶节点。然后，将从 到叶节点中所有点的距离以及相应的索引推送到优先队列 中。在 填满之前，将搜索到目前为止遇到的所有叶节点。如果 已满，并且搜索球 定义为 和 中最大距离 的球在当前 octant 的轴对齐框内，那么搜索就结束了。如果一个octant 不包含搜索球 ，那么以下三个条件之一必须满足：

其中 。如果以上条件都不满足，则搜索球在 octant 内。我们通过考察与搜索球 重叠的 octant 来更新 ，因为只有这些 octant 可能包含在所需邻域内的点。我们将 和 之间的距离定义为 , 其中 ,且 ，如果 ，否则 。 表示 与 重叠。为了加快速度，我们根据它们与 的距离对 的候选子octant 进行排序，并在 中获得8个不同的顺序。这样，越接近的octant越早被搜索，并且搜索尽早结束。

对于任意查询点 和半径 ，Radius Neighbors Search 方法找到满足 的每个点 。该过程类似于KNN搜索，但半径是固定的，而且k是不确定的。为了减少计算成本，在测试 是否完全包含 之前，我们先简单测试 是否大于