OctoMap

OctoMap: 基于八叉树的高效概率性三维建图框架

Armin Hornung - Kai M. Wurm • Maren Bennewitz •
Cyrill Stachniss • Wolfram Burgard

Received: 30 April 2012 / Accepted: 31 December 2012

摘要

三维模型提供了空间的体积表示，这对于多种机器人应用非常重要，包括飞行机器人和配备操作臂的机器人。本文提出了一个开源框架，用于生成体积三维环境模型。我们的建图方法基于八叉树，并使用概率占用估计。它不仅显式地表示已占用的空间，还表示空闲和未知区域。此外，我们还提出了一种八叉树地图压缩方法，保持三维模型的紧凑性。我们的框架作为一个开源C++库提供，并已经成功应用于多个机器人项目中。我们展示了一系列与实际机器人以及公开的真实世界数据集进行实验的结果。实验结果表明，我们的方法能够高效地更新表示，并且在保持最小内存需求的同时，持续一致地建模数据。

Keywords： 3D • Probabilistic • Mapping • Navigation

1 引言

多个机器人应用需要环境的三维模型。这些应用包括空中、海底、户外或外太空任务。然而，三维模型对于许多日常场景也很重要，例如移动操作和导航任务。

尽管三维地图是许多机器人系统的一个重要组成部分，但现有的可靠、高效的实现却很少。这种实现的缺乏导致了基本软件组件的重新创建，因此可以视为机器人研究中的瓶颈。因此，我们认为开发一个开源的三维建图框架将极大促进需要三维几何表示环境的机器人系统的发展。

大多数机器人应用需要概率表示、空闲、占用和未建图区域的建模，以及在运行时和内存使用上的效率。我们将详细讨论这三项要求。

• 概率表示：为了创建三维地图，移动机器人通过进行三维范围测量来感知环境。这些测量受到不确定性的影响：通常，范围测量的误差在几厘米的量级。但也可能会有看似随机的测量，这些测量是由反射或动态障碍物引起的。当任务是从这些噪声测量中创建环境的精确模型时，必须以概率方式考虑潜在的不确定性。然后，可以将多个不确定的测量融合成对环境真实状态的稳健估计。另一个重要的方面是，概率传感器融合允许整合来自多个传感器和多个机器人的数据。
• 未建图区域建模：在自主导航任务中，机器人只能为那些已通过传感器测量并被检测为空闲的区域规划无碰撞路径。相反，未建图区域需要被避免，因此地图必须表示这些区域。此外，未建图区域的知识在探索过程中至关重要。当地图被自主创建时，机器人必须规划其行动，以便在先前未建图的区域进行测量。
• 效率：地图是任何自主系统的核心组件，因为它在行动规划和执行过程中都会被使用。因此，地图需要在访问时间和内存消耗方面都具有高效性。从实践角度来看，内存消耗通常是三维建图系统的主要瓶颈。因此，模型必须在内存中紧凑，以便能够对大环境建图，机器人可以将模型保存在其主内存中，并且能够在多个机器人之间高效地传输。
• 

图1 用激光范围传感器扫描的树木的三维表示（从左到右）：点云、升高图、多级表面图，以及我们的体积（体素）表示。请注意，我们的体积表示显式地建模了空闲空间，但为了清晰起见，仅显示了占用的体积。

已经提出了几种方法来建模机器人中的三维环境。为了说明这一点，我们将我们的方法与三种常见的建图方法进行比较——结果的可视化如图1所示。在这个例子中，三维测量通过点云、升高图（Hebert等，1989）、多级表面图（Triebel等，2006）以及我们框架中的体积方式进行表示。之前的任何方法都没有满足我们上面列出的所有要求。点云存储大量的测量点，因此在内存效率方面不佳。此外，它们无法区分无障碍和未建图区域，也没有提供以概率方式融合多个测量的手段。升高图和多级表面图效率较高，但也不表示未建图区域。最重要的是，这些方法无法表示任意的三维环境，例如示例中的树枝。

在这项工作中，我们提出了OctoMap，一个基于八叉树的集成框架，用于表示三维环境。在我们的框架中，我们结合了之前三维环境建模方法的优点，以满足上面讨论的要求。我们方法的一个核心特性是，它允许高效且概率性地更新占用和空闲空间，同时保持最小的内存消耗。占用空间是通过距离传感器的端点获得的，例如激光测距仪，而空闲空间对应于传感器与端点之间的观测区域。作为我们方法的一个关键贡献，我们引入了一种压缩方法，通过在已建图的空闲区域和占用空间中局部合并一致的地图体积，减少内存需求。我们实现了这一方法，并通过使用各种公开的真实世界机器人数据集对其进行了全面评估，这些数据集包括室内和室外环境。

我们的开源实现以独立的C++库形式提供，免费使用。它已根据BSD许可证发布，可以从http://octomap.github.com获得。该库支持多个平台，如Linux、Mac OS和Windows。它已集成到机器人操作系统（ROS）中，并且可以在其他软件框架中以简单的方式使用。自2010年首次介绍（Wurm等，2010）以来，OctoMap框架不断改进，并在越来越多的机器人研究项目中得到应用。

本文的组织结构如下。下一节将详细讨论与三维数据结构和建图方法相关的工作，第三节我们将介绍OctoMap框架。第四节介绍实现细节，接着是第五节对所提框架的评估。最后，第六节通过案例研究展示OctoMap在机器人多个领域中的应用，展示其多样性和集成的便捷性。

2 相关工作

环境的三维模型是许多机器人系统的关键前提，因此它们已经成为研究的主题超过二十年。

一种常见的三维环境建模方法是使用大小相等的立方体体积网格，称为体素（voxels），来离散化建图区域。Roth-Tabak 和 Jain（1989）以及 Moravec（1996）提出了使用这种表示方法的早期工作。刚性网格的一个主要缺点是它们对内存的需求较大。网格地图需要初始化，以确保其至少与建图区域的边界框一样大，而不考虑实际的地图单元在体积中的分布。在大规模的户外场景中，或者当需要较细的分辨率时，内存消耗可能变得不可接受。此外，建图区域的范围需要提前知道，或者每次扩展地图区域时需要执行昂贵的复制操作。

通过直接存储三维范围测量，可以避免环境的离散化。环境中的占用空间通过范围传感器（如激光测距仪或立体摄像头）返回的三维点云进行建模。这种点云方法已在多个三维SLAM系统中使用，例如Cole和Newman（2006）提出的系统，以及Nüchter等（2007）的SLAM方法。该方法的缺点是既不建模空闲空间，也不建模未知区域，并且无法直接处理传感器噪声和动态物体。因此，点云仅适用于静态环境中的高精度传感器，并且当不需要表示未知区域时才合适。此外，这种表示方法的内存消耗随着测量数量的增加而增加，这是一个问题，因为它没有上限。

如果对建图区域做出某些假设，2.5D地图就足以建模环境。通常，使用一个二维网格来存储每个单元格的测量高度。在最基本的形式中，这会生成一个升高图，其中地图每个单元格仅存储一个值（Hebert等，1989）。这种地图足以建模环境的一个示例是Hadsell等（2009）描述的户外地形导航方法。只要有一个单一的表面供机器人用于导航，升高图就足够了，因为可以安全地忽略比车辆高的悬垂障碍物，如树木、桥梁或天桥。通过允许每个单元格有多个表面（Triebel等，2006；Pfaff等，2007），或使用对应不同结构类型的单元格类（Gutmann等，2008），可以放宽单一表面的严格假设。大多数2.5D地图的一个普遍缺点是它们不以体积方式表示环境，而是基于机器人的高度在垂直维度上对环境进行离散化。虽然这种方法对于使用固定机器人形态进行路径规划和导航是足够的，但该地图不能代表实际的环境，例如用于定位。

为了解决这个问题，Ryde 和 Hu（2010）提出了一种相关的方法。该方法为每个2D网格单元存储一个占用体素的列表。尽管这种表示方法是体积式的，但它并没有区分空闲和未知的体积。Dryanovski等（2010）在他们的多体积占用网格（Multi-Volume Occupancy Grid）方法中，为每个2D单元存储占用体素和空闲体素的列表。然而，与我们的方法相比，该方法需要提前知道地图的范围，地图更新计算量较大，并且不具有多分辨率能力。另一个潜在问题是，后续的地图更新无法细分现有的体积，从而导致环境模型不准确。同样，Douillard等（2010）将粗糙的升高图与较高分辨率的物体体素地图结合。与我们的工作不同，这种方法侧重于单次测量的三维分割，而不是将多个测量整合到环境模型中。

在机器人建图中，八叉树避免了固定网格结构的一个主要缺点：它们延迟初始化地图体积，直到需要整合测量数据。通过这种方式，建图环境的范围不需要提前知道，地图仅包含已测量的体积。如果树的内部节点得到正确更新，八叉树也可以作为多分辨率表示使用，因为它可以在任何级别进行剪裁，以获得更粗的子划分。八叉树用于建图的概念最早由Meagher（1982）提出。早期的工作主要集中在建模布尔属性，如占用（Wilhelms和Van Gelder，1992）。Payeur等（1997）使用八叉树将占用网格建图从2D扩展到3D，并引入了以概率方式建模占用和空闲空间的方法。Fournier等（2007）和Pathak等（2007）也采用了类似的方法。然而，与本文提出的方法不同，作者们没有明确解决地图压缩或地图中置信度有界的问题。

Fairfield等（2007）还提出了一种基于八叉树的三维地图表示。他们的地图结构叫做延迟引用计数八叉树（Deferred Reference Counting Octree），旨在实现高效的地图更新，特别是在粒子滤波SLAM的上下文中。为了实现地图的紧凑性，定期进行一种有损的最大似然压缩。与我们方法中使用的压缩技术相比，这种方法会丢失未来更新的概率信息。此外，过度自信的地图问题和多分辨率查询问题并未得到解决。

作为一种数据结构，八叉树被应用于多种领域，特别是在计算机图形学中，用于高效渲染（Botsch等，2002；Surmann等，2003；Laine和Karras，2010）以及在摄影测量领域用于存储和处理大型点云（Girardeau-Montaut等，2005；Elseberg等，2011）。另一个流行的应用是静态点云（Schnabel和Klein，2006）或点云流（Kammerl等，2012）的压缩。尽管我们的框架足够通用，也可以存储原始点云，但它的主要目的是将这些点云整合为一种内存高效的体积占用地图，因为点云作为机器人环境表示存在许多缺点，这在本节开头已经详细讨论过。

Yguel等（2007b）提出了一种基于Haar小波数据结构的三维地图。这种表示方法也是多分辨率的并且是概率性的。然而，作者并未深入评估其在三维建模中的应用。在他们的评估中，未知区域没有建模，并且仅使用了一个单一的模拟三维数据集。由于没有公开的实现，难以评估这种地图结构是否像八叉树一样高效。

表面表示方法，如三维法线分布变换（3D Normal Distribution Transform，Magnusson等，2007）或Surfels（Habbecke和Kobbelt，2007），最近已被用于三维路径规划（Stoyanov等，2010）和物体建模（Weise等，2009；Krainin等，2011）。类似地，Newcombe等（2011）提出了一种基于低成本深度相机和GPU处理的实时三维SLAM系统，用于重建室内场景中的密集表面。最近，该工作已扩展到较大的室内环境中（Whelan等，2012）。然而，表面表示方法无法区分空闲和未知空间，特别是在户外场景中可能需要大量内存，并且通常基于对环境的强假设。例如，在移动操作场景中，能够区分空闲和未知空间对于安全导航至关重要。

最后，据我们所知，目前尚无一个开源的三维占用建图框架实现能够满足引言中列出的要求。

3 OctoMap建图框架

本文提出的方法使用基于树的表示，以提供最大灵活性，适应建图区域和分辨率的变化。它执行概率占用估计以确保可更新性，并应对传感器噪声。此外，压缩方法确保了生成模型的紧凑性。

3.1 八叉树

八叉树是一种用于三维空间细分的层次数据结构（Meagher，1982；Wilhelms和Van Gelder，1992）。八叉树中的每个节点表示一个立方体体积中的空间，通常称为体素。这个体积递归地细分为八个子体积，直到达到给定的最小体素大小，如图2所示。最小体素大小决定了八叉树的分辨率。由于八叉树是层次化的数据结构，因此可以在任何级别进行剪裁，以获得更粗的子划分，只要相应地维护内部节点。图3展示了一个查询多个分辨率下占用体素的八叉树地图示例。

图2 八叉树示例，存储空闲（白色阴影）和占用（黑色）单元格。左侧显示体积模型，右侧显示相应的树形表示。

图3 通过限制查询的深度，可以随时获得同一地图的多个分辨率。占用体素在分辨率为 \(0.08 \, \mathrm{m}\)、\(0.64\) 和 \(1.28\) 米时显示。

在其最基本的形式中，八叉树可以用来建模布尔属性。在机器人建图的背景下，这通常是体积的占用情况。如果某个体积被测量为占用，则相应的八叉树节点会被初始化。在这种布尔设置下，任何未初始化的节点都可以是空闲的或未知的。为了解决这种模糊性，我们在树中显式地表示空闲体积。这些体积是在传感器和测量端点之间的区域创建的，例如通过射线投射（raycasting）确定的射线沿线。未初始化的区域则隐式地建模为未知空间。图4展示了一个包含空闲和占用节点的八叉树，数据来自实际激光传感器。

通过使用布尔占用状态或离散标签，可以实现八叉树的紧凑表示：如果一个节点的所有子节点都具有相同的状态（占用或空闲），则可以将其修剪掉。这将大大减少需要在树中维护的节点数量。

在机器人系统中，通常需要应对传感器噪声以及环境的临时或永久变化。在这种情况下，单一的离散占用标签将不够充分。相反，占用需要以概率方式建模，例如通过应用占用网格建图（Moravec和Elfes，1985）。然而，这种概率模型缺乏通过修剪实现无损压缩的能力。

本文提出的方法提供了将使用离散标签的八叉树紧凑性与概率建模的可更新性和灵活性结合的手段，具体内容将在第3.4节中讨论。

图4 从示例数据生成的八叉树地图。左：在走廊中使用倾斜激光测距仪记录的点云。中：从数据生成的八叉树，仅显示占用体素。右：八叉树的可视化，显示占用体素（深色）和空闲体素（白色）。空闲区域是通过清除从传感器原点到每个端点的射线空间得到的。无损修剪导致叶节点的大小不同，主要在右侧的空闲区域中可见。

在数据访问复杂性方面，八叉树由于其树结构，相比固定大小的3D网格需要额外开销。对包含\(n\)个节点、深度为\(d\)的树数据结构进行单次随机查询的复杂度为\(\mathcal{O}(d)=\mathcal{O}(\log n)\)。以深度优先的方式遍历完整的树则需要\(\mathcal{O}(n)\)的复杂度。需要注意的是，在实践中，我们的八叉树被限制为固定的最大深度\(d_{\text {max }}\)。这使得随机节点查找的复杂度为\(\mathcal{O}\left(d_{\max }\right)\)，其中\(d_{\max }\)为常数。因此，对于固定深度\(d_{\text {max }}\)，与相应的3D网格相比，额外开销是常量。在我们所有的实验中，使用了最大深度为16，这足以覆盖一个体积为\((655.36 \, \mathrm{m})^{3}\)，分辨率为1厘米的立方体。该设置的具体时间将在第5.5节中提供。

3.2 概率传感器融合

在我们的方法中，传感器读数是通过占用网格建图进行整合的，这一方法由Moravec和Elfes（1985）提出。给定传感器测量值\(z_{1:t}\)，叶节点\(n\)被占用的概率\(P\left(n \mid z_{1: t}\right)\)是根据以下公式估计的：

\[ \begin{align*} & P\left(n \mid z_{1: t}\right)= \\ & \quad\left[1+\frac{1-P\left(n \mid z_{t}\right)}{P\left(n \mid z_{t}\right)} \frac{1-P\left(n \mid z_{1: t-1}\right)}{P\left(n \mid z_{1: t-1}\right)} \frac{P(n)}{1-P(n)}\right]^{-1} \tag{1} \end{align*} \]

这个更新公式依赖于当前测量值\(z_{t}\)、先验概率\(P(n)\)和先前的估计\(P\left(n \mid z_{1: t-1}\right)\)。术语\(P\left(n \mid z_{t}\right)\)表示给定测量值\(z_{t}\)时，体素\(n\)被占用的概率。该值特定于生成\(z_{t}\)的传感器。我们在第5.1节中提供了有关实验中使用的传感器模型的详细信息。

通常假设均匀的先验概率，导致\(P(n)=0.5\)，并通过使用对数几率表示法，

公式(1)可以重写为

\[ \mathrm{L}\left(n \mid z_{1: t}\right)=\mathrm{L}\left(n \mid z_{1: t-1}\right)+\mathrm{L}\left(n \mid z_{t}\right)， \tag{2} \]

其中

\[ \mathrm{L}(n)=\log \left[\frac{P(n)}{1-P(n)}\right].\tag{3} \]

这种更新规则的表述允许更快的更新，因为乘法被加法所取代。在预计算的传感器模型的情况下，更新步骤中不必计算对数。请注意，对数几率值可以转换为概率，反之亦然，因此我们为每个体素存储此值，而不是占用概率。值得注意的是，对于某些传感器模型的对称配置，即更新为“命中”的节点与更新为“未命中”的节点具有相同的权重，这种概率更新的效果与类似于Kelly等（2006）的方法中计数命中和未命中的方式相同。

当三维地图用于导航时，通常会对占用概率\(P\left(n \mid z_{1: t}\right)\)应用阈值。当占用概率达到阈值时，体素被认为是占用的，否则被认为是空闲的，从而定义了两种离散状态。从公式(2)可以看出，要更改体素的状态，我们需要整合与定义其当前状态相同数量的观测值。换句话说，如果一个体素已经观察到空闲\(k\)次，那么它必须至少观察到占用\(k\)次，才能根据阈值被认为是占用的（假设在传感器模型中，空闲和占用的测量是等可能的）。虽然在静态环境中这种属性是可取的，但移动机器人通常面临环境的临时或永久变化，地图必须迅速适应这些变化。为了确保这种适应性，Yguel等（2007a）提出了一种钳制更新策略，该策略定义了占用估计的上下界。不是直接使用公式(2)，而是根据以下公式更新占用估计：

\[ \begin{align*} & \mathrm{L}\left(n \mid z_{1: t}\right)= \\ & \quad \max \left(\min \left(\mathrm{L}\left(n \mid z_{1: t-1}\right)+\mathrm{L}\left(n \mid z_{t}\right), l_{\max }\right), l_{\min }\right) \tag{4} \end{align*} \]

其中\(l_{\text{min}}\)和\(l_{\text{max}}\)表示对数几率值的下限和上限。直观地说，这种修改的更新公式限制了改变体素状态所需的更新次数。在我们的方法中应用钳制更新策略有两个优点：我们确保地图中的置信度保持在一定范围内，从而模型可以迅速适应环境变化。此外，我们还能够通过修剪压缩相邻体素（参见第3.4节）。正如我们将在第5.4节中讨论的，这导致了需要维护的体素数量的显著减少。通过钳制实现的压缩在完整概率方面不再是完全无损的，因为接近零和一的概率信息会丢失。然而，在钳制阈值之间，完整的概率值仍然得以保留。

概率与对数几率的关系

对数几率表示法在概率更新中具有计算上的优势，特别是在贝叶斯更新过程中。

---

1. 概率与几率

2. 概率 \(P\) : 表示事件发生的可能性, 取值范围在 0 到 1 之间。

3. 几率 Odds: 表示事件发生的概率与其不发生的概率之比。

几率的计算公式为:

\(\text { Odds }=\frac{P}{1-P}\)

举例:

• 如果事件发生的概率 \(P=0.75\), 则几率为:

\(\text { Odds }=\frac{0.75}{1-0.75}=\frac{0.75}{0.25}=3\)

这意味着事件发生的可能性是不发生的 3 倍。

---

1. 对数几率 (Log-Odds)

对数几率是几率的对数，通常以自然对数（底数为 \(e\) ）或以 \(\log _{10}\) 表示。对数几率的公式为:

\(\mathrm{L}=\ln \left(\frac{P}{1-P}\right)\)

或

\(\mathrm{L}=\log \left(\frac{P}{1-P}\right)\)

为什么使用对数几率?

• 对称性：对数几率可以取任意实数值，范围从负无穷到正无穷。当 \(P=0.5\) 时，对数几率为 0 。
• 方便计算：在概率更新中，使用对数几率可以将乘法转换为加法，提高计算效率。
• 线性更新：贝叶斯更新中的乘法关系在对数空间中变为加法，累加新的观测信息更加简单。

---

1. 从概率到对数几率的转换

2. 从概率到对数几率：

\(\mathrm{L}=\ln \left(\frac{P}{1-P}\right)\)

• 从对数几率到概率:

\(P=\frac{1}{1+e^{-\mathrm{L}}}\)

或者

\(P=\frac{e^{\mathrm{L}}}{1+e^{\mathrm{L}}}\)

举例:

• 如果 \(P=0.9\), 计算对数几率:

\(\mathrm{L}=\ln \left(\frac{0.9}{1-0.9}\right)=\ln \left(\frac{0.9}{0.1}\right)=\ln (9) \approx 2.197\)

• 如果 \(\mathrm{L}=-1\), 计算概率:

\(P=\frac{1}{1+e^{-(-1)}}=\frac{1}{1+e^1} \approx \frac{1}{1+2.718} \approx 0.2689\)

---

1. 在占用网格映射中的应用

在占用网格地图的更新中，使用对数几率有以下优势：

• 贝叶斯更新的简化

贝叶斯公式更新占用概率为：

\(P\left(n \mid z_{1: t}\right)=\frac{P\left(z_t \mid n\right) P\left(n \mid z_{1: t-1}\right)}{P\left(z_t \mid z_{1: t-1}\right)}\)

在对数几率空间中，更新公式简化为对数几率的加法：

\(\mathrm{L}\left(n \mid z_{1: t}\right)=\mathrm{L}\left(n \mid z_{1: t-1}\right)+\mathrm{L}\left(n \mid z_t\right)\)

其中 \(\mathrm{L}\left(n \mid z_t\right)=\ln \left(\frac{P\left(z_1 \mid n\right)}{P\left(z_i \mid-n\right)}\right)\) 是测量的对数似然比。

• 计算效率提高
• 加法运算更快：相比于乘法和除法，加法和减法的计算速度更快，数值稳定性更高。
• 避免下溢和上溢：在累乘小的概率值时，可能导致数值下溢。对数几率表示可以避免这个问题。

---

3.3 多分辨率查询

当测量数据被整合到我们的地图结构中时，概率更新仅针对八叉树中的叶节点进行。然而，由于八叉树是一种层次数据结构，我们可以利用树中的内部节点来启用多分辨率查询。观察到，当树的遍历仅到达某一深度时，且该深度不是叶节点的深度时，三维空间的细分会变得更粗。每个内部节点所覆盖的体积由其八个子节点占据，因此为了确定内部节点的占用概率，我们需要汇总其子节点的概率。有几种策略可以用于确定给定其八个子体积\(n_{i}\)的节点\(n\)的占用概率（Kraetzschmar等，2004）。

根据实际应用，可能使用平均占用 $$ \bar{l}(n)=\frac{1}{8} \sum_{i=1}^{8} \mathrm{L}\left(n_{i}\right) $$

• 均衡性：反映整个节点范围内的整体占用状态。
• 适合静态分析：对于需要整体趋势分析的任务，例如统计建图或环境评估，平均策略更适合。

或最大占用 $$ \hat{l}(n)=\max {i} \mathrm{L}\left(n\right) $$

• 保守性：如果任何子节点被认为是占用的，整个内部节点也被视为占用。
• 适合导航：特别适合路径规划和磁撞检测等任务, 可以确保机器人不会进入任何可能存在障碍的区域。

其中\(\mathrm{L}(n)\)返回节点\(n\)当前的对数几率占用值。使用最大子节点占用来更新内部节点可以被视为一种保守策略，非常适用于机器人导航。通过假设如果任何部分被测量为占用，则整个体积被认为是占用的，可以规划无碰撞的路径，因此我们系统中使用了最大占用更新。需要注意的是，在一个更加保守的设置中，\(\mathrm{L}(n)\)也可以定义为对未知单元格返回正的占用概率。图3展示了一个查询多个分辨率下占用体素的八叉树示例。

3.4 八叉树地图压缩

在第3.1节中，我们解释了如何通过修剪树来减少八叉树中离散占用状态的冗余信息，其中体素可以是占用或空闲的。相同的技术也可以应用于使用概率占用估计来建模占用和空闲空间的地图。然而，通常不能期望相邻节点的占用概率是相同的，即使这两个体素是由相同的物理障碍物占据的。传感器噪声和离散化误差可能导致不同的概率，因此会干扰依赖于相同节点信息的压缩方案。解决此问题的一种可能方法是对体素概率应用阈值，例如0.5，从而生成如Fairfield等（2007）所建议的离散状态估计。然而，在这种方法中，树修剪后无法恢复单个概率估计。

在我们的方法中，我们通过应用第(4)式中给出的钳制更新策略来实现地图压缩。每当体素的对数几率值达到下限\(l_{\text {min }}\)或上限\(l_{\text {max }}\)时，我们就认为该节点在我们的方法中是稳定的。直观地说，稳定的节点在较高的置信度下已经被测量为空闲或占用。在静态环境中，所有体素将在足够多的测量被整合后收敛到一个稳定状态。例如，在我们的实验中，五次一致的测量就足以将一个未知体素转化为稳定体素。如果一个内部树节点的所有子节点都是具有相同占用状态的稳定叶节点，那么这些子节点可以被修剪。如果将来有与相应内部节点状态相矛盾的测量数据被整合进来，则其子节点将被重新生成并更新。应用这种压缩只会导致接近\(P(n)=0\)和\(P(n)=1\)的概率信息丢失，而保留其中间的概率。在我们的实验中，将八叉树修剪和钳制相结合，导致最大压缩率为\(44 \%\)。

“如果将来有与相应内部节点状态相矛盾的测量数据被整合进来，则其子节点将被重新生成并更新。”

• 重新生成子节点：
• 如果将来有新的测量数据与内部节点的状态相矛盾（例如，之前认为是空闲的区域被新的观测测量为占用），该内部节点会重新生成子节点，并根据新的测量数据更新其状态。
• 动态环境中的适应性：
• 尽管修剪带来了压缩，但这种方法仍然能够适应动态环境的变化，因为修剪后的节点可以在需要时重新展开并更新。

在许多机器人导航任务中，如避障或定位，仅包含空闲或占用节点的最大似然地图就足够了。在这些情况下，可以执行基于占用阈值的有损压缩，如Fairfield等（2007）所建议的那样。对于这种压缩，所有节点都会转换为它们的最大似然（钳制）概率，然后进行树修剪。这将导致更大的压缩并减少内存需求。

图5 具有颜色信息的体积室内OctoMap的细节。完整地图覆盖了\(7.3\mathrm{m}\times7.9\mathrm{m}\times4.6\mathrm{m}\)，分辨率为2厘米。

3.5 扩展

3.5.1 具有丰富信息的地图

八叉树节点可以扩展以存储额外的数据，从而丰富地图表示。例如，体素可以存储地形信息、环境数据（如温度）或颜色信息。每个额外的体素属性需要一种方法，允许多个测量值进行融合。例如，我们扩展了我们的建图框架，以存储每个体素的平均颜色。这为用户创建了可视化效果，并实现了基于颜色的环境分类或基于外观的机器人定位（类似于（Einhorn等，2011；Mason等，2011））。它还可以作为创建彩色高分辨率表面网格（Hoppe等，1992）的起点。图5展示了一个八叉树地图，通过整合使用手持微软Kinect传感器记录的彩色点云生成。数据来自RGBD数据集中的freiburg1_360序列（Sturm等，2012），并通过RGB-D SLAM（Endres等，2012）进行对齐。

3.5.2 八叉树层级结构

我们开发了一个扩展，将层次结构依赖关系应用于我们的建图方法（Wurm等，2011）。该扩展在树结构中维护一个子地图集合，每个节点表示环境的一个子空间。在我们的系统中应用的细分是基于用户定义的输入分割和给定的空间关系，后者表达了分割之间的关系。

图6给出了一个层次结构的示例，该结构基于假设物体位于支撑平面之上。在此应用中，我们首先估计了输入中的支撑平面。然后，在输入数据中将这些支撑平面上的物体进行分割，并在单独的体积子地图中进行建模。因此，桌子是一个位于地板上的子地图，而几个家庭物品则表示为位于桌子上的子地图。

图6 桌面场景的层次八叉树模型。背景（黄色）、桌子（品红色）和物体（青色）由不同分辨率的单独八叉树地图表示。

与单一的整体环境地图相比，我们的层次方法具有多项优势：首先，每个子地图独立维护，建图参数（如分辨率）可以针对每个子地图进行调整。其次，子地图可以独立操作。例如，表示单个物体的子地图可以被移动，而其他子地图保持静态。第三，子地图的层次依赖关系可以在层次结构中编码。例如，桌子上的所有物体可以与桌子关联，如果桌子移动，那么物体也会随之移动。

该方法已在桌面操作的背景下进行了评估。桌子上的物体以非常高的分辨率进行建图，而桌子和背景结构则以较低的分辨率进行建图。这种方法使得模型的体积比表示完整场景的单一地图紧凑了约一个数量级。

4 实现细节

4.1 内存高效的节点实现

在传统的八叉树实现中，树中的每个节点除了存储数据负载外，还存储其中心位置的坐标、体素大小以及指向子节点的指针。然而，这会导致显著的内存开销。由于节点的位置和体素大小可以在遍历八叉树时重新构建，因此我们不在节点中显式存储这些信息，以减少内存开销。

为了减少内存使用，优化的八叉树实现不显式存储节点的中心位置和体素大小，而是通过以下方法动态重建这些信息：

1. 节点的位置计算：

2. 节点的位置可以根据其父节点的位置以及当前节点相对于父节点的索引（例如第几个子节点）动态计算。

3. 例如，对于一个三维八叉树：

   • 父节点的位置为\((x_{\text{parent}}, y_{\text{parent}}, z_{\text{parent}})\)。
   • 子节点的索引（0~7）决定其在父节点空间中的偏移量。
   • 子节点的中心位置可以由父节点的位置和当前深度层的体素大小计算得出。

4. 体素大小计算：

5. 每个节点的体素大小可以根据树的深度动态确定。

6. 例如，八叉树的深度为 d，根节点的体素大小为 \(S_{\text{root}}\)，则深度为 d 的节点的体素大小为：

\[ S_{\text{voxel}} = \frac{S_{\text{root}}}{2^d} \]

通常，八叉树节点需要维护其子节点的有序列表。这可以通过为每个节点使用八个指针直接实现。如果模型稀疏数据，这些指针的内存需求（\(8 \times 4\) 字节 = 32 字节，假设为32位架构）将导致显著的内存开销（Wilhelms和Van Gelder，1992）。我们通过为每个节点使用一个子节点指针指向包含八个指针的数组来克服这一问题（图7，左）。只有在节点确实具有子节点时，才分配这个数组，对于叶节点则不分配。因此，八叉树中的任何叶节点仅存储建图数据本身（例如，占用概率）和一个（空）指针。内部节点则额外存储八个指针指向它们的子节点。在我们评估中使用的与机器人相关的数据集，\(80\%-85\%\)的八叉树节点是叶节点。在我们的实验中，与为每个节点分配8个指针相比，上述实现节省了\(60\%-65\%\)的内存。

图7 左：图2中八叉树示例的第一个节点在内存中的样子，节点通过指针连接。数据作为表示占用的一个浮动值存储。右：图2中的完整树，作为紧凑的序列化位流存储。所有最大似然的占用信息可以仅用六个字节存储，使用两个比特表示每个节点的八个子节点标签（00: 未知；01: 占用；10: 空闲；11: 内部节点，子节点下一个在流中）。

为了存储每个体素的占用概率，使用一个浮动值（通常为4字节）即可表示对数几率值。这导致在32位架构下，内部节点的节点大小为40字节，叶节点为8字节。需要注意的是，大多数编译器为了运行时效率，会对成员数据进行内存对齐，即节点数据会填充到一个字长的倍数（32位架构为4字节）。64位架构可以以指针和字的大小是原来的两倍的代价，寻址大量内存。在这种架构下，内部节点的内存大小增加到80字节，叶节点的大小增加到16字节。需要注意的是，数据结构的实际大小（内部节点为76字节，叶节点为12字节）会通过大多数编译器填充到字长的倍数（64位架构为8字节）。

在我们的方法中，八叉树是均质的，即所有节点具有相同的结构并存储占用数据。尽管内部节点可以通过省略占用信息来节省8字节，但按照公式(5)或(6)维护它可以实现多分辨率查询，其中树遍历将在固定深度停止。

类之间的虚拟继承允许在运行时进行动态调度，但每个对象实例都需要额外的指向虚拟函数表（vtable）的指针。为了最小化内存占用，我们避免在八叉树节点实现中使用这种开销。我们对节点应用直接继承和类型转换，只有在八叉树类中才使用虚拟继承。这种方法只会导致每个八叉树地图一个指针的开销。

4.2 八叉树类型

我们框架中最常见的八叉树和节点类型总结在图8的UML图中。基本的八叉树功能在OcTreeBase中实现，基本的节点功能在OcTreeDataNode中实现。OcTreeDataNode是一个模板类，模板参数是存储在节点中的数据，而OcTreeBase是一个模板类，模板参数是节点类型。OccupancyOcTreeBase为树的实现添加了占用建图功能，如扫描插入和射线投射。主要的占用八叉树类OcTree继承自OccupancyOcTreeBase，并使用OcTreeNode作为节点。这个结构允许我们在不同层次灵活地扩展我们的框架，例如，可以扩展节点以存储自定义数据或为八叉树添加新功能。一个示例是ColorOcTree的实现，使用ColorOcTreeNodes（如图5所示）。这些节点在存储占用估计的同时，还存储颜色信息，如第3.5.1节所述。

图8 最常见的八叉树和节点类的UML图。

当前实现中，最大树深度限制为16层。这通过使用可计算的体素地址来实现快速的树遍历。然而，这个深度限制也对八叉树的最大空间范围提出了限制。例如，在1厘米的分辨率下，地图每个维度的最大覆盖范围为\(2^{16} \times 0.01 \, \mathrm{m}=655.36 \, \mathrm{m}\)。虽然这对大多数室内应用已经足够，但该实现可以直接扩展到32层深度，从而允许在1厘米分辨率下覆盖\(2^{32} \times 0.01 \, \mathrm{m}=42949672.96 \, \mathrm{m}\)。

4.3 地图文件生成

许多机器人应用需要将地图存储在文件中。这包括在设置阶段生成地图，并随后供移动机器人用于路径规划和定位的情况。另一个场景是多机器人系统，其中机器人之间交换地图。在这两种情况下，都需要一种紧凑的序列化表示，以最小化磁盘空间或通信带宽的消耗。

当最大似然估计足以完成任务时，可以生成最紧凑的文件。在这种情况下，节点的每个概率被丢弃。如前所述，在没有记录任何信息的体积中，可能在机器人系统中特别重要，例如在探索过程中。出于这个原因，我们在地图文件中显式地区分空闲区域和未知区域，并将节点编码为占用、空闲、未知或内部节点。

最大似然估计(Maximum Likelihood Estimation, MLE）是一种简化方法，假设每个节点的占用状态是其最可能的状态。

在这种表示中，每个节点的概率值（例如 P(n)或对数几率值）被丢弃，仅保留“占用”、“空闲”或“未知”的离散状态。

使用这些标签，八叉树地图可以递归地编码为紧凑的位流。每个节点仅由其八个子节点的标签表示。从根节点开始，每个不是叶节点的子节点都会递归地添加到位流中。叶节点不需要被添加，因为它们可以在解码过程中根据其标签重建。如图7（右）所示，位流编码的示意图。每行表示一个节点，最上面的一行对应于根节点。最后一行仅包含叶节点，因此不再添加其他节点。

在这种最大似然表示中，每个节点占用16位内存，每个子节点2位，生成一个紧凑的地图文件。在我们的实验中，即使对于大型室外环境，分辨率较高的情况下，文件大小也从未超过15MB，（详见第5.4节）。

也有一些应用需要存储和维护地图中的所有信息。这包括将硬盘空间用作二级存储并临时保存地图，直到需要再次访问时。另一个用例是存储额外的节点数据，如颜色或地形信息，这些信息在最大似然编码中会丢失。在这些情况下，我们通过存储节点的数据（占用和额外数据）以及每个节点8个比特来编码节点，这些比特指定是否存在子节点。然而，正如我们将在实验中展示的，这将导致文件大幅增加。

需要注意的是，类似于八叉树在内存中的表示，序列化流中不包含任何实际的三维坐标。为了重建地图，只需要知道根节点的位置。所有其他节点之间的空间关系都隐式地存储在编码中。

为了减少存储空间，八叉树地图被编码为紧凑的位流（bitstream）形式，具体方式如下：

1. 节点的编码：
2. 每个节点由其八个子节点的状态表示，使用两位（bits）表示每个子节点的标签：
   • 00：未知区域（未知状态）。
   • 01：占用节点（障碍物）。
   • 10：空闲节点（可通过区域）。
   • 11：内部节点（具有子节点）。
3. 递归编码规则：
4. 从根节点开始，所有非叶节点都会递归地添加到位流中。
5. 每个节点的八个子节点状态形成一组二进制标签，按顺序依次加入位流。
6. 如果一个子节点本身是内部节点，则递归编码其子节点。
7. 叶节点不需要额外添加，因为它们的状态在解码时可以根据标签直接恢复。
8. 位流的结束条件：
9. 最后一行仅包含叶节点，因此不再添加其他节点。

---

图7右侧展示了一个八叉树地图的紧凑位流编码示意图：

1. 最上层（根节点）：
2. 根节点的子节点标签是 00, 00, 11, 00, 00, 00, 10, 10。
3. 这表示根节点的第三个子节点是内部节点，其他子节点的状态为未知或空闲。
4. 第二层（递归编码内部节点）：
5. 对于根节点的第三个子节点（标记为 11），递归编码其八个子节点的状态。
6. 最后一层（叶节点）：
7. 最后一行仅包含叶节点的状态，不再递归，因为这些节点没有子节点。

通过这种递归结构，整个树被逐层展开为位流，而不需要显式存储每个节点的坐标或其他冗余信息。

---

5. 位流编码的优点

• 极高的空间效率：
• 使用2位表示每个子节点状态，大幅减少了存储空间。
• 无需存储多余的坐标或子节点指针信息。
• 支持快速解码：
• 解码时，从根节点开始逐层递归还原，直接从位流中提取节点状态和树结构。
• 适用于大规模场景：
• 这种方法特别适合处理大规模三维环境中的地图，能够有效降低存储和传输的开销。

---

6. 应用场景

• 路径规划与定位：
• 在路径规划中，只需使用最大似然状态（占用或空闲）即可。
• 紧凑文件形式使得机器人能快速加载地图。
• 多机器人协作：
• 紧凑的位流适用于多机器人之间的地图共享，减少通信带宽需求。
• 存档与回放：
• 大规模环境建模后，地图可以被高效存储，以供后续任务使用。

---

7. 总结

这段文字和图7右侧展示了一种通过紧凑的位流表示八叉树地图的方法：

1. 核心思想：节点的状态（占用、空闲、未知或内部节点）被压缩为2位标签，通过递归方式编码整个八叉树。
2. 实现方式：从根节点开始逐层递归编码，仅在需要时添加子节点状态；叶节点状态通过标签直接还原。
3. 优点：显著减少存储空间和传输带宽，同时保持地图功能性。
4. 应用：适用于路径规划、多机器人协作以及大规模地图存储的场景。

这种方法在实际应用中非常高效，是三维环境建模中常用的优化技术之一。

4.4 我们的OctoMap实现

OctoMap作为一个独立的C++库提供。它已根据BSD许可证发布，并可以从http://octomap.github.com获得。源代码经过详细文档化，库使用CMake支持多个平台（Linux和Mac OS X使用GCC，Windows使用MinGW或Visual Studio）。在机器人操作系统（ROS）中，OctoMap作为预编译的Debian包提供，例如，Ubuntu发行版。更多的ROS集成可以在包octomap_ros和octomap_msgs中找到。

图9 OctoMap可视化应用程序 octovis

OctoMap可以通过使用pkg-config或CMake构建系统中的find_package机制，轻松集成到任何其他框架中进行编译和链接。

该库还提供了一个基于OpenGL的3D可视化应用程序，用于查看存储的八叉树文件并从范围数据中逐步构建地图，这有助于故障排除和地图数据检查（见图9）。它还提供了基本的编辑功能。

4.4.1 集成传感器测量

通过调用占用八叉树类 OcTree 的方法 \(\text{insertRay}( \cdot )\)，可以使用射线投射（raycasting）集成单个范围测量。这会将测量的端点更新为占用，同时沿射线到传感器原点的所有其他体素被更新为空闲。

点云数据，例如来自3D激光扫描或立体摄像头的数据，通过\(\text{insertScan} (\cdot )\) 方法集成。这种批量操作已经过优化，比逐一跟踪每条射线更高效。

最后，八叉树中的单个节点可以通过调用 \(\text{updateNode}( \cdot )\) 方法通过点测量来更新。

4.4.2 访问数据

可以通过搜索节点的坐标来访问单个八叉树节点。为了高效的批量查询，我们的实现提供了迭代器，用于遍历八叉树，类似于标准的C++容器类。使用这些迭代器，可以查询所有节点、叶节点，或者查询位于特定边界框内的叶节点，或者根据其他标准对它们进行过滤。

射线交点查询，即从一个起点向给定方向投射射线直到它碰到占用体积，是机器人三维地图中的一个重要用例。这种查询用于可视性检查或使用范围传感器进行定位。因此，我们在 \(\text{castRay}( \cdot )\) 方法中提供了此功能。

5 评估

本文提出的方法已经使用多个真实世界数据集以及模拟数据集进行了评估。实验旨在验证所提出的表示方法是否满足引言中提出的要求。更具体地，我们展示了我们的方法能够充分建模各种类型的环境，并且它是一种可更新和灵活的地图结构，能够紧凑地存储。

为了进行评估，我们使用了当前版本的OctoMap 1.5.1。(最新版本为1.10.0)评估的数据集可以在线获得，并可以使用工具 graph2tree 将其从3D点云转换为八叉树地图，工具还会打印所有必要的统计信息。

5.1 激光测距数据的传感器模型

OctoMap可以与任何类型的距离传感器一起使用，只要有可用的逆传感器模型。由于我们的真实世界数据集主要是通过激光测距仪获取的，因此我们采用了基于光束的逆传感器模型，假设测量的端点对应于障碍物表面，并且传感器原点和端点之间的视线不包含任何障碍物。所有体积的占用概率初始化为均匀先验\(P(n)=0.5\)。为了高效地确定需要更新的地图单元格，执行射线投射操作，确定从传感器原点到测量端点的光束沿线的体素。为了提高效率，我们使用Bresenham算法的3D变体来近似光束（Amanatides和Woo，1987）。沿光束的体积按第3.2节所述进行更新，使用以下逆传感器模型：

\[ \mathrm{L}\left(n \mid z_{t}\right)= \begin{cases} l_{\text {occ }} & \text{如果光束在体积内被反射} \\ l_{\text {free }} & \text{如果光束穿过体积} \end{cases} \]

在我们的实验中，我们使用了对数几率值\(l_{\text{occ}} = 0.85\)和\(l_{\text{free}} = -0.4\)，分别对应占用体积和空闲体积的概率0.7和0.4。钳制阈值设置为\(l_{\text{min}} = -2\)和\(l_{\text{max}} = 3.5\)，分别对应占用体积和空闲体积的概率0.12和0.97。我们通过实验确定这些值最适合我们的用例，即通过激光测距仪建图大部分静态环境，同时仍保持地图的可更新性以适应偶尔的变化。通过调整这些可变阈值，可以实现更强的压缩。正如我们将在第5.6节中评估的那样，地图置信度和压缩之间存在权衡。

图10 激光扫描仪通过旋转以浅角度扫描平面表面。在第一次扫描时测量为占用的单元格（上图）在传感器旋转后在随后的扫描中被更新为空闲（下图）。占用单元格以灰色框显示，空闲单元格以白色显示。

图11 一个模拟的无噪声3D激光扫描（左）被集成到我们的3D地图结构中。传感器以浅角度扫描会导致不希望的离散化效应（中）。通过每个体积最多更新一次，地图正确地表示了环境（右）。为了清晰起见，仅显示占用的单元格。

5.2 来自真实传感器数据的三维模型

在本实验中，我们展示了我们的方法建模真实世界环境的能力。使用了各种不同的数据集。请注意，在实验中明确建模了空闲空间，但为了清晰起见，图中未显示空闲空间。

室内数据集名为FR-079走廊，使用配备SICK LMS激光测距仪的Pioneer2 AT平台在云台上记录。我们通过应用三维扫描匹配方法减少了里程误差。机器人在弗莱堡校园的079号楼走廊内行驶了三次，结果得到了66个3D扫描，总计600万个端点。在处理该数据集时，我们将激光光束的最大范围限制为10米。这去除了窗户外观察到的建筑物外的杂散测量。图12显示了生成的地图。

图12 FR-079走廊数据集的3D地图，自上而下查看。通过玻璃门部分观察到相邻房间的结构（场景大小：\(43.7 \, \mathrm{m} \times 18.2 \, \mathrm{m} \times 3.3 \, \mathrm{m}\)）。

一个相当大的室外数据集是在弗莱堡计算机科学校园记录的。它由81个密集的3D扫描组成，覆盖了\(292 \, \mathrm{m} \times 167 \, \mathrm{m}\)的区域，沿着723米的轨迹。这个数据集包含了2000万个端点。在进一步的实验中，我们使用了New College数据集（Smith等，2009）（Epoch C，总计1400万个端点）的激光测距数据。该数据记录在一个大规模的室外环境中，使用两个固定激光扫描仪，分别向机器人左右两侧进行扫描。对于该数据集，使用了由视觉里程计生成的优化机器人轨迹估计（Sibley等，2009）。结果生成的室外地图如图13所示。

图13 两个室外环境的八叉树地图，分辨率为0.2米。为了清晰起见，只有占用的体积被显示，高度通过颜色（灰度）编码进行可视化。上：弗莱堡校园数据集（场景大小：\(292 \, \mathrm{m} \times 167 \, \mathrm{m} \times 28 \, \mathrm{m}\)），下：新学院数据集（场景大小：\(250 \, \mathrm{m} \times 161 \, \mathrm{m} \times 33 \, \mathrm{m}\)）。

最后，我们将freiburg \(1 \_360\) RGBD数据集的数据集成到我们的地图表示中，总共有来自微软Kinect传感器的2.1亿个端点（见第3.5.1节）。最终地图如图5所示，表示一个办公室环境，分辨率为2厘米。在该地图中，我们还存储了每个体素的颜色信息。

5.3 地图精度

本实验展示了3D地图如何准确地表示用于构建该地图的数据。请注意，这一特定评估与底层八叉树结构无关，因为我们的方法能够将相同的数据建模为三维网格。我们通过测量正确建图的单元格占所有3D扫描的百分比来评估精度。如果它在地图和评估的3D扫描中具有相同的最大似然状态（空闲或占用），一个3D地图单元格被认为是正确建图的。在这里，扫描被处理为将其插入到已构建的地图中，即端点必须是占用的，并且传感器与端点之间的所有体素必须是空闲的。作为第二个度量标准，我们通过在构建地图时跳过每第五个扫描来进行交叉验证，并使用这些跳过的扫描来评估正确建图的单元格百分比。

地图数据集	精度	交叉验证
FR-079走廊\((5 \, \mathrm{cm})\)	\(97.27 \%\)	\(96.00 \%\)
弗莱堡校园（10 cm）	\(97.89 \%\)	\(95.80 \%\)
新学院（Ep. C）（10 cm）	\(98.79 \%\)	\(98.46 \%\)

表1 建图精度和交叉验证，表示评估的3D扫描和已构建地图之间正确建图单元格的百分比。对于精度，我们使用了所有扫描进行地图构建和评估。对于交叉验证，我们使用\(80\%\)的所有扫描来构建地图，剩余的\(20\%\)用于评估。

表1中的结果显示，我们的建图方法能够准确地表示环境。剩余的误差很可能是由于传感器噪声、离散化效应或扫描对齐不完全完美造成的。交叉验证结果只失去了很少的精度，这表明概率传感器模型产生了现实且可预测的结果。

5.4 内存消耗

在本实验中，我们评估了我们方法的内存消耗。我们处理了多个数据集，采用了不同的树分辨率。我们分析了使用八叉树压缩和最大似然压缩（将每个节点转换为完全空闲或占用的状态）前后的表示内存使用情况。为了进行比较，我们还确定了通过线性初始化内存的优化对齐的最小3D网格所需的内存量。根据第4.1节，存储在32位架构八叉树中的占用数据的内存消耗为：

\[ \mathrm{mem}_{\text{tree}} = n_{\text{inner}} \times 40 \, \mathrm{B} + n_{\text{leafs}} \times 8 \, \mathrm{B} \]

其中，\(n_{\text{inner}}\) 是内部节点的数量，\(n_{\text{leafs}}\) 是叶节点的数量。存储相同信息（一个浮动值表示占用概率）的最小3D网格的大小为：

\[ \mathrm{mem}_{\text{grid}} = \frac{x \times y \times z}{r^3} \times 4 \, \mathrm{B} \]

其中，\(x, y, z\) 是地图的最小包围盒在每个维度的大小，\(r\) 是地图分辨率。

此外，我们还使用第4.3节描述的完整概率模型和压缩二进制格式将每个地图写入磁盘，并评估了生成的文件大小。

表2显示了示例分辨率的内存使用情况。可以看出，特别是在大型室外环境中，可以实现很高的压缩比。在这些情况下，修剪会合并大量的空闲空间体积，未知空间区域不占用任何内存。需要注意的是，10厘米分辨率的室外数据集的3D网格甚至无法适配32位机器的可寻址主内存。另一方面，我们的地图结构也能够以适中的内存要求建模细粒度的室内环境。在非常狭小的空间中，优化对齐的3D网格可能比未压缩的建图八叉树占用更少的内存。然而，一旦使用压缩技术，这种效果会减小。

地图数据集	建图区域 \(\left[\mathrm{m}^{3}\right.\) ]	分辨率 [cm]	3D网格内存 [MB]	八叉树压缩内存 [MB]			文件大小 [MB]
				无	修剪	最大似然	完整	有损
FR-079走廊	\(43.7 \times 18.2 \times 3.3\)	5	78.88	73.55	41.62	24.72	15.76	0.67
		10	10.01	10.87	7.22	5.02	2.70	0.14
弗莱堡校园	\(292 \times 167 \times 28\)	10	5162.90	1257.57	990.66	504.76	379.70	13.82
		20	648.52	187.93	130.24	74.12	49.68	2.00
		80	10.58	4.55	4.12	3.09	1.53	0.08
新学院（Ep. C）	\(250 \times 161 \times 33\)	10	5058.76	607.92	395.42	230.33	148.75	6.40
		20	633.64	91.33	50.57	35.95	18.65	0.99
		80	10.13	2.34	1.79	1.69	0.63	0.05
freiburg1_360 (RGBD)	\(7.9 \times 7.3 \times 4.6\)	2	252.99*	159.97*	45.52*	20.05	21.59*	0.52
		5	16.19*	11.24*	4.55*	2.52	2.11*	0.07

表2：不同八叉树压缩类型与完整3D占用地图（称为3D网格）在32位架构上的内存消耗比较。八叉树内存压缩是通过将相同的子节点合并到父节点中实现的（称为修剪）。通过将每个节点转换为其最大似然值（完全空闲或占用），并随后修剪整个树，可以实现更高效但更有损的内存压缩。然后，可以将仅包含空闲和占用节点的最大似然树序列化为紧凑的二进制文件格式（称为有损文件）。(*): 体素包含来自RGBD数据集的完整颜色信息。

随着机器人的探索，内存使用量的变化如图14所示。在FR-079走廊的数据集中的扫描1-22和39-44，以及弗莱堡校园的数据集中的扫描1-50和65-81中，内存使用量随探索新区域而增加。在其余时间，先前建图的区域被重新访问，内存使用量保持几乎恒定，或者由于修剪而减少。

图14 建图FR-079走廊和弗莱堡校园数据集时的内存使用情况。

如预期的那样，内存使用量随树分辨率的增加呈指数增长。图15展示了这一效果，其中我们使用对数缩放进行绘图。

图15 分辨率对弗莱堡校园数据集内存使用的影响。请注意，使用了对数缩放。

表2显示了序列化二进制最大似然地图（称为“有损”）和完整概率模型（“完整”）的文件大小。需要注意的是，地图文件甚至可以通过使用标准文件压缩方法进一步压缩。即使是相当大的室外数据集（如弗莱堡校园和新学院）的地图，文件大小也小于14MB。

5.5 运行时间

在以下实验中，我们分析了在我们的框架中集成和访问数据所需的时间。所有的运行时间评估都在标准桌面CPU（Intel Core i7-2600，3.4 GHz）的单个核心上进行，针对不同的地图数据集。

5.5.1 地图生成

首先，我们分析了通过集成范围数据来生成地图所需的时间。此时间取决于地图分辨率和集成的光束长度。我们使用完整激光测距（最大距离为50米）以及将最大范围限制为10米的几种分辨率处理了FR-079走廊和弗莱堡校园数据集。每个光束的平均插入时间如图16所示。

图16 更新八叉树地图时插入一个数据点的平均时间，数据集包括弗莱堡校园和FR-079走廊。截断版本只插入最大传感器范围为10米的射线。

在我们的实验中，3D扫描通常包含约\(90000-250000\)个有效测量。通常，这样的扫描可以在不到一秒的时间内集成到地图中。这表明我们当前的实现即使面对RGBD相机的高数据量（这些相机以快速帧率输出最多300000个点）也能处理得很好，尽管测量范围较短。

在像弗莱堡校园数据集这样的长测量光束和大面积室外环境中，通过限制地图更新范围可以获得加速。然而，在室内环境中，由于只有少数传感器光束能到达较远的地方，限制传感器范围并不会明显加速处理。

5.5.2 地图查询

我们评估了使用迭代器遍历现有地图中所有叶节点（空闲或占用）所需的时间（见第4.4.2节）。查询的深度可以在运行时进行限制，在我们的数据结构中，这相当于在较粗分辨率的地图中进行查询。这使得在较粗分辨率足够时，可以更高效地进行树遍历。

图17展示了遍历多个地图的时间，遍历深度为16，表示完整的地图分辨率（深度截断=0）。该图还给出了限制查询深度时查询所有叶节点的时间。每增加一个深度截断，最小体素的边长就会加倍，遍历速度提高大约两倍。可以看到，地图遍历非常高效。即使是在完整的地图分辨率下，包含1087014个占用节点和3377882个空闲节点的弗莱堡校园大地图也可以在51毫秒内完成遍历。

图17 在多个地图中遍历所有八叉树叶节点所需的时间。通过限制查询的深度（称为深度截断），可以遍历较粗的地图。

5.6 钳制参数

最后，我们分析了钳制阈值对地图精度和压缩的影响。由于这些阈值为占用概率提供了下限和上限，因此与没有钳制的完整地图相比，接近\(P=0\)和\(P=1\)的信息会丢失。钳制地图表示了完整地图的近似，因此我们使用Kullback-Leibler散度（KLD），并对整个地图求和作为度量。由于占用是一个二元随机变量，具有离散状态空闲和占用，因此可以通过对所有地图节点\(n\)求和来计算从完整地图\(M_{f}\)到钳制地图\(M_{c}\)的KLD：

\[ \begin{align*} & \operatorname{KLD}\left(M_{f}, M_{c}\right)= \tag{10} \\ & \quad \sum_{n}\left(\ln \left(\frac{P(n)}{Q(n)}\right) P(n)+\ln \left(\frac{1-P(n)}{1-Q(n)}\right)(1-P(n))\right), \end{align*} \]

其中\(P(n)\)是节点\(n\)在\(M_{f}\)中的占用概率，而\(Q(n)\)是节点\(n\)在\(M_{c}\)中的占用概率。

图18显示了从\([0: 1]\)（无钳制，无损）到\([0.4: 0.6]\)（强钳制，损失最大）的一系列占用范围和不同地图的结果。我们选择的默认阈值范围\([0.12: 0.97]\)的值以细水平线表示，蓝色虚线表示内存消耗，红色表示KLD。这个钳制范围主要是为了在激光建图和环境中偶尔变化（如人通过扫描或门关闭）时表现最佳。可以看到，通过更强的钳制可以实现更高的压缩，但代价是失去地图的置信度。在最严重的情况下，一个传感器更新可能就足以将一个体素标记为完全空闲或占用，从而失去通过概率更新过滤噪声的能力。需要注意的是，尽管钳制对地图压缩有利，但即使没有钳制，损失压缩的地图也比3D网格小（参见表2）。

图18 不同钳制范围对我们三个数据集的地图压缩和精度的影响。更高的钳制导致占用范围更小，从而提高了八叉树压缩的效率（内存消耗，蓝色虚线）。Kullback-Leibler散度（KLD，红色）衡量了钳制表示和完整概率的未钳制地图之间的信息损失。为了对比，我们的默认钳制范围\([0.12:0.97]\)通过水平线在蓝色（虚线）表示内存消耗，红色表示KLD。

6 案例研究

自2010年首次推出以来（Wurm等，2010），OctoMap框架引起了广泛关注，并已应用于多个领域。这些应用包括6D定位（Hornung等，2010）、空中机器人自主导航（Heng等，2011；Müller等，2011）、类人机器人自主导航（Oßwald等，2012；Maier等，2012）、3D探索（Shade和Newman，2011；Dornhege和Kleiner，2011）、3D SLAM（Hertzberg等，2011）、3D臂导航（Ciocarlie等，2010）、语义地图（Blodow等，2011）和在杂乱环境中的导航（Hornung等，2012）。

接下来，我们将详细描述一些使用案例，以展示OctoMap库的多功能性和易集成性。

6.1 3D定位

在我们之前的工作中（Hornung等，2010），我们开发了一种基于OctoMap作为3D环境模型的定位方法。在这种方法中，类人机器人在复杂室内环境中的6D躯干姿态通过基于2D激光测距、IMU和关节编码器数据的蒙特卡洛定位来跟踪。对于粒子滤波观察模型，我们最初使用了端点模型，后来结合视觉观测使用了优化的射线投射方法进行局部优化（Oßwald等，2012）。最终的定位结果非常精确，甚至使类人机器人能够爬升螺旋楼梯。我们的实现是开源的，并使用了OctoMap中的射线投射功能（见第4.4.2节）。这使得它可以被其他机器人定位系统重复使用。

6.2 桌面操作

ROS的collider包基于3D点云构建了一个碰撞地图。来自多个传感器的数据，如倾斜激光和立体摄像机，使用OctoMap进行了融合。八叉树节点被扩展以存储时间戳属性，这使得在动态变化的环境中逐步清除节点成为可能。这种新的碰撞地图使得ROS臂导航和抓取流水线（Ciocarlie等，2010）能够动态响应变化并处理传感器噪声。与先前的固定大小体素网格不同，新的实现允许最初没有边界的工作空间，能够整合来自多个传感器的数据，并且更加高效地使用内存。

6.3 在杂乱环境中的导航

OctoMap还被用于创建一个移动操作的导航模块。在这个项目中，PR2机器人用两只手从一张桌子上拾起大物体，并通过狭窄的通道将其搬运到另一张桌子（Hornung等，2012）。该系统将3D传感器数据集成到OctoMap中。生成的3D占用地图随后被用来根据机器人的完整运动学配置和附加物体进行碰撞检查。多层投影2D地图和一个基于运动原语的随时规划器允许几乎实时地进行规划，并且具有有界次优性。基于OctoMap的导航系统和增量地图框架都已在ROS中开源1 开源2。

7 结论

在本文中，我们提出了OctoMap，一个用于三维建图的开源框架。我们的方法使用基于八叉树的高效数据结构，使得可以紧凑地表示内存并进行多分辨率地图查询。通过使用概率占用估计，我们的方法能够表示包含空闲和未知区域的体积3D模型。所提出的方法使用有界的每体积置信度，允许无损压缩方案并显著减少内存使用。我们通过多个真实世界数据集对方法进行了评估。结果表明，我们的方法能够准确地建模环境，同时最小化内存需求。

OctoMap可以轻松集成到机器人系统中，并且已经在多个机器人项目中成功应用。该实现作为BSD许可证下的C++源代码提供。数据集在线提供，供验证我们的实验结果并进行对比。

References

Amanatides J, Woo A (1987) A fast voxel traversal algorithm for ray tracing. In: Proceedings of Eurographics, Amsterdam, The Netherlands

Blodow N, Goron L, Marton Z, Pangercic D, Ruhr T, Tenorth M, Beetz \(M\) (2011) Autonomous semantic mapping for robots performing everyday manipulation tasks in kitchen environments. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)

Botsch M, Wiratanaya A, Kobbelt L (2002) Efficient high quality rendering of point sampled geometry. In: EGRW ' 02 : Proc. of the 13th Eurographics workshop on Rendering, pp 53-64

Ciocarlie M, Hsiao K, Jones EG, Chitta S, Rusu RB, Sucan IA (2010) Towards reliable grasping and manipulation in household environments. In: Intl. Symposium on Experimental Robotics (ISER)

Cole D, Newman P (2006) Using laser range data for 3D SLAM in outdoor environments. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA)

Dornhege C, Kleiner A (2011) A frontier-void-based approach for autonomous exploration in 3D. In: IEEE International Symposium on Safety, Security and Rescue Robotics (SSRR)

Douillard B, Underwood J, Melkumyan N, Singh S, Vasudevan S, Brunner C, Quadros A (2010) Hybrid elevation maps: 3D surface models for segmentation. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)

Dryanovski I, Morris W, Xiao J (2010) Multi-volume occupancy grids: An efficient probabilistic 3D mapping model for micro aerial vehicles. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)

Einhorn E, Schröter C, Groß HM (2011) Attention-driven monocular scene reconstruction for obstacle detection, robot navigation and map building. Robotics \& Autonomous Systems 59(5):296-309

Elseberg J, Borrmann D, Nüchter A (2011) Efficient processing of large 3d point clouds. In: Proc. of the XXIII Int. Symp. on Information, Communication and Automation Technologies (ICAT '11)

Endres F, Hess J, Engelhard N, Sturm J, Cremers D, Burgard W (2012) An evaluation of the RGB-D SLAM system. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA)

Fairfield N, Kantor G, Wettergreen D (2007) Real-time SLAM with octree evidence grids for exploration in underwater tunnels. Journal of Field Robotics

Fournier J, Ricard B, Laurendeau D (2007) Mapping and exploration of complex environments using persistent 3D model. In: Computer and Robot Vision, 2007. Fourth Canadian Conf. on, pp 403-410

Girardeau-Montaut D, Roux M, Marc R, Thibault G (2005) Change detection on points cloud data acquired with a ground laser scanner. International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences 36:30-35

Gutmann JS, Fukuchi M, Fujita M (2008) 3D perception and environment map generation for humanoid robot navigation. Int J Rob Res 27(10):1117-1134

Habbecke M, Kobbelt L (2007) A surface-growing approach to multiview stereo reconstruction. In: Proc. of the IEEE Conf. on Computer Vision and Pattern Recognition (CVPR)

Hadsell R, Bagnell JA, Hebert M (2009) Accurate rough terrain estimation with space-carving kernels. In: Proc. of Robotics: Science and Systems (RSS)

Hebert M, Caillas C, Krotkov E, Kweon IS, Kanade T (1989) Terrain mapping for a roving planetary explorer. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA)

Heng L, Meier L, Tanskanen P, Fraundorfer F, Pollefeys M (2011) Autonomous obstacle avoidance and maneuvering on a vision-guided MAV using on-board processing. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA)

Hertzberg C, Wagner R, Birbach O, Hammer T, Frese U (2011) Experiences in building a visual slam system from open source components. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA)

Hoppe H, DeRose T, Duchamp T, Mcdonald J, Stuetzle W (1992) Surface reconstruction from unorganized points. SIGGRAPH Computer Graphics 26(2):71-78

Hornung A, Wurm KM, Bennewitz M (2010) Humanoid robot localization in complex indoor environments. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)

Hornung A, Phillips M, Jones EG, Bennewitz M, Likhachev M, Chitta S (2012) Navigation in three-dimensional cluttered environments for mobile manipulation. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA)

Kammerl J, Blodow N, Rusu RB, Gedikli S, Beetz M, Steinbach EG (2012) Real-time compression of point cloud streams. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA)

Kelly A, Stentz A, Amidi O, Bode M, Bradley DM, Diaz-Calderon A, Happold M, Herman H, Mandelbaum R, Pilarski T, Rander P, Thayer S, Vallidis N, Warner R (2006) Toward reliable off road autonomous vehicles operating in challenging environments. J of Robotics Research 25(5-6):449-483

Kraetzschmar G, Gassull G, Uhl K (2004) Probabilistic quadtrees for variable-resolution mapping of large environments. In: Ribeiro MI, Victor SJ (eds) Proc. of the 5th IFAC/EURON Symposium on Intelligent Autonomous Vehicles, Lisbon, Portugal

Krainin M, Henry P, Ren X, Fox D (2011) Manipulator and object tracking for in-hand 3d object modeling. J of Robotics Research 30(11):1311-1327

Laine S, Karras T (2010) Efficient sparse voxel octrees. In: ACM SIGGRAPH Symposium on Interactive 3D Graphics and Games

Magnusson M, Duckett T, Lilienthal AJ (2007) Scan registration for autonomous mining vehicles using 3D-NDT. Journal of Field Robotics 24(10):803-827

Maier D, Hornung A, Bennewitz M (2012) Real-time navigation in 3d environments based on depth camera data. In: Proc. of the IEEERAS Int. Conf. on Humanoid Robots (Humanoids)

Mason J, Ricco S, Parr R (2011) Textured occupancy grids for monocular localization without features. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA)

Meagher D (1982) Geometric modeling using octree encoding. Computer Graphics and Image Processing 19(2):129-147

Moravec H (1996) Robot spatial perception by stereoscopic vision and 3D evidence grids. Tech. Rep. CMU-RI-TR-96-34, Robotics Institute, Pittsburgh, PA

Moravec H, Elfes A (1985) High resolution maps from wide angle sonar. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA), St. Louis, MO, USA, pp 116-121

Müller J, Kohler N, Burgard W (2011) Autonomous miniature blimp navigation with online motion planning and re-planning. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)

Newcombe R, Izadi S, Hilliges O, Molyneaux D, Kim D, Davison A, Kohli P, Shotton J, Hodges S, Fitzgibbon A (2011) KinectFusion: Real-time dense surface mapping and tracking. In: Mixed and Augmented Reality (ISMAR), 2011 10th IEEE International Symposium on, IEEE, pp 127-136

Nüchter A, Lingemann K, Hertzberg J, Surmann H (2007) 6D SLAM—3D mapping outdoor environments: Research articles. J Field Robot 24(8-9):699-722

Oßwald S, Hornung A, Bennewitz M (2012) Improved proposals for highly accurate localization using range and vision data. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)

Pathak K, Birk A, Poppinga J, Schwertfeger S (2007) 3D forward sensor modeling and application to occupancy grid based sensor fusion. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)

Payeur P, Hebert P, Laurendeau D, Gosselin C (1997) Probabilistic octree modeling of a 3-d dynamic environment. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA)

Pfaff P, Triebel R, Stachniss C, Lamon P, Burgard W, Siegwart R (2007) Towards mapping of cities. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA), Rome, Italy

Roth-Tabak Y, Jain R (1989) Building an environment model using depth information. Computer 22(6):85-90

Ryde J, Hu H (2010) 3D mapping with multi-resolution occupied voxel lists. Autonomous Robots 28(2):169-185

Schnabel R, Klein R (2006) Octree-based point-cloud compression. In: Symposium on Point-Based Graphics 2006, Eurographics

Shade R, Newman P (2011) Choosing where to go: Complete 3D exploration with stereo. In: Proc. of the IEEE Int. Conf. on Robotics \& Automation (ICRA)

Sibley G, Mei C, Reid I, Newman P (2009) Adaptive relative bundle adjustment. In: Proc. of Robotics: Science and Systems (RSS)

Smith M, Baldwin I, Churchill W, Paul R, Newman P (2009) The new college vision and laser data set. International Journal for Robotics Research (IJRR) 28(5):595-599, DOI http://dx.doi.org/10. 1177/0278364909103911

Stoyanov T, Magnusson M, Andreasson H, Lilienthal AJ (2010) Path planning in 3d environments using the normal distributions transform. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS) Sturm J, Engelhard N, Endres F, Burgard W, Crem

ers D (2012) A benchmark for the evaluation of RGB-D slam systems. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), http://cvpr.in.tum.de/data/datasets/rgbd-dataset/download

Surmann H, Nüchter A, Hertzberg J (2003) An autonomous mobile robot with a 3d laser range finder for 3d exploration and digitalization of indoor environments. Robotics and Autonomous Systems 45(3):181-198

Triebel R, Pfaff P, Burgard W (2006) Multi-level surface maps for outdoor terrain mapping and loop closing. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)

Weise T, Wismer T, Leibe B, Van Gool L (2009) In-hand scanning with online loop closure. In: ICCV Workshops

Whelan T, Kaess M, Fallon M, Johannsson H, Leonard J, McDonald J (2012) Kintinuous: Spatially extended KinectFusion. Tech. rep., URL http://hdl.handle.net/1721.1/71756

Wilhelms J, Van Gelder A (1992) Octrees for faster isosurface generation. ACM Trans Graph 11(3):201-227

Wurm KM, Hornung A, Bennewitz M, Stachniss C, Burgard W (2010) OctoMap: A probabilistic, flexible, and compact 3D map representation for robotic systems. In: Proc. of the ICRA 2010 Workshop on Best Practice in 3D Perception and Modeling for Mobile Manipulation

Wurm KM, Hennes D, Holz D, Rusu RB, Stachniss C, Konolige K, Burgard W (2011) Hierarchies of octrees for efficient 3d mapping. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), San Francisco, CA, USA

Yguel M, Aycard O, Laugier C (2007a) Update policy of dense maps: Efficient algorithms and sparse representation. In: Field and Service Robotics, Results of the Int. Conf., FSR 2007, vol 42, pp 23-33

Yguel M, Keat CTM, Braillon C, Laugier C, Aycard O (2007b) Dense mapping for range sensors: Efficient algorithms and sparse representations. In: Proceedings of Robotics: Science and Systems