该博客内容发表在泡泡机器人公众号上，请尊重泡泡机器人公众号的版权声明

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<script type="math/tex" id="MathJax-Element-1">kf_1,kf_2,kf_3</script>在窗口里，经过时间t，第四个关键帧kf4<script type="math/tex" id="MathJax-Element-2">kf_4</script>加入优化,此时我们需要去掉kf1<script type="math/tex" id="MathJax-Element-3">kf_1</script>，只对关键帧kf2,kf3,kf4<script type="math/tex" id="MathJax-Element-4">kf_2,kf_3,kf_4</script>进行优化。这样就始终保持待优化变量的个数，而固定了计算量。在上面的过程中，新的关键帧到来时，我们直接丢弃了关键帧1和关键帧2,3之间的约束，直接只用新的关键帧4和2,3构建的约束来对帧2,3,4的位姿进行新的优化，因此一个很自然的问题是，优化后的kf2,kf3<script type="math/tex" id="MathJax-Element-5">kf_2,kf_3</script>的位姿和原来kf1<script type="math/tex" id="MathJax-Element-6">kf_1</script>的约束肯定就被破坏了，原来kf1<script type="math/tex" id="MathJax-Element-7">kf_1</script>的一些约束信息就被损失了。那么，我们如何做到即使用滑动窗口固定计算量又充分保留信息呢？因此下面我们要对sliding window进行一个彻底的分析。额，感觉有点水深，像一个坑，别急，喝口水，后面不是一个坑是一个湖在等你。

sliding window技术

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

<script type="math/tex; mode=display" id="MathJax-Element-8">\mathbf{z}_{ij} = \mathbf{h}_{ij}(\mathbf{x}_i,\mathbf{x}_j)+\mathbf{n}_{ij}</script>公式中xi,xj<script type="math/tex" id="MathJax-Element-9">\mathbf{x}_i,\mathbf{x}_j</script>表示图优化的顶点，比如相机位姿或三维坐标点，zij<script type="math/tex" id="MathJax-Element-10">\mathbf{z}_{ij}</script>表示两个顶点之间相对关系的测量值。nij<script type="math/tex" id="MathJax-Element-11">\mathbf{n}_{ij}</script>是一个零均值的测量高斯噪声nij～N(0,Λ?1ij)<script type="math/tex" id="MathJax-Element-12">\mathbf{n}_{ij}\sim \mathcal{N}(0,\Lambda^{-1}_{ij})</script>，我们通过最大似然估计来优化变量：<script type="math/tex; mode=display" id="MathJax-Element-13">\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)</script>由于服从高斯分布，所以上述问题近似于求解下面的最小二乘问题：
<script type="math/tex; mode=display" id="MathJax-Element-14">\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)</script>由于hij()<script type="math/tex" id="MathJax-Element-15"> \mathbf{h_{ij}}()</script>非线性，上面的方程我们需要对hij()<script type="math/tex" id="MathJax-Element-16"> \mathbf{h_{ij}}()</script>进行泰勒展开，然后使用Gauss-Newton迭代法来求解，在第k次迭代中，能够通过求解下面方程得到迭代增量δx<script type="math/tex" id="MathJax-Element-17">\delta \mathbf{x}</script>：<script type="math/tex; mode=display" id="MathJax-Element-18">\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}}</script>其中J(k)ij=?hij?x∣x^(k)<script type="math/tex" id="MathJax-Element-19">\mathbf{J}_{ij}^{(k)}=\frac{\partial \mathbf{h}_ij}{\partial\mathbf{x}}\mid_{\mathbf{\hat{x}}^{(k)}}</script>表示在当前状态(迭代k-1次后)时的雅克比,那么k+1时刻的状态为x(k+1)=x(k)+δx(k)<script type="math/tex" id="MathJax-Element-20">\mathbf{x}^{(k+1)}=\mathbf{x}^{(k)}+\delta\mathbf{x}^{(k)}</script>，重复这个迭代过程直到收敛。在上面这个最小二乘问题中，随着加入的变量xi<script type="math/tex" id="MathJax-Element-21">\mathbf{x}_{i}</script>越来越多，计算量将越来越大，因此我们需要去掉一些变量。这就用到了接下来提到的marginalization技术。

Marginalization和Schur Complement

假设要marginalize掉的变量为xm<script type="math/tex" id="MathJax-Element-22">\mathbf{x}_m</script>, 和这些待丢弃变量有约束关系的变量用xb<script type="math/tex" id="MathJax-Element-23">\mathbf{x}_b</script>表示，窗口中其他变量为xr<script type="math/tex" id="MathJax-Element-24">\mathbf{x}_r</script>，即x=[xm,xb,xr]T<script type="math/tex" id="MathJax-Element-25">\mathbf{x}=[\mathbf{x}_m,\mathbf{x}_b,\mathbf{x}_r]^T</script>。相应的测量值为z={zb,zr}={zm,zc,zr}<script type="math/tex" id="MathJax-Element-26">\mathbf{z}=\left \{ \mathbf{z}_b,\mathbf{z}_r \right \}=\left \{ \mathbf{z}_m,\mathbf{z}_c,\mathbf{z}_r \right \}</script>，其中zb={zm,zc}<script type="math/tex" id="MathJax-Element-27">\mathbf{z}_b=\left \{ \mathbf{z}_m,\mathbf{z}_c \right \}</script>。为了有助于理解，看下图所示

假设窗口中有x0,x1,x2,x3,x4<script type="math/tex" id="MathJax-Element-28">\mathbf{x}_0,\mathbf{x}_1,\mathbf{x}_2,\mathbf{x}_3,\mathbf{x}_4</script>五个状态，需要marg掉x1<script type="math/tex" id="MathJax-Element-29">\mathbf{x}_1</script>，而x0,x2,x3<script type="math/tex" id="MathJax-Element-30">\mathbf{x}_0,\mathbf{x}_2,\mathbf{x}_3</script>和x1<script type="math/tex" id="MathJax-Element-31">\mathbf{x}_1</script>有约束关系。因此对应之前我们定义好的变量有xm=x1,xb=[x0,x2,x3]T,xr=x4.<script type="math/tex" id="MathJax-Element-32">\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.</script>，相应的约束为zm={z01,z12,z13},zc={z0,z03,z23},zr={z04,z34}<script type="math/tex" id="MathJax-Element-33">\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\}</script>。

现在，需要丢掉变量xm<script type="math/tex" id="MathJax-Element-34">\mathbf{x}_m</script>，而去优化xb,xr<script type="math/tex" id="MathJax-Element-35">\mathbf{x}_b,\mathbf{x}_r</script>。为了不丢失信息，正确的做法是把xm,xb<script type="math/tex" id="MathJax-Element-36">\mathbf{x}_m,\mathbf{x}_b</script>之间的约束zm<script type="math/tex" id="MathJax-Element-37">\mathbf{z}_m</script>封存成状态xb<script type="math/tex" id="MathJax-Element-38">\mathbf{x}_b</script>的先验信息，简单地说就是告诉xr<script type="math/tex" id="MathJax-Element-39">\mathbf{x}_r</script>，我和xb<script type="math/tex" id="MathJax-Element-40">\mathbf{x}_b</script>之前是有约定的，你不能只按照你的约定胡来。封存先验信息就是如下公式，在zm<script type="math/tex" id="MathJax-Element-41">\mathbf{z}_m</script>条件下xb<script type="math/tex" id="MathJax-Element-42">\mathbf{x}_b</script>的概率：

<script type="math/tex; mode=display" id="MathJax-Element-43">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)</script>上式就是把xm,xb<script type="math/tex" id="MathJax-Element-44">\mathbf{x}_m,\mathbf{x}_b</script>之间的约束封存成了xb～N(x^b,Λ?1t)<script type="math/tex" id="MathJax-Element-45">{\mathbf{x}}_b\sim \mathcal{N}(\hat{\mathbf{x}}_b, \mathbf{\Lambda}^{-1}_t)</script>先验信息。带着先验信息去优化xb,xr<script type="math/tex" id="MathJax-Element-46">\mathbf{x}_b,\mathbf{x}_r</script>就不会损失信息了。

为了求解(x^b,Λ?1t)<script type="math/tex" id="MathJax-Element-47">(\hat{\mathbf{x}}_b, \mathbf{\Lambda}^{-1}_t)</script>，我们只需要求解

<script type="math/tex; mode=display" id="MathJax-Element-48">\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)</script>在求解这个非线性最小二乘的时候，我们可以得到其信息矩阵（Hessian）如下<script type="math/tex; mode=display" id="MathJax-Element-49">\mathbf{H}=\begin{bmatrix} \mathbf{H}_{mm} &\mathbf{H}^T_{bm} \\ \mathbf{H}_{bm} & \mathbf{H}_{bb} \end{bmatrix}</script>一般的，我们计算Hx=b<script type="math/tex" id="MathJax-Element-50">Hx=b</script>就能得到x<script type="math/tex" id="MathJax-Element-51">x</script>，然而这里我们不需要计算xm<script type="math/tex" id="MathJax-Element-52">\mathbf{x}_m</script>，因此可以对H矩阵进行Schur Complement分解就能直接求解xb<script type="math/tex" id="MathJax-Element-53">\mathbf{x}_b</script>：<script type="math/tex; mode=display" id="MathJax-Element-54">(\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</script>因此，我们即得到了x^b<script type="math/tex" id="MathJax-Element-55">\hat{\mathbf{x}}_b</script>,又得到了Λt=(Hbb?HbmH?1mmHTbm)<script type="math/tex" id="MathJax-Element-56">\mathbf{\Lambda_t}=(\mathbf{H}_{bb}-\mathbf{H}_{bm}\mathbf{H}_{mm}^{-1}\mathbf{H}_{bm}^T)</script>。一旦这个先验信息得到确定，之前公式(1)中求解xm,xb,xr<script type="math/tex" id="MathJax-Element-57">\mathbf{x}_m,\mathbf{x}_b,\mathbf{x}_r</script>全状态问题就可以丢掉xm<script type="math/tex" id="MathJax-Element-58">\mathbf{x}_m</script>而不损失信息：<script type="math/tex; mode=display" id="MathJax-Element-59">\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)</script>如果我们直接丢掉xm<script type="math/tex" id="MathJax-Element-60">\mathbf{x}_m</script>，也不引入先验，那最多算是丢失了信息，然而上述过程中，稍微不注意就可能人为引入错误信息而慢慢导致系统崩溃。下面就来讨论下First Estimate Jacobians.

First Estimate Jacobians

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

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

<script type="math/tex; mode=display" id="MathJax-Element-61">\mathcal{c}(\mathbf{x})=\mathcal{c}_m(\mathbf{x}_m,\mathbf{x}_b)+\mathcal{c}_r(\mathbf{x}_b,\mathbf{x}_r)</script>因此，我们就能得到：<script type="math/tex; mode=display" id="MathJax-Element-62">\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))</script>求上面这个方程，我们可以先最小化cm(xm,xb)<script type="math/tex" id="MathJax-Element-63">\mathcal{c}_m(\mathbf{x}_m,\mathbf{x}_b)</script>，注意这一步和我们求解先验信息时是一样的，我们对它在最优值附近二阶泰勒展开得到：<script type="math/tex; mode=display" id="MathJax-Element-64">\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)</script>其中<script type="math/tex; mode=display" id="MathJax-Element-65">\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}</script>分别是雅克比和Hessien矩阵,注意，他们求导时的变量(即线性化点)是x^m,x^b<script type="math/tex" id="MathJax-Element-66">\hat{\mathbf{x}}_m,\hat{\mathbf{x}}_b</script>。我们依然使用schur分解marg掉xm<script type="math/tex" id="MathJax-Element-67">\mathbf{x}_m</script>，但是这次我们选择求解xm<script type="math/tex" id="MathJax-Element-68">\mathbf{x}_m</script>:<script type="math/tex; mode=display" id="MathJax-Element-69">\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))</script>把计算出来的xm<script type="math/tex" id="MathJax-Element-70">\mathbf{x}_m</script>带入公式(4)就能得到：<script type="math/tex; mode=display" id="MathJax-Element-71">\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)</script>其中Λt<script type="math/tex" id="MathJax-Element-72">\mathbf{\Lambda}_t</script>我们已经知道，而gt<script type="math/tex" id="MathJax-Element-73">\mathbf{g}_t</script>就是公式(4)消去xm<script type="math/tex" id="MathJax-Element-74">\mathbf{x}_m</script>得到的：<script type="math/tex; mode=display" id="MathJax-Element-75">\mathbf{g}_t=\mathbf{g}_{mb}-\mathbf{H}_{bm}\mathbf{H}^{-1}_{mm}\mathbf{g}_{mm}</script>注意，它就是上面那个近似损失函数(5)的一阶导数，我们求解最小值的时候不就是令一阶导数等于0吗，所以在x^b<script type="math/tex" id="MathJax-Element-76">\hat{\mathbf{x}}_b</script>处有gt=0<script type="math/tex" id="MathJax-Element-77">\mathbf{g}_t=0</script>.

现在我们把损失函数cm(xm,xb)<script type="math/tex" id="MathJax-Element-78">\mathcal{c}_m(\mathbf{x}_m,\mathbf{x}_b)</script>去掉了xm<script type="math/tex" id="MathJax-Element-79">\mathbf{x}_m</script>得到了无信息损失的近似函数公式(5), 那我们把公式(5)代入公式:

<script type="math/tex; mode=display" id="MathJax-Element-80">\mathcal{c}(\mathbf{x})=\mathcal{c}_m(\mathbf{x}_m,\mathbf{x}_b)+\mathcal{c}_r(\mathbf{x}_b,\mathbf{x}_r)</script><script type="math/tex; mode=display" id="MathJax-Element-81">\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)</script>我们在最小化上式求解的过程中，如果雅克比采用的是margxm<script type="math/tex" id="MathJax-Element-82">\mathbf{x}_m</script>时的值，即对xb<script type="math/tex" id="MathJax-Element-83">\mathbf{x}_b</script>的求导是采用的x^b<script type="math/tex" id="MathJax-Element-84">\hat{\mathbf{x}}_b</script>,由于gt=0<script type="math/tex" id="MathJax-Element-85">\mathbf{g}_t=0</script>，此时公式(6)就等价于公式(3)。如果不这么做，而是采用和xr<script type="math/tex" id="MathJax-Element-86">\mathbf{x}_r</script>一起优化后不断迭代得到的新xb<script type="math/tex" id="MathJax-Element-87">\mathbf{x}_b</script>去计算雅克比，这时gt!=0<script type="math/tex" id="MathJax-Element-88">g_t!=0</script>那我们的公式(6)相对于公式(3)，就引入了人为伪造的信息，系统就会慢慢被破坏。

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

DSO中的windowed optimization

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

我们可以看到在优化过程中有：
<script type="math/tex; mode=display" id="MathJax-Element-92">\mathbf{x}^{new}\leftarrow \mathbf{\delta}+\mathbf{x}\\ \mathbf{\zeta}\leftarrow \mathbf{x}\oplus\mathbf{\zeta}_0</script>作者一再强调，求雅克比时要在x=0，即ζ0<script type="math/tex" id="MathJax-Element-93">\mathbf{x}=0，即\mathbf{\zeta}_0</script>处去求，就是第一次计算得到的雅克比，别每次随着状态变量的更新而重新计算雅克比。知道了这些概念，再读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》