Real-time Globally Consistent 3D Grid Mapping 论文翻译

全局一致的实时3D网格建图

摘要: 对于移动机器人在复杂未知的三维（3D）环境的路径规划，通常需要精确的3D环境表示。在本文中，我们提出了一种新方法,通过网格图更新策略和高效的数据结构实时生成全局一致的三维网格图。 我们把SLAM模块提供的点云转换到世界坐标系，然后用网格地图模块处理它们以产生全局一致的3D网格图。 室内场景的实验结果表明,我们的方法能够高效的更新3D网格地图,并且能进行实时的闭环更新.

1.介绍

即时定位与地图构建（SLAM）包括同时估计机器人的姿势和环境地图[1]。大多数移动机器人任务也要求机器人能够实时的检测障碍物和规划在未知环境中到达目的地的无碰撞路径。但是,SLAM系统[3]，[10]，[11]，[12]建立的地图普遍是密集地图，半
密集地图或稀疏地图的格式, 不适合路径规划。传统上，2D网格图可以应用于路径规划。但是，它们还不足够无人驾驶飞行器或水下航行器应用[2]。在某些情况下，当对建图区域有一定假设,2.5D网格图是另一种选择[5]，[6]。但是，2.5D地图一个主要的的缺点是它们通常使用2D网格,每个元胞存储测量的高度。因此，它们不足以模拟多尺度环境。通过允许每个元胞存储多个表面,移动机器人在多尺度环境中运行是可能的[15]。由于大多数2.5D地图离散了环境的垂直维度,而不是表示以体积的方式，因此很难精确表示多尺度环境，并指示移动机器人是否能通过多尺度环境如桥梁，隧道或多层建筑。对于在复杂位置环境,包含多尺度和3D障碍的移动机器人路径规划,一种精确的3D体积环境表示更受欢迎。

一种高效的压缩3D网格地图内存消耗的方式是八叉树(octree).一种基于八叉树的使用概率来表示占用和自由空间的方法在[13]中介绍。但是，它没有明确地解决了地图压缩的问题。另外,[4]还提​​出了另一种基于八叉树的三维地图表示。它找到了一种有效的方法在基于粒子滤波器的SLAM中执行地图更新。但是，它执行一个有损最大似然压缩，周期的丢弃概率信息。有损压缩使得未来的更新变得不可能并导致缺乏路径规划的信息。为了克服这些缺陷，一种名为OctoMap的高效方法被提出[7]，它可以根据概率占用估计和八叉树生成体积三维环境模型。此外，它是一个开源框架，并已被使用在许多研究项目中。但是，OctoMap只能在收集整体环境信息之后生成全局一致的3D表示。除此以外，由于缺乏一个更新策略，它构建的网格地图将无法修改漂移并忽略闭环修正[3]，[4]，[16]。因此，如何建立一个全局一致的3D网格图仍然是一个悬而未决的问题。

本文介绍的工作试图实时生成全局一致的3D网格图。 示例性结果如图1所示。所提出的系统包括视觉SLAM模块和网格建图模块。 提出了一种更新策略来更新OctoMap方法创建的三维网格图。 视觉SLAM模块以点云的形式提供稠密地图，其被提供给网格建图模块。 然后，网格建图模块处理点云数据以实时地创建全局一致的3D网格图。 我们还提出了一种基于SLAM系统中关键帧标识号（关键帧ID）的高效数据结构，这可以加速更新过程并生成更加压缩的3D网格图。 此外，我们的系统可以利用视觉SLAM模块检测和优化的闭环来更新3D网格图，从而保证地图的全局一致性。

2.相关工作

建模3D环境的一种流行方法是使用等大小的立方体积网格（体素）来离散环境空间。 [9]中提出的网格图需要提前初始化，这意味着应该预先知道建图区域的范围，它所需的内存可能是很大的(望而却步的)。为了适应占用网格建图，引入了基于概率和八叉树的方法来模拟占用空间和自由空间[13]。在[4]中也提出了另一种基于八叉树的3D地图表示。它们的地图结构，延缓引用计数八叉树，使其有效地进行地图更新。但是为了实现地图紧凑性，它丢弃概率信息，使其成为有损最大似然压缩。为了克服这些缺点，OctoMap基于八叉树和概率占用估计生成体积三维环境模型[7]。它在内存中高效紧凑。

只知道建模环境的方法对于在线路径规划是不够的。我们还应该找到一种实时获取周围环境精确信息的方法。基于SLAM，[4]中的工作提出了一种由Rao-Blackwellized粒子滤波器组成的方法，它基于有效的八叉树数据结构为环境提供三维证据网格图。但是，系统没有考虑闭环。

由于使用RGB-D传感器的视觉SLAM可用于生成可用于3D网格建图的稠密地图，因此本节主要关注RGB-D SLAM。 KinectFusion提出了一种体积截断的有符号距离函数的新型GPU实现，并且仅使用来自Kinect的深度数据就能够实现室内场景的实时3D重建[11]。但是，KinectFusion不考虑闭环，它的模型使场景限制在固定的小区域。为了将该模型应用于可扩展空间，随后的研究改进了记忆效率低的常规体素网格。一个简单的空间散列方案被用于空间压缩，这使得在线系统能够进行可扩展的体积重建[12]。然而，由于数据量庞大，实时重新集成或重新融合基于网格的地图仍然是一个具有挑战性的问题。另一种方法是ORB-SLAM，该系统基于近年来最先进的算法构建，设计为一个新系统，对所有主要SLAM任务使用相同的功能：跟踪，映射，重定位和循环优化[10]。其为重建选择点和关键帧的策略使得系统具有鲁棒性，并且其生成的地图紧凑且可跟踪。

由于在路径规划的背景下，环境的精确3D体积表示是首选。 [8]中的在线系统使用深度相机数据来生成基于OctoMap的环境3D表示。 但是当地图建成后，它无法更新，导致地图的漂移无法修改。 其他一些基于OctoMap的系统也可以生成3D环境表示。 虽然它们是在线系统，但是闭环的结果不能反映在OctoMap中，因为它们都缺乏更新策略[3]，[16]。

我们在本文中提出的系统是实时运行的。 它使用视觉SLAM模块提供周围环境的强大密集地图。 我们提出了更新策略和有效的数据结构来更新由OctoMap方法创建的3D网格地图，以生成全局一致的网格地图。

3.全局一致的实时3D网格建图

我们系统的框架如图2所示，其中包括视觉SLAM模块和网格建图模块。 我们实现了基于ORB-SLAM的视觉SLAM模块，并使用RGB-D摄像头对其进行扩展以提取点云。 然后，在网格建图模块中，我们使用基于关键帧的实时更新策略来实现全局一致的网格地图。 我们提出了一种有效的数据结构和压缩策略，以保证基本更新信息的完整性以及最小化内存和运行时消耗。

A.视觉SLAM模块

视觉SLAM模块向网格建图模块提供环境的点云数据。它由姿势跟踪线程，局部建图线程和闭环线程组成，如图2所示。

位姿跟踪线程负责处理每帧图像以获得姿态估计，并决定何时插入新的关键帧。当应添加新的关键帧时，将基于RGB-D图像和世界坐标系中的当前姿势来计算对应于当前关键帧的点云。然后，点云数据将被传输到网格建图模块中。

本地建图线程执行局部束调整（BA）以优化关键帧姿势和地图点位置。在covisibility图[10]中，当前关键帧与一组关键帧连接。比较当前关键帧和前一个关键帧的连接关键帧，我们认为这些不再在连接关键帧中的关键帧在短期内是稳定的。因此，我们称它们为稳定的关键帧。稳定关键帧的点云基于局部BA优化后的位姿计算，然后将在网格地图模块中使用。

闭环线程主要执行闭环检测和位姿图优化。在关键帧之间检测到闭环之后，将开始位姿图优化。优化完成后，删除指令将被发送到网格建图模块以删除旧的八叉树。然后，原来的局部网格地图模块的目标将被更改为新的八叉树。同时，将基于新位姿重新计算所有点云，然后将其传递到由目标是新八叉树的关键帧组织的网格地图模块。

B.网格地图模块

使用实时更新策略，网格地图模块处理从视觉SLAM模块接收的点云数据，以产生全局一致的3D网格图。此外，我们实现了基于关键帧ID的高效数据结构和压缩策略。

通过扩展OctoMap，我们继承了内存有效的实现和概率建模方法[7]。当我们将数据插入八叉树时，相应节点的相关占用概率将增加，直到达到[17]中提出的阈值。然后，体素将被认为稳定地以高置信度占据。其他地方被归类为未知区域。

此外，我们将对应于不同关键帧的点云视为空间中的单独重叠组。对应于关键帧的点云的相对位置被认为与该关键帧刚性连接。由于优化是基于视觉SLAM模块中的关键帧执行的，因此我们将相应的关键帧ID附加到网格地图模块中的点云。因此，在视觉SLAM模块中的BA过程之后，我们可以根据相应的关键帧位姿更新来更新点云的位置。

(1)高效的数据结构: 我们继承了OctoMap的内存高效实现，用于3D网格图的数据结构[7]。八叉树中的任何内部节点仅存储映射数据本身（例如，占用概率和颜色）和子指针（如果存在）。此外，我们在数据结构中维护关键帧ID。如图3所示，每个节点包含一组数字，这些数字是内部点的相应关键帧的关键帧ID。我们将此结构称为关键帧ID集。

当八叉树以下列方式改变时，我们维护数据结构。首先，应该通过遍历更新的父节点来更新关键帧ID集。应将新添加的关键帧ID添加到其父节点的关键帧ID集中。然后，为了避免碰撞，有关的内部节点应该采用其子节点的平均颜色和最大占用概率。

关于八叉树更新效率，我们的数据结构比传统的八叉树更有效。详细地说，更新操作分为两个步骤。首先，我们搜索八叉树以找到有关节点。然后，我们更新相应的占用概率。在传统的八叉树中，搜索方法基于协调或遍历所有节点，导致显著的时间成本，尤其是当节点数量很大时。相反，在我们的数据结构中，我们可以通过搜索关键帧ID来追踪相关节点。由于父节点将始终包含其子节点的关键帧ID集中的元素（关键帧ID），因此当发生更新操作时，搜索路径可以限于包含关键帧ID的若干节点。因此，搜索时间消耗限于可接受的小的消耗。

(2)地图压缩策略: 为了最小化网格图的内存消耗，我们将压缩策略应用于地图。 如前所述，具有相同关键帧ID的点云中的点的相对位置相对于关键帧是恒定的。 因此，我们可以推断出关键帧ID集中具有相同元素的八叉树节点被相同的关键帧观察到，因此，它们的相对位置也相对于这些关键帧是恒定的。

因此，剪枝条件可以是：当八个子节点具有相同的关键帧ID集时，它们可以被修剪，如图3中的灰色所示。并且修剪节点的父节点应该采用其子节点的最大占用率 ，适用于路径规划。 这样，将保持对路径规划有用的信息，并且由于节点数量的减少，可以提高修剪地图的更新效率。

(3)实时更新策略: 如图2所示，我们的更新策略包括以下三个过程：原始网格地图，局部网格地图和闭环网格地图。

原始网格建图过程将世界框架中的点云插入到八叉树中。每个点云由视觉SLAM模块提供，根据RGB-D图像和相应的位姿计算。当在视觉SLAM模块中检测到闭环时，新数据将被插入到新的八叉树中。此外，该过程将遍历八叉树中的节点，以确定它们是否满足每20个关键帧中压缩策略的剪枝条件。

在局部网格建图过程中，当在视觉SLAM模块中执行局部BA时，接收的数据包括稳定关键帧的关键帧ID及其优化的对应点云。在更新由关键帧ID搜索的相关八叉树节点的占用概率之后，优化的点云数据将被插入到我们的八叉树中。与原始网格映射相同，当在可视SLAM模块中检测到循环闭包时，新数据将插入到新的八叉树中。

在闭环网格建图过程中，在视觉SLAM模块中执行闭环优化之后，将更新循环中几乎所有关键帧的位姿。如果我们重用在本地网格建图中应用的更新方法，那么它将是低效的并且无法在线执行。因此，我们放弃旧的八叉树并将所有新数据插入到新的八叉树中。详细地，我们重新计算视觉SLAM模块中的所有点云数据并将它们插入到新的八叉树中。由于维护数据结构的时间成本与八叉树节点的大小成线性关系，我们更新关键帧ID集并在收到每20个关键帧时执行剪枝操作。这不会影响地图的长期稳健性，并且可以有效地减少时间成本，这使得该程序可以在线执行。

4.实验和评估

A.实验设定

我们的系统在机器人操作系统（ROS）[14]中实现，它促进了视觉SLAM模块和网格建图模块之间的通信。 我们在数量和质量上评估我们的系统，证明其准确性和实时性。 在实验中处理的RGB-D数据集由我们实验室中安装有Kinect传感器的Turtlebot平台记录。 该系统在配备Ubuntu 14.04 64位操作系统，Intel核心i7-3610QM（8核@ 2.30GHZ）CPU和8GB RAM的笔记本电脑上运行。

B.定性地图表现

由于3D环境地图的地面实况数据不可用，我们分析了网格图的定性性能。图4表示在闭环（图4a和图4b）之后由视觉SLAM模块生成的密集图和仅通过原始网格建图在我们的实验中生成的3D网格图（图4c和图4d），由原始网格生成的密集图使用局部网格建图（图4e和图4f）和我们的完整更新策略进行建图，包括原始网格建图，局部网格建图和闭环网格建图（图4g和图4h）。图4d是图4c的红色框中的位置的放大图，图4d的红色框中的位置也是在我们的实验数据中发生的闭环位置。同一行中其他图形之间的关系与图4c和图4d相同。

在我们的实验中，当实验数据以每秒18帧的速度运行时，视觉SLAM模块可以正常处理它，因此我们假设由它计算的点云通常与要建图的环境一致。由于密集地图是基于这些点云生成的，因此它可以作为定性的基本事实。

从图4a中可知，图4c的红色框中的相同位置只有一条线，但是在图4c和图4e的相应位置，有两条线彼此重叠。在它们相应的放大图中，我们可以看到图4d中红色框中的位置由多个重叠层组成，而在图4f中，相应位置具有两个单独的地平面。这是由局部BA操作产生的，该操作优化了相邻关键帧的姿态，因此本地地图更准确。因此，在机器人沿着其路径导航并获得闭环之后，使用局部BA构建的地图出现两个单独的地平面。此外，在图4h中，当完成环闭合时，在相同的位置，单独的地平面再次合并成一个层，而没有多余的多层。图4h中的网格图最适合图4b中的点云，这意味着我们构建的网格图与稠密的环境图一致。

C.时间消耗

在本节中，我们通过我们提出的系统分析时间成本来显示其实时性能。 我们讨论了数据结构的好处以及压缩策略。 此外，时间成本取决于建图区域的大小和网格图的分辨率。 在我们的实验中，建图区域是我们实验室的办公室（14.3 * 15 * 3.3立方米），分辨率为5厘米。

我们的系统包括视觉SLAM模块和网格建图模块。 由于视觉SLAM模块基于可在线执行的ORB-SLAM实现，我们将主要关注网格建图模块的时间成本。 在网格建图模块中，耗时的过程是：1）维护数据结构。 2）执行八叉树更新操作时更新八叉树节点。 3）修剪冗余节点，满足我们的地图压缩策略的剪枝条件。 4）在闭环网格地图中使用全局BA之后的数据重新生成新的八叉树。

(1)维护数据结构的时间成本：图5给出了在原始网格期间维护数据结构的时间成本演变。如预期的那样，时间消耗随着八叉树节点的数量线性增加，这几乎是数倍于已处理的关键帧。偶尔的波动是由于新节点将新元素引入关键帧ID集，这导致了八叉树中耗时的全局更新。并且检测到闭环导致关键帧号270周围的突然下降。更具体地，在全局BA之后使用数据替换旧的八叉树导致节点数量的短暂减少，这进一步使得更新关键帧ID集的时间成本也相应减少。

(2)更新八叉树节点的时间成本：在局部网格建图中的优化之后，应该更新八叉树中的节点，即，通过关键帧ID搜索，然后减少相应的可能性。

使用我们提出的数据结构和带有和不带压缩策略的传统数据结构更新八叉树节点的时间成本如图6所示。在III-B1中，可以使用我们提出的数据结构找到目标节点搜索有限节点，因此可以将时间成本限制为小的恒定时间（在5ms内）。相反，使用传统的数据结构应该遍历整个八叉树，因此随着节点数量的增加，时间成本会线性增加，这几乎是关键帧ID数量的倍数。通过压缩策略，减少了冗余节点，从而减少了要遍历的节点数量。修剪后的地图的更新效率提高了约14％。此外，波动与关键帧ID有关，其中一些涉及过多的相应节点，因此更新时间变得显著。

总之，由于我们的数据结构和压缩策略，更新效率提高了84％-94％，使其可以实时运行。

(3)修剪节点的时间成本：在处理来自每个关键帧的数据期间修剪满足修剪条件的节点的时间成本如图7所示。时间随着关键帧的数量线性增加直到关键帧数160左右。突然下降是因为没有节点满足修剪条件，并且遍历节点以确定它们是否满足修剪条件的时间成本非常接近于零。另外，波动的原因是与新节点连接的八叉树满足剪枝​​条件并且删除操作消耗一些时间。因此，八叉树节点的数量减少，使得下一个剪枝操作的时间减少。

(4)闭环网格建图的时间成本：在闭环网格建图中的全局优化之后，旧的八叉树应该被新的八叉树替换，这需要再次重新计算整个网格图。正如我们在Sect III-B3中提到的那样，我们每20个关键帧执行维护数据结构操作，这可以将时间成本降低到可接受的小值。在本实验中，243162个节点可以在3.9270s内构建，满足在线系统的要求。

5.总结

在本文中，我们提出了一种实时全局一致的3D网格建图方法。 它利用更新策略，高效的数据结构和基于关键帧的压缩策略。 实验结果表明，建图方法可以处理闭环，并可以生成全局一致的在线网格图。 此外，与OctoMap相比，我们的高效数据结构减少了八叉树的更新时间。 本文的另一个贡献是我们公开提供与此工作相关的源代码1。 实验中的数据集和结果网格图也是公开的2。 实验也在网上视频中显示。

 

参考资料

[1] Cesar Cadena, Luca Carlone, Henry Carrillo, Yasir Latif, Davide Scaramuzza, José Neira, Ian Reid, and John J Leonard. Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age. IEEE Transactions on Robotics, 32(6):1309–1332, 2016.
[2] Alberto Elfes. Using occupancy grids for mobile robot perception and navigation. Computer, 22(6):46–57, 1989.
[3] Felix Endres, Jurgen Hess, Jurgen Sturm, Daniel Cremers, and Wolfram Burgard. 3-d mapping with an rgb-d camera. IEEE Transactions on Robotics, 30(1):177–187, 2014.
[4] Nathaniel Fairfield, George Kantor, and David Wettergreen. Real-time slam with octree evidence grids for exploration in underwater tunnels. Journal of Field Robotics, 24(1-2):03–21, 2007.
[5] Raia Hadsell, J Andrew Bagnell, Daniel F Huber, and Martial Hebert. Accurate rough terrain estimation with space-carving kernels. In Robotics: Science and Systems, volume 2009, 2009.
[6] Martial Herbert, Claude Caillas, Eric Krotkov, In-So Kweon, and Takeo Kanade. Terrain mapping for a roving planetary explorer. In Robotics and Automation, 1989. Proceedings., 1989 IEEE International Conference on, pages 997–1002. IEEE, 1989.
[7] Armin Hornung, Kai M Wurm, Maren Bennewitz, Cyrill Stachniss, and Wolfram Burgard. Octomap: An efficient probabilistic 3d mapping framework based on octrees. Autonomous Robots, 34(3):189–206, 2013.
[8] Daniel Maier, Armin Hornung, and Maren Bennewitz. Real-time navigation in 3d environments based on depth camera data. In Humanoid Robots (Humanoids), 2012 12th IEEE-RAS International Conference on, pages 692–697. IEEE, 2012.
[9] H Moravec. Robot spatial perceptionby stereoscopic vision and 3d evidence grids. Perception, 1996.
[10] Raul Mur-Artal, Jose Maria Martinez Montiel, and Juan D Tardos. Orb-slam: a versatile and accurate monocular slam system. IEEE Transactions on Robotics, 31(5):1147–1163, 2015.
[11] Richard A Newcombe, Shahram Izadi, Otmar Hilliges, David Molyneaux, David Kim, Andrew J Davison, Pushmeet Kohi, Jamie Shotton, Steve Hodges, and Andrew Fitzgibbon. Kinectfusion: Real- time dense surface mapping and tracking. In Mixed and augmented reality (ISMAR), 2011 10th IEEE international symposium on, pages 127–136. IEEE, 2011.
[12] Matthias Nießner, Michael Zollhöfer, Shahram Izadi, and Marc Stamminger. Real-time 3d reconstruction at scale using voxel hashing. ACM Transactions on Graphics (TOG), 32(6):169, 2013.
[13] Pierre Payeur, Patrick Hébert, Denis Laurendeau, and Clément M Gosselin. Probabilistic octree modeling of a 3d dynamic environment. In Robotics and Automation, 1997. Proceedings., 1997 IEEE International Conference on, volume 2, pages 1289–1296. IEEE, 1997.
[14] Morgan Quigley, Ken Conley, Brian Gerkey, Josh Faust, Tully Foote, Jeremy Leibs, Rob Wheeler, and Andrew Y Ng. Ros: an open-source robot operating system. In ICRA workshop on open source software, volume 3, page 5. Kobe, 2009.
[15] Rudolph Triebel, Patrick Pfaff, and Wolfram Burgard. Multi-level surface maps for outdoor terrain mapping and loop closing. In Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on, pages 2276–2282. IEEE, 2006.
[16] Thomas Whelan, Michael Kaess, John J Leonard, and John McDonald. Deformation-based loop closure for large scale dense rgb-d slam. In Intelligent Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on, pages 548–555. IEEE, 2013.
[17] Manuel Yguel, Olivier Aycard, and Christian Laugier. Update policy of dense maps: Efficient algorithms and sparse representation. In Field and Service Robotics, pages 23–33. Springer, 2008.