本节学习IMU预积分，推导离散时间下的IMU预积分公式，并解读相应代码。
VINS-Mono/Fusion代码学习系列：
从零学习VINS-Mono/Fusion源代码（一）：主函数
从零学习VINS-Mono/Fusion源代码（二）：前端图像跟踪
从零学习VINS-Mono/Fusion源代码（三）：IMU预积分公式推导
从零学习VINS-Mono/Fusion源代码（四）：误差卡尔曼滤波
从零学习VINS-Mono/Fusion源代码（五）：VIO初始化
从零学习VINS-Mono/Fusion源代码（六）：后端优化
VINS-Mono论文中给出的世界坐标系W下，k到k+1时刻的连续时间积分：
注意：此公式定义中虚部在前，实部在后
四元数求导：
将位置速度姿态从世界坐标系w转到bk坐标系下，两边同乘转移矩阵，得到IMU预积分项.
IMU预积分项只与IMU输出有关，与位姿无关，当位姿得到优化后，也不需要重新对IMU做积分.
相邻两个图像帧时刻[bk, bk+1]之间有多个imu数据，计算i到i+1时刻的imu预积分量.
然后，依次更新姿态、速度、位置.
IMU预积分的部分都是在/vins_estimator/src/factor/integration_base.h这个头文件中实现的，找到midPointIntergration()
输入参数：
void midPointIntegration(double _dt, //时间戳
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, //k时刻加计和陀螺输出
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
| 参数 | 属性 |
|---|---|
| _dt | 时间戳 |
| acc_0, gyr_0 | k时刻加速度计和陀螺仪 |
| acc_1, gyr_1 | k+1时刻加速度计和陀螺仪 |
| delta_p, delta_q, delta_v, | k时刻位置、速度、姿态 |
| linearized_ba, linearized_bg | k时刻加速度计和陀螺仪 |
| result_delta_p, result_delta_q, result_delta_v | k+1时刻位置、速度、姿态 |
| result_linearized_ba, result_linearized_bg | k+1时刻加速度计和陀螺仪 |
| update_jacobian=1 | 求解雅克比矩阵 |
这里的k都是针对于imu采样值，不是相机帧
//预积分更新
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;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;