VINS-Mono代码学习记录--IntegrationBase类详解

IntegrationBase类详解

成员变量

IntegrationBase类是VINS-Mono中用于处理IMU预积分相关功能的类，位于vins_estimator/src/factor/integration_base.h头文件中，包含的成员变量如下：

double dt; // 每次预积分的时间周期长度
Eigen::Vector3d acc_0, gyr_0; // t时刻对应的IMU测量值
Eigen::Vector3d acc_1, gyr_1; // t+1时刻对应的IMU测量值

const Eigen::Vector3d linearized_acc, linearized_gyr; // k帧图像时刻对应的IMU测量值
Eigen::Vector3d linearized_ba, linearized_bg; // 加速度计和陀螺仪零偏，在[k,k+1]区间上视为不变

Eigen::Matrix<double, 15, 15> jacobian, covariance; // 预积分误差的雅克比矩阵
Eigen::Matrix<double, 18, 18> noise; //系统噪声矩阵

double sum_dt; //所有IMU预积分区间的总时长，由于量测的不同步性，不一定有sum_dt = (k+1)-k
Eigen::Vector3d delta_p; // 位置预积分
Eigen::Quaterniond delta_q; // 旋转四元数预积分
Eigen::Vector3d delta_v; // 速度预积分

std::vector<double> dt_buf; // 用于存储每次预积分时间dt的寄存器
std::vector<Eigen::Vector3d> acc_buf; // 用于存储每次预积分加速度量测的寄存器
std::vector<Eigen::Vector3d> gyr_buf; // 用于存储每次预积分角速度量测的寄存器

成员函数

构造函数（IntegrationBase）

VINS-Mono要求预积分中的参数必须进行初始化，因此移除了默认构造函数，仅允许使用如下所示的唯一构造函数：

 IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                    const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
        : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
          linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},
            jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},
          sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()}
//p,v,q,ba,bg每个都是3维，所以是15×1维，这里影响雅克比和协方差的维度
    {
        // 噪声矩阵
        // ACC_N, GYR_N: 加速度计和陀螺白噪声均值
        // ACC_W, GYR_W: 加速度计和陀螺随机游走
        noise = Eigen::Matrix<double, 18, 18>::Zero();
        noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
        noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();
    }

该构造函数接收第k帧时刻对应的加速度、角速度、加速度计零偏和陀螺仪零偏，并完成各成员变量的初始化，

中值积分函数（midPointIntegration）