VINS-Mono代码学习记录（六）---processIMU( )

客官°小女子只卖身不卖艺 2023-05-30 13:58 17阅读 0赞

#### 一、processIMU（ ） ####

main( )---->process( )---->processIMU( )  
函数原型为：

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

这一部分是对IMU进行处理, 传入参数为imu信息的时间间隔，线加速度，角速度。代码注释如下：

//IMU预积分
    void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
    {
    
        //[1]判断是不是第一个imu消息，如果是第一个imu消息，则将当前传入的线加速度和角速度作为初始的加速度和角速度
        if (!first_imu)//first_imu为false表示当前上报的imu数据为第一个imu数据
        {
            first_imu = true;
            acc_0 = linear_acceleration;
            gyr_0 = angular_velocity;
        }
    
        //[2]创建预积分对象
        // IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)];
        if (!pre_integrations[frame_count])
        {
            pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};//这四个参数可以去对比IntegrationBase类的构造函数
        }
    
    
        //[3]当滑动窗口内有图像帧数据时，进行预积分 frame_count表示滑动窗口中图像数据的个数
        if (frame_count != 0)
        {
    
            //
            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);
    
            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);
    
            // [4]更新Rs、Ps、Vs三个向量数组。Rs为旋转向量数组，Ps为位置向量数组，Vs为速度向量数组，数组的大小为WINDOW_SIZE + 1
            int j = frame_count;         
            Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
            Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
            Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
            Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
            Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
            Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
            Vs[j] += dt * un_acc;
        }
        //[5]当frame_count==0的时候表示滑动窗口中还没有图像帧数据，不需要进行预积分，只进行线加速度和角速度初始值的更新
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }

在这个函数中，使用到 IntegrationBase 类的实例\*pre\_integrations\[(WINDOW\_SIZE + 1)\]以及这个类的push\_back()函数，所以 接下来一起来看一下这个integration\_base.h这个头文件，这个文件中定义了IntergrationBase类，定义了中值积分的函数等等。

#### 二、integration\_base.h介绍 ####

#pragma once
    
    #include "../utility/utility.h"
    #include "../parameters.h"
    
    #include <ceres/ceres.h>
    using namespace Eigen;
    
    class IntegrationBase
    {
      public:
        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()}
    //p,v,q,ba,bg每个都是3维，所以是15×1维，这里影响雅克比和协方差的维度
        {
            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();
        }
    
        void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)//把传递的参数值push到相应的vector中
        {
            dt_buf.push_back(dt);
            acc_buf.push_back(acc);
            gyr_buf.push_back(gyr);
            propagate(dt, acc, gyr);
        }
    
        void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
        {
            sum_dt = 0.0;
            acc_0 = linearized_acc;
            gyr_0 = linearized_gyr;
            delta_p.setZero();
            delta_q.setIdentity();
            delta_v.setZero();
            linearized_ba = _linearized_ba;
            linearized_bg = _linearized_bg;
            jacobian.setIdentity();//雅克比初值设为单位阵
            covariance.setZero();//协方差矩阵初值为0矩阵
            for (int i = 0; i < static_cast<int>(dt_buf.size()); i++)
                propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);
        }
    
        //离散形式下PVQ增量误差，这一部分基本上是根据公式来求的，求误差的雅克比和协方差的迭代更新公式
        void 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)
        {
            //ROS_INFO("midpoint integration");a_q为相对预计分参考系的旋转四元数
            Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);//k时刻的加速度
            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);//k+1时刻的加速度
            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;//位置P的更新
            result_delta_v = delta_v + un_acc * _dt;//速度v的更新
            result_linearized_ba = linearized_ba;//ba的迭代，短时间内认为值不变，所以直接赋值就好
            result_linearized_bg = linearized_bg;//bg同上
            //是否更新IMU预计分测量关于IMU Bias的雅克比矩阵！！！离散形式下的
            if(update_jacobian)
            {
                Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;//角速度
                //计算_acc_0这个观测线加速度对应的实际加速度
                Vector3d a_0_x = _acc_0 - linearized_ba;//线加速度
                Vector3d a_1_x = _acc_1 - linearized_ba;
                Matrix3d R_w_x, R_a_0_x, R_a_1_x;
                /**
                            *         | 0      -W_z     W_y |
                            * [W]_x = | W_z     0      -W_x |
                            *         | -W_y   W_x       0  |
                           */
                R_w_x<<0, -w_x(2), w_x(1),
                    w_x(2), 0, -w_x(0),
                    -w_x(1), w_x(0), 0;
                R_a_0_x<<0, -a_0_x(2), a_0_x(1),
                    a_0_x(2), 0, -a_0_x(0),
                    -a_0_x(1), a_0_x(0), 0;
                R_a_1_x<<0, -a_1_x(2), a_1_x(1),
                    a_1_x(2), 0, -a_1_x(0),
                    -a_1_x(1), a_1_x(0), 0;
                /**
                 * matrix.block(i,j, p, q) : 表示返回从矩阵(i, j)开始，每行取p个元素，每列取q个元素所组成的临时新矩阵对象，原矩阵的元素不变；
                 * matrix.block<p,q>(i, j) :<p, q>可理解为一个p行q列的子矩阵，该定义表示从原矩阵中第(i, j)开始，获取一个p行q列的子矩阵，
                 * 返回该子矩阵组成的临时矩阵对象，原矩阵的元素不变；
                */
    
    
                MatrixXd F = MatrixXd::Zero(15, 15);
                F.block<3, 3>(0, 0) = Matrix3d::Identity();
                F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                                      -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
                F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
                F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
                F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
                F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
                F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
                F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                                      -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
                F.block<3, 3>(6, 6) = Matrix3d::Identity();
                F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
                F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
                F.block<3, 3>(9, 9) = Matrix3d::Identity();
                F.block<3, 3>(12, 12) = Matrix3d::Identity();
                //cout<<"A"<<endl<<A<<endl;
    
                MatrixXd V = MatrixXd::Zero(15,18);
                V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;
                V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;
                V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
                V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3);
                V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;
                V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt;
                V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;
                V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;
                V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt;
                V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3);
                V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
                V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
    
                //step_jacobian = F;
                //step_V = V;
                jacobian = F * jacobian;//雅克比的更新,迭代公式：J_(k+1)=F*J_k，J_0=I
                /**
                 * covariance为协方差，协方差的迭代公式：P_(k+1) = F*P_k*F^T + V*Q*V^T，P_0 = 0
                 * P_k就是协方差，Q为noise，其初值为18*18的单位矩阵
                */
    
                covariance = F * covariance * F.transpose() + V * noise * V.transpose();//协方差的更新
            }
    
        }
    
        void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
        {
            dt = _dt;
            acc_1 = _acc_1;
            gyr_1 = _gyr_1;
            Vector3d result_delta_p;
            Quaterniond result_delta_q;
            Vector3d result_delta_v;
            Vector3d result_linearized_ba;
            Vector3d result_linearized_bg;
    
            midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
                                linearized_ba, linearized_bg,
                                result_delta_p, result_delta_q, result_delta_v,
                                result_linearized_ba, result_linearized_bg, 1);
    
            //checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
            //                    linearized_ba, linearized_bg);
            delta_p = result_delta_p;
            delta_q = result_delta_q;
            delta_v = result_delta_v;
            linearized_ba = result_linearized_ba;
            linearized_bg = result_linearized_bg;
            delta_q.normalize();
            sum_dt += dt;
            acc_0 = acc_1;
            gyr_0 = gyr_1;  
         
        }
    //求残差的表达式
        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);//O_P = 0,O_R = 3, O_V = 6, O_BA = 9, O_BG = 12
            Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
    
            Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
    
            Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
            Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);
    
            Eigen::Vector3d dba = Bai - linearized_ba;
            Eigen::Vector3d dbg = Bgi - linearized_bg;
    
            Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
            Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
            Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
    
            residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;//位置的误差
            residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();//旋转的误差
            residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;//速度的误差
            residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
            residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
            return residuals;//上面的都是为了写residuals
        }
    
        double dt;
        Eigen::Vector3d acc_0, gyr_0;
        Eigen::Vector3d acc_1, gyr_1;
    
        const Eigen::Vector3d linearized_acc, linearized_gyr;
        Eigen::Vector3d linearized_ba, linearized_bg;
    
        Eigen::Matrix<double, 15, 15> jacobian, covariance;
        Eigen::Matrix<double, 15, 15> step_jacobian;
        Eigen::Matrix<double, 15, 18> step_V;
        Eigen::Matrix<double, 18, 18> noise;
    
        double sum_dt;
        Eigen::Vector3d delta_p;
        Eigen::Quaterniond delta_q;
        Eigen::Vector3d delta_v;
    
        std::vector<double> dt_buf;
        std::vector<Eigen::Vector3d> acc_buf;
        std::vector<Eigen::Vector3d> gyr_buf;
    
    };

这一部分代码对应着IMU的测量误差模型，以及误差的协方差和雅克比矩阵的更新方式，以及残差的构成，都是公式推导的，得提前把公式看明白。  
proceIMU（ ）–> pre\_integrations\[frame\_count\]->push\_back( )–> propagate( )–>midPointIntegration() 大概这样一个信息流向。  
再补一张图：![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng_size_16_color_FFFFFF_t_70]

这就是IMU的处理过程啦！

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20191110223555404.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3dlaXhpbl8zODUwNDI3Ng==,size_16,color_FFFFFF,t_70