【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 }\left\{\underset{marginalizationresidual}{{\left|\left|r_p-H_p\chi \right|\right|}^2}+\underset{IMUresidual}{\sum _{k\in B}{\left|\left|r_B({\hat{z}}_{b_{k+1}}^{b_k},\chi )\right|\right|}^2}+\underset{visualresidual}{\sum _{(l,j)\in C}{\left|\left|r_c({\hat{z}}_l^{c_j},\chi )\right|\right|}_{P_l^{c_i}}^2}\right\}$$
整体的VIO的cost function由上式中的三个部分组成：
1、由滑动窗口中被去掉(marginalize)的节点和特征点构成的约束，也叫prior（先验）
2、IMU 运动模型的误差, 每相邻的两帧IMU之间产生一个residual
3、视觉的误差, 单个特征点l在相机cj下的投影会产生一个residual
在k时刻，滑动窗口中待估计的系统状态量定义如下：
$$\begin{array}{c}\chi =[x_n,x_{n+1},...,x_{n+N},x_c^b,{\lambda }_0,{\lambda }_1,...,{\lambda }_m]\\ x_k=[p_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g],k\in [n,n+N]\\ x_c^b=[p_c^b,q_c^b]\end{array}$$
其中，$x_k$表示k时刻IMU在惯性系（世界坐标系world）的位置、速度、姿态，和body系下的加速度和角速度的偏置，15维。N是滑动窗口的尺寸，$\lambda$是特征点对应的逆深度，$x_c^b$是相机内参，6维。
所以总体BA要估计的残差$\chi$是一个15×N+6+M长度的量，要注意的是：这里最后我们需要维护的量中仅考虑每帧相机时刻的姿态，而由于IMU的更新速度通常比相机要快很多，也就是说两帧相机之间会有很多IMU数据，这些数据将会被积分转化为上述的位置、速度、姿态，最后是不参与优化的。
解释一下逆深度：特征点在归一化平面和相机坐标系下的坐标关系为：
$$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\frac{1}{\lambda }\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$其中$\lambda =\frac{1}{z}$称为逆深度，使用逆深度的原因是使用xyz时，其值可能会特别大（如很远的点），采用逆深度和uv可以节省存储空间。
归一化平面：在相机前z=1处的假想平面，将像素平面上的像素坐标转换为米制坐标。

视觉部分

视觉部分的BA可以参考十四讲，但是与纯视觉不同的是，
特征点的逆深度在第i帧中初始化得到，在第j帧中又被观测到，则其在第j帧中的预测坐标表示如下：
$$\left[\begin{array}{c}{x}_{c_j}\\ y_{c_j}\\ z_{c_j}\\ 1\end{array}\right]={\mathbf{T}}_{bc}^{-1}{\mathbf{T}}_{w{b}_j}^{-1}{\mathbf{T}}_{w{b}_i}{\mathbf{T}}_{bc}\left[\begin{array}{c}\frac{1}{\lambda }{u}_{c_i}\\ \frac{1}{\lambda }{v}_{c_i}\\ \frac{1}{\lambda }\\ 1\end{array}\right]$$
其中${\mathbf{T}}_{bc}$表示body坐标系到相机坐标系的变换矩阵，是一个固定的矩阵，${\mathbf{T}}_{w{b}_j}^{-1}$是j时刻body坐标系到惯性系的转换矩阵，${\mathbf{T}}_{w{b}_i}$同理。那么在VIO中重投影误差可以表示为：
$${\mathbf{r}}_c=\left[\begin{array}{c}\frac{x_{c_j}}{z_{c_j}}-u_{c_j}\\ \frac{y_{c_j}}{z_{c_j}}-v_{c_j}\end{array}\right]$$

IMU部分

从第i帧到第j帧对IMU的测量值进行积分，即可得到相对的PVQ(对应平移、速度、旋转)：
$$\begin{array}{rl}{p}_{b_j}^w& =p_{b_k}^w+v_{b_k}^w\Delta t_k+\int {\int }_{t\in [i,j]}\left(R_t^w({\hat{a}}_t-b_{a_t})-g^w\right)d{t}^2\\ v_{b_j}^w& =v_{b_k}^w+{\int }_{t\in [i,j]}\left(R_t^w({\hat{a}}_t-b_{a_t})-g^w\right)dt\\ q_{b_j}^w& =q_{b_k}^w⨂{\int }_{t\in [i,j]}\frac{1}{2}\Omega ({\hat{\omega }}_t-b_{w_t})q_t^{b_k}dt\end{array}$$
由于这个式子中存在一个$R_t^w$（惯性系下的旋转，$q_{b_k}$同理），每一次积分都要重新估计这个旋转，使得积分变得十分复杂，所以我们要用到IMU的预积分，由于$q_{w{b}_t}=q_{w{b}_i}⨂q_{b_i{b}_t}$，则可以将$q_{w{b}_i}$提出来，将以下的式子定义为预积分量：
$$\begin{array}{rl}{\alpha }_{b_i{b}_j}& =\int {\int }_{t\in [i,j]}\left(q_{b_i{b}_t}{a}^{b_t}\right)\delta t^2\\ {\beta }_{b_i{b}_j}& ={\int }_{t\in [i,j]}\left(q_{b_i{b}_t}{a}^{b_t}\right)\delta t\\ q_{b_i{b}_j}& ={\int }_{t\in [i,j]}{q}_{b_i{b}_t}⨂\left[\begin{array}{c}0\\ \frac{1}{2}{w}^{bt}\end{array}\right]\delta t\end{array}$$
则可以得到新的PVQ积分表达式（太长了不想敲了）

最后得到预积分的误差表达式：

这就是最后三项中的IMU residual

预积分方差

IMU的测量值我们看做是有误差的，且误差服从高斯分布，那么在两帧之间的时间内的多个IMU数据形成的预积分量的误差也需要我们进行表示，高斯分布经过线性变换还是高斯分布，假设有一个线性变换A，那么协方差的递推公式如下（协方差表示的是数据之间的协同性，在这里指的就是IMU不同参数误差之间的联系，这有一篇科普：协方差和协方差矩阵）：

将相邻时刻的误差线性传递方差表示为：

其中

表示状态量误差

表示测量噪声，可以看到状态量就是指的PVQ，而测量噪声则是指的IMU本身的偏置。
根据之前说的性质，协方差矩阵表示为：

那么这个式子我们将其抽象为$x_k=f(x_{k-1},u_{k-1})$,这里的f就明显是非线性的，为了能用上之前的性质，我们就需要对它进行线性化。举例来说，从0积分到10的数据记为$x_{10}$，那么新来一帧IMU数据，$x_{11}$就表示为上式中左边的量，$u_{10}$表示输入量。
这里线性化有两种方法，第一种是基于泰勒展开的，第二种是求一个随时间变化的递推方差。
第一种：对上面式子进行变换

得到

F和G就是一阶泰勒展开的雅各比矩阵，分别是$x_k$对$x_{k-1}$和$u_{k-1}$的，$\delta x_{k-1}$和$n_{k-1}$分别表示两者的误差。
第二种：有空填坑。。。
最后我们得到的预积分误差传递方程如下：

那么F和G怎么计算呢？由于其计算量非常大，先给出最后的结果：

其中

在这里科普一句，雅各比矩阵可以简单的理解为导数在多维方程中的表达形式，这个矩阵的第一行就是等式左边的第一个因变量对所有变量的偏导，第一列同理，是所有因变量对第一个变量的偏导。而在这里我们把所有偏导简写一下：
对于F和G的推导是一个工作量较大的任务，首先看到F的对角项都是单位阵，这是因为对角项相当于对自己上一时刻求导，举例说明：

这是速度的预积分展开为中值积分的形式，它是第三项，即f33（对上一时刻速度的雅各比）可以表示为：

其他的过长，有空再填坑

系统残差函数雅各比

这里的雅各比是指的系统函数的雅各比，而上面的F和G是预积分递推的雅各比。

视觉的雅克比

将之前的重投影误差的表达式转换成三维坐标下的形式：

${\mathbf{f}}_{c_j}$表示j时刻相机的坐标，我们用$r_c$来表示重投影误差，其对于各个变量的雅克比矩阵可以表示为下面这样：

其中

分别表示
根据链式法则，$\frac{\partial r_c}{所有状态变量}=\frac{\partial r_c}{\partial {\mathbf{f}}_{c_j}}\frac{\partial {\mathbf{f}}_{c_j}}{所有状态变量}$ ，其中：

之后就可以求${\mathbf{f}}_{c_j}$对各个状态量的偏导，包括对i和j时刻的状态量：
例如
就是由一开始的式子对pbi那一项求偏导，其他项全部为0，只留下和pbi相乘的两个旋转矩阵。
其他的复杂一点的举一个例子来说：

与角度相关的就是标红的旋转矩阵，其他项求导为0，所以化简为：

这里第二行的合并见上面的式子，对fcj求雅克比之后得到：

这里用到的技巧是上一篇中写到的，旋转点的右扰动雅克比可以表示为：

这样上面的式子就很好理解了，p就表示的是上面的fbi，^和x的意义相同表达形式不同而已。
其他的项求解的方式类似，不再赘述。具体结果太长我就不在这贴出来了。

IMU的雅克比

有空填坑。。。

总结

这讲里面涉及到很多公式的推导，在课程里也并没有讲的很清楚，还是得靠自己去查资料推。。对新手太不友好了。。。
回顾之前说的最小二乘的求解步骤：

1 找到一个合适的关于状态量x 的残差函数fi(x)
2 计算残差函数对状态量x 的雅克比J。
3 选定cost function 以及其参数。
4 LM 算法求解。

第一步中的残差函数就是我们的三项residual，x指的就是各项待优化的量，而我们花大时间推导的雅克比则是为了在实际编程中，将这些参数送给我们的solver，例如VINS里使用的ceres就需要这些参数。