简介

DM-VIO: Delayed Marginalization Visual-Inertial Odometry
DM-VIO: 延迟边缘化惯性视觉里程计

花了两天时间捏着鼻子把这篇论文翻译完了，很多术语和状态的表达方式可能是和这个团队以前的工作DSO以及VI-DSO保持了一致，所以看起来很是费劲，然后整个论文的行文在翻译之后仍然感觉云里雾里，估计需要将DSO和VI-DSO有一个相对的了解再看这片论文比较合适。

本篇文章只用做学习备忘，因为在翻译过程中存在很多生硬的表述，有可能偏离原意，不具备参考价值，慎用。。。

0.摘要

本篇提出DM-VIO(基于延迟边缘化的单目视觉惯性里程计)，该方法使用了两种新的技术：延迟边缘化和位姿图BA。DM-VIO在计算视觉残差时使用了一种基于动态权重的光学BA。系统采用边缘化这一常用策略，以限制更新时间(限制更新时间是什么意思？？？边缘化在VIO的作用是将滑窗中较老的帧移除并将其位姿转为其他帧估计的先验信息)，但是这种边缘化是不可逆转的，并且一旦完成边缘化对于边缘化处的线性化点(一阶泰勒展开点)就会被固定(导致线性化点不一致性的出现)。为了解决这一问题文章提出延迟边缘化策略：维持第二个因子图，使得边缘化可以被延迟。这一策略使得之后再次进入这个延迟的图时，可以得到一个新的具备一致性线性化点的边缘化先验。此外，延迟边缘化策略能够使得我们能够向已经被边缘化的状态量加入IMU信息。这是我们提出的位姿图集束调整的基础，我们使用它来进行IMU初始化。与之前的IMU初始化工作相比，它能够捕获完整的光度不确定性，提高尺度估计。为了应对初始无法观测的尺度问题，我们在IMU初始化完成后继续优化主系统的尺度和重力方向。我们在EuRoC、TUM-VI和4Seasons数据集上评估了我们的系统，其中包括飞行无人机、手持设备和车载大型场景。由于提出的IMU初始化，我们的系统超过了视觉惯性里程计的最先进工作，甚至在只使用一个相机和IMU的情况下超过了双目惯性方法。该代码将在vision.in.tum.de/dm-vio上发布。

1.引言

视觉（惯性）里程计在机器人、自动驾驶和增强现实中的应用越来越多。在上述任务中，相机和惯性测量单元（IMU）的组合颇受欢迎，且两种传感器可以实现互补，进而实现高度准确和鲁棒的系统。对于单目相机+IMU的最小配置，IMU还可用于恢复真实的尺度信息。然而，这种刻度并不总是可观的，最常见的退化情况是恒定速度的运动(恒速运动时加速度为0,无法形成有效约束)。因此，该类系统(VIO)的初始化时间可能会比较长，而且依赖于实际的运动情况。如果急于初始化，会影响到初值的计算，进而影响到后面估计的性能(对于单目VIO来说一个好的初始化结果十分重要，因为后面的估计都是基于初始化结果进行计算的，如果一开始的值偏差就比较大，在后续跟踪时会影响到最优估计的收敛)。这个问题也是过去双目相机+VIO系统优于单目VIO的原因。

大多数以前的系统(如vins-mono,orb-slam3等)在初始化时都是将视觉里程计和IMU初始化并行(分开)实现，在视觉和惯性里程计均初始化成功后再执行视觉-惯性系统。但是，这会在初始化时对初始化时间和性能进行权衡，初始化应该越短越好，但是如果太短，无法获得足够的尺度信息。

VI-DSO[6]以任意尺度即时初始化，并显式地优化主系统中的尺度。这可以产生高度准确的尺度估计，但是在尺度被正确估计之前这种方法会显著地增加时间成本。此外，系统还可能在初始尺度误差非常高的情况下失败，比如在大型户外环境中的运动估计。

我们提出了这两种策略的组合：与前者类似，我们从一个视觉里程计系统开始，同时并行运行一个IMU初始化线程。但在IMU初始化后，我们仍然估计尺度和重力方向作为主系统中的显式优化变量(不明白？哪里结合了，反而有点向vins，在视觉和惯性分别初始化后进行视觉惯性对齐)，使得系统能够快速收敛和并得到较为精确的估计。

这种初始化策略可能会导致以下三个问题：

如何在IMU初始化器中正确地捕获视觉上的不确定性。
如何将有关尺度和IMU变量的信息从IMU初始化器传输到主系统？
如果是尺度估计变化，如何才能保持一致的边缘化先验？

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

在本篇提出延迟边缘化，对于上述三个问题都得到了比较好的解决。这个方法主要是维持第二个(位姿图？？？不明所以)，该位姿图用于延迟边缘化，该方法只需要少量计算开销，但实现了以下三个关键技术：

将IMU因子加入到延迟因子图中执行所提出的位姿图BA(需明确是如何加入的)
这个用于IMU初始化的延迟边缘化因子图可以被撤销边缘化，为主系统提供了一个包含IMU信息的边缘化先验
当主系统中尺度发生显著变化时，可以在边缘化先验中进行更新(trigger marginalization replacement暂时不知道什么意思)

基于以上三个技巧可以得到一个十分准确的初始化，对于长时间的不可观性也可以很鲁棒(？？？)。基于上述想法我们提出一个视觉惯性里程计系统，该系统实现了一个集成了动态权重的光学前端。

我们在三个比较有挑战性的数据集上评估了所提出的方法：使用飞行器录制的EUROC数据集，手持相机录制的TUM-VI数据集，4SEASON数据集。后者具有长时间的恒定速度，对单惯性里程计提出了特殊的挑战。
在这里插入图片描述

我们所提出的系统性能在惯性视觉里程计中最佳，甚至超过一些双目惯性视觉里程计。本篇文章的主要贡献如下：

提出一种延迟边缘化策略，在保持边缘化优势的同时弥补了边缘化的缺陷(线性化点早期固定带来的不一致问题)
位姿图集束调整(PGBA)结合了姿态图优化的效率与完整的集束调整的不确定性。(需要进行理解)
一种最先进的视觉惯性里程计系统，具有一种新的多级IMU初始化器和动态加权光度因子。(需要进行理解)

2.相关工作

最初大部分的视觉里程计和SLAM系统采用基于特征的方法，对于后端使用滤波的方法或者非线性优化。后来直接法被提出来，通过优化光度误差函数实现状态估计，该方法可以在稠密、半稠密和稀疏点云上进行操作。

MSCKF算法表明视觉和惯性测量的结合可以极大地提高里程计的准确性和鲁棒性，后来许多基于相机和IMU紧耦合的视觉里程计以及SLAM系统被提出来。

单目视觉惯性系统的初始化并非易事，因为需要足够的运动才能观察到尺度。大多数系统先进行一个纯视觉的初始化，并使用视觉的结果完成IMU的初始化。和这些系统相比，我们选择在直接主系统中优化尺度信息。我们发现ORB-SLAM3系统在初始化完成后同样会继续优化尺度，但这是一个单独的优化，该优化对之前所有的位姿进行了调整，但是需要在初始化后的75s内完成优化操作。[23]同样选择在主系统中继续优化尺度信息，它们没有办法在主系统和初始值设定项之间传递协方差，因此它们没有达到相同的精度水平。与所有这些系统不同，提议的延迟边缘化允许我们的IMU初始值设定项捕获完整的视觉不确定性，并不断优化主系统中的规模。

VI-DSO以任意的尺度进行初始化并在主系统中优化尺度信息，并且也使用了动态边缘化来处理主系统中的大规模的尺度变化。与VI-DSO相比，我们提出一个独立的IMU初始化器，使用延迟边缘化(一种动态的广度误差权重)替代动态边缘化，使得系统估计更加精确和鲁棒。

3.研究方法

3.1 一些符号的定义

定义了一些符号的表示方法，这里的位姿是指从世界坐标系到相机坐标系。

3.2 基于直接法的视觉惯性BA

DM-VIO的核心是对所有关键帧进行视觉惯性联合BA。正如通常所做的那样，我们把视觉和IMU的估计变量放在同一个代价函数中进行优化。对于视觉部分的残差，我们使用DSO中的直接法计算方法。为了将IMU数据集成到BA中，我们采用对两个关键帧之间IMU数据进行预积分的处理。

我们使用L-M算法优化以下代价函数：
在这里插入图片描述

其中E_prior包含了第一帧位姿、重力方向和第三节C部分提出的边缘化先验。下面我们将描述各能量项(残差？？？)和优化状态变量。

光度误差： 光度误差函数基于DSO算法。我们优化一组活跃的关键帧F,每个关键帧都包含一组特征点P_i(这里应该指的是对应的空间点吧？？)，每个点被投影到可以观测到该点的关键帧中，则光度误差可以由以下公式进行计算：
在这里插入图片描述

代价函数的具体定义需要看DSO。

动态光学权重： 上式第三项包含对于第一个位姿的先验和重力方向。在图像质量较差的情况下，系统应主要依赖于惯性数据。然而，由于使用的光度成本函数，糟糕的图像质量往往会导致非常大的光度残差。与IMU相比，它极大地增加了光度权重。为了解决它，我们使用了一个动态的光度误差权重。
在这里插入图片描述

其中λ是静态权重分量，θ是激活误差相关权重的阈值。这有效地将均方根光度误差归一化为最大值λθ，类似于阈值鲁棒成本函数[25]。与公式（3）中的Huber范数（后者对违反光度假设的单个点进行加权）相反，该权重解决了整体图像质量较差并增加IMU相对权重的情况。在我们的实验中，我们选择 θ = 8。

优化变量： 我们优化了尺度和重力方向作为显式变量。虽然BA原则上也可以改变尺度和全局方向，但当显式优化它们时，收敛性会得到改善[6]。另外，我们在图像帧V中表示视觉因子的姿态，在IMU帧I中表示IMU因子的姿态。而IMU帧有一个米制真实尺度和一个对准重力方向的z轴，即视觉图像帧可以有一个任意的尺度和旋转，这是在视觉系统的初始化过程中定义的(因为视觉部分在定义之后会通过IMU数据对齐尺度和重力方向)。为了建模该变量，我们使用s和旋转矩阵R_V_I。因为YAW角对于IMU来说不可观，我们固定最近的R_V_I坐标系。我们使用下式来转换坐标系：
在这里插入图片描述
其中S_I_V为sim(3)中的元素，包含旋转、平移和尺度信息。其他的变量也被转为sim(3)，但是最终的结果(什么的结果？？？？)的尺度为1并且为SE(3)中的元素。

完整的优化变量为：
在这里插入图片描述
其中s_i是所有关键帧的状态，定义如下：

其中v_i表示关键帧所对应的速度信息，b_i表示偏置(？？？？什么的偏置，加速计和陀螺仪的吗？？是一个几维向量？？)，a_i和b_i是仿射变换的亮度参数，d表示关键帧中活跃点的逆深度信息。光度残差的优化使用DSO中自定义集成的SIMD加速代码实现，其他因子使用GTSAM进行优化。

IMU误差： 我们使用[26]中提出的方法对IMU数据进行预积分，和[27]中那样作为一种智能因子使用，并进一步进行了改进。关于IMU的误差，我们将IMU的状态定义为以下形式：
在这里插入图片描述

imu的状态由方程5中对状态s的优化得到。给定一个i时刻的IMU数据，可以通过预积分得到j时刻的先验状态以及对应的协方差。由此产生的惯性误差函数会惩罚当前状态估计值与预测状态的偏差。
在这里插入图片描述

3.3 基于舒尔补的部分边缘化

我们使用舒尔补边缘化老的变量。当边缘化集合β中的变量时，我们收集所有依赖于这些变量以及和这些变量关联变量α中的因子，这些因子组成了马尔可夫毯(马尔可夫毯是什么？？？？？)。这些因子在当前状态估计中被线性化，形成一个线性系统。
在这里插入图片描述

我们使用舒尔补然后可以得到新的线性系统：
在这里插入图片描述

这个线性化系统组成了和集合α中的变量关联的边缘化先验，如图2所示。

在进行BA时我们让关键帧的数量维持在8帧以内，边缘化策略来自于DSO，这意味着不同于固定的滑窗(smothing应该就是指滑窗吧????),我们并不总是边缘化最老的位姿，而是会维持新位姿和老位姿之间的连接，只要他们具备相同的视野。如DSO中展示的那样，这种方式优于固定的滑动窗口算法。当要边缘化一个位姿时，图像帧中剩余的点会首先被边缘化，这些点对应的残差会被丢弃。这样保留了Hessian矩阵的稀疏性以及足够的信息。

3.4 延迟边缘化

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

边缘化也带来了严重的缺点：如果不重新执行整个边缘化程序，就不可能重新实现一组变量的边缘化。同时，为了保持边缘化先验一致性，FEJ方法[28]必须被应用。这意味着所有连接变量的线性化点都必须是固定在边缘化时。这对于视觉惯性里程计来说尤其成问题，因为边缘化先验因子与边缘化时的尺度有关，但是尺度可能会变化剧烈。我们使用延迟边缘化策略来克服缺点。

有效地消除部分边缘化，以捕获姿态图束调整的完整光度概率分布。
使用IMU信息更新最初的视觉边缘先验在IMU初始化后。重新线化化马尔可夫括号中的变量，同时保持所有的视觉和大多数惯性信息。

延迟边缘化的想法是，边缘化不能消除，但可以推迟。延迟图中的边缘化与原始图中的边缘化具有相同的运行时间。

为了归一化边缘化先验信息，我们维持可第二个延迟边缘化先验以及相应的因子图。在这个延迟因子图中，图像帧的边缘化会延迟一个图像帧数d。我们注意到，延迟边缘化保留了与原始图表中相同的边缘化顺序。当将这个图转换为固定的滑窗时，会马上导致较大的马尔可夫毯，增加系统的运行时间。如图2b所示，我们描述了对位姿P1的延迟边缘化过程。马尔可夫毯仅仅包含P0,P2和P3。如果我们对最老的帧P0进行边缘化，则马尔可夫毯将会包含P1到P7所有的帧，进而导致更高的运算时间。在延迟因子图中的边缘化和在原初的因子图中进行边缘化所花费的时间i相同。基于延迟边缘化的因子图包含了和原初因子图中相同的所有光度因子，在位姿被边缘化的同时相应的观测点也被边缘化。这意味着延迟图中的每个线性光度系数都恰好连接到生成相应因子时处于活动状态的 Nf = 8个关键帧。通过保持边缘化顺序，在延迟边缘化因子图中的马尔可夫毯通常和原初因子图中的马尔可夫毯尺寸相同。因此舒尔补所花费的时间是相同的。这意味着即使对于任意大的延迟，延迟边缘化的开销也非常小，因为它只相当于每个延迟图的额外边缘化过程。

3.5 IMU初始化中的位姿图BA

PGBA利用延迟边缘化来进行IMU初始化。其思想是用IMU因子填充延迟图，并优化所有变量。我们使用GTSAM[29]库优化图，使用lm优化器。在这种优化中，所有的点都被边缘化了。它是位姿图优化(PGO)和集束调整(BA)的组合。与BA相比，我们不更新我们对点深度的估计，也不重新调整光度误差。与PGO不同的是，我们不使用姿态之间的约束，而是使用“八元”约束，它连接全部帧并捕获BA的全部概率分布。相比较的PGO因此，我们的解决方案更准确，同时比full BA快得多。

1.填充因子图
定义一帧Pi与最新的位姿Pk直接相连，并且Pi到Pk之间的位姿都没有被边缘化。我们假定延迟边缘化因子图中的第一帧P_conn也和最新的图像帧相连。在图2c中，P_conn即为P2。从P2开始我们对所有连续帧都插入IMU因子和偏置因子。

我们不能在P_conn帧之前开始，因为不能将IMU因子在不连续的KF之间进行插入。由于边缘化的顺序是不固定的，这意味着我们需要在优化位姿的时候不使用与位姿相对应的IMU变量。

可以证明，在没有IMU变量的情况下，因子图中最多可以有Nf−2个位姿。原因是所有非关联的位姿在某个时刻同样处于活跃状态。这意味着我们至少有d-Nf+2个位姿可以添加IMU因子。实际上，我们选择Nf=8,延迟帧数d=100，这表示即使在最坏的情况下依然会有93个IMU因子参与优化。正如之前所解释的那样，固定滑窗的边缘化会导致hessian矩阵变得稠密，或者导致视觉系统的性能欠佳，因此我们所提出的策略能够实现一个较好的权衡。

2.优化： 我们使用GTSAM优化库中的LM优化器优化因子图，优化器使用Ceres默认的参数设置。我们称这个优化为位姿图BA，因为他结合了位姿图优化和BA的思想。和BA相比，我们不再对跟踪点的深度信息进行更新，也不对光度误差项进行线性化。和位姿图优化相比，我们不使用位姿之间的二元约束(二元约束是什么意思？？？)，而是使用一个八元约束，他关联Nf个(8个)帧并且可以得到BA完整的概率分布。通过使用固定延迟，它也在运行时受到限制，即使它可以随时执行而不会丢失任何先前的视觉信息。

3.Readvancing: 我们所提出的延迟边缘化和位姿图BA的另一个优势在于我们可以获得一个主系统的边缘化先验，包含了所有视觉和惯性信息。对于这一点，我们重新使用边缘化因子图进行PGAB，该功能通过连续边缘化在主图中被边缘化的所有变量实现。同样的这样保存了边缘化的顺序，意味着在每一步边缘化时马尔可夫毯都会有一个固定的最大尺寸。因此逐步进行边缘化比一次性边缘化所有变量显著的快速，这允许可以计算更大规模的矩阵求逆。图2D展示了这一过程。

3.6 鲁棒的多阶段IMU初始化

在这里插入图片描述

我们的初始化策略主要基于以下三个创新点：

变量是未知的（在我们的例子中，尺度、重力方向和bias），而其他变量接近最优时，它是最有效的首先只优化未知变量，并修复其他变量。
通过联合优化所有变量，获取全协方差，可以得到最准确的结果。
当被边缘化时，连通变量必须接近最优值，否则边缘化先验就会变得不一致。

对于这个初始化器我们使用一个延迟帧数d=100的单独的延迟边缘化因子图来实现。即使在第一次初始化之后，这个延迟边缘化因子图仅维持视觉因子，并不包含IMU因子，以方便进行边缘化替换。

初步的IMU初始化： 为此，我们只考虑最后的d=100个关键帧，并将它们与IMU因子连接起来。类似于ORB-SLAM3[5]中仅用于初始化的惯性优化，在这个优化中，我们固定姿态并使用单一的偏差。我们只优化了速度、偏置、重力方向和尺度。重力方向是通过计算的前两个关键帧之间平均加速度计的平均值来初始化，比例初始化为1，偏差和速度为0。这中优化方式的精度要略低于PGBA但仅用于初始化。在IMU初始化之后，我们计算尺度的边缘化协方差，如果低于阈值则继续进行PGAB。正如[31]中所述，考虑IMU的噪声参数对于IMU初始化十分关键。但对于我们的方法，它只是PGBA的初始化，此外还对光度噪声属性进行建模。

PGBA IMU 单元： 我们执行如3-E节所述的PGBA方法。之后，我们再次对尺度的边际协方差进行阈值判断，以确定优化是否成功。当未满足更严格的阈值 θ_reinit 时，我们使用结果进行初始化，但之后将执行另一个 PGBA 以使用更准确的值重新初始化。这种重新初始化使我们能够将 θ_init 设置为一个相对较大的值，从而允许更早地在主系统中使用 IMU 数据。

边缘化替换： 在IMU初始化之后，我们检测和在边缘化时使用的初始尺度估计s_fej相比尺度s的变化量。如果这个变化量超过阈值θ_marge,则对边缘化先验中的尺度进行替换。对于边缘化尺度替换，我们通过引入带有IMU因子的延迟边缘化因子图重新构造了一个PGBA因子图(如图2c所示)。与PGBA不同，我们在此图中没有进行优化，而只是重新推进它以获得更新的边缘化先验。这个新的先验仍然会包含所有视觉因子和至少d-Nf+1=93个IMU因子。如果在之前的边缘化先验中有超过50%的IMU因子丢失则不使用边缘化替换策略。这个过程展示了延迟边缘化是如何被用于FEJ更新的，解决了边缘化过程的主要问题。

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