imu预积分_VINS-Mono源码解读(二)：IMU预积分

预积分的公式网上有很多推导过程的讲解，这里就不详细介绍了，放出一个连接，供参考：

VINS-Mono理论学习——IMU预积分 Pre-integration （Jacobian 协方差）​blog.csdn.net

程序文件里关于IMU与积分具体实现的文件一共有两个：

• VINS-Mono/vins_estimator/src/factor/imu_factor.h
• VINS-Mono/vins_estimator/src/factor/integration_base.h

后者被前者调用，是一些更具体的函数的封装

下面就放出各个函数代码吧，代码是和公式一一对应的，所以没有太多的流程梳理

1. propagate：IMU预积分传播方程

propagate(

double _dt, //时间间隔

const Eigen::Vector3d &_acc_1, //线加速度

const Eigen::Vector3d &_gyr_1) //角速度

该函数位于integration_base.h文件中，其作用是积分计算两个关键帧之间IMU测量的变化量：

• 旋转delta_q 速度delta_v 位移delta_p
• 加速度的biaslinearized_ba 陀螺仪的Bias linearized_bg
• 同时维护更新预积分的Jacobian和Covariance,计算优化时必要的参数

void 

2. midPointIntegration：中值积分递推Jacobian和Covariance

midPointIntegration(

double _dt, //时间间隔

const Eigen::Vector3d &_acc_0, //上次测量加速度

const Eigen::Vector3d &_gyr_0, //上次测量角速度

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)//是否更新雅克比

该函数位于integration_base.h文件中，其作用是进行中值积分，这样比直接积分更精确

void 

3. repropagate：根据新的bias重新计算预积分

repropagate(

const Eigen::Vector3d &_linearized_ba, //新的加速度偏差

const Eigen::Vector3d &_linearized_bg)//新的角速度偏差

优化过程中Bias会更新，需要根据新的bias重新计算预积分

该函数位于integration_base.h文件中，起作用是根据新的加速度和角速度偏差重新计算预积分的值

void 

4. evaluate：计算残差

evaluate(

const Eigen::Vector3d &Pi, //第i帧位置

const Eigen::Quaterniond &Qi, //第i帧姿态

const Eigen::Vector3d &Vi, //第i帧速度

const Eigen::Vector3d &Bai, //第i帧加速度偏差

const Eigen::Vector3d &Bgi, //第i帧角速度偏差

const Eigen::Vector3d &Pj, //第j帧位置

const Eigen::Quaterniond &Qj, //第j帧姿态

const Eigen::Vector3d &Vj, //第j帧速度

const Eigen::Vector3d &Baj, //第j帧加速度偏差

const Eigen::Vector3d &Bgj)//第j帧角速度偏差

该函数位于integration_base.h文件中，其作用是计算第i帧和第i帧之间的残差

Eigen

5. Evaluate：计算对应的残差和雅克比矩阵

Evaluate(

double const *const *parameters, //parameters[0~3]分别对应了4组优化变量的参数块

double *residuals, //残差

double **jacobians)//雅克比矩阵

该函数位于文件imu_factor.h中

virtual