预处理 | VINS-Mono 论文公式推导与代码解析分讲

概述

Monocular visual-inertial odometry with relocalization achieved via nonlinear graph optimization-based, tightly-coupled, sliding window, visual-inertial bundle adjustment.
代码（注释版）：cggos/vins_mono_cg
1. 测量预处理
1.1 前端视觉处理
Simple feature processing pipeline
自适应直方图均衡化
掩模处理，特征点均匀分布
提取图像Harris角点
KLT金字塔光流跟踪
连续帧跟踪
本质矩阵(RANSAC)去除外点
发布feature_points(id_of_point, un_pts, cur_pts, pts_velocity)

Keyframe selectionCase
1: Rotation-compensated average feature parallax is larger than a thresholdCase
2: Number of tracked features in the current frame is less than a threshold
All frames are used for optimization, but non-keyframes are removed first

1.2 IMU 预积分

IMU 测量方程忽略地球旋转，IMU 测量方程为

预积分方程
（1）IMU integration in world frame
由上面的 IMU 测量方程积分就可以计算出下一时刻的 p、v 和 q ：

（2）IMU integration in the body frame of first pose of interests

为避免重新传播 IMU 观测值，选用 IMU 预积分模型，从世界坐标系转为本体坐标系

则预积分 IMU 测量模型（估计值）为

离散状态下采用中值法积分的预积分方程（预积分测量值）为

其中

midPointIntegration中的相关代码：
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_0 = delta_q * (_acc_0 - linearized_ba);
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;

// 预积分的过程中Bias没有发生改变
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;

误差状态方程IMU
误差状态向量

根据 ESKF 中 5.3.3 The error-state kinematics 小节公式

对于中值积分下的误差状态方程为

简写为

所以

展开得

其中

令

则简写为

此处 F′ 即代码中 F，相关代码见 midPointIntegration。

最后得到 IMU 预积分测量关于 IMU Bias 的 雅克比矩阵、IMU 预积分测量的协方差矩阵 和 噪声的 协方差矩阵 Q，初始状态下的雅克比矩阵和协方差矩阵为单位阵和零矩阵

当 bias 估计轻微改变时，我们可以使用如下的一阶近似对中值积分得到的预积分测量值进行矫正，而不重传播，从而得到更加精确的预积分测量值（ bias 修正的线性模型）

此时，可以与卡尔曼滤波对比一下：