本篇是我在学习 VINS-Mono 时对其工作流程的一个总结。VINS-Mono 的功能模块可包括五个部分：数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线程，分别是：前端图像跟踪、后端非线性优化 (其中初始化和 IMU 预积分在这个线程中) 、闭环检测、闭环优化。本人才学疏浅，有理解不深和错误的地方，请多多指教！

VINS-Mono 工作流程

参考：

• vins-mono 原始论文：

  VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator, Tong Qin, Peiliang Li, Zhenfei Yang, Shaojie Shen, IEEE Transactions on Robotics pdf

• 沈劭劼老师于 2017 年在泡泡机器人上讲的公开课的 ppt

• 崔华坤老师整理的 VINS-Mono 公式推导

系统总览

VINS-Mono 的功能模块可包括五个部分：数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线程，分别是：前端图像跟踪、后端非线性优化 (其中初始化和 IMU 预积分在这个线程中) 、闭环检测、闭环优化。

各模块的主要功能：

1. 图像和 IMU 预处理

   • 图像：提取图像 Harris 角点，利用金字塔光流跟踪相邻帧，通过 RANSAC 去除异常点，最后将跟踪到的特征点 push 到图像队列中，并通知后端进行处理
   • IMU: 将 IMU 数据进行积分，得到当前时刻的位置、速度和旋转 (PVQ)，同时计算在后端优化中将用到的相邻帧的预积分增量，及预积分误差的 Jacobian 矩阵和协方差项

2. 初始化

   首先，利用 SFM 进行纯视觉估计滑窗内所有帧的位姿及 3D 点逆深度

   最后与 IMU 预积分进行 Align 求解初始化参数

   初始化这一步非常非常重要，单目视觉中很多的东西比如 imu 的 bias、尺度等信息都是无法直接观测出来的，都是需要算出来的。整个系统是一个非常非线性化的系统，非线性系统若收敛肯定是要初始值的，特别是这种对状态都无法直接观测的情况下。vins-mono 花了比较大的精力对初始值进行处理。

   

3. Local 滑窗优化

   将视觉约束、IMU 约束和闭环约束放在一个大的目标函数中进行非线性优化，求解滑窗内所有帧的 PVQ、bias 等

   初始化之后，就进入了一个基于非线性优化的，滑动窗口 Visual-Inertial Bundle Adjustment。将视觉约束、IMU 约束和闭环约束放在一个大的目标函数中进行非线性优化，求解滑窗内所有帧的 PVQ、bias 等。

   基于 Graph 的形式给表示出来，并且维护一个滑动窗口来保证算 VI Odometry 时的运算复杂度保持在一个固定的状态下。

   绿色的帧就是回环检测的到的，跟当前的滑动窗口里的已知位置的蓝色帧有共同观测，然后就能根据这些已知位置的帧做一个重定位。vins-mono 里的重定位也是一个 紧耦合 的形式，即回环检测的帧和当前的帧之间的位置信息是通过他们互相看到一套同样的特征点的一个互匹配得到的。并且 Relocalization 后，与后面的 Global Pose Graph 在某种程度上也牵扯在一起。

   

4. 闭环检测

   利用 DBoW2 进行闭环检测

5. 闭环优化 - 全局 Pose Graph 的优化

   后端的一个大规模 Pose Graph 优化，保证全局一致性。

   在不断地运行中，会把 keyframe 给保存下来，然后通过不断地回环检测检测到了一些特征点匹配，把这些匹配给通过一个 Pose Graph 的优化把它们的误差给消除掉。也就相当于是前面那个 sliding window 是一直可以基于这个 Pose Graph 做一个重定位的操作的。

   

输入处理

图像前端

• 特征提取和跟踪采用 Harris corners 和 KLT tracker

• 在图像的连续帧之间进行 track

• RANSAC 对外点进行第一次的剔除

• 采用了 Unified camera model 这样可以用于多种类型相机比如大 FOV 的鱼眼相机

  

对于关键帧的提取，满足下面两种情况即被选为关键帧：

• 旋转补偿的、达到一定视差的帧
• 跟踪的特征点的数量低于一定值时要提取新的特征，则这一帧即也被选为特征点

IMU 预积分

利用 IMU 数据的时候，希望在积分 IMU 测量值的时候，不依赖于 IMU 在世界坐标系下的位姿信息。其实跳出来想，我们其实想要知道的，直接是两次相机观测时二者的一个相对的旋转就可以了。所以可以以两次的积分点（相机的两次观测）的前者为基础坐标系，算出来后者在前者坐标系下的相对 PVQ 就可以了。

示意图如下：

具体的中间过程公式就比较 formulation 了。具体推导过程见文末。

系统初始化

Very, very, very important! 如果初始化得不好，整个系统尤其是尺度，最后是无法收敛到一个正确的值的。

初始化采用视觉和 IMU 的松耦合方案，首先用 SFM 求解滑窗内所有帧的位姿，和所有路标点的 3D 位置，然后跟 IMU 预积分的值对齐，求解重力方向、尺度因子、陀螺仪 bias 及每一帧对应的速度。

Pipline:

1. Monocular vision-only SfM，得到一个 up-to-scale 的平移信息和一个没有跟重力对齐、但是相对比较准确的旋转信息
2. 相机和 IMU 之间的 rotation 标定（TODO 外参不标平移？）
3. Visual-inertial alignment。预积分得到的那个 $\alpha$ 和 $\beta$ 和相机 sfm 得到的 translation 对起来，这样就有尺度了

Monocular vision-only SfM

• 1 秒，一个很小的窗口，10 帧
• Up-to-scale，locally drift-free 的 position estimates
• 有帧之间的旋转，但是没有和重力对齐

Camera-IMU rotation calibration

这个旋转对整个系统的精确度是非常重要的，若差了那么一点整个系统可能都崩了。但从另一方面来说，这个值就很容易被激化，相当于就是容易被标定出来。

标定过程就是一个经典的手眼标定问题。（TODO）先得出 IMU 之间的旋转增量 $R_{b_1}^{b_0}$，$R_{b_2}^{b_1}$，$R_{b_3}^{b_2}...$，Camera 之间的旋转增量 $R_{c_1}^{c_0}$，$R_{c_2}^{c_1}$，$R_{c_3}^{c_2}...$，然后找一个中间的旋转，让两者的 sequence 对起来。这个就由手眼标定模型来求解，有线性的方法可以直接解出来，速度还比较快。

Visual-inertial alignment

经过上面两个步骤后，现在系统里相机和 IMU 的旋转就知道了，目前还有系统的初速度、重力向量和尺度都是未知的。即待优化变量：$\mathcal {X}_{I}=\left [\mathbf {v}_{b_{0}}^{c_{0}}, \mathbf {v}_{b_{1}}^{c_{0}}, \cdots \mathbf {v}_{b_{n}}^{c_{0}}, \mathbf {g}^{c_{0}}, s\right]$

加上 IMU 的好处就是可以得到一个较精确的速度信息，在很多系统里，比如无人机的控制中，速度是非常重要的。此外，对于重力向量，其实就是相当于能得到一个系统的 roll 和 pitch 这两个姿态角，有了重力向量后，我们就可以将上面标定好的系统坐标系与重力对齐。尺度信息可以利用 imu 预积分的值，可以把尺度拉到一个对的情况下。整个形式的示意图如下：

求解这个 alignment 的问题：

这个初始化仍存在的问题，但是影响不大：

• IMU 的 bias 未被初始化

  其中陀螺仪的倒还好说，可从静止的 measurement 中观测出来

  加速度计就比较棘手了

• 在高海拔场景，由于视差变化比较小，imu 的积分时间变得很长，这时候就不 OK 了。解决办法：用微分来替代积分。沈老师的 2017 ICRA 的另一篇论文中已经提到解决了。

Local BA with Relocalization

这个的全称比较长，但也充分体现了这个 Local BA 的特性：

Nonlinear graph optimization-based, tightly-coupled, sliding window, visual-inertial bundle adjustment

这个 BA 的所有估计量：

相机的位置、速度、旋转、IMU 的 bias、特征点的逆深度、相机 - IMU 的外参

$\begin {aligned} \mathcal {X} &=\left [\mathrm {x}_{0}, \mathrm {x}_{1}, \cdots \mathrm {x}_{n}, \mathrm {x}_{c}^{b}, \lambda_{0}, \lambda_{1}, \cdots \lambda_{m}\right] \\ \mathrm {x}_{k} &=\left [\mathrm {p}_{b_{k}}^{w}, \mathrm {v}_{b_{k}}^{w}, \mathrm {q}_{b_{k}}^{w}, \mathrm {b}_{a}, \mathrm {b}_{g}\right], k \in [0, n] \\ \mathrm {x}_{c}^{b} &=\left [\mathrm {p}_{c}^{b}, \mathrm {q}_{c}^{b}\right] \end {aligned}$

这个时候构建优化函数：

$\begin {aligned} \min _{\mathcal {X}}\left\{\left\|\mathbf {r}_{p}-\mathbf {H}_{p} \mathcal {X}\right\|^{2}+\sum_{k \in \mathcal {B}}\left\|\mathbf {r}_{\mathcal {B}}\left (\hat {\mathbf {z}}_{b_{k+1}}^{b_{k}}, \mathcal {X}\right)\right\|_{\mathbf {P}_{b_{k+1}}^{b_{k}}}^{2}\right.&+\\ &\left.\sum_{(l, j) \in \mathcal {C}} \rho\left (\left\|\mathbf {r}_{\mathcal {C}}\left (\hat {\mathbf {z}}_{l}^{c_{j}}, \mathcal {X}\right)\right\|_{\mathbf {P}_{l}^{c_{j}}}^{2}\right)\right\} \end {aligned}$

其中每一项的作用：

第一项是滑窗边缘化操作的误差；

边缘化操作的示意图：

把滑窗内的最老帧丢出滑窗但仍要保存它对系统的约束和影响，采用舒尔补得到一个类似于先验概率的项。（具体见边缘化操作的文档）

第二项是 IMU 测量值（预积分的误差）;

第三项是视觉的重投影误差

实现过程中采用 Ceres Solver 进行了求解。

关于求解的质量的定性讨论：

• 求解过程中的数值稳定性的问题依然存在，比纯 vSLAM 更严重。

  • 在手持和空中机器人上表现较好
  • 纯在地面 2D 移动的地面机器人上表现较差
  • 匀速运动和纯旋转会 hold 不住

• 远距离场景中性能下降（Downgraded performance in distanced scenes）

整个 Local BA 只能运行在 10Hz 左右，在大多数场景中均不够用，尤其是在无人机控制这种应用中。

所以要进行 speed up：

• motion-only visual-inertial bundle 将系统帧率提升到 30Hz

• IMU forward propagation 来把帧率提到 100Hz

  

回环

Loop closure 采用的就是 DBoW2。

Feature 的检索：

• Try to retrieve matches for features (200) that are used in the VIO
• BRIEF descriptor match
• Geometric check
  • Fundamental matrix test with RANSAC
  • At least 30 inliers

输出：

• Loop closure frames with known pose
• Feature matches between VIO frames and loop closure frames

下面图中的绿色的帧就是 Loop closure frames with constant pose，找出与当前帧 match 的 feature。

然后 Relocalization 做了个紧耦合的优化；相当于是在前面 Local BA 的优化函数中加了一最后一项。

Global Pose Graph Optimization

为了保证全局一致性，全局当然需要做一个 Global 位姿图的优化。

后端优化的 pose graph 是一个 4-DOF 的 pose graph。因为俯仰和滚转角通过重力向量的观测即可得到。

优化的这个残差：

完整过程看这个 9 个图即可了（前面也放了）：

附：IMU 预积分详细推导

问题提出

IMU 的好处：IMU 加在传统的 V-SLAM 系统中后，除了更获取尺度信息以外，还有很多更多的很好的 “副作用”，比如可以很好的增强系统的鲁棒性，特别是在快速旋转等场景，视觉挂掉了，IMU 还可以撑一会儿。这些东西都是在一个统一的传感器融合框架中进行的。

首先，对 IMU 的测量值进行建模：

$\begin {aligned} \hat {\mathbf {a}}_{t} &=\mathbf {a}_{t}+\mathbf {b}_{a_{t}}+\mathbf {R}_{w}^{t} \mathbf {g}^{w}+\mathbf {n}_{a} \\ \hat {\mathbf {\omega}}_{t} &={\mathbf {\omega}}_{t}+\mathbf {b}_{\omega_{t}}+\mathbf {n}_{\omega} \end {aligned}$

（1）

其中 $\mathbf {n}_{a}$ 和 $\mathbf {n}_{w}$ 是白噪声，服从高斯分布 $\mathbf {n}_{a} \sim \mathcal {N}\left (\mathbf {0}, \boldsymbol {\sigma}_{a}^{2}\right), \mathbf {n}_{\omega} \sim \mathcal {N}\left (\mathbf {0}, \boldsymbol {\sigma}_{\omega}^{2}\right)$；$\mathbf {b}_{a}$ 和 $\mathbf {b}_{w}$ 分别是加速度计的 bias，服从 $\dot {\mathbf {b}}_{a_{t}}=\mathbf {n}_{b_{a}}, \quad \dot {\mathbf {b}}_{w_{t}}=\mathbf {n}_{b_{w}}$。

在一般的系统中，IMU 的测量频率要比视觉频率高好多，IMU 状态太多所以不可能去估计它的每一个状态。所以考虑到 积分 integration 的方式，使得 IMU 和视觉达到一个相同的频率。积分的示意图如下：

IMU 的积分在传统的惯导体系中，一般是在 世界坐标系 中进行积分的，在第 k 帧到第 k+1 帧的过程中，PVQ（位置、速度和旋转）积分公式如下：

$\begin {aligned} \mathbf {p}_{b_{k+1}}^{w}=\mathbf {p}_{b_{k}}^{w} &+\mathbf {v}_{b_{k}}^{w} \Delta t_{k} \\ &+\iint_{t \in\left [t_{k}, t_{k+1}\right]}\left (\mathbf {R}_{t}^{w}\left (\hat {\mathbf {a}}_{t}-\mathbf {b}_{a_{t}}-\mathbf {n}_{a}\right)-\mathbf {g}^{w}\right) d t^{2} \end {aligned}$

$\begin {array}{l}{\mathbf {v}_{b_{k+1}}^{w}=\mathbf {v}_{b_{k}}^{w}+\int_{t \in\left [t_{k}, t_{k+1}\right]}\left (\mathbf {R}_{t}^{w}\left (\hat {\mathbf {a}}_{t}-\mathbf {b}_{a_{t}}-\mathbf {n}_{a}\right)-\mathbf {g}^{w}\right) d t} \\ {\mathbf {q}_{b_{k+1}}^{w}=\mathbf {q}_{b_{k}}^{w} \otimes \int_{t \in\left [t_{k}, t_{k+1}\right]} \frac {1}{2} \mathbf {\Omega}\left (\hat {\omega}_{t}-\mathbf {b}_{w_{t}}-\mathbf {n}_{w}\right) \mathbf {q}_{t}^{b_{k}} d t}\end {array}$

（2）

其中

$\Omega (\omega)=\left [\begin {array}{cc}{-\lfloor\omega\rfloor_{\times}} & {\omega} \\ {-\omega^{T}} & {0}\end {array}\right],\lfloor\omega\rfloor_{\times}=\left [\begin {array}{ccc}{0} & {-\omega_{z}} & {\omega_{y}} \\ {\omega_{z}} & {0} & {-\omega_{x}} \\ {-\omega_{y}} & {\omega_{x}} & {0}\end {array}\right]$

预积分模型目前的 Community 在提出了不止一种，这里就只放 vins-mono 中用到的。

上面的公式是论文中贴出的 IMU 在世界坐标系下的积分形式。下面的公式是沈老师公开课 PPT 中的提到公式。这里上下只是公式的表述不一样，本质是一样的。前两个式子，论文中的更加严谨，ppt 里的表述则把白噪声给去掉了；关于旋转 q 的式子，上面论文版的公式直接将 q 在机体坐标系下进行计算了，ppt 里给出的是 q 在世界坐标系下的旋转。具体关于角速度和四元数 q 的关系、q 的微分公式，包括上面的那个 $\Omega$ 的式子，可以在秦永元老师的《惯性导航（第一版）》p299、300 页有具体推导。

$\begin {aligned} \mathbf {p}_{b_{k+1}}^{w}=\mathbf {p}_{b_{k}}^{w} &+\mathbf {v}_{b_{k}}^{w} \Delta t_{k} \\ &+\iint_{t \in [k, k+1]}\left (\mathbf {R}_{t}^{w}\left (\hat {\mathbf {a}}_{t}-\mathbf {b}_{a_{t}}\right)-\mathbf {g}^{w}\right) d t^{2} \end {aligned}$

$\mathbf {v}_{b_{k+1}}^{w}=\mathbf {v}_{b_{k}}^{w}+\int_{t \in [k, k+1]}\left (\mathbf {R}_{t}^{w}\left (\hat {\mathbf {a}}_{t}-\mathbf {b}_{a_{t}}\right)-\mathbf {g}^{w}\right) d t$

$\mathbf {q}_{b_{k+1}}^{w}=\mathbf {q}_{b_{k}}^{w} \otimes \int_{t \in [k, k+1]} \mathbf {q}_{t}^{w} \otimes\left [\begin {array}{c}{0} \\ {\frac {1}{2}\left (\hat {\omega}_{t}-\mathbf {b}_{w_{t}}\right)}\end {array}\right] d t$

这里的问题就出来了，IMU 的积分公式需要用到 $\mathbf {R}_{t}^{w}$（根据沈老师 ppt，还包括 $\mathbf {q}_{t}^{w}$，但 $\mathbf {q}_{t}^{w}$ 可以直接推导出，而不需要预积分公式，即论文中的公式），即 body 的姿态与世界坐标系之间的转换关系！关于这个旋转矩阵存在导致的问题，沈老师公开课和论文中讲到了两种说法（其实都挺有道理的，但论文中的说法 更能解释为什么叫做 “预” 积分）：

• 沈老师公开课中的解释看起来更加的简单：

  本来 IMU 积分就是为了求出机体在世界坐标系下的姿态的，但积分又需要知道其在世界坐标系下的姿态，这就好像一个 “鸡生蛋蛋生鸡” 的问题

• 论文中给出的说法：

  IMU 的积分需要依赖与第 k 帧的机体与世界坐标系的 $\mathbf {v}_{b_{k}}^{w}$ 和 $\mathbf {R}_{t}^{w}$，当我们在后端进行非线性优化时，需要迭代更新第 k 帧的 $\mathbf {v}_{b_{k}}^{w}$ 和 $\mathbf {R}_{t}^{w}$，这将导致我们需要根据每次迭代后值重新进行积分工作，将非常耗时。

  论文原话：

  It can be seen that the IMU state propagation requires rotation, position and velocity of frame bk . When these starting states change, we need to re-propagate IMU measurements. Especially in the optimization-based algorithm, every time we adjust poses, we will need to re-propagate IMU measurements between them. This propagation strategy is computationally demanding. To avoid re-propagation, we adopt pre-integration algorithm.

解决办法 - IMU 预积分

预积分的测量值

所以这个时候需要解决的问题就是，希望在积分 IMU 测量值的时候，不依赖于 IMU 在世界坐标系下的位姿信息。其实跳出来想，我们其实想要知道的，直接是两次相机观测时二者的一个相对的旋转就可以了。所以可以以两次的积分点（相机的两次观测）的前者为基础坐标系，算出来后者在前者坐标系下的相对 PVQ 就可以了。示意图如下：

然后就正式开始预积分公式的推导了，若要做一个上述这样的坐标转换，其实也很容易，只用在（2）式的三个公式均左乘一个世界坐标系到机体的旋转即可。在第 k 帧到第 k+1 帧的过程中，PVQ 公式如下：

$\begin {aligned} \mathbf {R}_{w}^{b_{k}} \mathbf {p}_{b_{k+1}}^{w} &=\mathbf {R}_{w}^{b_{k}}\left (\mathbf {p}_{b_{k}}^{w}+\mathbf {v}_{b_{k}}^{w} \Delta t_{k}-\frac {1}{2} \mathbf {g}^{w} \Delta t_{k}^{2}\right)+\boldsymbol {\alpha}_{b_{k+1}}^{b_{k}} \\ \mathbf {R}_{w}^{b_{k}} \mathbf {v}_{b_{k+1}}^{w} &=\mathbf {R}_{w}^{b_{k}}\left (\mathbf {v}_{b_{k}}^{w}-\mathbf {g}^{w} \Delta t_{k}\right)+\boldsymbol {\beta}_{b_{k+1}}^{b_{k}} \\ \mathbf {q}_{w}^{b_{k}} \otimes \mathbf {q}_{b_{k+1}}^{w} &=\gamma_{b_{k+1}}^{b_{k}} \end {aligned}$

（3）

其中，

$\begin {array}{l}{\boldsymbol {\alpha}_{b_{k+1}}^{b_{k}}=\iint_{t \in\left [t_{k}, t_{k+1}\right]} \mathbf {R}_{b_t}^{b_{k}}\left (\hat {\mathbf {a}}_{t}-\mathbf {b}_{a_{t}}-\mathbf {n}_{a}\right) d t^{2}} \\ {\boldsymbol {\beta}_{b_{k+1}}^{b_{k}}=\int_{t \in\left [t_{k}, t_{k+1}\right]} \mathbf {R}_{b_t}^{b_{k}}\left (\hat {\mathbf {a}}_{t}-\mathbf {b}_{a_{t}}-\mathbf {n}_{a}\right) d t} \\ {\gamma_{b_{k+1}}^{b_{k}}=\int_{t \in\left [t_{k}, t_{k+1}\right]} \frac {1}{2} \boldsymbol {\Omega}\left (\hat {\boldsymbol {\omega}}_{t}-\mathbf {b}_{w_{t}}-\mathbf {n}_{w}\right) \gamma_{t}^{b_{k}} d t}\end {array}$

（4）

$\boldsymbol {\gamma}_{b_{k+1}}^{b_{k}}$ 又可写作（四元数形式）：

$\gamma_{b_{k+1}}^{b_{k}}=\int_{t \in [k, k+1]} \gamma_{b_{t}}^{b_{k}} \otimes\left [\begin {array}{cc}{0} \\ {\frac {1}{2}\left (\hat {\omega}_{t}-\mathbf {b}_{w_{t}}\right)}\end {array}\right] d t$

（5）

这样一做，可以看出，这些预积分公式中的项均可以在选择在以第 k 帧为参考帧的前提下，被单独观测出来。看公式可以看出来，$\boldsymbol {\alpha}_{b_{k+1}}^{b_{k}}, \boldsymbol {\beta}_{b_{k+1}}^{b_{k}}$ 仅仅依赖于 t 时刻时的帧与 $b_k$ 即参考帧之间的旋转 $\mathbf {R}_{b_t}^{b_{k}}$，而这个旋转是可以通过陀螺仪测出的角速度来积分（也就是 $\boldsymbol {\gamma}_{b_{k+1}}^{b_{k}}$）得到的。

积分项 $\boldsymbol {\alpha}_{b_{k+1}}^{b_{k}}, \boldsymbol {\beta}_{b_{k+1}}^{b_{k}}, \boldsymbol {\gamma}_{b_{k+1}}^{b_{k}}$ 在积分的时候依赖了需要优化的变量：imu 的 bias。这将导致的问题是，当每次迭代时，我们得到一个新的 bias，又得根据公式 (4) 重新对第 k 帧和第 k+1 帧之间的 IMU 预积分，非常耗时。论文中提到，若关于 bias 的估计变化时，若变化不大，就仅仅通过 $\boldsymbol {\alpha}_{b_{k+1}}^{b_{k}}, \boldsymbol {\beta}_{b_{k+1}}^{b_{k}}, \boldsymbol {\gamma}_{b_{k+1}}^{b_{k}}$ 的一阶近似值来调整它们，否则就重新做一次 re-propagation，这样做在 optimization-based 算法中可以节省很大的计算资源。

通过一阶近似值来调整它们的公式：

$\begin {aligned} \alpha_{b_{k+1}}^{b_{k}} & \approx \alpha_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\alpha} \delta b_{a}+J_{b_{\omega}}^{\alpha} \delta b_{\omega} \\ \beta_{b_{k+1}}^{b_{k}} & \approx \hat {\beta}_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\beta} \delta b_{a}+J_{b_{\omega}}^{\beta} \delta b_{\omega} \\ \gamma_{b_{k+1}}^{b_{k}} & \approx \hat {\gamma}_{b_{k+1}}^{b_{k}} \otimes\left [\frac {1}{2} J_{b_{\omega}}^{\gamma} \delta b_{\omega}\right] \end {aligned}$

这样前面那个鸡生蛋蛋生鸡的问题也就被解决掉了。相当于也就是把全局旋转的东西挪到外面去了，这样 IMU 就可以在不知道全局旋转的情况下进行积分。从直觉上来看，IMU 预积分的 $\boldsymbol {\alpha}_{b_{k+1}}^{b_{k}}, \boldsymbol {\beta}_{b_{k+1}}^{b_{k}}$ 也就相当于是速度和位置的变化量在一个自由落体的坐标系里面的值。

上面还只是连续形式的积分，所以还需要将连续的积分形式进行离散化，欧拉法、中值法、龙格 - 库塔（RK4）等多种方法均可以（三者的具体形式 链接）。论文中贴的公式是用的形式最简单的欧拉法进行离散化的，具体代码在实现的时候采用的是中值法，并且将白噪声项 $\mathbf {n}_{a}$ 和 $\mathbf {n}_{w}$ 假设为 0。

• 欧拉法进行离散：

  $\begin {array}{l}{\hat {\boldsymbol {\alpha}}_{i+1}^{b_{k}}=\hat {\boldsymbol {\alpha}}_{i}^{b_{k}}+\hat {\boldsymbol {\beta}}_{i}^{b_{k}} \delta t+\frac {1}{2} \mathbf {R}\left (\hat {\gamma}_{i}^{b_{k}}\right)\left (\hat {\mathbf {a}}_{i}-\mathbf {b}_{a_{i}}\right) \delta t^{2}} \\ {\hat {\boldsymbol {\beta}}_{i+1}^{b_{k}}=\hat {\boldsymbol {\beta}}_{i}^{b_{k}}+\mathbf {R}\left (\hat {\gamma}_{i}^{b_{k}}\right)\left (\hat {\mathbf {a}}_{i}-\mathbf {b}_{a_{i}}\right) \delta t} \\ {\hat {\gamma}_{i+1}^{b_{k}}=\hat {\gamma}_{i}^{b_{k}} \otimes\left [\begin {array}{c}{1} \\ {\frac {1}{2}\left (\hat {\boldsymbol {\omega}}_{i}-\mathbf {b}_{w_{i}}\right) \delta t}\end {array}\right]}\end {array}$

  （6）

• 中值法进行离散：

  $\begin {aligned} \hat {\alpha}_{i+1}^{b_{k}} &=\hat {\alpha}_{i}^{b_{k}}+\hat {\beta}_{i}^{b_{k}} \delta t+\frac {1}{2} \overline {\hat {a}}_{i}^{\prime} \delta t^{2} \\ \hat {\beta}_{i+1}^{b_{k}} &=\hat {\beta}_{i}^{b_{k}}+\overline {\hat {a}}_{i}^{\prime} \delta t \\ \hat {\gamma}_{i+1}^{b_{k}} &=\hat {\gamma}_{i}^{b_{k}} \otimes \hat {\gamma}_{i+1}^{i}=\hat {\gamma}_{i}^{b_{k}} \otimes\left [\frac {1}{2} \overline {\widehat {\omega}}_{i}^{\prime} \delta t\right] \end {aligned}$

  （7）

  其中：

  $\overline {\hat {a}}_{i}^{\prime}=\frac {1}{2}\left [q_{i}\left (\hat {a}_{i}-b_{a_{i}}\right)+q_{i+1}\left (\hat {a}_{i+1}-b_{a_{i}}\right)\right]$

  $\overline {\widehat {\omega}}_{i}^{\prime}=\frac {1}{2}\left (\widehat {\omega}_{i}+\widehat {\omega}_{i+1}\right)-b_{\omega_{i}}$

  （8）

  这一块对应代码 vins_estimator/src/factor/integration_base.h 文件的 63-69 行：

  //ROS_INFO ("midpoint integration");
  Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
  Vector3d un_gyr = 0.5 * (_gyr_0 +_gyr_1) - linearized_bg;
  result_delta_q = delta_q *Quaterniond (1, un_gyr (0)* _dt / 2, un_gyr (1) *_dt / 2, un_gyr (2)* _dt / 2);
  Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
  Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
  result_delta_p = delta_p + delta_v *_dt + 0.5* un_acc *_dt*_dt;
  result_delta_v = delta_v + un_acc * _dt;

至此，IMU 预积分的测量值就得到了。

预积分的更新

有了测量值后，测量值的误差传播肯定也是要计算的。即 PVQ 增量的误差、协方差和 Jacobian。

首先定义 $\boldsymbol {\gamma}_{b_{k+1}}^{b_{k}}$ 的误差项，是在 mean 附近加一个三维的小扰动 $\delta \boldsymbol {\theta}_{t}^{b_{k}}$：

$\gamma_{t}^{b_{k}} \approx \hat {\gamma}_{t}^{b_{k}} \otimes\left [\begin {array}{c}{1} \\ {\frac {1}{2} \delta \theta_{t}^{b_{k}}}\end {array}\right]$

（9）

在 t 时刻，误差项的导数为：

$\left [\begin {array}{c}{\delta \dot {\boldsymbol {\alpha}}_t^{b_{k}}} \\ {\delta \dot {\boldsymbol {\beta}}_{t}^{b_{k}}} \\ {\delta \dot {\boldsymbol {\theta}}_{t}^{b_{k}}} \\ {\delta \dot {\mathbf {b}}_{a_{t}}} \\ {\delta \dot {\mathbf {b}}_{w_{t}}}\end {array}\right]=\left [\begin {array}{ccccc}{0} & {\mathbf {I}} & {0} & {0} & {0} \\ {0} & {0} & {-\mathbf {R}_{t}^{b_{k}}\left\lfloor\hat {\mathbf {a}}_{t}-\mathbf {b}_{a_{t}}\right\rfloor_{\times}} & {-\mathbf {R}_{t}^{b_{k}}} & {0} \\ {0} & {0} & {-\left\lfloor\hat {\omega}_{t}-\mathbf {b}_{w_{t}}\right\rfloor_{\times}} & {0} & {-\mathbf {I}} \\ {0} & {0} & {0} & {0} & {0} \\ {0} & {0} & {0} & {0} & {0}\end {array}\right]\left [\begin {array}{c}{\delta \boldsymbol {\alpha}_{t}^{b_{k}}} \\ {\delta \boldsymbol {\beta}_{t}^{b_{k}}} \\ {\delta \boldsymbol {\theta}_{a_{t}}^{b_{k}}} \\ {\delta \mathbf {b}_{a_{t}}} \\ {\delta \mathbf {b}_{w_{t}}}\end {array}\right]$

$+\left [\begin {array}{cccc}{0} & {0} & {0} & {0} \\ {-\mathbf {R}_{t}^{b_{k}}} & {0} & {0} & {0} \\ {0} & {-\mathbf {I}} & {0} & {0} \\ {0} & {0} & {\mathbf {I}} & {0} \\ {0} & {0} & {0} & {\mathbf {I}}\end {array}\right]\left [\begin {array}{l}{\mathbf {n}_{a}} \\ {\mathbf {n}_{w}} \\ {\mathbf {n}_{b_{a}}} \\ {\mathbf {n}_{b_{w}}}\end {array}\right]=\mathbf {F}_{t} \delta \mathbf {z}_{t}^{b_{k}}+\mathbf {G}_{t} \mathbf {n}_{t}$

（10）

可以先简写为

$\delta \dot {z}_{t}^{b_{k}}=F_{t} \delta z_{t}^{b_{k}}+G_{t} n_{t}$

所以可根据导数的定义：

$\delta \dot {z}_{t}^{b_{k}}=\lim _{\delta t \rightarrow 0} \frac {\delta z_{t+\delta t}^{b_{k}}-\delta z_{t}^{b_{k}}}{\delta t}$

$\begin {aligned} \delta z_{t+\delta t}^{b_{k}} &=\delta z_{t}^{b_{k}}+\delta \dot {z}_{t}^{b_{k}} \delta t=\left (\mathbf {I}+\mathbf {F_{t}} \delta t\right) \delta z_{t}^{b_{k}}+\left (\mathbf {G_{t}} \delta t\right) n_{t} \\ &=\mathbf {F} \delta z_{t}^{b_{k}}+\mathbf {V} n_{t} \end {aligned}$

（11）

这个公式，看起来和 EKF 中对非线性系统进行线性化的过程一样。这里的 $\delta z_{t+\delta t}^{b_{k}}$ 的公式，表示下一个时刻的 IMU 测量量是和上一个时刻呈线性关系的。这样，就可以根据当前时刻的值来预测出下一个时刻的均值和方差。公式 (11) 给出了均值的预测，协方差的预测：

$\mathbf {P_{t+\delta t}^{b_{k}}=\left (I+F_{t} \delta t\right) P_{t}^{b_{k}}\left (I+F_{t} \delta t\right)^{T}+\left (G_{t} \delta t\right) Q\left (G_{t} \delta t\right)^{T}}$

（12）

公式（12）即协方差的迭代公式，初始值论文中提到可以设为 $$\mathbf {P}{b{k}}^{b_{k}}=0$$，$\mathbf {Q}$ 为四个高斯噪声的协方差组成的对角阵：

$\mathbf {Q}^{12 \times 12}=\left [\begin {array}{cccc}{\sigma_{a}^{2}} & {0} & {0} & {0} \\ {0} & {\sigma_{w}^{2}} & {0} & {0} \\ {0} & {0} & {\sigma_{b_{a}}^{2}} & {0} \\ {0} & {0} & {0} & {\sigma_{b_{w}}^{2}}\end {array}\right]$

根据（11）式还可求出误差项的 Jacobian 迭代公式：

$\mathbf {J_{t+\delta t}}=\left (\mathbf {I}+\mathbf {F_{t}} \delta t\right) \mathbf {J_{t}}$

（13）

其中 Jacobian 的初值为 $\mathbf {J_{b_k}} = \mathbf {I}$

所以可根据这个大雅克比的 bias 对应的子块更新 bias，当 bias 大更新导致需要重新预积分的时候，可以用下面的一阶展开公式（上面已提到过，更新策略上文已提到）：

$\begin {aligned} \alpha_{b_{k+1}}^{b_{k}} & \approx \alpha_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\alpha} \delta b_{a_k}+J_{b_{\omega}}^{\alpha} \delta b_{\omega_k} \\ \beta_{b_{k+1}}^{b_{k}} & \approx \hat {\beta}_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\beta} \delta b_{a_k}+J_{b_{\omega}}^{\beta} \delta b_{\omega_k} \\ \gamma_{b_{k+1}}^{b_{k}} & \approx \hat {\gamma}_{b_{k+1}}^{b_{k}} \otimes\left [\frac {1}{2} J_{b_{\omega}}^{\gamma} \delta b_{\omega_k}\right] \end {aligned}$

（14）

所以这个时候就可以写出完整的 IMU measurement model（由（3）式推出），对应上面提到的协方差矩阵和雅克比阵：

$\left [\begin {array}{c}{\hat {\boldsymbol {\alpha}}_{b_{k+1}}^{b_{k}}} \\ {\hat {\boldsymbol {\beta}}_{b_{k+1}}^{b_{k}}} \\ {\hat {\boldsymbol {\gamma}}_{b_{k+1}}^{b_{k+1}}} \\ {\mathbf {0}} \\ {\mathbf {0}}\end {array}\right]=\left [\begin {array}{cc}{\mathbf {R}_{w}^{b_{k}}\left (\mathbf {p}_{b_{k+1}}^{w}-\mathbf {p}_{b_{k}}^{w}+\frac {1}{2} \mathbf {g}^{w} \Delta t_{k}^{2}-\mathbf {v}_{b_{k}}^{w} \Delta t_{k}\right)} \\ {\mathbf {R}_{w}^{b_{k}}\left (\mathbf {v}_{b_{k+1}}^{w}+\mathbf {g}^{w} \Delta t_{k}-\mathbf {v}_{b_{k}}^{w}\right)} \\ {\mathbf {q}_{b_{k}}^{w^{-1}} \otimes \mathbf {q}_{b_{k+1}}^{w}} \\ {\mathbf {b}_{a b_{k+1}}-\mathbf {b}_{a b_{k}}} \\ {\mathbf {b}_{w b_{k+1}}-\mathbf {b}_{w b_{k}}}\end {array}\right]$

（15）

至此完整地描述了通过 IMU 测量的，在当前的 sliding-window 中两帧之间状态的空间和不确定性关系。

但是，上面的式子的形式还均为连续形式下的推导，在代码的实现过程中还需要实现离散化：

代码中仍然使用中值法来进行离散化。公式的具体推导形式较为复杂，为了行文简洁这里就先暂时不贴了，此处直接放推导好的结果（参考《崔华坤 VINS 论文及代码解析》）：

$\left [\begin {array}{c}{\delta \alpha_{k+1}} \\ {\delta \theta_{k+1}} \\ {\delta \beta_{k+1}} \\ {\delta b_{a_{k+1}}} \\ {\delta b_{w_{k+1}}}\end {array}\right]=\left [\begin {array}{ccccc}{I} & {f_{01}} & {\delta t} & {f_{03}} & {f_{04}} \\ {0} & {f_{11}} & {0} & {0} & {-\delta t} \\ {0} & {f_{21}} & {I} & {f_{23}} & {f_{24}} \\ {0} & {0} & {0} & {I} & {0} \\ {0} & {0} & {0} & {0} & {I}\end {array}\right]\left [\begin {array}{c}{\delta \alpha_{k}} \\ {\delta \theta_{k}} \\ {\delta \beta_{k}} \\ {\delta b_{a_{k}}} \\ {\delta b_{w_{k}}}\end {array}\right]$

$+\left [\begin {array}{cccccc}{v_{00}} & {v_{01}} & {v_{02}} & {v_{03}} & {0} & {0} \\ {0} & {\frac {-\delta t}{2}} & {0} & {\frac {-\delta t}{2}} & {0} & {0} \\ {-\frac {R_{k} \delta t}{2}} & {v_{21}} & {-\frac {R_{k+1} \delta t}{2}} & {v_{23}} & {0} & {0} \\ {0} & {0} & {0} & {0} & {\delta t} & {0} \\ {0} & {0} & {0} & {0} & {0} & {\delta t}\end {array}\right]\left [\begin {array}{c}{n_{a_{k}}} \\ {n_{w_{k}}} \\ {n_{a_{k+1}}} \\ {n_{w_{k+1}}} \\ {n_{b_{a}}} \\ {n_{b_{w}}}\end {array}\right]$

（16）

其中：

$f_{01}=\frac {\delta t}{2} f_{21}=-\frac {1}{4} R_{k}\left (\hat {a}_{k}-b_{a_{k}}\right)^{\wedge} \delta t^{2}-\frac {1}{4} R_{k+1}\left (\hat {a}_{k+1}-b_{a_{k}}\right)^{\wedge}\left [I-\left (\frac {\widehat {\omega}_{k}+\widehat {\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta t^{2}$

$\begin {aligned} f_{03}=&-\frac {1}{4}\left (R_{k}+R_{k+1}\right) \delta t^{2} \\ f_{04}=& \frac {\delta t}{2} f_{24}=\frac {1}{4} R_{k+1}\left (\hat {a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{3} \\ f_{11}=& I-\left (\frac {\widehat {\omega}_{k}+\widehat {\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t \end {aligned}$

$\begin {aligned} f_{21}=-\frac {1}{2} R_{k}\left (\hat {a}_{k}-b_{a_{k}}\right)^{\wedge} \delta t-\frac {1}{2} R_{k+1}\left (\hat {a}_{k+1}-b_{a_{k}}\right)^{\wedge}\left [I-\left (\frac {\widehat {\omega}_{k}+\widehat {\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta t \end {aligned}$

$\begin {array}{c}{f_{23}=-\frac {1}{2}\left (R_{k}+R_{k+1}\right) \delta t} \\ {f_{24}=\frac {1}{2} R_{k+1}\left (\hat {a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2}} \\ {v_{00}=-\frac {1}{4} R_{k} \delta t^{2}} \\ {v_{01}=v_{03}=\frac {\delta t}{2} v_{21}=\frac {1}{4} R_{k+1}\left (\hat {a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2} \frac {\delta t}{2}} \\ {v_{02}=-\frac {1}{4} R_{k+1} \delta t^{2}} \\ {v_{21}=v_{23}=\frac {1}{4} R_{k+1}\left (\hat {a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2}}\end {array}$

（17）

将式子（16）简写为 $$\delta z_{k+1}^{15 \times 1}=F^{15 \times 15} \delta z_{k}^{15 \times 1}+V^{15 \times 18} Q^{18 \times 1}$$ 相应的，对应的雅克比的迭代公式：

$J_{k+1}^{15 \times 15}=F J_{k}$

协方差的迭代公式为：

$P_{k+1}^{15 \times 15}=F P_{k} F^{T}+V Q V^{T}$

初始值 $P_k = 0$，$Q$ 为噪声项的对角协方差矩阵：

$Q^{18 \times 18}=\left [\begin {array}{cccccc}{\sigma_{a}^{2}} & {0} & {0} & {0} & {0} & {0} \\ {0} & {\sigma_{w}^{2}} & {0} & {0} & {0} & {0} \\ {0} & {0} & {\sigma_{a}^{2}} & {0} & {0} & {0} \\ {0} & {0} & {0} & {\sigma_{w}^{2}} & {0} & {0} \\ {0} & {0} & {0} & {0} & {\sigma_{b_{a}}^{2}} & {0} \\ {0} & {0} & {0} & {0} & {0} & {\sigma_{b_{w}}^{2}}\end {array}\right]$

这些形式与代码中均与代码 vins_estimator/src/factor/integration_base.h 中 midPointIntegration 函数第 73-126 行是对应上的。（TODO 但是噪声项的前四项相差了一个负号，mark）