泡泡图灵智库，带你精读机器人顶级会议文章
标题：DM-VIO: Delayed Marginalization Visual-Inertial Odometry
作者：Lukas von Stumberg and Daniel Cremers
机构：Technical University of Munich
来源：RA-L
编译：GUOCHENG
审核:  YANGLINGHAO
这是泡泡图灵智库推送的第745篇文章，欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权
摘要
大家好，今天为大家带来的文章是　
DM-VIO: Delayed Marginalization Visual-Inertial Odometry
本文提出了一种基于延迟边缘化和位姿图BA的单目视觉惯性里程计系统DM-VIO。DM-VIO使用动态权重对视觉光度残差进行BA。边缘化策略常用于保证系统更新时间，但边缘化后很难恢复，并且连接变量的线性化点不能修改。本文提出了延迟边缘化策略，其思想是建立第二个因子图，该因子图的边缘化将会延迟，可以在之后通过访问延迟图，使用新的一致线性化点产生更新的边缘化。此外，延迟边缘化使我们能够将IMU的信息关联到已经被边缘化的状态，这是提出的位姿图BA的基础，使用它来进行IMU初始化。与以往的IMU初始化工作相比，该方法能够捕获全光度不确定度，使用新的一致线性化点产生更新的边缘化。为了解决初始尺度不可观的问题，IMU初始化完成后，在主系统中继续优化尺度和重力方向。实验证明，该系统只使用单目VIO的情况下超过了性能最好的双目VIO。
主要工作与贡献
  1. 延迟边缘化，弥补了边缘化的缺陷并保留了边缘化的优势
  2. 位姿图BA(PGBA)，结合了位姿图优化的效率和BA的完全不确定性。
  3. 视觉惯性里程计系统，具有新的多级IMU初始化器和动态加权的光度因子。

算法流程
Direct Visual-Inertial Bundle Adjustment
DM-VIO的核心是对所有关键帧进行视觉惯性光束法平差计算，建立联合能量函数同时优化视觉和IMU变量，对于视觉部分，选择了基于DSO的方法，DSO是一个非常准确和鲁棒的直接法视觉里程计系统。为了将IMU数据融合到BA中，在关键帧之间进行预积分。使用Levenberg-Marquardt算法优化以下能量函数
Eprior为位姿和重力方向的先验信息。
Photometric error：光度能量以[17]为基础。优化了一组激活状态关键帧F，每个关键帧包含一组点Pi。每个点p被投影到可见的所有关键帧obs(p)中，并计算光度能量函数:
Dynamic photometric weight：在图像质量较差的情况下，系统主要依靠惯性数据。然而，由于使用了光度成本函数，不好的图像质量往往会导致非常大的光度残差，相比IMU有效地增加了光度权重。为了弥补这一点，我们提出了一种动态光度权重W(ephoto)调整方法，ephoto为光度误差均方根。
λ是一个静态权重分量，θ是衡量阈值。当整体图像质量较差的情况，增加了IMU的相对权重。在实验中选择θ = 8。
Optimized variables：我们将尺度和重力方向作为显式变量进行优化，虽然BA也可以改变尺度和全局方向，但显式优化可以提高收敛性，为此，在视觉帧V中表示视觉因子的位姿，在IMU帧I中表示IMU因子的位姿，IMU帧具有尺度和与重力方向同向的z轴，而视觉帧可以具有任意比例和旋转，这是在视觉系统初始化期间定义的。为了建立模型，我们优化了比例s和旋转RV-I。由于使用IMU不能观测到偏航，我们固定RV-I的最后一个坐标。我们使用以下方法在坐标系之间进行转换
其中SI-V是具有单位旋转和平移的Sim(3)元素，尺度为s。其他变量被转换为Sim(3)，但请注意，结果的尺度为1以及在SE(3)中。
优化后的完整状态为：
si是所有活动关键帧的状态，定义为:
其中vi为速度，bi为偏差，ai和bi为仿射亮度参数，dji为关键帧中的活动点的逆深度。通过集成来自[17]的simd加速代码来优化光度残差以及其它因子GTSAM。
IMU Error：我们采用[26]中提出的IMU预积分，在[27]中实现为智能因子，并在[24]中进一步改进。对于这种能量我们使用IMU状态sIi= {PIi, vi, bi}。考虑到之前的状态sIi，预积分数据为我们提供了状态sIi的预测值以及协方差矩阵。由此产生的惯性误差函数惩罚当前状态估计与预测状态之间的偏差。
Partial Marginalization using the Schur Complement
我们用舒尔补将旧的变量边缘化。当边缘化一组变量β时，收集所有依赖于它们的因子以及连接变量α，组成Markov Blankets。这些因素在当前状态估计时线性化，得到线性系统:
我们应用舒尔补得到了新的线性系统
这个线性系统在α中形成了关于所有变量的边缘化。
在BA中，最多保留8个关键帧。边缘化策略根据[17]设计的：与固定延迟平滑器不同，并不总是边缘化最旧的位姿，而是保持最新和最旧位姿的组合，只要它们不离开视野。如[17]所示，这比用于视觉里程计的固定延迟平滑器要好。当边缘化一个位姿时，首先在框架中所有剩余的点被边缘化，剩余的活跃点的残差被删除。这在保留足够信息的同时保留了Hessian的稀疏性。
Delayed Marginalization
在前一节中解释的边缘化概念的优点是能够得到完整的概率分布。事实上，只要边缘因子不重新线性化，求解得到的较小的系统等价于求解更大的原始系统。然而，它也有严重的缺点：恢复一组变量的边缘化不可能不重新做整个边缘化过程。同时，为了保持边缘化的先验一致性，必须应用First-Estimates Jacobians (FEJ)[28]。所有关联变量的线性化点只要它们连接到边缘先验就必须是固定的。对于视觉惯性里程计来说，当第一个关键帧被边缘化时，尺度就与边缘化的先验信息相关。[6]中引入了动态边缘化来解决这一问题，但它的应用局限于单个一维变量，即尺度，当尺度快速变化时，它会丢失大部分先验惯性信息。 在这里，我们介绍了延迟边缘化，避免了边缘化的缺点，同时保留了优势：
  1. 有效地恢复部分边缘化，以获得位姿图束调整的完整光度概率分布 
  2. 在IMU初始化后，使用IMU信息更新初始的视觉边缘化 
  3. 重新逼近Markov blanket中的变量，同时保留所有视觉和大部分惯性的信息。 

延迟边缘化的理念是边缘化无法恢复，但可以延迟边缘化，除了正常进行边缘化，我们还增加了延迟边缘化和相应的因子图。在延迟因子图中，帧的边缘化是在延迟为d的情况下执行的。点在延迟因子图中仍然同时被边缘化，从而导致光度系数线性化。我们注意到，延迟后保留了与原始图相同的边缘化顺序。对于这个图，切换到一个固定的滞后平滑器将立即导致一个更大的Markov blanket影响系统运行时间。例如，在图2b中，我们描述了P1的延迟边缘化。Markov blanket仅包含P0、P2和P3。 如果我们将最老的帧P0边缘化，马尔可夫覆盖层将包含P1− P7，导致更高的运行时间。延迟图中的边缘化与原始图中的边缘化具有相同的运行时间。
延迟因子图中包含与原始因子图相同的光度因子，同时点被边缘化。延迟因子中的每个线性光度系数都与Nf=8的关键帧精确相连，这些关键帧在生成相应系数时处于活动状态。通过保持边缘化顺序，延迟因子图中的Markov blanket始终与原始图中的Markov blanket大小相同。因此，舒尔补的运行时间是相同的。这意味着即使对于任意大的延迟，延迟边缘化的开销也非常小，因为它只相当于每个延迟因子图的额外边缘化过程。 
图1 Delayed Marginalization and PGBA
Pose Graph Bundle Adjustment for IMU Initialization
PGBA利用延迟边缘化进行IMU初始化。其思想是用IMU因子填充延迟因子图，并优化所有变量
Populating the graph:让一个帧Pi直接连接到最新的姿势Pk，如果所有姿势Pj，i<j<k未被边缘化。延迟图中的第一帧Pconn，它仍然直接连接到最新帧。如图1c中所示的P2帧处将IMU因子和偏差因子插入到所有连续的帧中。
Optimization:使用GTSAM[29]库中的Levenberg-Marquardt优化器，并使用提供的Ceres默认设置，优化因子图。在优化中，所有的点都被边缘化了，该优化方法是常规位姿图优化（PGO）和光束平差（BA）的组合，因此称之为位姿图光束法平差算法。与BA不同，不更新点深度的估计值，也不重新逼近光度误差项。与PGO不同，在姿势之间不使用二进制约束，而是使用八进制约束，它连接Nf帧并捕获BA的全概率分布。因此，与PGO相比，该方法更准确，同时比BA快得多。通过使用固定延迟，即使它可以在任何时间执行，不会丢失任何先验视觉信息，也会影响运行时间。
Readvancing:延迟边缘化和PGBA的另一个优点是，可以获得系统的边缘化优先权，捕获所有视觉和惯性信息。为此，通过依次边缘化主因子图中已边缘化的所有变量来实现的。同样，这样做是为了保持边缘化顺序，在每个边缘化步骤中，Markov blanket有一个固定的最大值。因此，逐步边缘化比一次性边缘化所有变量要快得多，这将有更大的矩阵求逆。图1d显示了readvancing的结果。 
图2 多阶段IMU初始化 
Robust Multi-Stage IMU Initialization
初始化策略基于以下三点： 
  1. 当一些变量未知（尺度、重力方向和偏差）而其他变量接近最佳值时，首先只优化未知变量并修复其他变量是最有效的。
  2. 通过联合优化所有变量，获取完整的协方差，可以获得最精确的结果。 
  3. 当边缘化时，关联变量必须接近最优值，否则边缘化先验变得不一致。 

由此提出了1）粗略的IMU初始化，2）PGBA，以及3）边缘化替换（图2）。请注意，该过程是在“初始化主VIO”之后，主VIO系统III-B（图2左侧绿色框）已经并行运行。 对于这个初始器设定项，使用一个延迟为d=100的延迟图。即使在第一次初始化之后，该延迟图始终只包含视觉因素，而不包含IMU因素，以便于边缘化替换。 
Coarse IMU Initialization：为此，只考虑最新的d＝100的关键帧，并将它们与IMU因子连接起来。与ORB-SLAM3中用于初始化的纯惯性优化类似，在该优化中，固定姿势并使用单个偏差。只优化速度、偏差、重力方向和尺度。重力方向通过平均前两个关键帧之间的加速计测量值初始化，比例初始化为1，偏移和速度初始化为0。这种优化不如PGBA精确，但可以作为它的初始化。 优化后，我们计算尺度cov(s)的边缘协方差，如果未超过阈值θinit，则继续计算PGBA 。如[31]所示，考虑IMU噪声参数对于IMU初始化至关重要，粗略IMU初始化满足这一点。但对于我们的方法来说，这只是PGBA的初始化，它还模拟了光度噪声特性。 
PGBA IMU Init : 在对IMU初始化计算BA之后，之后，再次对尺度的边缘协方差设置阈值，以确定优化是否成功。当不满足更严格的阈值θreinit时，使用该结果进行初始化，但将执行另一个PGBA重新初始化。这种重新初始化可以使我们能够将θinit设置为一个相对较大的值，从而允许更早地在主系统中使用IMU数据。
Marginalization Replacement : 在IMU初始化之后，观察尺度s与之前边缘化中使用的第一次估计尺度sfej相比的变化程度。如果这一变化超过阈值θmarg，即δs:=max(s，sfej)/min(s，sfej)>θs，采用边缘化替换。对于边缘化替换，通过使用IMU因子填充延迟图来构建PGBA图，见图1c。与PGBA不同的是，没有在该图中进行优化，而只是对其进行重新升级以获得更新的边缘化。这个新的先验信息仍然包含所有视觉因素，至少包含最新的d−Nf+1=93个IMU因子。如果上一个先验IMU因子丢失超过阈值θlost=50%，将禁用边缘化替换。该过程显示了如何利用延迟边缘化来更新FEJ值，从而克服边缘化的主要问题之一。
在实时模式下，单独的线程中执行粗略的IMU初始化和PGBA，延迟边缘化对于IMU初始化非常重要，它允许PGBA从BA中获取全部协方差。使主系统生成一个边缘化优先级，包含来自初始化中的所有IMU信息。最后，当初始化后尺度发生变化时更新边缘化先验信息。
实验结果

 RMSE A TE in m on the TUM-VI dataset
Cumulative error plot for the TUM-VI dataset 
Cumulative error plot for the 4Seasons datase
如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【泡泡机器人SLAM】公众号。
点击阅读原文， 即可获取本文下载链接。
欢迎来到泡泡论坛，这里有大牛为你解答关于SLAM的任何疑惑。
有想问的问题，或者想刷帖回答问题，泡泡论坛欢迎你！
泡泡网站：www.paopaorobot.org
泡泡论坛：http://paopaorobot.org/bbs/
泡泡机器人SLAM的原创内容均由泡泡机器人的成员花费大量心血制作而成，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！
商业合作及转载请联系[email protected]
继续阅读
阅读原文