【视觉SLAM】DM-VIO: Delayed Marginalization Visual-Inertial Odometry

L. v. Stumberg and D. Cremers, “DM-VIO: Delayed Marginalization Visual-Inertial Odometry,” in IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 1408-1415, April 2022, doi: 10.1109/LRA.2021.3140129.

论文阅读方法：Title，Abstract——>是否符合研究方向——>Conclusion——>是否感兴趣——>图表（实际效果）——>Introduction（吹嘘效果）——>Result（论文核心部分）——>Experiment实验方法，过程中随时可以停止阅读

摘要

我们提出了DM-VIO，这是一种基于两种新技术的单目视觉惯性里程计系统，称为延迟边缘化和位姿图BA。DM-VIO使用视觉残差的动态权重执行BA。我们采用边缘化，这是一种流行的策略，可以限制更新时间，但它不容易逆转，并且必须固定连接变量的线性化点。为了克服这个问题，我们提出延迟边缘化：这个想法是维护第二个因子图，其中边缘化被延迟。这使我们能够稍后重新推进此延迟图，从而在具有新的一致线性化点之前产生更新的边缘化。这是提议的姿势图束调整的基础，我们将其用于 IMU 初始化。与之前关于IMU初始化的工作相比，它能够捕获完整的光度不确定性，从而改善了尺度估计。为了应对最初无法观测到的尺度，我们在IMU初始化完成后，继续优化主系统中的尺度和重力方向。我们在 EuRoC、TUM-VI 和 4Seasons 数据集上评估我们的系统，这些数据集包括飞行无人机、大型手持设备和汽车场景。由于提议的IMU初始化，我们的系统超越了视觉惯性里程计的最新技术，甚至在仅使用单个相机和IMU的情况下优于立体惯性方法。该代码将在以下位置发布： http://vision.in.tum.de/dm-vio .

一、引言

视觉（惯性）里程计在机器人、自动驾驶和增强现实中的应用越来越相关。用于此任务的相机和惯性测量单元（IMU）的组合是一个流行且明智的选择，因为它们是互补的传感器，从而形成高度准确和强大的系统[1]。在单个摄像机的最小配置中，IMU 还可用于恢复公制刻度。然而，这种刻度并不总是可观察到的，最常见的退化情况是恒定速度的运动[2]。因此，根据轨迹，此类系统的初始化可能需要任意长的时间。更糟糕的是，当过早初始化时，IMU 实际上会降低性能。IMU初始化的困难是为什么立体惯性方法在过去优于单惯性方法。

大多数以前的系统[3]-[5]最初并行运行仅视觉里程计和IMU初始化。完成后，视觉惯性系统启动。这引入了初始化周期持续时间的权衡：它应该尽可能短，因为在此期间主系统中不使用 IMU 信息。但是，如果太短，比例估计将不准确，从而导致性能不佳。

VI-DSO [6] 而是使用任意刻度立即初始化，并在主系统中显式优化标度。这会产生高度准确的比例估计值，但它会显著增加正确估计比例的时间。此外，在初始缩放误差非常高的情况下，例如在大型户外环境中，它可能会失败。

我们提出了两种策略的组合：与前者类似，我们从仅视觉系统开始，并行运行 IMU 初始值设定项。但是在IMU初始化之后，我们仍然将尺度和重力方向估计为主系统中的显式优化变量。这导致快速收敛和高精度的系统。

此初始化策略可能导致三个问题：

• 如何在 IMU 初始值设定项中正确捕获视觉不确定性。

• 如何将有关比例和 IMU 变量的信息从 IMU 初始值设定项传输到主系统？

• 如果规模估计发生变化，如何保持先前的一贯边缘化？

VI-DSO[6]试图通过引入动态边缘化来解决3问题，这确实使边缘化因素保持一致，但在此过程中丢失了太多信息。

在这项工作中，我们提出了延迟边缘化，这为所有这三个问题提供了有意义的答案。这个想法是保持第二个延迟边缘化先行，其开销很小，但支持三种技术：

• 我们可以用新的 IMU 因子填充延迟因子图，以执行建议的姿势图束调整 （PGBA）。这是 IMU 初始化的基础，它可以捕获完整的光度不确定性，从而提高准确性。

• 用于 IMU 初始化的图形可以重新推进，从而在主系统的 IMU 信息之前提供边缘化。

• 当主系统中的规模发生显著变化时，我们可以触发边缘化替代。

这些技术的组合使初始值设定项具有高度的准确性，即使对于长时间的不可观测性，它也具有鲁棒性。在此基础上，我们实施了一个视觉惯性里程计（VIO）系统，该系统具有与新的动态光度测量权重集成的光度前端。

我们在三个具有挑战性的数据集（图1）上评估了我们的方法，捕获了三个域：飞行无人机记录的EuRoC数据集[7]，手持设备捕获的TUM-VI数据集[8]和汽车4Seasons数据集[9]。后者具有长时间的恒定速度，对单惯性里程计提出了特殊的挑战。

我们表明，我们的系统超越了视觉惯性里程计的最新技术，甚至优于立体惯性方法。总之，我们的贡献是：

• 延迟边缘化弥补了边缘化的弊端，同时保留了优势。

• 位姿图BA （PGBA） 将位姿图优化的效率与BA的完全不确定性相结合。

• 最先进的视觉惯性里程计系统，具有新颖的多级 IMU 初始值设定器和动态加权光度系数。

我们将发布我们方法的完整源代码。

二、相关工作

最初，大多数视觉里程计和SLAM系统都是基于特征的[10]，要么使用过滤[11]，要么使用非线性优化[12]，[13]。最近，已经提出了直接方法，该方法优化了光度误差函数，并且可以在密集[14]，[15]，半密集[16]或稀疏点云[17]上运行。

Mourikis和Roumeliotis[1]已经表明，视觉和惯性测量的紧密集成可以大大提高测程法的准确性和鲁棒性。之后，提出了许多紧密耦合的视觉惯性里程计[18]，[19]和SLAM系统[3]，[5]，[20]，[21]。

单目视觉惯性系统的初始化并非易事，因为需要足够的运动才能观察到尺度[2]，[22]。大多数系统 [3]–[5] 从仅视觉系统开始，并使用其输出进行单独的 IMU 初始化。与这些系统相比，我们继续在主系统中明确优化规模。我们注意到 ORB-SLAM3 [5] 在初始化后也会继续细化比例，但这是一个单独的优化，修复了所有姿势，并且仅在初始化后 75 秒之前执行。[23] 还继续优化主系统中的尺度，但与我们相反，它们没有办法在主系统和初始值设定项之间传递协方差，因此它们没有达到相同的精度水平。与所有这些系统不同，提议的延迟边缘化允许我们的IMU初始值设定项捕获完整的视觉不确定性，并不断优化主系统中的规模。

VI-DSO [6] 立即使用任意刻度进行初始化，并在主系统中显式优化标度。它还引入了动态边缘化，以处理主系统中随之而来的大规模变化。与之相比，我们提出了一个单独的IMU初始值设定项，延迟边际化作为动态边缘化的更好替代方案，动态光度误差权重，以及更多的改进，从而大大提高了准确性和鲁棒性。

很多博士推荐重点读Related Work，因为这里可以知道研究领域的演变现状，知道这篇paper是如何基于之前的工作进行改进创新的。

三、方法

A.符号

定义了公式推导需要的符号

B.直接视觉惯性BA

手动推导BA

C. 使用舒尔补数的部分边缘化

这个线性系统在连接所有变量之前形成边缘化α (图 2 a）。

我们最多保留Nf=8束调整期间的关键帧1.边缘化策略是从[17]中接管的：这意味着与固定滞后平滑不同，我们并不总是边缘化最旧的姿势，而是保持新旧姿势的组合，只要它们不离开视野。如[17]所示，这优于视觉里程计的固定滞后平滑器。边缘化姿势时，首先将帧中托管的所有剩余点边缘化，并丢弃具有剩余活动点的残差。这保留了黑森州的稀疏性，同时保留了足够的信息。

D. 延迟边缘化

上一节中解释的边缘化概念具有捕获完整概率分布的优点。事实上，求解由此产生的更小的系统等同于求解大得多的原始系统，只要边缘化因素不被重新线性化。

但是，它也具有严重的缺点：如果不重做整个边缘化程序，就不可能恢复一组变量的边缘化。此外，为了保持边缘化先前的一致性，必须应用第一估计雅各布派（FEJ）[28]。这意味着所有连接变量的线性化点必须在它们连接到边缘化之前立即固定。这对于视觉惯性里程计尤其成问题，因为一旦第一个关键帧被边缘化，尺度就与边缘化相关联，但可能会发生重大变化。在[6]中，引入了动态边缘化来解决这个问题，但它的应用仅限于单个一维变量，即尺度，并且当尺度快速变化时会丢失大多数先验惯性信息。

在这里，我们介绍延迟边缘化，它规避了边缘化的缺点，同时保留了优势。它使我们能够：

• 有效地撤消部分边缘化，以捕获姿势图丛调整的完整光度概率分布（第 III-E 节）。

• 在 IMU 初始化之后，使用 IMU 信息更新最初仅视觉边缘化。

• 在马尔可夫毯中重新线性化变量，同时保留所有视觉和大部分惯性信息。

延迟边缘化的想法是边缘化无法撤消，但可以延迟：除了正常的边缘化先验之外，我们还维护第二个延迟边缘化先验和相应的因子图。 在此延迟图中，帧的边缘化以d.在延迟图中，点仍然同时被边缘化，导致光度系数线性化。我们注意到，保留了与原始图表中相同的边缘化顺序。为此图切换到固定滞后平滑将立即导致更大的马尔可夫毯危及系统的运行时。例如，在图2 b中，我们描述了延迟的边缘化P1.马尔可夫毯子只包含P0,P2和P3.如果我们反而边缘化最古老的框架P0，马尔可夫毯将包含P1−P7，从而延长运行时间。

延迟图中的边缘化与原始图中的边缘化具有相同的运行时间：延迟图包含与原始图相同的光度系数，并且点同时被边缘化。这意味着延迟图中的每个线性化光度系数都恰好连接到Nf=8生成相应因子时处于活动状态的关键帧。通过保持边缘化顺序，延迟图中的马尔可夫毯始终与原始图中的马尔可夫毯具有相同的大小。因此，舒尔补码的运行时间是相同的。这意味着即使对于任意大的延迟，延迟边缘化的开销也非常小，因为它只相当于每个延迟图的额外边缘化过程。

E. IMU 初始化的位姿图BA

位姿图优化+BA，基于IMU初始化

F. 强大的多级 IMU 初始化

我们的初始化策略基于三个见解：

• 当某些变量未知（在我们的例子中是尺度、重力方向和偏差）而其他变量接近最佳时，最有效的方法是首先优化未知变量并修复其他变量。

• 通过联合优化所有变量，捕获全协方差，可以获得最准确的结果。

• 边缘化时，连接变量必须接近最优值，否则边缘化先验变得不一致。

这些观察结果激发了1）粗略的IMU初始化，2）PGBA，以及3）边缘化替换（图3）。请注意，在“初始化主 VIO”之后，主 VIO 系统 III-B（绿框）已经并行运行。

对于此初始值设定项，我们使用延迟为d=100.即使在第一次初始化之后，此延迟图也将始终仅包含视觉因素，而不包含IMU因素，以方便边缘化替换。

在实时模式下，我们在单独的线程中执行粗略的 IMU 初始化和 PGBA。请注意，建议的延迟边缘化对于此 IMU 初始化的重要性。它允许PGBA从光度束调整中捕获完整的协方差。通过读取，这也使我们能够在主系统之前生成边缘化，其中包含来自初始值设定项的所有 IMU 信息。最后，它用于在初始化后尺度发生变化时更新边缘化。

四、结果

我们在EuRoC数据集[7]，TUM-VI数据集[8]和4Seasons数据集[9]上评估了我们的方法，分别涵盖了飞行无人机，手持序列和自动驾驶。我们鼓励读者观看补充视频，该视频显示了4Seasons和TUM-VI幻灯片的定性实时结果1。我们还在 http://vision.in.tum.de/dm-vio 提供的补充中提供消融研究和运行时间评估。

除非另有说明，否则所有实验均在同一台MacBook Pro 2013（i7 at 2.3 GHz）上以实时模式进行，该MacBook Pro 2013用于在[6]中生成结果，而不使用GPU。由于 ORB-SLAM3 在 MacOS 上不受官方支持，因此我们在稍强的台式机上展示了它的结果，该桌面采用英特尔酷睿 i7-7700 K，频率为 4.2 GHz，这与他们论文中使用的 PC 非常相似。

所有方法针对 EuRoC 评估 10 次，针对每个序列的其他数据集评估 5 次。在 [17] 之后，结果以累积误差图表示，该图显示了跟踪了多少个序列（y 轴），其精度优于 x 轴上的阈值。我们执行南E（3）将轨迹与提供的地面实况对齐，并报告均方根误差 （RMSE），也称为绝对轨迹误差 （ATE）。在 TUM-VI 和 4Seasons 上，轨迹长度可能会有很大差异，因此我们报告了%，我们用它计算漂移=RMSE⋅100长度.我们还显示了与其他论文中的数字进行比较的表格，并报告了我们方法的每个序列的中位数结果。

A. EuRoC 数据集

EuRoC数据集[7]是迄今为止最流行的视觉惯性数据集，并且已经对其评估了许多强大的方法。在表I中，我们将视觉惯性里程计的最新技术进行比较，所有结果都没有闭环。我们的方法在RMSE方面明显优于所有其他方法。最接近的竞争对手是玄武岩[20]，这是一种立体惯性方法，可在2个序列上实现较小的误差。我们还观察到数据集上迄今为止报告的最低平均比例误差，证实了我们在 IMU 初始化方面的贡献对性能有积极影响。在补充中，我们提供了运行时评估，显示跟踪平均需要 10.34 毫秒，关键帧处理需要 53.67 毫秒。延迟边缘化导致关键帧线程中 0.44 毫秒或 0.8% 的开销。

B. TUM-VI数据集

TUM-VI数据集[8]是一个非常具有挑战性的手持数据集，具有大规模的室内和室外场景，甚至序列从管子上滑下来，几乎覆盖了完整的图像。由于长时间沿直线行走，立体方法在这里具有优势，因为它们仍然可以通过不断运动来观察比例。我们将表II中[8]中评估的最先进的视觉惯性里程计方法进行比较。我们的方法在大多数序列上明显优于VINS-Mono [3]中的其他单目方法，即使与立体声方法相比，它在16个序列上也显示出最佳结果，平均漂移为0.472。最接近的竞争对手仍然是玄武岩，它在 8 个序列上取得了最佳结果，平均漂移为 0.939。

在这个数据集上，我们还评估了ORB-SLAM3 [5]，这是最先进的视觉惯性SLAM系统。这并不完全公平，因为 ORB-SLAM3 使用环路闭合（不能禁用），与其他方法相比具有优势。我们发现这种比较仍然很有帮助，因为它可以得出有关潜在里程计的结论。我们对每个序列进行了5次评估ORB-SLAM3，并使用作者提供的代码和设置重现了它们的结果。对于此比较，我们还评估了VI-DSO[6]，结果如图4所示。我们观察到，ORB-SLAM3 在某些序列上更准确，这要归功于其非常强大的闭环系统。但是，我们的方法总体上更健壮。这表明将闭环和映射重用集成到我们的系统中将是一个有趣的未来研究方向。

C. 四季数据集

4Seasons数据集[9]是一个非常新的汽车数据集，与大多数其他汽车数据集相比，它具有时间同步良好的视觉惯性传感器。图像的下部被汽车引擎盖遮挡，因此我们裁剪了底部的 96 像素，我们对所有方法都这样做。由于这是在 4Seasons 数据集上评估的第一种测程方法，因此我们确保以相同的方式确定所有方法的 IMU 噪声参数，以确保公平比较：我们手动读取了 IMU 数据手册中提供的艾伦方差图中的加速度计和陀螺仪噪声密度和偏置随机游走。为了处理未建模的效果，我们遵循[8]并将噪声值膨胀不同数量，以确定所有方法的最佳设置。对于每种方法，我们尝试了分别膨胀1，10，100，1000的噪声模型，并选择了最佳结果的配置。对于 VI-DSO 和我们的方法，我们通过在 x 和 y 轴上的平移之前添加一个零来稍微修改视觉初始值设定项，并且还添加了一个阈值来停止为小于 0.01 m 的平移创建关键帧（后者没有为 VI-DSO 激活，因为它没有改善它的结果）。否则，参数与其他实验相同。对于玄武岩，我们尝试了所有三种提供的默认配置，具有最佳噪声值，以找到最佳设置。在为每种方法选择配置后，我们执行一次最终评估，每个序列运行所有 30 个序列 5 次。

结果如图 5 所示。很明显，汽车场景对于单目方法来说非常具有挑战性。这是意料之中的，因为它自然具有许多具有恒定运动的拉伸，其中无法观察到比例，这对 IMU 初始化构成了挑战。得益于我们新颖的IMU初始化，DM-VIO不仅在数据集上运行良好，而且在使用单目图像且无闭环的情况下，甚至优于立体惯性ORB-SLAM3和玄武岩。

五、结论和未来工作

我们提出了一种单目视觉惯性里程计系统，该系统的性能优于最先进的，甚至是立体惯性方法。得益于新颖的 IMU 初始值设定项，它在飞行、手持和汽车场景中运行良好，扩展了单目方法的适用性。我们的 IMU 初始化的基础是延迟边缘化，这也支持位姿图BA。

我们预计这种方法将激发这方面的进一步研究。延迟边缘化的想法可以应用于更多用例，例如，在边缘化设置中重新激活旧关键帧以实现地图重用。位姿图BA也可以应用于长期循环闭包。最后，我们的开源系统易于扩展，因为所有优化都与GTSAM集成，允许快速添加新因素。这可用于 GPS 集成、车轮里程计等。

德国慕尼黑工业大学的计算机视觉研究组的两位教授做的工作，单目视觉惯性里程计，在三大数据集上都进行了实验，效果达到最先进，甚至超过了双目视觉系统。创新点在于延迟边缘化，IMU初始化时使用位姿图BA的方法进行位姿估计。代码开源，是2022年看到最好的论文，没有之一。