第 3 讲基于优化的 IMU 与视觉信息融合

VIO-shenlan

---

1 基于 Bundle Adjustment 的 VIO 融合

$$\underset{\mathbf{q}, \mathbf{p}, \mathbf{f}}{\arg \min } \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|\pi\left(\mathbf{q}_{w c_{i}}, \mathbf{p}_{w c_{i}}, \mathbf{f}_{j}\right)-\mathbf{z}_{f_{j}}^{c_{i}}\right\|_{\Sigma_{i j}}$$

关于优化函数的构建，可以在像素平面上构建重投影误差，也可以在归一化平面上构建重投影误差。发现在归一化平面上构建优化问题在写代码和推公式的时候少了一个内参矩阵K，大大简单了很多。

这个地方要了解BA的求解细节，

2 最小二乘问题的求解

2.1 基础算法

Gaussian-Newton算法

$$\left(\mathbf{J}^{\top} \mathbf{J}\right) \Delta \mathbf{x}_{\mathrm{gn}}=-\mathbf{J}^{\top} \mathbf{f}$$
LM 算法
$$\left(\mathbf{J}^{\top} \mathbf{J}+\mu \mathbf{I}\right) \Delta \mathbf{x}_{\operatorname{lm}}=-\mathbf{J}^{\top} \mathbf{f} \quad \text { with } \mu \geq 0$$

阻尼因子 µ 的更新策略有很多种，不同的人设计的算法细节上有区别，同一个算法也不可能在所有的数据上都表现很好。其中g2o中的LM阻尼因子更新策略通过比例因子来确定的

$$\rho=\frac{F(\mathbf{x})-F\left(\mathbf{x}+\Delta \mathbf{x}_{\operatorname{lm}}\right)}{L(\mathbf{0})-L\left(\Delta \mathbf{x}_{\operatorname{lm}}\right)}$$

注意这个$\frac{1}{2}$和分母上的$\frac{1}{2}$会消除掉，就得到了上面g2o算法流程里面的$\rho$的更新。

---

2.2 鲁棒核函数的实现

最小二乘中遇到 outlier 怎么处理？核函数如何在代码中实现？有多种方法，这里主要介绍 g2o 和 ceres 中使用的Triggs Correction

其中$\Delta s_{k}$的计算如下

$$\begin{aligned} \Delta s_{k} &=\left\|f_{k}(\mathbf{x}+\Delta \mathbf{x})\right\|^{2}-\left\|f_{k}(\mathbf{x})\right\|^{2} \\ & \approx\left\|f_{k}+\mathbf{J}_{k} \Delta \mathbf{x}\right\|^{2}-\left\|f_{k}(\mathbf{x})\right\|^{2} \\ &=2 f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x} \end{aligned}$$
代入公式14，得到
$$\begin{aligned} \frac{1}{2} \rho(s) \approx & \frac{1}{2}\left(\rho^{\prime}\left[2 f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}\right]\right.\\ &+\frac{1}{2} \rho^{\prime \prime}\left[2 f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}\right]^{2}+\text { const } ) \\ \approx & \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+\frac{1}{2} \rho^{\prime}(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+\rho^{\prime \prime}(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top} f_{k} f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+c o n s t \\=& \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k} \Delta \mathbf{x}+\frac{1}{2}(\Delta \mathbf{x})^{\top} \mathbf{J}_{k}^{\top}\left(\rho^{\prime} I+2 \rho^{\prime \prime} f_{k} f_{k}^{\top}\right) \mathbf{J}_{k} \Delta \mathbf{x}+c o n s t \end{aligned}$$

对该公式求和后，对变量$\Delta x$求导，令其等于 0 ，得到

$$\begin{aligned} \sum_{k} \mathbf{J}_{k}^{\top}\left(\rho^{\prime} I+2 \rho^{\prime \prime} f_{k} f_{k}^{\top}\right) \mathbf{J}_{k} \Delta \mathbf{x} &=-\sum_{k} \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k} \\ \sum_{k} \mathbf{J}_{k}^{\top} W \mathbf{J}_{k} \Delta \mathbf{x} &=-\sum_{k} \rho^{\prime} f_{k}^{\top} \mathbf{J}_{k} \end{aligned}$$

2.3 最小二成LM求解步骤

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

3 VIO 残差函数的构建

系统需要优化的状态量

$$\begin{aligned} \mathcal{X} &=\left[\mathbf{x}_{n}, \mathbf{x}_{n+1}, \ldots, \mathbf{x}_{n+N}, \lambda_{m}, \lambda_{m+1}, \ldots, \lambda_{m+M}\right] \\ \mathbf{x}_{i} &=\left[\mathbf{p}_{w b_{i}}, \mathbf{q} w b_{i}, \mathbf{v}_{i}^{w}, \mathbf{b}_{a}^{b_{i}}, \mathbf{b}_{g}^{b_{i}}\right]^{\top}, i \in[n, n+N] \end{aligned}$$

3.1 视觉重投影误差

一个特征点在归一化相机坐标系下的估计值与观测值的差。

$$\mathbf{r}_{c}=\left[\begin{array}{ll}{\frac{x}{z}-u} \\ {\frac{y}{z}-v}\end{array}\right]$$
待估计的状态量为特征点的三维空间坐标 $(x, y, z)^T$，观测值$(u, v)^T$ 为特征在相机归一化平面的坐标。

3.2 预积分模型

3.2.1 IMU数学模型

值得注意的是这里的加号，我们认为重力加速度是一个标量。实际上当加速度计静止的时候，IMU的实际读数就是接近于9.8的，这和我们的公式也是一致的，便于我们编程。

3.2.2 Level1 —— 朴素的IMU积分形式

从第 $i$ 时刻的 PVQ 对 IMU 的测量值进行积分得到第 $j$ 时刻的 PVQ:

注意这个是相对世界坐标系的。

3.2.3 Level2 —— 将相对世界坐标系转为相对上一帧图像

下面的这个是精髓

然后我们就得到IMU预积分形式

3.2.4 Level3 —— IMU预积分的连续形式

预积分量仅仅跟 IMU 测量值有关，它将一段时间内的 IMU 数据直接积分起来就得到了预积分量

3.2.5 Level4 —— IMU预积分的离散形式

重新整理下 PVQ 的积分公式，有：

3.2.6 Level5 —— IMU预积分mid-point连续形式（For Coding）

这个也是VINS程序中使用到的形式，只不过需要转换为离散形式，这个要注意的是编程的时候算式的更新顺序！！

3.3 IMU预积分误差

IMU预积分误差关联的状态量如下：

• 两个时刻的$PVQ, b_a, b_g$
  $P_{wb_i}, V_{i}^w, q_{b_iw}, b^a_i, b^g_i$
  $P_{wb_j}, V_{j}^w, q_{wb_j}, b^a_j, b_j^g$
• 两个时刻中间的预积分量
  $\alpha_{b_ib_j}, q_{b_ib_j}, \beta_{b_ib_j}$

3.4 预积分量方差的计算

3.3.1 为什么要计算预计分量方差？

有了IMU预积分就可以计算两个图像帧之间的IMU积分量得到五个状态量的值$PVQ,b_a, b_g$。虽然得到了$j$时刻的IMU的状态量，为了构建IMU的误差项，还需要针对我们这五个状态量的协方差。有了协方差才能很好的确定我当前的状态量的估计好坏程度。

3.3.2 怎么计算？

• 基于一阶泰勒展开的误差递推方程
• 基于误差随时间变化的递推方程

实际上这两种推导方式是可以相互转换的，第一种方式比较直观，那为什么还要有第二种方式呢？因为在很多系统中，比如我们的VIO系统，我们已经知道了状态的导数和状态之间的转移矩阵。第二种才是本质！！

确定误差传递的状态量，噪声量，然后开始构建传递方程。

4 残差 Jacobian 的推导

视觉重投影残差的 Jacobian
IMU 预积分残差的雅克比