VINS中的滑动窗口
1. 图优化模型的概括
有如下优化系统
\[\begin{array}{l}
\boldsymbol{\xi}=\underset{\boldsymbol{\xi}}{\operatorname{argmin}} \frac{1}{2} \sum_{i}\left\|\mathbf{r}_{i}\right\|_{\boldsymbol{\Sigma}_{i}}^{2}\\
\boldsymbol{\xi}=\left[\begin{array}{l}
\boldsymbol{\xi}_{1} \\
\boldsymbol{\xi}_{2} \\
\ldots \\
\boldsymbol{\xi}_{6}
\end{array}\right], \mathbf{r}=\left[\begin{array}{l}
\mathbf{r}_{12} \\
\mathbf{r}_{13} \\
\mathbf{r}_{14} \\
\mathbf{r}_{15} \\
\mathbf{r}_{56}
\end{array}\right]
\end{array}
\]
对应的图模型如下所示：
- 圆：圈表示顶点，需要优化估计的变量，可以使位姿（位置和姿态），3D点，加速度角速度零偏等等可以优化的变量。
- 边：表示顶点之间构建的残差，有一元边（只连接一个顶点），二元边，多元边。至于到底是几元边和具体问题有关，简单来说就是你残差的构建需要几个待优化变量，需要一个就是一元边，两个就是两元边，多个就是多元边，一般一些开源库只支持到二元边，再往上就是多元边。具体的残差可以是视觉残差，IMU预积分残差，先验残差等。
上述最小二乘为题对应的高斯牛顿的解为：
\[\underbrace{\mathbf{J}^{\top} \boldsymbol{\Sigma}^{-1} \mathbf{J}}_{\mathbf{H} \text { or } \boldsymbol{\Lambda}} \delta \boldsymbol{\xi}=\underbrace{-\mathbf{J}^{\top} \boldsymbol{\Sigma}^{-1} \mathbf{r}}_{\mathbf{b}}
\]
公式中的雅可比矩阵为：
\[\mathbf{J}=\frac{\partial \mathbf{r}}{\partial \boldsymbol{\xi}}=\left[\begin{array}{c}
\frac{\partial \mathbf{r}_{12}}{\partial \xi} \\
\frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}} \\
\frac{\partial \mathbf{r}_{14}}{\partial \xi} \\
\frac{\partial \mathbf{r}_{15}}{\partial \xi} \\
\frac{\partial \mathbf{r}_{56}}{\partial \boldsymbol{\xi}}
\end{array}\right]=\left[\begin{array}{l}
\mathbf{J}_{1} \\
\mathbf{J}_{2} \\
\mathbf{J}_{3} \\
\mathbf{J}_{4} \\
\mathbf{J}_{5}
\end{array}\right], \mathbf{J}^{\top}=\left[\begin{array}{lllll}
\mathbf{J}_{1}^{\top} & \mathbf{J}_{2}^{\top} & \mathbf{J}_{3}^{\top} & \mathbf{J}_{4}^{\top} & \mathbf{J}_{5}^{\top}
\end{array}\right]
\]
矩阵乘法公式（前一个公式）可以写成连加:
\[\sum_{i=1}^{5} \mathbf{J}_{i}^{\top} \boldsymbol{\Sigma}_{i}^{-1} \mathbf{J}_{i} \delta \boldsymbol{\xi}=-\sum_{i=1}^{5} \mathbf{J}_{i}^{\top} \boldsymbol{\Sigma}_{i}^{-1} \mathbf{r}_{i}
\]
雅可比矩阵和信息矩阵都具有稀疏性，由于每个残差只和某几个状态量有关，因此，雅克比矩阵求导时，无关项的雅克比为 0。比如本例中 \(\boldsymbol{\xi}\) 应该是有五个，图中有五个顶点，但是大部分残差是只需要其中的很少顶点就可以计算完毕，因此残差在对其他顶点计算雅可比的时候就是 0，\(\mathbf{r}_{13}\)残差的构建就止和 \(\boldsymbol{\xi_1,\xi_3}\)有关，因此关于其他顶点的雅可比矩阵均为0。
\[\mathbf{J}_{2}=\frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}}=\left[\begin{array}{llllll}
\frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}_{1}} & \mathbf{0} & \frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}_{3}} & \mathbf{0} & \mathbf{0} & \mathbf{0}
\end{array}\right] \\
\]
计算其信息矩阵可以得到：
\[\begin{aligned}
\boldsymbol{\Lambda}_{2}=\mathbf{J}_{2}^{\top} \boldsymbol{\Sigma}_{2}^{-1} \mathbf{J}=\left[\begin{array}{ccccccc}
\left(\frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}_{1}}\right)^{\top} \boldsymbol{\Sigma}_{2}^{-1} \frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}_{1}} & \mathbf{0} & \left(\frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}_{1}}\right)^{\top} \boldsymbol{\Sigma}_{2}^{-1} \frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}_{3}} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\
\left(\frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}_{3}}\right)^{\top} \boldsymbol{\Sigma}_{2}^{-1} \frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}_{1}} & \mathbf{0} & \left(\frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}_{3}}\right)^{\top} \boldsymbol{\Sigma}_{2}^{-1} \frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}_{3}} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0}
\end{array}\right]
\end{aligned}
\]
同理，可以计算 \(\boldsymbol{\Lambda}_{1},\boldsymbol{\Lambda}_{3},\boldsymbol{\Lambda}_{4},\boldsymbol{\Lambda}_{5}\)并且也是稀疏的。
信息矩阵组装过程的可视化，将五个残差的信息矩阵加起来，得到样例最终的信息矩阵 \(\Lambda\)，可视化如下：
2. 基于边际概率的滑动窗口算法
-
为什么SLAM需要滑动窗口算法？
- 随着 VSLAM 系统不断往新环境探索，就会有新的相机姿态以及看到新的环境特征，最小二乘残差就会越来越多，信息矩阵越来越大，计算量将不断增加。
- 为了保持优化变量的个数在一定范围内，需要使用滑动窗口算法动态增加或移除优化变量。
-
滑动窗口算法的具体流程
- 增加新的变量进入最小二乘系统优化。
- 如果变量数目达到了一定的维度，则移除老的变量。
- SLAM 系统不断循环前面两步。
Question: 怎么移除老的变量？直接丢弃这些变量吗？
Answer: 利用边际概率移除老的变量。
直接丢弃变量和对应的测量值，会损失信息。正确的做法是使用边际概率，将丢弃变量所携带的信息传递给剩余变量。下图为例子中使用边际概率移除变量 \(\xi_1\)，信息矩阵的变化过程:
可以看到随着 \(\xi_1\)的移除，信息矩阵可以被分为四个部分，然后通过舒尔补的方式去除左上角的部分，注意这里是通过等式变换得到的，因此不会造成信息的损失，但是通过结果也可以发现，原先稀疏的信息矩阵变得稠密，原本中不相关的两个变量此时也变得相关了。这里也简单提一下一个小tips，如果 marg 掉那些不被其他帧观测到的路标点，不会显著地使 H 矩阵变得稠密。 而要 marg 掉的路标点中， 对于那些被其他帧观测到路标点，要么就别设置为 marg，要么就宁愿丢弃，这就是 OKVIS 中用到的策略。这里我们marg的\(\xi_1\)明显关联的太多，就会使得H矩阵变得稠密。
3. 滑动窗口中的 FEJ 算法
还是上面的例子，我们已经移除了\(\xi_1\)，现在加入新的变量 \(\xi_7\)，新的变量和旧的变量\(\xi_2\)之间存在观测信息，能构建残差\(\boldsymbol{r}_{27}\)，图模型如下所示：
新残差加上之前 marg 留下的信息，构建新的最小二乘系统，对应的信息矩阵的变化如下图所示：
我们来系统的回顾一下前面提到的了例子的整个流程，如图所示：
- 红色为被marg掉的变量以及测量约束。
- 绿色为跟marg变量有关的保留变量。
- 蓝色为和marg变量无关联的变量。
- 如图，在 \(t\in[0,k]\)时刻，系统中状态量为 \(\xi_i,i\in[1,6]\)。第 \(k^{‘}\) 时刻，加入新的观测和状态量 \(\xi_7\)。
- 在第 k 时刻，最小二乘优化完以后， marg 掉变量\(\xi_1\)。被 marg 的状态量记为 \(x_m\), 剩余的变量 \(\xi_i,i\in [2, 5]\) 记为 \(x_r\)。
- marg 发生以后， \(x_m\) 所有的变量以及对应的测量将被丢弃。同时，这部分信息通过 marg 操作传递给了保留变量 \(x_r\)， marg 变量的信息跟\(\xi_6\)不相关。
- 第 \(k^{‘}\) 时刻，加入新的观测和状态量 \(\xi_7\)（记作\(x_n\)）以及对应的观测，开始新的一轮的最小二乘优化。
marg 发生后，留下的到底是什么信息？
marg 发生后，留下的到底是什么信息？
marg 发生后，留下的到底是什么信息？
重要的事情我们问三遍哈哈，marg 前, 变量 \(\mathbf{x}_{m}\) 以及对应测量 \(\mathcal{S}_{m}\) 构建的最小二乘信息矩阵为，注意这里的正定方程的构建是只使用了 marg 掉的变量和与 marg 变量存在关联的变量以及之间的约束。
\[\begin{aligned}
\mathbf{b}_{m}(k) & =\left[\begin{array}{l}
\mathbf{b}_{m m}(k) \\
\mathbf{b}_{m r}(k)
\end{array}\right]=-\sum_{(i, j) \in \mathcal{S}_{m}} \mathbf{J}_{i j}^{\top}(k) \boldsymbol{\Sigma}_{i j}^{-1} \mathbf{r}_{i j} \\
\boldsymbol{\Lambda}_{m}(k) & =\left[\begin{array}{ll}
\boldsymbol{\Lambda}_{m m}(k) & \boldsymbol{\Lambda}_{m r}(k) \\
\boldsymbol{\Lambda}_{r m}(k) & \boldsymbol{\Lambda}_{r r}(k)
\end{array}\right]=\sum_{(i, j) \in \mathcal{S}_{m}} \mathbf{J}_{i j}^{\top}(k) \boldsymbol{\Sigma}_{i j}^{-1} \mathbf{J}_{i j}(k)
\end{aligned}
\]
marg 后，变量 \(\mathbf{x}_{m}\) 的测量信息传递给了变量 \(\mathbf{x}_{r}\) ：
\[\begin{array}{l}
\mathbf{b}_{p}(k)=\mathbf{b}_{m r}(k)-\boldsymbol{\Lambda}_{r m}(k) \boldsymbol{\Lambda}_{m m}^{-1}(k) \mathbf{b}_{m m}(k) \\
\boldsymbol{\Lambda}_{p}(k)=\boldsymbol{\Lambda}_{r r}(k)-\boldsymbol{\Lambda}_{r m}(k) \boldsymbol{\Lambda}_{m m}^{-1}(k) \boldsymbol{\Lambda}_{m r}(k)
\end{array}
\]
下标 p 表示 prior. 即这些信息将构建一个关于 \(x_r\) 的先验信息。我们可以从 \(b_p(k),\Lambda_p(k)\)中反解出一个残差 \(r_p(k)\) 和对应的雅克比矩阵 \(J_p(k)\)。需要注意的是，随着变量 \(x_r(k)\) 的后续不断优化变化，残差\(r_p(k)\) 或者 \(b_p(k)\) 也将跟着变化，但雅克比 \(J_p(k)\) 则固定不变了。
变量一旦丢弃，对应的测量肯定在程序中也就不复存在了。
新测量信息和旧测量信息构建新的系统。在 \(k′\) 时刻， 新残差 \(r_{27}\) 和先验信息 \(b_p(k), Λ_p(k)\)以及残差 \(r_{56}\) 构建新的最小二乘问题， \(J_p(k)\) 和 \(r_p(k)\) 为先验部分对应的雅克比和残差：
\[\begin{array}{c}
\mathbf{b}\left(k^{\prime}\right)=\boldsymbol{\Pi}^{\top} \mathbf{J}_{p}(k) \mathbf{r}_{p}\left(k^{\prime}\right)-\sum_{(i, j) \in \mathcal{S}_{a}\left(k^{\prime}\right)} \mathbf{J}_{i j}^{\top}\left(k^{\prime}\right) \boldsymbol{\Sigma}_{i j}^{-1} \mathbf{r}_{i j}\left(k^{\prime}\right) \\
\boldsymbol{\Lambda}\left(k^{\prime}\right)=\boldsymbol{\Pi}^{\top} \boldsymbol{\Lambda}_{p}(k) \boldsymbol{\Pi}+\sum_{(i, j) \in \mathcal{S}_{a}\left(k^{\prime}\right)} \mathbf{J}_{i j}^{\top}\left(k^{\prime}\right) \boldsymbol{\Sigma}_{i j}^{-1} \mathbf{J}_{i j}\left(k^{\prime}\right)
\end{array}
\]
其中 \(\boldsymbol{\Pi}=[\bold{I}_{dim\ \bold{x}_r}, 0]\) 用来将矩阵的维度进行扩张。\(S_a\) 用来表示除了被 marg 掉的测量以外的其他测量，如 \(r_{56},r_{27}\)。
- 出现的问题：
- 由于被 marg 的变量以及对应的测量已被丢弃，先验信息 \(\Lambda p(k)\)中关于 \(x_r\) 的雅克比在后续求解中没法更新。比如说其中包含的 \(r_{12},r_{13},r_{14},r_{15}\) 的残差，其实是作为先验信息保存了下来的。但是由于变量\(ξ_1\)已经不在了，这些残差中关于 \(ξ_1\) 的雅可比已经无法更新了。
- 但 \(x_r\) 中的部分变量还跟其他残差有联系, 如 \(ξ_2, ξ_5\)。这些残差如\(r_{27}\) 对 \(ξ_2\) 的雅克比会随着 \(ξ_2\) 的迭代更新而不断在最新的线性化点处计算。
滑动窗口算法导致的问题
信息矩阵的零空间变化，滑动窗口算法优化的时候，信息矩阵如公式如上所示变成了两部分，且这两部分计算雅克比时的线性化点不同。这可能会导致信息矩阵的零空间发生变化，从而在求解时引入错误信息。下面的图片很少的说明了这个问题。
多个解的问题，变成了一个确定解。不可观的变量，变成了可观的
-
单目 SLAM 系统 7 自由度不可观： 6 自由度姿态 + 尺度。
-
单目 + IMU 系统是 4 自由度不可观： yaw 角 + 3 自由度位置不可观。roll 和 pitch 由于重力的存在而可观，尺度因子由于加速度计的存在而可观。
-
滑动窗口中的问题。
- 滑动窗口算法中，对于同一个变量，不同残差对其计算雅克比矩阵时线性化点可能不一致，导致信息矩阵可以分成两部分，相当于在信息矩阵中多加了一些信息，使得其零空间出现了变化。
-
解决办法： First Estimated Jacobian
- FEJ 算法：不同残差对同一个状态求雅克比时，线性化点必须一致。这样就能避免零空间退化而使得不可观变量变得可观。
- 比如: toy example 中计算 \(r_{27}\) 对 \(ξ_2\) 的雅克比时， \(ξ_2\) 的线性话点必须和 \(r_{12}\)对其求导时一致。