VINS 细节系列 - 窗口优化

DSO中除了完善直接法估计位姿的误差模型外(加入了仿射亮度变换，光度标定，depth优化)，另一个核心就是像okvis一样使用sliding window来优化位姿，Engel也专门用了一节来介绍它。sliding window 就像c++中的队列，队首进来一个新人，队尾出去一个老人，它更像王朝中武将的新老交替，老将解甲归田，新人受window大王的重用，然而安抚老将不得当，会使得SLAM王朝土崩瓦解。对于初次接触sliding window的初学者来说，window大王安抚老将，振兴SLAM王朝的三件法宝“First Estimate Jacobians”，“Marginalization”，“Schur complement”实在让人有点摸不清头脑。原谅我的口水话，接下来我将用尽量直观简洁的方式进行描述。

本文将包括如下三个方面：

• 为什么要使用sliding window ?
• 什么是sliding window？ Marginalization, Schur Complement, First Estimate Jacobians；
• DSO中是如何使用windowed optimization的？

一、为什么要使用sliding window ？

        在基于图优化的SLAM技术中，无论是pose graph还是bundle adjustment都是通过最小化损失函数来达到优化位姿和地图的目的。然而，当待优化的位姿或特征点坐标增多时，优化过程的计算量也随着增大。因此不能无限制的添加待优化的变量，而是使用滑动窗口技术来限制计算量在一定范围。

        比如，一开始有 三个关键帧 kf1, kf2, kf3 在窗口里 （窗口大小为3），经过时间t，第四个关键帧 kf4 加入优化,此时我们需要去掉 kf1，只对 关键帧 kf2,kf3,kf4  进行优化。这样就始终保持待优化变量的个数，而固定了计算量。在上面的过程中，新的关键帧到来时，我们直接丢弃了关键帧1和关键帧2,3之间的约束，直接只用新的关键帧4和2,3构建的约束来对帧2,3,4的位姿进行新的优化，因此一个很自然的问题是，优化后的kf2,kf3的位姿和原来 kf1 的约束肯定就被破坏了，原来kf1的一些约束信息就被损失了。那么，我们如何做到即使用滑动窗口固定计算量又充分保留信息呢？因此下面我们要对sliding window进行一个彻底的分析。 

二、sliding window技术

在这部分，我们从基本的 graph based slam出发，逐步分析当新的优化变量加入时，如何优雅的去掉旧变量，在固定计算量的同时又保留信息并且不破坏系统的一致性。我们知道图优化SLAM问题中两个顶点之间的边有如下的形式：

三、Marginalization 和 Schur Complement

补充：

注意：如果我们直接丢掉 xm，也不引入先验，那最多算是丢失了信息，然而上述过程中，稍微不注意就可能人为引入错误信息而慢慢导致系统崩溃。下面就来讨论下First Estimate Jacobians.

四、First Estimate Jacobians

在marg的时候，我们需要不断迭代计算H矩阵和残差b，而迭代过程中，状态变量会被不断更新，计算雅克比时我们要 fix the linearization point。也就是计算雅克比时求导变量要固定，而不是用每次迭代更新以后的x去求雅克比，这就是所谓的用第一次得到的雅克比（First Estimate Jacobians）。在之前介绍的泡泡机器人的推文或我的博文中都已经直观的介绍了这里面的原理，在这里我们将采用更理论的方式来进行分析。假设之前求最小二乘的损失函数可以表达成：

五、DSO中的windowed optimization

在DSO论文的2.3节，在进行窗口优化Gauss-Newton迭代的时候，作者特意强调要使用 First Estimate Jacobians 技术。作者将优化前的状态定义为ζ0,高斯牛顿迭代过程的总的累计量定义为x,高斯牛顿迭代中每一步得到的增量δ。作者用了下面一个图来讲解这些变量的关系：

六、扩展

上面只是理论的一些推导，在实际应用中还要考虑稀疏矩阵H会变得稠密。仔细想一想，我们在marg的过程中，要去掉变量还要保留他的信息，把以前一个大的H矩阵丢掉一些维度压缩到一个小的H矩阵里，不可避免的就会使得原本稀疏的H矩阵变得稠密，这就是所谓的fill-in。DSO,OKVIS的作者在使用的时候都使用了一些策略那尽量维持稀疏性，在上面提到的我的另一篇博文中有详细介绍，这里不再赘述。（转载请注明作者和出处：知行合一_白巧克力亦唯心_CSDN博客 未经允许请勿用于商业用途）

ref:
[1] 《Direct Sparse Odometry》
[2] 《Decoupled, Consistent Node Removal and Edge Sparsification for Graph-based SLAM》