深蓝学院从零开始手写VIO（四）——后端优化实践

声明：本专栏文章为深蓝学院《从零开始手写VIO》课程个人学习笔记，更多学习资源请咨询深蓝学院相关课程。

本节课为主要以VINS-Mono为例，讲解VIO后端优化中的一些实践trick。

非线性最小二乘问题求解的优化方法

根据前几讲的内容可知，最小二乘问题的求解往往可以归结为求解如下所示的正则方程（normal equation）过程：
$$\begin{array}{rl}{\mathbf{J}}^T{\mathbf{\Sigma }}^{-1}\mathbf{J}\Delta \mathbf{x}& =-{\mathbf{J}}^T{\mathbf{\Sigma }}^{-1}\mathbf{r}\\ \mathbf{\Lambda }\Delta \mathbf{x}& =-\mathbf{b}\end{array}$$

显然在信息矩阵$\mathbf{\Lambda }$满秩的情况下，上式的解为$\Delta \mathbf{x}=-{\mathbf{\Lambda }}^{-1}\mathbf{b}$。但在实际应用中，除了相机位姿外，滑动窗口还存在大量的路标点，这会使得$\mathbf{\Lambda }$维数很大，而求解高维矩阵的逆会带来极大地计算负担，并不是我们所期望的。

使用舒尔补加速求解

我们知道信息矩阵$\mathbf{\Lambda }$是一个稀疏矩阵，特别是对于主对角线上的分块矩阵，${\mathbf{\Lambda }}_{aa}$和${\mathbf{\Lambda }}_{bb}$来说，二者是一个对角矩阵，而求解对角矩阵的逆是一件非常容易的事情。如下式所示，我们将优化变量$\Delta \mathbf{x}$分为位姿和路标点两部分：
$$\left[\begin{array}{cc}{\mathbf{\Lambda }}_{pp}& {\mathbf{\Lambda }}_{pl}\\ {\mathbf{\Lambda }}_{lp}& {\mathbf{\Lambda }}_{ll}\end{array}\right]\left[\begin{array}{c}\Delta {\mathbf{x}}_p\\ \Delta {\mathbf{x}}_l\end{array}\right]=-\left[\begin{array}{c}\Delta {\mathbf{b}}_p\\ \Delta {\mathbf{b}}_l\end{array}\right]$$

随后对等式两边同时乘以矩阵$\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{\Lambda }}_{pl}{\mathbf{\Lambda }}_{ll}^{-1}\\ 0& \mathbf{I}\end{array}\right]$将其化简为下三角形式有：
$$\begin{array}{rl}\left[\begin{array}{cc}{\mathbf{\Lambda }}_{pp}& {\mathbf{\Lambda }}_{pl}\\ {\mathbf{\Lambda }}_{lp}& {\mathbf{\Lambda }}_{ll}\end{array}\right]\left[\begin{array}{c}\Delta {\mathbf{x}}_p\\ \Delta {\mathbf{x}}_l\end{array}\right]& =-\left[\begin{array}{c}\Delta {\mathbf{b}}_p\\ \Delta {\mathbf{b}}_l\end{array}\right]\\ \left[\begin{array}{cc}\mathbf{I}& -{\mathbf{\Lambda }}_{pl}{\mathbf{\Lambda }}_{ll}^{-1}\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}{\mathbf{\Lambda }}_{pp}& {\mathbf{\Lambda }}_{pl}\\ {\mathbf{\Lambda }}_{lp}& {\mathbf{\Lambda }}_{ll}\end{array}\right]\left[\begin{array}{c}\Delta {\mathbf{x}}_p\\ \Delta {\mathbf{x}}_l\end{array}\right]& =-\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{\Lambda }}_{pl}{\mathbf{\Lambda }}_{ll}^{-1}\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}\Delta {\mathbf{b}}_p\\ \Delta {\mathbf{b}}_l\end{array}\right]\\ \left[\begin{array}{cc}{\mathbf{\Lambda }}_{pp}-{\mathbf{\Lambda }}_{pl}{\mathbf{\Lambda }}_{ll}^{-1}{\mathbf{\Lambda }}_{lp}& 0\\ {\mathbf{\Lambda }}_{lp}& {\mathbf{\Lambda }}_{ll}\end{array}\right]\left[\begin{array}{c}\Delta {\mathbf{x}}_p\\ \Delta {\mathbf{x}}_l\end{array}\right]& =-\left[\begin{array}{c}\Delta {\mathbf{b}}_p-{\mathbf{\Lambda }}_{pl}{\mathbf{\Lambda }}_{ll}^{-1}\Delta {\mathbf{b}}_l\\ \Delta {\mathbf{b}}_l\end{array}\right]\end{array}$$

可见$\left({\mathbf{\Lambda }}_{pp}-{\mathbf{\Lambda }}_{pl}{\mathbf{\Lambda }}_{ll}^{-1}{\mathbf{\Lambda }}_{lp}\right)\Delta {\mathbf{x}}_{pp}=-(\Delta {\mathbf{b}}_p-{\mathbf{\Lambda }}_{pl}{\mathbf{\Lambda }}_{ll}^{-1}\Delta {\mathbf{b}}_l)$，有了$\Delta {\mathbf{x}}_{pp}$之后可进一步通过求解${\mathbf{\Lambda }}_{lp}\delta {\mathbf{x}}_{pp}+{\mathbf{\Lambda }}_{ll}\Delta {\mathbf{x}}_{ll}=\Delta {\mathbf{b}}_l$得到$\Delta {\mathbf{x}}_{ll}$。显然，在利用舒尔补对正则方程消元之后，我们只需要求解一个维数更小的线性方程组即可获得对应的解向量。

信息矩阵不满秩的处理方法

信息矩阵$\mathbf{\Lambda }$可以是一个非常理想的情况，实际上，在大多数应用中，信息矩阵$\mathbf{\Lambda }$并不满秩（例如前面提到过的单目vSLAM具有7个自由度，单目VIO具有4个自由度）。针对这一问题，可以有如下几种处理方法：

• 使用LM求解：由于LM求解过程中会对信息矩阵$\mathbf{\Lambda }$添加阻尼项$\lambda \mathbf{I}$，添加之后的信息矩阵必定是满秩的（零行的对角线位置都加上了$\lambda$）。该方法虽然能够保证正则方程有解，但解可能会向零空间便宜（这里具体原因涉及现行代数求解的一些知识，暂时还不太理解）。
• 添加先验约束：信息矩阵$\mathbf{\Lambda }$不满秩的根本原因在于整个系统求解的初值不确定，因此如果将状态量的初值固定住，就可以有效约束系统的不确定性。常用的方法包括：1）对信息矩阵中的${\mathbf{\Lambda }}_{11}$添加单位矩阵$\mathbf{I}$；2）强行将${\mathbf{\Lambda }}_{11}$认为设定为极大值（例如$10^{15}$）;3) 将Jacobian矩阵中对应的项${\mathbf{J}}_{11}$设为零。（上述三种方法的本质都是使得初值对应的$\Delta {\mathbf{x}}_0=0$，从而固定初值。）
• 固定位姿：将起始相机位姿设为基准位姿，使用下一个帧间位姿（或特征点）固定尺度。

Solver的代码框架

采用类似g2o的图优化方式，使用定点存储优化变量，使用边存储误差函数。

滑动窗口算法

$$\begin{array}{rl}\mathbf{\Lambda }& ={\mathbf{\Lambda }}_p+{\mathbf{J}}_a^T{\mathbf{\Sigma }}_a^{-1}{\mathbf{J}}_a\\ \mathbf{b}& ={\mathbf{b}}_p+{\mathbf{J}}_a^T{\mathbf{\Sigma }}_a^{-1}\mathbf{r}\end{array}$$

回顾上一讲中如上式所示的滑窗边缘化过程。由于在边缘化之后，包含边缘化变量的残差函数$\mathbf{r}({\mathbf{x}}_m,{\mathbf{x}}_r)$已经被丢弃，其对应的Jacobian矩阵${\mathbf{J}}_p$将保持不变，相应的先验信息矩阵（${\mathbf{\Lambda }}_p={\mathbf{J}}^T{\mathbf{\Sigma }}^{-1}\mathbf{J}$）也将保持不变。同时，为了避免解的零空间漂移，我们需要限定${\mathbf{J}}_a^T{\mathbf{\Sigma }}_a^{-1}{\mathbf{J}}_a$保持和${\mathbf{\Lambda }}_p$处于同样的线性化点。

但对于先验残差来说，虽然${\mathbf{J}}_p$仍然保持不变，但保留下的各变量残差$\mathbf{r}({\mathbf{x}}_r,{\mathbf{x}}_r)$将随着迭代过程的变化继续变化，此时若先验残差${\mathbf{b}}_p$仍然表示在原线性化点时的估计残差，同样会使得解空间出现漂移。但由于残差${\mathbf{b}}_p$表示的是当前的估计值，我们不能固定其不变（否则待求解变量一直在原地踏步，就失去意义了），因此我们需要使得先验残差跟随当前的估计值进行变化。常用的方法为对其进行一阶泰勒近似：
$${\mathbf{b}}_p^{\prime }={\mathbf{b}}_p+\frac{\partial {\mathbf{b}}_p}{\partial {\mathbf{x}}_r}\Delta {\mathbf{x}}_r={\mathbf{b}}_p-{\mathbf{\Lambda }}_p\Delta {\mathbf{x}}_r$$