Learning VINS-Mono/Fusion source code from scratch (3): Derivation of IMU pre-integration formula

In this section, we learn about IMU pre-integration, derive the IMU pre-integration formula in discrete time, and interpret the corresponding code.

VINS-Mono/Fusion code learning series:
Learning VINS-Mono/Fusion source code from scratch (1): Main function
learning VINS-Mono/Fusion source code from scratch (2): Front-end image tracking
learning VINS-Mono/Fusion from scratch Source code (3): IMU pre-integration formula derivation and
learning VINS-Mono/Fusion from scratch source code (4): Error Kalman filter
learning VINS-Mono/Fusion from scratch source code (5): VIO initialization
learning VINS- from scratch Mono/Fusion source code (6): back-end optimization

1 Representation method of rotation

• Rotation matrix R
  The rotation matrix R of 3x3 uses 9 quantities to describe 3 degrees of freedom, which introduces additional constraints (orthogonal matrix RR^T=I), making the derivation very difficult.
• Rotation vector $\vec{n}The\; representation\; of\; \theta$
  is compact, but it is periodic and cannot be distinguished beyond 2pi.
• Euler angle
  The default rotation order of Euler angle is ZYX, which corresponds to heading (yaw), pitch (pitch), and roll (roll) respectively; the advantage is that it is intuitive, but there is a universal joint deadlock problem.
• Quaternions
  are compact, have no singularities, but are over-parameterized (using four numbers to represent 3 degrees of freedom, and the requirement is a unit quaternion)
  
  
  

2 Continuous time integration

In the world coordinate system W given in the VINS-Mono paper, the continuous time integral from k to k+1 moment:

Note: In the definition of this formula, the imaginary part comes first and the real part comes last

Quaternion derivation:

3 IMU pre-integration

3.1 Continuous time integration

Transfer the position, velocity and attitude from the world coordinate system w to the bk coordinate system, and multiply both sides by the transfer matrix to obtain the IMU pre-integration term. The IMU pre-integration term is only
related to the IMU output and has nothing to do with the pose. When the pose is optimized, There is no need to re-integrate the IMU.

3.2 Discrete-time median integral

There are multiple imu data between two adjacent image frame times [bk, bk+1]. Calculate the imu pre-integration amount from i to i+1. Then, update the attitude
, speed, and position in sequence.

3.3 Implementation of median integral code

The IMU pre-integration part is implemented in the header file /vins_estimator/src/factor/integration_base.h. Find
the input parameters of 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)

parameter	Attributes
_dt	Timestamp
acc_0, gyr_0	k time accelerometer and gyroscope
acc_1, gyr_1	accelerometer and gyroscope at k+1 time
delta_p, delta_q, delta_v,	k time position, speed, attitude
linearized_ba, linearized_bg	k time accelerometer and gyroscope
result_delta_p, result_delta_q, result_delta_v	Position, speed, attitude at k+1 time
result_linearized_ba, result_linearized_bg	accelerometer and gyroscope at k+1 time
update_jacobian=1	Solving the Jacobian Matrix

The k here are all for imu sampling values, not camera frames

 //预积分更新
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;