DM-VIO简析

今天主要是针对DMVIO/DM-VIO的简析，中文网上有的东西都太少了，只能靠看完论文和组员们一起改代码。Lukas组这个东西在中文网被称为有史以来最好的VIO，但是实际过程中我们还是发现了许多不完美的地方。。。(比如ZUPT更新改造中该有的问题仍然在那里，不过根据实际在强力硬件上的运行来看确实比VINS-MONO强，但是明显不如VINS-MONO的工程版)

既然有简析，后续肯定会做包括函数/代码在内的一些关键详解。现在代码拆解还没到那个阶段，只能先把数论延续性和关键点的理解拆出来。计划春节前做好吧。

首先讲个前提，要搞懂这东西首先要有SLAM基础和VIO基础，了解所有Jacorbian和Hession相关，了解各种7788的耦合方式，熟悉因子图(Factor Graph)。一定要熟悉TUM和DSO相关系统的传承和延续，不然你不知道我在说啥。。。DSO相关的参考资料已经很多了(推荐高翔博士的“DSO详解”，但是注意里面关于3个核心迭代的描述略有偏差)。另外既然是简析，肯定也是个人理解，不代表一定正确。

先发个比较重要的图，字比较丑，不要介意。。。

先说明一下，此图虽丑但是非常重要。。。作为一个40岁的老工程师大家就别太计较了。。。后面的详解估计会是其他同学来做，保证不会这么难看了。

要了解DM-VIO，必须得了解DSO和工大TUM的历史，这个Github和公开资料上很多不赘述了。重点讲Lukas组。

DM-VIO是VI-DSO的延续，VI-DSO因为实际使用效果较差，国内各组在跟的应该比较少。

但实际动态边缘化是一种非常精妙的思路，我也不知道这几年发生什么了（疫情？），最终L组并没有在这条路上一路走到黑，而是出了DM-VIO这个超工程但是令人无力吐槽的优秀版本。

在之前必须熟悉DSO中的基础（以下2式必须熟悉）：

光度代价：

光度误差：

这2个式子不解释了，不了解的同学不适合阅读本文。

首先要牢记：无论任何多传感器融合融态系统，尤其是VIO系统，无论选择什么样的传感器组合，又或者采用何种算力平台，又或者怎么去做权重和约束策略，亦或者各种各样的ZUPT，甚至这个系统的基础可用性判断benchmark，就是是否能在完成SLAM的同时合理控制尺度(旋转与平移)。

这篇因为是DM-VIO的简析，关于VI-DSO中的核心只做简述：

1.VI-DSO的核心是动态边缘化，其核心思维就是通过尺度因子s和重力方向因子Rv_I来构建一个动态边缘化，以尝试保持一致的边缘化先验(见附图右边因子图)。其中VI-DSO工程化了一个关键帧<=0.5秒的策略，但是边缘化中可以随时违背，以确保长期和持续的关联。优化的本身就是尺度s。

2.M因子(边缘化因子)在Margin时不固定尺度的线性化点并保留3个边缘化先验，当尺度开始飘飞前(或理解为尺度估计离线性化交点远时)，进行重置！通过Mvisual/Mhalf/Mcurr来进行控制，分别指代包含之前状态中与尺度无关的信息(不可用其推断全局尺度)/设置线性化点以来所有的尺度信息/最接近当前值的尺度信息

3.初始化和DSO一样仍然是非常的高开销易失败，很容易在初始误差高的情况下直接失败，导致快速的尺度飘飞(尤其是户外)，尺度飘飞也是VIO里最常见的问题了，ZUPT和各种各样的手段都是为了把这玩意掌控住。

4. SIM(3)系，这个在DMVIO和VIDSO都是通用的，就是为了以尺度/重力初始值来优化，除了基础的度量系SE(3)(世界系)，建立了一个DSO坐标系中度量系能被尺度收缩/旋转的版本。

5. VI-DSO后端优化通用性，DM-VIO也采用了同样的方式，用Gstam库进行后端优化以及完成IMU预积分。比较简单，不赘述。

好了开始讲DM-VIO：

本质一句话：

只要边缘因子不重新线性化，求解由此产生的更小系统就等同于求解更大的原始系统。

这句话就是无力吐槽的部分了，我来按我的理解翻译一下哈：

就是如果我采用系统运行时非常古早的信息来求解系统，选用的边缘因子(M因子)不进行重线性化(边缘化和重线性化是SLAM的2个基操)，那被我求解而产生的这个更小的系统就相当于我求解了更大的原始系统。这就是延迟边缘化。

这里是DM-VIO关键式：

Epj和Ephoto延续至DSO，这里的小ephoto是Ephoto/Nresidual(残差个数)开均方根，本质是利用光度误差形成视觉权重。这里的θ和主流程中各个奇怪的θxx不是一码事，代表一个阈值。

E(s)这个表述，熟悉VINS-MONO的同学一看就知，和VINS三权重是一码事，权重和约束是控制尺度的基础。不过VINS-MONO是通过各自的协方差矩阵Σ来做的，其先验Prior是为了保持上一个重线性化点的状态的约束，但是这个约束很松，犹如一根细线，很容易被视觉或IMU权重带走(事实也需要这么设计)，我这块说的比较抽象，但是直观理解就是这个意思。

yaw不可观，一堆转换弄出Rv_I，和s联合构建出：

(这是完整的需要被优化的状态变量)

还有一个活动关键帧状态Si(Active frame)，也是DM-VIO中最重要的函数

里面分别是状态值-齐次坐标4x4的SE(3)或SIM(3) ，速度，bias(Ba/Bg都有，2个列向量)，2个关键光度参数，所有点(2000个默认)的逆深度。

imu因子误差：

舒尔补/cholesky分解那些比较简单清晰的就不列了。 

这里补个马尔科夫毯一目了然图：

 使变量A条件独立的一张毯子，切断A与世界和整个系统的联系。

看完这些知识后，开始讲DM-VIO因子图：

因子图其实算是一目了然了：

1.DM-VIO最大的特点是构建了2个完全不同作用的视觉BA。

其中一个是全局视觉因子图，另一个是延迟边缘化因子图。

在例图中给的Nf=4，代表延迟了4个激活帧(或关键帧)，同时在延迟边缘化b因子图中d=4(但这个延迟是高达d=100个关键帧的！)如系统以1秒30fps对应7-8个KF生成为例，相当于使用了10余秒前的信息(所以称为古早)

但实际上这是一种非常好的策略。

2.两个BA其实是同一套BA，根据Nf值的延时来对时，如因子图中的2条虚线就是进行对时的指向线。使Main和Delay的因子图完全保持了一致。

3.然后M掉P1后，使用被延迟的这一帧加入了PGBA(这里我理解为Populate gragh with IMU factors)，迁移并入IMU因子进行尺度归一，关键就是优化s(见前式)。一定要注意这里使用的M因子对应延迟边缘化中被并入帧的M因子。

4.一定要注意因子图中已线性化的光度因子和激活光度因子的位置与对应关系！每一个激活的光度因子包含了多个三元因子(每个关联一个逆深度/Host frame/Target frame，DSO基础知识)，在因子图中总结成了一个链接所有激活关键帧的因子。

5.向前推进PGBA，完整地完成边缘化，并优化s

最后讲右下角的主流程图(可以认为就是初始化的流程图)：

初始化是整个DSO与其分支最重要的部分，要注意在这类系统中初始化/前端/后端是不严格区分的，可以理解为同步不间歇进行。

1.主流程图分为了视觉里程计和VIO两个部分，在这里可以理解为松耦合。当然在最后算E(s)时又是紧耦合了，看不懂就看看前面那句话。

2.这里一共设定了一堆θ，分别是θinit=1,θreinit=0.5,θlost=50,θmarg，这一堆都是经验值

3.这一堆阈值的整个初始化优化的目标是使：cov(s)<=0.5，Nlost>=50，Nlost指代丢失的IMU因子数量。第一个s的协方差控制在比较小的范围很好理解，满足系统设计主旨。Nlost很难理解，指至少需要优化到有50个kf没有和imu因子链接上，并不存在IMU变量才能停止边缘化替换。实际指的是禁用和终止边缘化替换，使主系统继续的关键条件。

4.主流程中最关键的优化迭代运算动作是：PGBA/初始化主VIO/延迟边缘化替换，均在因子图中明确了动作，优化目标见3。

简述完毕，给一下我的基础结论：

在DMVIO实际拆解动作中，我们发现了一系列的问题，首先2次BA成倍地增加了开销，大量的判断带来了不间断的迭代和优化运算，间接影响了原来非常漂亮简洁的DSO（虽然DSO比较难懂，但是代码水平是很高的）。如简述所描述，整个DM-VIO的实现非常工程思维(但是却没太考虑开销，嵌入式系统难以承受，ZUPT问题依旧)，DM-VIO完全舍弃了VI-DSO动态边缘化的思路，但是达到了很好的效果。延迟边缘化相对动态边缘化孰优孰劣当然最终是由结果决定的，DM-VIO也证明了这一点。

这个系统对大部分工程师来说整体不算友好，它完善地解决了IMU初始化器中正确捕获视觉不确定性的问题，非常优秀地改善了初始化，成功将尺度和IMU变量信息从初始化器传递到了主系统，最终也通过这一系列特别的延迟边缘化工程策略，保持了当尺度估计变化时一致的边缘化先验。佩服之余也不禁有感慨，希望后续有更好的详解，同时更希望有厉害的同学去解决依然存在的问题。

继续完善SLAM/VSLAM的前进路径从来都是曲折和漫长的，少不了我们一起的努力，如何平衡精度与开销，是SLAMer长期的课题。