VINS-mono 代码解析——IMU预积分processIMU( )实现

最新推荐文章于 2023-10-21 16:19:32 发布

TB81266

最新推荐文章于 2023-10-21 16:19:32 发布

阅读量2.4k

点赞数 2

分类专栏： VINS

本文链接：https://blog.youkuaiyun.com/qq_43247439/article/details/107322934

版权

本文深入解析VINS-mono中的processIMU函数，涉及IntegrationBase类、push_back()、evaluate()、propagate()、midPointIntegration()以及repropagate()。介绍了预积分在IMU数据处理中的关键步骤，如状态变化量计算、残差与雅可比矩阵的构建，以及中值积分法的应用。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

文章目录

前言

在VINS-mono中,estimator/src/estimator_node.cpp中,从主函数依次为:

main( )---->process( )---->processIMU( )

其中,processIMU( )函数的原型为:

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)

一 processIMU()函数

// IMU预积分，中值积分得到当前PQV作为 优化 初值
void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
   
     // 1.判断是不是第一个imu消息，如果是第一个imu消息，则将当前传入的线加速度和角速度作为初始的加速度和角速度
    if (!first_imu)
    {
   
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }

     // 2.IMU 预积分类对象还没出现，创建一个对象
    if (!pre_integrations[frame_count])
    {
   
        pre_integrations[frame_count] = new IntegrationBase{
   acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }
    //当frame_count==0的时候表示滑动窗口中还没有图像帧数据，不需要进行预积分，只进行线加速度和角速度初始值的更新
    if (frame_count != 0)
    {
   
         // 3.预积分操作
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        //if(solver_flag != NON_LINEAR)
            tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

        // 4.dt、加速度、角速度加到buf中
        dt_buf[frame_count].push_back(dt);
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

        int j = frame_count;
        // 5.采用的是中值积分的传播方式         
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;// a0=Q(a^-ba)-g 已知上一帧imu速度
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];// w=0.5(w0+w1)-bg
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;// a1 当前帧imu速度
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);// 中值积分下的加速度a=1/2(a0+a1)
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;// P=P+v*t+1/2*a*t^2
        Vs[j] += dt * un_acc;// V=V+a*t

    }
    // 6.当frame_count==0的时候表示滑动窗口中还没有图像帧数据，不需要进行预积分，只进行线加速度和角速度初始值的更新
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

1 IMU 预积分IntegrationBase类

预积分类：加速度计、陀螺仪、线性加速度计ba、陀螺仪bg、雅克比矩阵初始化、协方差矩阵15*15、dt、PVQ

IntegrationBase() = delete;
    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()}

    {
   
        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();
    }

2 push_back()函数

void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
    {
   
        dt_buf.push_back(dt);
        acc_buf.push_back(acc);
        gyr_buf.push_back(gyr);
        //进入预积分阶段
        propagate(dt, acc, gyr);
    }

3 evaluate（）函数

预计分量
在这里插入图片描述
构建残差


    Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
                                          const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
    {
   
        Eigen::Matrix<double, 15, 1> residuals;// 残差

        Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
        Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);

        Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG

最低0.47元/天解锁文章