从零学习VINS-Mono/Fusion源代码（三）：IMU预积分公式推导

本节学习IMU预积分，推导离散时间下的IMU预积分公式，并解读相应代码。

VINS-Mono/Fusion代码学习系列：
从零学习VINS-Mono/Fusion源代码（一）：主函数
从零学习VINS-Mono/Fusion源代码（二）：前端图像跟踪
从零学习VINS-Mono/Fusion源代码（三）：IMU预积分公式推导
从零学习VINS-Mono/Fusion源代码（四）：误差卡尔曼滤波

1 旋转的表示方法

• 旋转矩阵 R
  3x3的旋转矩阵R用9个量来描述3自由度，引入了额外约束（正交矩阵RR^T=I），求导十分困难
• 旋转向量 $n\theta$
  表示形式紧凑，但是具有周期性，超过2pi就无法区分了
• 欧拉角
  欧拉角默认旋转顺序为Z-Y-X，分别对应航向（yaw），俯仰（pitch），横滚（roll）；优势是直观，但存在万向节死锁问题
• 四元数
  紧凑，没有奇异，但是过参数化（用四个数表示3自由度，要求是单位四元数）
  
  
  

2 连续时间积分

VINS-Mono论文中给出的世界坐标系W下，k到k+1时刻的连续时间积分：

注意：此公式定义中虚部在前，实部在后

四元数求导：

3 IMU预积分

3.1 连续时间积分

将位置速度姿态从世界坐标系w转到bk坐标系下，两边同乘转移矩阵，得到IMU预积分项.
IMU预积分项只与IMU输出有关，与位姿无关，当位姿得到优化后，也不需要重新对IMU做积分.

3.2 离散时间中值积分

相邻两个图像帧时刻[bk, bk+1]之间有多个imu数据，计算i到i+1时刻的imu预积分量.
然后，依次更新姿态、速度、位置.

3.3 中值积分代码实现

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;