【VIO笔记（学习VINS的必备基础）】第五讲（1/2） 手写VIO后端

系列教程来自某学院，侵权删除。
学习完这一系列课程再去看VINS才能做到不吃力，不然直接撸网上的各种VINS解析完全云里雾里-_-!

非线性最小二乘问题求解:solver

回忆上一讲的内容，最小二乘的系统可以表示为：

其中$\xi$是状态量，r表示残差，${\Sigma }_i$表示协方差。求解这个最小二乘实际上最后会落到迭代求下面这个正定方程：

在上一讲里我们把它写成了连加形式，便于理解：

而要求解这个方程，直接取H的逆计算量大，上一讲提到使用舒尔补来求逆，具体来说，考虑纯视觉的BA问题，我们可以把$\mathbf{H}\Delta x=b$写成：

其中，下标p表示pose，下标l表示landmark，那么求$\Delta x$的问题可以看成分别求$\Delta x_p^{\ast }$和$\Delta x_l^{\ast }$，再拼接起来。例如我们先求$\Delta x_p^{\ast }$，利用舒尔补把H矩阵化为下三角：
$\left[\begin{array}{cc}{H}_{pp}^{\prime }& 0\\ X& X\end{array}\right]\left[\begin{array}{c}\Delta x_p^{\ast }\\ \Delta x_l^{\ast }\end{array}\right]=\left[\begin{array}{c}-b_p^{\prime }\\ -b_l^{\prime }\end{array}\right]$
对应的等式右边b也会发生变化，在这里等式左边H矩阵下面两项X的值不重要，使用第一行乘$\Delta x$可以得到变化后的b’，如下：

那么$\Delta x_p^{\ast }$可以表示为左边矩阵的逆乘等式右边，而这个矩阵的维度与之前的H相比降低了很多，方便求解，因为原H矩阵用图表示为下：

左上角是相机姿态，一个滑动窗口内只有十几个，而路标点往往会达到几百个，所以可以减少整个的计算量，得到了$\Delta x_p^{\ast }$后，通过原始的式子可以计算出$\Delta x_l^{\ast }$：

这样就计算出了整个$\Delta x$，那么接下来我们回顾一下求解器的整体流程图：

首先我们要确定误差函数r以及它的雅克比矩阵J，这个在第三讲里面已经讨论过了，之后我们利用最小二乘法求状态量的最佳估计，可以使用舒尔补等方法来进行计算，对于不是slam的问题会使用蓝色框里的一些方法来迭代求解，最后进行不断的迭代直到得到$x^{\ast }$。
最后我们要说一下求解过程中的一些问题，我们知道H矩阵通常不满秩，求解时无法直接求逆，所以我们使用LM法时，由于要添加一个阻尼因子，可以使得H矩阵满秩，然而由于相机都是相对的估计，所以会出现零空间变化的问题，通俗的说其优化后的轨迹的零点可能不是真实空间中的零点，为了解决这个问题，有以下几种方法：
1、添加先验。假设第一帧的x为零点，为了使其不会变化，我们给第一个pose的信息矩阵H加上一个单位阵I，即${\mathbf{H}}_{[}$