DSO 中的Windowed Optimization / 憋错料

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

在此之前，泡泡群里王京和张腾在知乎写过First Estimate Jacobians的回答，范帝楷也在《OKVIS的理论推导(下)》中对marginalization进行了描述，这些都可以在泡泡历史推文中找到，我也写过一篇《SLAM中的marginalization 和 Schur complement》的博客。虽然资料已经很全了，这里还是想结合DSO[1]，以及另一篇文献[2]对windowed optimization涉及到的知识点进行一个全面的讲解。

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

1. 为什么要使用sliding window ?

2. 什么是sliding window？ Marginalization, Schur Complement, First Estimate Jacobians

3. DSO中是如何使用windowed optimization的？

为什么要使用sliding window？

在基于图优化的SLAM技术中，无论是pose graph还是bundle adjustment都是通过最小化损失函数来达到优化位姿和地图的目的。然而，当待优化的位姿或特征点坐标增多时，优化过程的计算量也随着增大。因此不能无限制的添加待优化的变量，而是使用滑动窗口技术来限制计算量在一定范围。比如，一开始有三个关键帧kf1,kf2,kf3在窗口里，经过时间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问题中两个顶点之间的边有如下的形式：

zij=hij(xi,xj)+nij

公式中xi,xj表示图优化的顶点，比如相机位姿或三维坐标点，zij表示两个顶点之间相对关系的测量值。nij是一个零均值的测量高斯噪声nij～N(0,Λ?1ij)，我们通过最大似然估计来优化变量：

x^=argmaxxp(z|x)=argmaxx∏p(zij|xi,xj)

由于服从高斯分布，所以上述问题近似于求解下面的最小二乘问题：

x^=argminx∑||zij?hij(xi,xj)||2Λij(1)

由于hij()非线性，上面的方程我们需要对hij()进行泰勒展开，然后使用Gauss-Newton迭代法来求解，在第k次迭代中，能够通过求解下面方程得到迭代增量δx：

δx=argminδx∑||zij?hij(x^(k)i,x^(k)j)?J(k)ijδx||2Λij

其中J(k)ij=?hij?x∣x^(k)表示在当前状态(迭代k-1次后)时的雅克比,那么k+1时刻的状态为x(k+1)=x(k)+δx(k)，重复这个迭代过程直到收敛。在上面这个最小二乘问题中，随着加入的变量xi越来越多，计算量将越来越大，因此我们需要去掉一些变量。这就用到了接下来提到的marginalization技术。

Marginalization和Schur Complement

假设要marginalize掉的变量为xm, 和这些待丢弃变量有约束关系的变量用xb表示，窗口中其他变量为xr，即x=[xm,xb,xr]T。相应的测量值为z={zb,zr}={zm,zc,zr}，其中zb={zm,zc}。为了有助于理解，看下图所示

假设窗口中有x0,x1,x2,x3,x4五个状态，需要marg掉x1，而x0,x2,x3和x1有约束关系。因此对应之前我们定义好的变量有xm=x1,xb=[x0,x2,x3]T,xr=x4.，相应的约束为zm={z01,z12,z13},zc={z0,z03,z23},zr={z04,z34}。

现在，需要丢掉变量xm，而去优化xb,xr。为了不丢失信息，正确的做法是把xm,xb之间的约束zm封存成状态xb的先验信息，简单地说就是告诉xr，我和xb之前是有约定的，你不能只按照你的约定胡来。封存先验信息就是如下公式，在zm条件下xb的概率：

p(xb|zm)=∫xmp(xb,xm|zm)dxm≈N(x^b,Λ?1t)

上式就是把xm,xb之间的约束封存成了xb～N(x^b,Λ?1t)先验信息。带着先验信息去优化xb,xr就不会损失信息了。

为了求解(x^b,Λ?1t)，我们只需要求解

argminxb,xm∑(i,j)∈zm12||zij?hij(xi,xj)||2Λij(2)

在求解这个非线性最小二乘的时候，我们可以得到其信息矩阵（Hessian）如下

H=[HmmHbmHTbmHbb]

一般的，我们计算Hx=b就能得到x，然而这里我们不需要计算xm，因此可以对H矩阵进行Schur Complement分解就能直接求解xb：

(Hbb?HbmH?1mmHTbm)x^b=bb?HbmH?1mmbm

因此，我们即得到了x^b,又得到了Λt=(Hbb?HbmH?1mmHTbm)。一旦这个先验信息得到确定，之前公式(1)中求解xm,xb,xr全状态问题就可以丢掉xm而不损失信息：

argminx12||x^b?xb||2Λt+∑(i,j)∈(zc,zr)12||zij?hij(xi,xj)||2Λij(3)

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

First Estimate Jacobians

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

假设之前求最小二乘的损失函数可以表达成：

c(x)=cm(xm,xb)+cr(xb,xr)

因此，我们就能得到：

minxc=minxb,xr(minxmc(xm,xb,xr))=minxb,xr(minxmcm(xm,xb)+cr(xb,xr))

求上面这个方程，我们可以先最小化cm(xm,xb)，注意这一步和我们求解先验信息时是一样的，我们对它在最优值附近二阶泰勒展开得到：

cm(xm,xb)?cm(x^m,x^b)+gT[xm?x^mxb?x^b]+12[xm?x^mxb?x^b]TH[xm?x^mxb?x^b](4)

其中

g=[gmmgmb],H=[HmmHbmHTbmHbb]

分别是雅克比和Hessien矩阵,注意，他们求导时的变量(即线性化点)是x^m,x^b。我们依然使用schur分解marg掉xm，但是这次我们选择求解xm:

xm=x^m?H?1mm(gmm+HTbm(xb?x^b))

把计算出来的xm带入公式(4)就能得到：

minxcm(xm,xb)?minxζ+gTt(xb?x^b)+12(xb?x^b)TΛt(xb?x^b)(5)

其中Λt我们已经知道，而gt就是公式(4)消去xm得到的：

gt=gmb?HbmH?1mmgmm

注意，它就是上面那个近似损失函数(5)的一阶导数，我们求解最小值的时候不就是令一阶导数等于0吗，所以在x^b处有gt=0.

现在我们把损失函数cm(xm,xb)去掉了xm得到了无信息损失的近似函数公式(5), 那我们把公式(5)代入公式:

c(x)=cm(xm,xb)+cr(xb,xr)

c′r(xb,xr)=gTt(xb?x^b)+12(xb?x^b)TΛt(xb?x^b)+∑(i,j)∈(zc,zr)12||zij?hij(xi,xj)||2Λij(6)

我们在最小化上式求解的过程中，如果雅克比采用的是margxm时的值，即对xb的求导是采用的x^b,由于gt=0，此时公式(6)就等价于公式(3)。如果不这么做，而是采用和xr一起优化后不断迭代得到的新xb去计算雅克比，这时gt!=0那我们的公式(6)相对于公式(3)，就引入了人为伪造的信息，系统就会慢慢被破坏。

如果有了这个概念，我们再回到DSO的论文中，就不难理解论文中的公式了。