简单理解VIO： 三 基于优化的IMU与视觉信息融合

基于Bundle Adjustment的VIO融合

视觉SLAM中的Bundle Adjustment问题

已知：

• 状态初始值，根据里程计大致位置，可以计算出机器人的位置与特征的位置。如： q = [0.96, 0, 0, 0.25], p = [1, 2, 0], f = [2, 4, 0].
• 观测量：图像观测到的特征的像素坐标 z = [100, 0];

目标函数：
最小化所有观测像素的残差：
$arg min \Sigma ||z_i - \pi (q, p, f)||$
这里的 $\pi(q, p, f)$是两个步骤，首先将特征点转换到局部坐标，然后把局部坐标转换到像素坐标，第二步的过程为：
$x_{pix} = f_x \frac{x}{z}$
相机局部坐标系为：前z，右x，下y，在二维情况中，y一直为0.

最小二乘问题的求解

关于泰勒展开式的理解： 泰勒展开通过一定阶数的表达式来代表（近似）一个复杂的函数。一般来说，求取泰勒展开式有两种：

• 已知复杂表达式，且复杂表达式可导，那直接通过一阶导数和二阶导数来求取。
• 当不知道复杂表达式，或者不可导时，可以通过在其上采样，通过采样点来求取泰勒展开。

在自动驾驶中，一般采用三次曲线来描述车道线，它其实就是现实生活中，复杂车道线的一个三次泰勒展开式。车身坐标系其实就是展开点。

一种比较简单的记忆泰勒展开式的方法：一次项使用直线，二次项使用抛物线，分别在0点处的情况来说明。

关于线性问题，如果是线性问题，其泰勒展开式，就可以描述其在整个可行解区间的近似，直接使用最小二乘解一次即可。

Hessian H的性质：当一阶导数为0时：

1. 如果H正定，特征值都大于0，则一阶导数单增，此处为最小值。
2. 如果H负定，特征值都小于0，则一阶导数单减，此处为最大值。
3. 如果H不定，特征值有正有负，此处不确定，为鞍点。

迭代下降法

下降法的初衷，就是找到一个方向，然后确定步长。正如下楼梯，如果简化为一个1维的问题，那就是超楼下指的方向就下降方向(-x), 然后确定使用线搜索确定步长。

最速下降法

梯度的负方向下降速度最快。也就是沿着切线方向。在最后容易震荡。适合一开始
用 $y = \sin(x)$来理解。

牛顿法

求二阶最优，最稳定，但是速度慢，适合最后几步。
牛顿法，本质就是初中时学的，二次方程的根：对于方程 $ax^2 + bx +c = 0$, 其最值在 $x = -\frac{b}{2a}$, 其中 $a = 1/2*H, b = J$, 显然，这种解法不一定能找到得最小值，只适用于快要接近于最优值了。

阻尼法

增加一个阻尼因子，在二次项上增加，调节a的大小。

高斯牛顿

高斯牛顿法，本质就是F函数被表示为了一个二次型，即 $F = f*f$. 然后把牛顿法用上，就成了高斯牛顿，真简单。

LM

将阻尼因子动态调节，就成了列文伯格马夸尔特算法。

鲁棒核函数

VIO残差函数的构建

系统需要优化的状态量

VIO中基于逆深度的重投影误差

IMU测量值积分

IMU预积分

残差Jacobian的推导