从零开始学习VIO笔记 --- 第三讲：基于优化的IMU和视觉信息融合

一. 预备知识

1.1 知识点概述

PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。（双目和RGB-D相机上的方法，双目可以通过三角化得到landmark 3D坐标，RGB-D则可以直接读取深度图获取像素平面点对应的深度信息/具体如何利用 见后续文章）
PnP 问题可以通过线性方法，DLT（直接线性变化）P3P ，EPnP 等求解，或者非线性优化求解，也就是通过BA优化求解。

线性方法，往往是先求相机位姿，再求空间点位置；而非线性优化则是把它们都看成优化变量，放在一起优化，我们把相机和三维点放在一起进行最小化问题，统称为Bundle Adjustment。

1. 构建最小化重投影误差问题，可以解PnP问题，也就是对误差函数，使用高斯牛顿/LM算法迭代的方式，解得相机的位姿，这是针对两帧图像之间的问题，也是BA的方式，属于slam的前端。
2. 另一种是后端优化中，通过多帧的相机位姿以及其观测到的 landmark 构建最小化投影误差函数，优化相机位姿和 3D的特征点/landmark。

于是我们分别对上面两种情况进行介绍。

1.2 PnP 中的重投影误差函数

构建的函数如下：

具体求解： 见单独的文章

1.3 后端优化中重投影误差函数/BA问题

优化问题组成：

• 待优化的状态量：特征点的空间坐标，相机的位姿
• 测量值/观测值：特征点在不同图像上的图像坐标(u,v)

构建重投影误差函数，最小化误差函数，得到最优的状态量
（理论投影值减去实际观测值）

可以使用 g2o或者ceres求解

二. VIO 信息融合问题

2.1 问题提出

前面构建的误差函数，误差项只是包含了 相机位姿和landmark。 现如今需要通过紧耦合方式融合imu信息。那么需要构建关于IMU的误差。

2.2 线性最小二乘简介

线性最小二乘求解: 最速下降法和牛顿法

非线性线性最小二乘求解: Gauss-Newton 和 LM

最小二乘中遇到 outlier，通过添加鲁棒核函数

Gauss-Newton：
整体的残差函数为$F(x)$ ,非线性最小二乘，是单独对里面残差项$f_i\left(x\right)$分析

$$F(x)=\frac{1}{2}\sum _{i=1}^m{\left(f_i\left(x\right)\right)}^2$$
将残差组合成向量的形式


得：$$F(x+△x)\approx F(x)+△x^T{J}^{T}f+\frac{1}{2}△x^T{J}^{T}J△x$$
上式对 $△x$进行一阶求导得：$$\frac{dF(x+△x)}{d△x}=J^{T}f+J^{T}J△x$$
令导数为0， 即得：
$$J^{T}J△x_{gn}=-J^{T}f$$

或写为 $H△x_{gn}=b$, 使用$J^{T}J$近似 $H$,

具体见后续

2.3 VIO 残差函数的构建

2.3.1 VIO 误差函数：

$$\underset{\chi }{\min }\{\underset{marginalization/priorresidual}{\underbrace{{\Vert r_p-H_p\chi \Vert }_{{\sum }_p}^2}}+\underset{IMUresidual}{\underbrace{\sum _{i\in B}{\Vert r_b(z_{b_i{b}_{i+1}},\chi )\Vert }_{{\sum }_{b_i{b}_{i+1}}}^2}}+\underset{imageresidual}{\underbrace{\sum _{(i,j)\in F}{\Vert r_f(z_{f_j}^{c_i},\chi )\Vert }_{{\sum }_{f_j}^{c_i}}^2}}\}$$

1、由滑动窗口中被去掉(marginalize)的节点和特征点构成的约束，也叫prior（先验）
2、IMU 运动模型的误差, 每相邻的两帧IMU之间产生一个residual/预计分误差，下面会详细介绍
3、视觉的投影误差, 单个特征点 $f_j$ 在相机$c_i$下的投影会产生一个residual
每一部分的误差和右下角${\sum }_{}$为协方差

2.3.2 系统需要优化的状态量

系统需要优化的状态量
为了节约计算量采用滑动窗口形式的 Bundle Adjustment, 在 i 时刻，滑动窗口内待优化的系统状态量定义如下:
$$\begin{array}{c}\chi =\left[{\mathbf{x}}_n,{\mathbf{x}}_{n+1},\dots ,{\mathbf{x}}_{n+N},{\lambda }_m,{\lambda }_{m+1},\dots ,{\lambda }_{m+M}\right]\\ \\ {\mathbf{x}}_i={\left[p_{b_i}^w,v_{b_i}^w,q_{b_i}^w,b_a^{b_i},b_g^{b_i}\right]}^T,i\in [n,n+N]\end{array}$$

1. $\chi$ 为系统需要优化的状态量；
2. ${\mathbf{x}}_i$ 包含 i 时刻 IMU 机体的在惯性坐标系中的位置$p$，速度$v$，姿态$q$，以及 IMU 机体坐标系中的加速度和角速度的偏置量估计$b_a$ , $b_g$；
3. $\lambda$ 是逆深度
4. $n,m$ 分别是机器人状态量和路标在滑动窗口里的起始时刻；
5. $N$ 是滑动窗口中关键帧数量
6. $M$是被滑动窗口内所有关键帧观测到的路标数量

2.3.3 视觉重投影误差

定义：一个特征点在归一化相机坐标系（z=1平面）下的估计值与观测值的差。
$${\mathbf{r}}_c=\left[\begin{array}{l}\frac{x}{z}-u\\ \frac{y}{z}-v\end{array}\right]$$

其中，待估计的状态量为特征点的三维空间坐标 $(x,y,z{)}^{\top }$，观测值$(u,v{)}^{\top }$ 为特征在相机归一化平面的坐
标。

这儿选择的观测值是在归一化平面的下，也可以选择在像素平面,需要相机内参K 。。。。（后续补上）

2.3.4 逆深度参数化

特征点在相机坐标系与归一化相机坐标系下的坐标关系为:
$$\underset{cameraframe}{\underbrace{\left[\begin{array}{c}x\\ y\\ z\end{array}\right]}}=\frac{1}{\lambda }\underset{z=1frame}{\underbrace{\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]}}$$
其中 $\lambda =1/z$ 称为逆深度 ; 使用逆深度的原因是使用xyz时，其值可能会特别大（如很远的点）,不易优化，并且采用逆深度和uv可以节省存储空间。归一化平面：在相机前z=1处的假想平面，将米制坐标转换为像素平面上的像素坐标。观测值$(u,v{)}^{\top }$ 已知，只需要优化逆深度值，便可以得到优化之后的特征点三维空间坐标 $(x,y,z{)}^{\top }$

2.3.5 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]$$

2.3.6 IMU 测量值积分

更为详细公式推导见上一讲VIO 中的 imu模型
从第 i 时刻的 PVQ 对 IMU 的测量值进行积分得到第 j 时刻的 PVQ：
$$\begin{array}{l}{\mathbf{p}}_{w{b}_j}={\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t+{\iint }_{t\in [i,j]}\left({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}-{\mathbf{g}}^w\right)\delta t^2\\ {\mathbf{v}}_j^w={\mathbf{v}}_i^w+{\int }_{t\in [i,j]}\left({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}-{\mathbf{g}}^w\right)\delta t\\ \mathbf{q}{w}_{b_j}={\int }_{t\in [i,j]}{\mathbf{q}}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$

1. 其中 $\mathbf{a}$ 与 $\omega$都是测量值；要想结果更准确当然都需要减去各自的偏差bias；
2. 式中 $\mathbf{q}$ 为惯性系/世界坐标系 $w$下body的旋转，四元数表达，也可以是${\mathbf{R}}_t^w$；
3. 在每一次进行imu积分操作时， ${\mathbf{q}}_{b_t}^w$ 每次都需要重新估计这个旋转，使得积分变得复杂，所以我们使用imu预积分

2.3.6 IMU 预积分

将积分模型转为预积分模型 ：$${\mathbf{q}}_{w{b}_t}={\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_t}$$

那么，PVQ 积分公式中的积分项则变成相对于第 i 时刻的姿态，而不是相对于世界坐标系的姿态：
p w b j = p w b i + v i w Δ t − 1 2 g w Δ t 2 + q w b i ∬ t ∈ [ i , j ] ( q b i b t a b t ) δ t 2 v j w = v i w − g w Δ t + q w b i ∫ t ∈ [ i , j ] ( q b i b t a b t ) δ t q w b j = q w b i ∫ t ∈ [ i , j ] q b i b t ⊗ [ 0 1 2 ω b t ] δ t \begin{array}{l} \mathbf{p}_{w b_{j}}=\mathbf{p}_{w b_{i}}+\mathbf{v}_{i}^{w} \Delta t-\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}+\mathbf{q}_{w b_{i}} \iint_{t \in[i, j]}\left(\mathbf{q}_{b_{i} b_{t}} \mathbf{a}^{b_{t}}\right) \delta t^{2} \\ \mathbf{v}_{j}^{w}=\mathbf{v}_{i}^{w}-\mathbf{g}^{w} \Delta t+\mathbf{q} w b_{i} \int_{t \in[i, j]}\left(\mathbf{q}_{b_{i} b_{t}} \mathbf{a}^{b_{t}}\right) \