VINS-Mono代码阅读笔记（五）：vins_estimator中IMU预积分处理

1.前言

本篇紧接着上一篇笔记 VINS-Mono代码阅读笔记（四）：vins_estimator中process线程代码分析 来分析IMU数据的相关处理代码流程。

先上一张图从代码结构上看一下IMU处理在process函数中的位置。

如上图所示，process函数中有三个for循环：

1）第一个for循环也就是最外层的for循环是在遍历measurements，这样可以获取每个measurement中的imu_msg和img_msg数据。

2）第二个for循环是遍历第一个for循环中获取的每一个measurement中的first元素，也就是IMU数据信息。并对每一个IMU数据调用estimator.processIMU函数进行预积分处理。

3）第三个for循环是遍历每一个img_msg中的特征点信息，所有的特征点信息构成了一个image信息，然后调用estimator.processImage对图像信息进行处理。

本篇笔记，我主要是学习和分析imu数据处理的代码和相关算法思想。

2.调用processIMU之前的处理

//2.遍历measurements，其实就是遍历获取每一个img_msg和其对应的imu_msg对数据进行处理
        for (auto &measurement : measurements)
        {
            auto img_msg = measurement.second;
            double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
            //遍历和当前img_msg时间上“对齐”的IMU数据
            for (auto &imu_msg : measurement.first)
            {
                double t = imu_msg->header.stamp.toSec();
                double img_t = img_msg->header.stamp.toSec() + estimator.td;
                if (t <= img_t)//imu数据比图像数据早到
                {
                    //第一次的时候current_time值为-1
                    if (current_time < 0)
                        current_time = t;
                    //计算时间间隔
                    double dt = t - current_time;
                    ROS_ASSERT(dt >= 0);
                    current_time = t;
                    //dx,dy,dz分别是IMU在三个轴方向上的加速度
                    dx = imu_msg->linear_acceleration.x;
                    dy = imu_msg->linear_acceleration.y;
                    dz = imu_msg->linear_acceleration.z;
                    //rx,ry,rz分别是IMU在三个轴方向上的角速度
                    rx = imu_msg->angular_velocity.x;
                    ry = imu_msg->angular_velocity.y;
                    rz = imu_msg->angular_velocity.z;
                    //对每一个IMU值进行预积分
                    estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
                    //printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);

                }
                else//图像数据比IMU数据早到
                {
                    double dt_1 = img_t - current_time;
                    //t为imu的时间，dt_2表示imu消息的时间戳和图像数据时间戳之间的差值
                    double dt_2 = t - img_t;
                    //此时，用img_t来更新current_time
                    current_time = img_t;
                    ROS_ASSERT(dt_1 >= 0);
                    ROS_ASSERT(dt_2 >= 0);
                    ROS_ASSERT(dt_1 + dt_2 > 0);
                    /**
                     * 这块是计算当前情况下的线加速度和角速度，但是为什么要这样做？？？？还没想明白，为什么不是下面这样？？？
                     * double w1 = dt_1 / (dt_1 + dt_2);
                     * double w2 = dt_2 / (dt_1 + dt_2);
                     * */
                    double w1 = dt_2 / (dt_1 + dt_2);
                    double w2 = dt_1 / (dt_1 + dt_2);
                    dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
                    dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
                    dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
                    rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
                    ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
                    rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
                    //对每一个IMU值进行预积分
                    estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
                    //printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
                }
            }

通过以上代码，可以看到调用processIMU之前，主要是计算本次IMU消息距上次的IMU消息之间的时间间隔、本次IMU数据中IMU在三个轴方向上的线加速度向量、IMU在三个轴上的角速度向量，其实也就是processIMU函数的三个参数。

IIMU预积分，是要通过积分的方式计算出图像帧的时刻所对应的P（位置）、V（速度）、Q（旋转矩阵）。由于一帧image图像消息对应多个IMU消息，所以针对每一个IMU消息的时间戳，需要判断IMU的时间戳和image消息的时间戳之间的大小关系，也就是需要判断IMU消息早于image消息先收到，还是IMU消息晚于image消息收到。

1）IMU数据时间戳早于image数据时间戳

这种情况下的时间间隔就是两次IMU消息时间戳的间隔，线加速度和角速度直接从Imu_msg中获取就ok了。

2）IMU数据时间戳晚于image数据时间戳

这种情况下，由于图像数据比IMU数据早到，所以时间间隔自然是时间戳大的减去时间戳小的。但是对于三个轴上的线加速度dx,dy,dz和三个轴方向上的角速度rx,ry,rz的计算，我不是特别明白这里w1、w2的取值，为什么不是double w1 = dt_1 / (dt_1 + dt_2);   double w2 = dt_2 / (dt_1 + dt_2);呢？

有哪位朋友看到本篇笔记的时候如果清楚的话麻烦给我解释一下这块，谢谢。

3.processIMU函数分析

/**
 * 处理IMU数据
 * linear_acceleration 线加速度
 * angular_velocity    角速度
 * */
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.创建预积分对象
     * 首先，pre_integrations是一个数组，里面存放了(WINDOW_SIZE + 1)个指针，指针指向的类型是IntegrationBase的对象
    */
    if (!pre_integrations[frame_count])
    {
        //创建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);

        dt_buf[frame_count].push_back(dt);
        //当前的线加速度和角速度存放到先加速度buffer和角速度buffer当中
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

        int j = frame_count;
        /**
         * 4.更新Rs、Ps、Vs三个向量数组。
         * Rs为旋转向量数组，Ps为位置向量数组，Vs为速度向量数组，数组的大小为WINDOW_SIZE + 1
         * 那么，这三个向量数组中每个元素都对应的是每一个window
        */
        //计算上一时刻的加速度，前边乘一个旋转矩阵Rs[j]的作用是进行坐标系转换
        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);
        //位移（位置）更新，位置是在之前的基础上加上当前的位移量，使用的是位移公式：s = v*t + 1/2*a*t^2
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
        //速度更新，使用的是速度公式：v = a * t a是加速度，t是时间
        Vs[j] += dt * un_acc;
    }
    //更新acc_0和gyr_0的值，本次的线加速度和角速度作为下一个IMU消息的前一个状态值
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

1）IMU预积分指的是对两个时刻的IMU数据通过预计分的方式来计算出PVQ，那么，就会存在初始化时刻的状态设置问题。first_imu为判断是否是第一个imu数据的标志，first_imu为false表示当前上报的imu数据为第一个imu数据，此时，就将当前的IMU线加速度和角速度的值赋值给初始状态的线加速度acc_0和初始状态的角速度gyr_0。

2）对滑动窗口内每个帧创建预积分对象。每新到一个图像帧，就会创建一个IntegrationBase对象存入pre_integrations数组当中。每次更新的acc_0和gyr_0作为IMU上一个状态时候的线加速度和角速度值，Bas[frame_count]和Bgs[frame_count]为对应于id为frame_count这一帧图像的线加速度偏置和角速度偏置值。

3）预积分是需要和滑动窗口中的图像信息结合起来的，frame_count表示滑动窗口中图像数据的个数，当frame_count==0的时候表示滑动窗口中还没有图像帧数据，所以不需要进行预积分，只进行线加速度和角速度初始值的更新。

当frame_count!=0的时候，说明滑动窗口中已经有图像帧的数据了，此时就可以对该图像帧对应的imu进行预积分。

pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

预积分操作就是放在这里IntegrationBase类型对象的push_back函数中的。

4）更新Rs、Ps和Vs三个数组的值。这三个数组的大小为滑动窗口大小+1，这里按照图像帧的id来计算得到滑动窗口中每个图像帧所对应的旋转矩阵、位置和速度值。这三个值在后边进行窗口滑动和进行图像位姿初始化的时候需要使用。

4.IMU预积分代码

进入预积分的函数是IntegrationBase当中的push_back函数。该函数代码如下：

/**
     * dt  时间间隔
     * acc 陀螺仪的线加速度
     * gyr 陀螺仪的角速度
    */
    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);
    }

可以看到，该函数中将传入的参数dt、acc、gyr放进对应的buffer中后，调用propagate进行了预积分（IMU传播）操作。propagate函数代码如下：

   /**
     * 预计分计算
     * _dt    时间间隔
     * _acc_1 线加速度
     * _gyr_1 角速度
    */
    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);
        //更新PQV
        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;
     
    }

该函数中调用了midPointIntegration函数，采用中点积分方法。这里需要清楚的是，大多数情况下讲解预积分推导的理论知识的时候都是从连续的情况开始的，但是现实中IMU测量到的值是一个个的离散状态下的值，所以实际编写代码过程中都是在离散情况下来操作的。

midPointIntegration函数代码如下：

 /**
     * 该函数是以中值点的方式进行预积分求解PVQ的，需要注意的是这里使用的是离散形式的预积分公式
     * 参数中_0代表上次测量值，_1代表当前测量值，delta_p，delta_q，delta_v代表相对预积分初始参考系的位移，旋转四元数，以及速度
     * 从k帧预积分到k+1帧，则参考系是k帧的imu坐标系
    */
    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");
        //delta_q为相对预计分参考系的旋转四元数，线加速度的测量值减去偏差然后和旋转四元数相乘表示将线加速度从世界坐标系下转到了body(IMU)坐标系下
        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
        //计算平均角速度
        Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
        //对平均角速度和时间的乘积构成的旋转值组成的四元数左乘旋转四元数，获得当前时刻body中的旋转向量（四元数表示）
        result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
        //用计算出来的旋转向量左乘当前的加速度，表示将线加速度从世界坐标系下转到了body坐标系下
        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
        //计算两个时刻的平均加速度
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        //当前的位移：当前位移=前一次的位移+(速度×时间)+1/2×加速度的×时间的平方
        //匀加速度运动的位移公式：s_1 = s_0 + v_0 * t + 1/2 * a * t^2
        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
        //速度计算公式：v_1 = v_0 + a*t
        result_delta_v = delta_v + un_acc * _dt;
        // 预积分的过程中Bias并未发生改变，所以还保存在result当中
        result_linearized_ba = linearized_ba;
        result_linearized_bg = linearized_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;
            //计算_acc_1这个观测线加速度对应的实际加速度
            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;
            //F是一个15行15列的数据类型为double，数据全部为0的矩阵，Matrix创建的矩阵默认按列存储
            MatrixXd F = MatrixXd::Zero(15, 15);
            /**
             * matrix.block(i,j, p, q) : 表示返回从矩阵(i, j)开始，每行取p个元素，每列取q个元素所组成的临时新矩阵对象，原矩阵的元素不变；
             * matrix.block(i, j) :可理解为一个p行q列的子矩阵，该定义表示从原矩阵中第(i, j)开始，获取一个p行q列的子矩阵，
             * 返回该子矩阵组成的临时矩阵对象，原矩阵的元素不变；
            */
            //从F矩阵的(0,0)位置的元素开始，将前3行3列的元素赋值为单位矩阵
            /**
             * 下面F和V矩阵为什么这样构造，是需要进行相关推导的。这里的F、V矩阵的构造理解可以参考论文附录中给出的公式
            */
            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"<(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;
            /**
             * 求矩阵的转置、共轭矩阵、伴随矩阵：可以通过成员函数transpose()、conjugate()、adjoint()来完成。注意：这些函数返回操作后的结果，
             * 而不会对原矩阵的元素进行直接操作，如果要让原矩阵进行转换，则需要使用响应的InPlace函数，如transpoceInPlace()等
            */
            //雅克比jacobian的迭代公式：J_(k+1)​=F*J_k​，J_0​=I
            jacobian = F * jacobian;
            /**
             * 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();
        }

    }

对于该代码的理解在代码注释中已经写了。需要清楚的是，预积分的理论知识和推导还是要花一些心思去理解学习的，但是牛顿第二定律是这里进行相关积分的基础。也就是说，位移的导数是速度，速度的导数是加速度。那么，我们对IMU测量到的加速度进行时间上的积分获得的就是速度，在对速度进行积分获取的就是位移。

这里列出几个个人学习过程中觉得比较好的预积分相关论文和文章帮助理解：

VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator 

On-Manifold Preintegration for Real-Time Visual-Inertial Odometry

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

VIO初始化相关Paper简单梳理   （虽然是讲初始化的，但是里边重点提到了预积分）

从零开始的 IMU 状态模型推导

IMU预积分总结与公式推导--邱笑晨：

IMU预积分总结与公式推导（一） 

IMU预积分总结与公式推导（二） 

IMU预积分总结与公式推导（三） 

IMU预积分总结与公式推导（四）