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

一、视觉SLAM中的Bundle Adjustment问题

已知：

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

目标函数：
最小化所有观测像素的残差：
$$argmin\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、迭代下降法

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

1.1、最速下降法

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

1.2、牛顿法

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

1.3、阻尼法

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

1.4、高斯牛顿

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

1.5、LM

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

2、鲁棒核函数

三、VIO残差函数的构建

1、系统需要优化的状态量

滑动窗口中，待优化的变量包含：body的PVQ(位置速度姿态)，特征点的位置，角速度和角速度的偏置量。

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

因为前面已经假设了二维的场景，相机坐标系下的feature与图像坐标系下的观测之间的关系为：$u=f\ast \frac{x}{z}$, 因为是二维场景，所以相机的观测都在一条水平线上$v=f\ast \frac{y}{z}=0$. 实例检验：当x = 1m, z = 1m, y = 0m, f = 500, u = 500, v = 0;
残差的定义则为
$$r_c=[\begin{array}{c}u-f\ast \frac{x}{z}\\ v-f\ast \frac{y}{z}\end{array}]=[\begin{array}{c}u-f\ast \frac{x}{z}\\ 0\end{array}]$$

3、IMU测量值积分

我们的状态量，是怎么与IMU测量值发生关系的？前一时刻的PVQ，通过对IMU测量值积分，得到下一个时刻的PVQ。具体来说：P-通过假设匀加速直线运动模型；V-通过匀加速直线运动模型，Q-通过匀角速度运动模型。实例检验：角速度1°/s, 加速度1m/s^2, 速度1m/s, 时间t=0-1s; 初始状态：$P_0=[0,0],V_0=[0,0],Q_0=0°$。 经过一秒后，$P_1=[0.5,0.5],V_1=[1,1],Q_1=1°$$
写成通用形式：
$$q_j=\int q_t\otimes [0,1/2w]\delta t$$
$$v_j=v_i+\int q_{t}a\delta t$$
$$p_j=p_i+\int v_t\delta t+1/2\int \int q_{t}a\delta t$$
这里的观测值a与w都是局部坐标系下的观测值。
缺点： 我们可以看到，积分里面的量包含了观测量，也包含了待优化的姿态，那么，我们每一步迭代都需要重新计算积分里面的量。而积分是一个计算量比较大的操作。比如两个相邻的姿态之间，有是个加速度值，那么就是计算10*3次，如果迭代10次，那么就需要对积分量进行300次计算，如果有100个姿态，则计算量高达3万次。

4、IMU预积分

将上面的三个式子变成：
$$q_j=q_i\int q_{it}\otimes [0,1/2w]\delta t$$
$$v_j=v_i+q_i\int q_{it}a\delta t$$
$$p_j=p_i+\int v_t\delta t+1/2q_i\int \int q_{it}a\delta t$$
这样变换之后，与积分相关的内容，其实是一个常数了，只需要计算一次，也就是说，上面的三万次计算量，变成了30*100 = 3千次，计算量缩小了十倍。
预计分的本质： 将状态量变成了观测量。

四、残差Jacobian的推导

（@todo）

五、结论

1. 通过预计分，计算量优化了n倍，n是迭代的次数。这个倍数很重要，比如以前的1hz, 可以提高到10hz, 就能衍生出更多的应用场景了。