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都是通过最小化损失函数来达到优化位姿和地图的目的。然而，当待优化的位姿或特征点坐标增多时，优化过程的计算量也随着增大。因此不能无限制的添加待优化的变量，而是使用滑动窗口技术来限制计算量在一定范围。比如，一开始有三个关键帧$kf_1,kf_2,kf_3$在窗口里，经过时间t，第四个关键帧$kf_4$加入优化,此时我们需要去掉$kf_1$，只对关键帧$kf_2,kf_3,kf_4$进行优化。这样就始终保持待优化变量的个数，而固定了计算量。在上面的过程中，新的关键帧到来时，我们直接丢弃了关键帧1和关键帧2,3之间的约束，直接只用新的关键帧4和2,3构建的约束来对帧2,3,4的位姿进行新的优化，因此一个很自然的问题是，优化后的$kf_2,kf_3$的位姿和原来$kf_1$的约束肯定就被破坏了，原来$kf_1$的一些约束信息就被损失了。那么，我们如何做到即使用滑动窗口固定计算量又充分保留信息呢？因此下面我们要对sliding window进行一个彻底的分析。额，感觉有点水深，像一个坑，别急，喝口水，后面不是一个坑是一个湖在等你。

sliding window技术

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

$$\mathbf{z}_{ij} = \mathbf{h}_{ij}(\mathbf{x}_i,\mathbf{x}_j)+\mathbf{n}_{ij}$$ 公式中 $\mathbf{x}_i,\mathbf{x}_j$表示图优化的顶点，比如相机位姿或三维坐标点， $\mathbf{z}_{ij}$表示两个顶点之间相对关系的测量值。 $\mathbf{n}_{ij}$是一个零均值的测量高斯噪声 $\mathbf{n}_{ij}\sim \mathcal{N}(0,\Lambda^{-1}_{ij})$，我们通过最大似然估计来优化变量： $$\mathbf{\hat{x}}=\underset{\mathbf{x}}{\operatorname{argmax}}p(\mathbf{z}|\mathbf{x})=\underset{\mathbf{x}}{\operatorname{argmax}}\prod p(\mathbf{z}_{ij}|\mathbf{x}_i,\mathbf{x}_j)$$ 由于服从高斯分布，所以上述问题近似于求解下面的最小二乘问题：
$$\hat{\mathbf{x}} = \underset{\mathbf{x}}{\operatorname{argmin}}\sum{||\mathbf{z}_{ij} - \mathbf{h}_{ij}(\mathbf{x}_i,\mathbf{x}_j)||}^2_{\Lambda_{ij} } \quad\quad (1)$$ 由于 $\mathbf{h_{ij}}()$非线性，上面的方程我们需要对 $\mathbf{h_{ij}}()$进行泰勒展开，然后使用Gauss-Newton迭代法来求解，在第k次迭代中，能够通过求解下面方程得到迭代增量 $\delta \mathbf{x}$： $$\delta \mathbf{x}=\underset{\delta\mathbf{x}}{\operatorname{argmin}}\sum||\mathbf{z}_{ij}-\mathbf{h}_{ij}(\mathbf{\hat{x}}_i^{(k)}, \mathbf{\hat{x}}_j^{(k)}) -\mathbf{J}_{ij}^{(k)}\delta\mathbf{x}||^2_{\Lambda_{ij}}$$ 其中 $\mathbf{J}_{ij}^{(k)}=\frac{\partial \mathbf{h}_ij}{\partial\mathbf{x}}\mid_{\mathbf{\hat{x}}^{(k)}}$表示在当前状态(迭代k-1次后)时的雅克比,那么k+1时刻的状态为 $\mathbf{x}^{(k+1)}=\mathbf{x}^{(k)}+\delta\mathbf{x}^{(k)}$，重复这个迭代过程直到收敛。在上面这个最小二乘问题中，随着加入的变量 $\mathbf{x}_{i}$越来越多，计算量将越来越大，因此我们需要去掉一些变量。这就用到了接下来提到的marginalization技术。

Marginalization和Schur Complement

假设要marginalize掉的变量为$\mathbf{x}_m$, 和这些待丢弃变量有约束关系的变量用$\mathbf{x}_b$表示，窗口中其他变量为$\mathbf{x}_r$，即$\mathbf{x}=[\mathbf{x}_m,\mathbf{x}_b,\mathbf{x}_r]^T$。相应的测量值为$\mathbf{z}=\left \{ \mathbf{z}_b,\mathbf{z}_r \right \}=\left \{ \mathbf{z}_m,\mathbf{z}_c,\mathbf{z}_r \right \}$，其中$\mathbf{z}_b=\left \{ \mathbf{z}_m,\mathbf{z}_c \right \}$。为了有助于理解，看下图所示

假设窗口中有 $\mathbf{x}_0,\mathbf{x}_1,\mathbf{x}_2,\mathbf{x}_3,\mathbf{x}_4$五个状态，需要marg掉 $\mathbf{x}_1$，而 $\mathbf{x}_0,\mathbf{x}_2,\mathbf{x}_3$和 $\mathbf{x}_1$有约束关系。因此对应之前我们定义好的变量有 $\mathbf{x}_m=\mathbf{x}_1, \mathbf{x}_b=[\mathbf{x}_0,\mathbf{x}_2,\mathbf{x}_3]^T,\mathbf{x}_r=\mathbf{x}_4.$，相应的约束为 $\mathbf{z}_m=\left\{ \mathbf{z}_{01},\mathbf{z}_{12},\mathbf{z}_{13}\right\},\mathbf{z}_c=\left\{ \mathbf{z}_{0},\mathbf{z}_{03},\mathbf{z}_{23}\right\},\mathbf{z}_r=\left\{ \mathbf{z}_{04},\mathbf{z}_{34}\right\}$。

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

$$p(\mathbf{x}_b|\mathbf{z}_m)=\int_{\mathbf{x}_{m}}{p(\mathbf{x}_b,\mathbf{x}_m | \mathbf{z}_m)d\mathbf{x}_m}\approx \mathcal{N}(\hat{\mathbf{x}}_b, \mathbf{\Lambda}^{-1}_t)$$ 上式就是把 $\mathbf{x}_m,\mathbf{x}_b$之间的约束封存成了 ${\mathbf{x}}_b\sim \mathcal{N}(\hat{\mathbf{x}}_b, \mathbf{\Lambda}^{-1}_t)$先验信息。带着先验信息去优化 $\mathbf{x}_b,\mathbf{x}_r$就不会损失信息了。

为了求解$(\hat{\mathbf{x}}_b, \mathbf{\Lambda}^{-1}_t)$，我们只需要求解

$$\underset{\mathbf{x}_b,\mathbf{x}_m}{\operatorname{argmin}}\sum_{(i,j)\in \mathbf{z}_m}\frac{1}{2}||\mathbf{z}_{ij}-\mathbf{h}_{ij}(\mathbf{x}_i,\mathbf{x}_j)||^2_{\mathbf{\Lambda}_{ij}}\quad\quad(2)$$ 在求解这个非线性最小二乘的时候，我们可以得到其信息矩阵（Hessian）如下 $$\mathbf{H}=\begin{bmatrix} \mathbf{H}_{mm} &\mathbf{H}^T_{bm} \\ \mathbf{H}_{bm} & \mathbf{H}_{bb} \end{bmatrix}$$ 一般的，我们计算 $Hx=b$就能得到 $x$，然而这里我们不需要计算$\mathbf{x}_m$，因此可以对H矩阵进行Schur Complement分解就能直接求解 $\mathbf{x}_b$： $$(\mathbf{H}_{bb}-\mathbf{H}_{bm}\mathbf{H}_{mm}^{-1}\mathbf{H}_{bm}^T)\hat{\mathbf{x}}_b=\mathbf{b}_b-\mathbf{H}_{bm}\mathbf{H}_{mm}^{-1}\mathbf{b}_m$$ 因此，我们即得到了 $\hat{\mathbf{x}}_b$,又得到了 $\mathbf{\Lambda_t}=(\mathbf{H}_{bb}-\mathbf{H}_{bm}\mathbf{H}_{mm}^{-1}\mathbf{H}_{bm}^T)$。一旦这个先验信息得到确定，之前公式(1)中求解 $\mathbf{x}_m,\mathbf{x}_b,\mathbf{x}_r$全状态问题就可以丢掉 $\mathbf{x}_m$而不损失信息： $$\underset{\mathbf{x}}{\operatorname{argmin}}\frac{1}{2}||\hat{\mathbf{x}}_b-\mathbf{x}_b||^2_{\mathbf{\Lambda}_{t}} + \sum_{(i,j)\in (\mathbf{z}_c,\mathbf{z}_r)}\frac{1}{2}||\mathbf{z}_{ij}-\mathbf{h}_{ij}(\mathbf{x}_i,\mathbf{x}_j)||^2_{\mathbf{\Lambda}_{ij}}\quad\quad(3)$$ 如果我们直接丢掉 $\mathbf{x}_m$，也不引入先验，那最多算是丢失了信息，然而上述过程中，稍微不注意就可能人为引入错误信息而慢慢导致系统崩溃。下面就来讨论下First Estimate Jacobians.

First Estimate Jacobians

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

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

$$\mathcal{c}(\mathbf{x})=\mathcal{c}_m(\mathbf{x}_m,\mathbf{x}_b)+\mathcal{c}_r(\mathbf{x}_b,\mathbf{x}_r)$$ 因此，我们就能得到： $$\underset{\mathbf{x}}{\operatorname{min}}\mathcal{c}=\underset{\mathbf{x}_b,\mathbf{x}_r}{\operatorname{min}}(\underset{\mathbf{x}_m}{\operatorname{min}}\mathcal{c}(\mathbf{x}_m,\mathbf{x}_b,\mathbf{x}_r))=\underset{\mathbf{x}_b,\mathbf{x}_r}{\operatorname{min}}(\underset{\mathbf{x}_m}{\operatorname{min}}\mathcal{c}_m(\mathbf{x}_m,\mathbf{x}_b)+\mathcal{c}_r(\mathbf{x}_b,\mathbf{x}_r))$$ 求上面这个方程，我们可以先最小化 $\mathcal{c}_m(\mathbf{x}_m,\mathbf{x}_b)$，注意这一步和我们求解先验信息时是一样的，我们对它在最优值附近二阶泰勒展开得到： $$\mathcal{c}_m(\mathbf{x}_m,\mathbf{x}_b)\simeq\mathcal{c}_m(\hat{\mathbf{x}}_m,\hat{\mathbf{x}}_b)+\mathbf{g}^T\begin{bmatrix} \mathbf{x}_m-\hat{\mathbf{x}}_m\\ \mathbf{x}_b-\hat{\mathbf{x}}_b \end{bmatrix}+\frac{1}{2}\begin{bmatrix} \mathbf{x}_m-\hat{\mathbf{x}}_m\\ \mathbf{x}_b-\hat{\mathbf{x}}_b \end{bmatrix}^T\mathbf{H}\begin{bmatrix} \mathbf{x}_m-\hat{\mathbf{x}}_m\\ \mathbf{x}_b-\hat{\mathbf{x}}_b \end{bmatrix}\quad\quad(4)$$ 其中 $$\mathbf{g}=\begin{bmatrix} \mathbf{g}_{mm}\\ \mathbf{g}_{mb} \end{bmatrix},\mathbf{H}=\begin{bmatrix} \mathbf{H}_{mm} &\mathbf{H}^T_{bm} \\ \mathbf{H}_{bm} & \mathbf{H}_{bb} \end{bmatrix}$$ 分别是雅克比和Hessien矩阵,注意，他们求导时的变量(即线性化点)是 $\hat{\mathbf{x}}_m,\hat{\mathbf{x}}_b$。我们依然使用schur分解marg掉 $\mathbf{x}_m$，但是这次我们选择求解 $\mathbf{x}_m$: $$\mathbf{x}_m=\hat{\mathbf{x}}_m-\mathbf{H}^{-1}_{mm}(\mathbf{g}_{mm}+\mathbf{H}^{T}_{bm}(\mathbf{x}_b-\hat{\mathbf{x}}_b))$$ 把计算出来的 $\mathbf{x}_m$带入公式(4)就能得到： $$\underset{\mathbf{x}}{\operatorname{min}}\mathcal{c}_m(\mathbf{x}_m,\mathbf{x}_b)\simeq\underset{\mathbf{x}}{\operatorname{min}}\zeta+\mathbf{g}^T_t(\mathbf{x}_b-\hat{\mathbf{x}}_b)+\frac{1}{2}(\mathbf{x}_b-\hat{\mathbf{x}}_b)^T\mathbf{\Lambda}_t(\mathbf{x}_b-\hat{\mathbf{x}}_b)\quad\quad(5)$$ 其中 $\mathbf{\Lambda}_t$我们已经知道，而 $\mathbf{g}_t$就是公式(4)消去 $\mathbf{x}_m$得到的： $$\mathbf{g}_t=\mathbf{g}_{mb}-\mathbf{H}_{bm}\mathbf{H}^{-1}_{mm}\mathbf{g}_{mm}$$ 注意，它就是上面那个近似损失函数(5)的一阶导数，我们求解最小值的时候不就是令一阶导数等于0吗，所以在 $\hat{\mathbf{x}}_b$处有 $\mathbf{g}_t=0$.

现在我们把损失函数$\mathcal{c}_m(\mathbf{x}_m,\mathbf{x}_b)$去掉了$\mathbf{x}_m$得到了无信息损失的近似函数公式(5), 那我们把公式(5)代入公式:

$$\mathcal{c}(\mathbf{x})=\mathcal{c}_m(\mathbf{x}_m,\mathbf{x}_b)+\mathcal{c}_r(\mathbf{x}_b,\mathbf{x}_r)$$ $$\mathcal{c}^{'}_r(\mathbf{x}_b,\mathbf{x}_r)=\mathbf{g}^T_t(\mathbf{x}_b-\hat{\mathbf{x}}_b)+\frac{1}{2}(\mathbf{x}_b-\hat{\mathbf{x}}_b)^T\mathbf{\Lambda}_t(\mathbf{x}_b-\hat{\mathbf{x}}_b)+\sum_{(i,j)\in (\mathbf{z}_c,\mathbf{z}_r)}\frac{1}{2}||\mathbf{z}_{ij}-\mathbf{h}_{ij}(\mathbf{x}_i,\mathbf{x}_j)||^2_{\mathbf{\Lambda}_{ij}}\quad\quad(6)$$ 我们在最小化上式求解的过程中，如果雅克比采用的是marg $\mathbf{x}_m$时的值，即对 $\mathbf{x}_b$的求导是采用的 $\hat{\mathbf{x}}_b$,由于 $\mathbf{g}_t=0$，此时公式(6)就等价于公式(3)。如果不这么做，而是采用和 $\mathbf{x}_r$一起优化后不断迭代得到的新 $\mathbf{x}_b$去计算雅克比，这时 $g_t!=0$那我们的公式(6)相对于公式(3)，就引入了人为伪造的信息，系统就会慢慢被破坏。

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

DSO中的windowed optimization

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

我们可以看到在优化过程中有：
$$\mathbf{x}^{new}\leftarrow \mathbf{\delta}+\mathbf{x}\\ \mathbf{\zeta}\leftarrow \mathbf{x}\oplus\mathbf{\zeta}_0$$ 作者一再强调，求雅克比时要在 $\mathbf{x}=0，即\mathbf{\zeta}_0$处去求，就是第一次计算得到的雅克比，别每次随着状态变量的更新而重新计算雅克比。知道了这些概念，再读DSO的论文就会容易许多。

扩展

上面只是理论的一些推导，在实际应用中还要考虑稀疏矩阵H会变得稠密。仔细想一想，我们在marg的过程中，要去掉变量还要保留他的信息，把以前一个大的H矩阵丢掉一些维度压缩到一个小的H矩阵里，不可避免的就会使得原本稀疏的H矩阵变得稠密，这就是所谓的fill-in。DSO,OKVIS的作者在使用的时候都使用了一些策略那尽量维持稀疏性，在上面提到的我的另一篇博文中有详细介绍，这里不再赘述。

（转载请注明作者和出处：http://blog.csdn.net/heyijia0327 未经允许请勿用于商业用途）

【版权声明】泡泡机器人SLAM的所有文章全部由泡泡机器人的成员花费大量心血制作而成的原创内容，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能否进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！
【注】商业转载请联系刘富强（liufuqiang_robot@hotmail.com）进行授权。普通个人转载，请保留版权声明，并且在文章下方放上“泡泡机器人SLAM”微信公众账号的二维码即可。

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