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<p,q>(i, j) :<p, q>可理解爲一個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"<<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;
            /**
             * 求矩陣的轉置、共軛矩陣、伴隨矩陣：可以通過成員函數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代碼閱讀筆記（六）：vins_estimator中圖像處理1

這裏列出幾個個人學習過程中覺得比較好的預積分相關論文和文章幫助理解：

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預積分總結與公式推導（四）