【VIO笔记（学习VINS的必备基础）】第三讲 基于优化的IMU和视觉融合的实现原理

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

融合框架

基于优化的VIO融合原理上是从视觉的BA发展而来，其代价函数是多个重投影误差的和，详见十四讲。在VIO中还需要考虑IMU的数据，因此需要对多个传感器的数据进行融合，这里分为松耦合和紧耦合，目前比较火的方案如VINS等都使用紧耦合。

最小二乘

最小二乘定义是找到一个x使得代价函数F(x)的取值最小，F(x)是由多个f(x)的平方求和组成的。所以这个问题叫做最小二乘，意义是求f(x)的二次方之和的最小值。在SLAM中这个问题基本上是非线性的，所以对该F(x)求解析形式的导数形式是不现实的（即无法使用直接求解法），一般需要进行泰勒展开使用迭代下降法求解。
$$F(\mathbf{x}+\Delta \mathbf{x})=F(\mathbf{x})+\mathbf{J}(\mathbf{x}-{\mathbf{x}}_k)+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}({\mathbf{x}}_k)\Delta \mathbf{x}+o^n$$
其中的J和H分别是F对x的一阶导和二阶导矩阵。
最小二乘的求解有一阶和二阶梯度下降法，但是由于其本身具有较大缺陷（一阶法过于贪心，会走出锯齿路线；二阶法需要计算H矩阵），一般不会在实际SLAM系统中使用。

G-N和L-M

针对上述方法的问题，我们采用更实用的高斯牛顿法和LM法，其中GN法是对f(x)进行泰勒展开，从而避免H的计算。上述的最速下降法适用于迭代的开始，而牛顿法适用于最优值附近，那么将两者结合的方案就是LM法。
GN法对f（x）进行一阶泰勒展开，
并将上式带入F（x），得到：

此时将F（x）的表达式与牛顿法做对比，可以发现使用J来近似表达了H，从而避免了H的计算，对上式求$\Delta x$的一阶导并令其等于0，得到：

也就是我们经常看到的$\mathbf{H}\Delta x=\mathbf{b}$，求解这个方程就能得到待估计的x。
而LM法则在上述增量方程中加入了阻尼因子：

它的作用是保证左边的H矩阵正定，确保迭代方向是朝着下降的方向进行，μ 非常大时，接近最速下降法；μ 比较小时，接近高斯牛顿法。它的更新策略在g2o和ceres里采取的是Nielsen策略：

那么整个最小二乘的求解步骤总结为：
1 找到一个合适的关于状态量x 的残差函数fi(x)
2 计算残差函数对状态量x 的雅克比J。
3 选定cost function 以及其参数。
4 LM 算法求解。
这些量也是我们接下来要在VIO系统中弄清楚的。

VIO残差函数

$$\underset{\chi }{\min }\{\underset{marginalizationresidual}{\underbrace{{\left|\left|r_p-H_p\chi \right|\right|}^2}}+\underset{IMUresidual}{\underbrace{\sum _{k\in B}{\left|\left|r_B({\hat{z}}_{b_{k+1}}^{b_k},\chi )\right|\right|}^2}}+\underset{visualresidual}{\underbrace{\sum _{(l,j)\in C}\left|\left|r_c({\hat{z}}_l^{c_j},\chi )\right|\right|}}$$