VINS-MONO源码学习（一）：IMU预积分processIMU

最新推荐文章于 2024-04-22 18:52:37 发布

矢三郎的狸猫

最新推荐文章于 2024-04-22 18:52:37 发布

阅读量632

点赞数 2

分类专栏： VINS-MONO 文章标签： slam

本文链接：https://blog.csdn.net/weixin_43242765/article/details/121523327

版权

VINS-MONO 专栏收录该内容

4 篇文章 0 订阅

订阅专栏

这个系列记录一下学习VINS-MONO源码的学习过程，可能会出现一些错误，希望各位及时指出。

从estimator_node.cpp的主线程process()开始说起，在getMeasurements之后，我们得到了若干组measurement，也就是两个关键帧之间的若干个IMU数据和一个关键帧的图像数据。对于每个measurement，取出所有的IMU数据，执行send_imu函数，计算两个IMU数据之间的时间间隔dt，把线加速度和角速度数据传递给预积分函数estimator.processIMU()：

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
    if (!first_imu)
    {
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }

    if (!pre_integrations[frame_count])
    {
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[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);

        int j = frame_count;         
        //noise是zero mean Gassu，在这里忽略了
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
        //下面都采用的是中值积分的传播方式，noise被忽略了
        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;
    }
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

这里涉及到了IntegrationBase类（在integration_base.h中定义），主要包括这些变量：

    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;

IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)];
pre_integrations是一个大小为滑动窗口大小的数组，里面装的是指向IntegrationBase的指针。如果下标为frame_count的指针还没指向内容，就new一个IntegrationBase出来。（这么看来frame_count应该一直在滑动窗口大小的范围内，这个变量到底是怎么更新的后面再看）

if (!pre_integrations[frame_count])
    {
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }

在后面的计算过程中，首先执行了IntegrationBase对象的push_back操作：

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

这个操作把dt，acc，gyr值push进了IntegrationBase的成员变量中

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

再执行propagate，积分计算两个关键帧之间IMU测量的变化量：旋转delta_q 速度delta_v 位移delta_p，加速度的biaslinearized_ba 陀螺仪的Bias linearized_bg。acc_0和gyr_0分别是上一次IMU数据的线加速度和角速度，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);
        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：（不会打公式，有时间的话后面在补吧-_-||）
这里的已知量是dt，ai测量值，ai+1测量值，wi测量值，wi+1测量值。需要求解：
delta_q（两帧间的旋转，被初始化为单位四元数），delta_v（两帧间的速度，被初始化为0向量），delta_p（两帧间的位移，被初始化为0向量）。由于IMU的频率大于图像的频率，所以在这两帧之间存在着m个IMU的数据。对于i（i属于[0,m]）时刻的IMU数据，通过累加的方式（也就是积分，由于是离散的数据，因此准确的说是中值积分）就可以得到两帧之间的PVQ变化，这个就是预积分最后的输出。
代码中先算出wi两次测量值的中值，据此可以算出新的旋转qi+1（牢记高博告诉我们的，更新四元数之后要normalize！），再用qi+1算出ai+1，得出ai和ai+1的中值。有了线加速度的中值，可以求出i+1时刻的速度和位移。
在这里插入图片描述
后面的代码更新了误差项的Jacobian：

if(update_jacobian)
        {
            Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
            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;

            //skew matrix这里是对应的反对称矩阵
            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;

            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;
            covariance = F * covariance * F.transpose() + V * noise * V.transpose();
        }

其实就是在算F矩阵和V矩阵，用来更新Jacobian和Covariance。
在这里插入图片描述
其中，

上面的代码和公式是完全对应的。至此，IMU预积分部分的代码已经基本搞清了。

参考：崔华坤

矢三郎的狸猫

关注

2
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
VINS-MONO源码学习（一）：IMU预积分processIMU

这个系列记录一下学习VINS-MONO源码的学习过程，可能会有一些我自己的理解，大概率会出现一些错误，希望各位及时指出。从estimator_node.cpp的主线程process()开始说起，在getMeasurements之后，我们得到了若干组measurement，也就是两个关键帧之间的若干个IMU数据和一个关键帧的图像数据。对于每个measurement，取出所有的IMU数据，执行send_imu函数，计算两个IMU数据之间的时间间隔dt，把线加速度和角速度数据传递给预积分函数estimator.
复制链接

扫一扫