ROS-3DSLAM(15):视觉部分visual estimator 第九节 factor3

2021@SDUSC
2021年12月19日星期日——2021年12月24日星期五

一、背景简介：

这一周我分析的代码是factor文件夹

运动模型的IMU计算中得到的残差：相关代码为imu_factor.h和integration_base.h

这部分涉及到的是IMU预积分的相关知识。

在分析雷达部分的时候，我已经初步接触了IMU预积分的知识，这里再来回顾一下：

IMU预积分回顾：

为了有个清晰的认识，首先说下预计分的作用。大家知道，用IMU的slam、vio算法有很多，有滤波器的比如MSCKF，有基于图优化的比如VINS，OKVIS，ORB-SLAM等。

在紧耦合的优化slam中，IMU就是提供了两个关键帧的相对测量，从而构建误差函数对关键帧姿态的迭代优化。当然实际应用中不会是这么简单的形式，这里面要对各个变量分别求取误差，然后求雅克比矩阵。

预积分的功能是为了在优化问题中确定一个测量项，来使得它能够与估计项来进行优化求解。预积分得到的是两个关键帧之间的相对变化。

预积分具体的数学计算：涉及到大量的数学计算，等到之后再深入学习。

简单的来说，首先是对两个关键帧 i、j，利用Δt来进行积分运算。

得到两帧之间的相对变化。

需要注意的地方是bias，因为求解的过程中，默认bias是已知的。

但是bias（加速的的，角速度的）是在不断变化的，我们在优化的过程中也需要估计bias。

为了避免重复计算，需要优化方程，利用新的bias来更新预积分得到的值。经过推导之后得到最终可以利用偏差更新的值。

IMU预积分原理深入介绍：

预积分的误差和线性传递方程

经过上边的简单介绍，我们可以确定：通过预积分获得了更为精简的的数据，用1个结果来代表若干个特征点。

但是同时这若干的点所具有的不确定度（噪声方差）的数据也损失了。也就是我们在减少计算的同时损失了一小部分数据。

这样，我们就不能确定我们得到的IMU的预积分点的不确定性，需要对其进行处理，推导这部分的误差。

• 获取预积分的误差
• 相邻时刻的误差传递线性方程
• 预积分协方差矩阵

预积分的雅克比行列式和协方差（更多的推导）

两帧之间的PVQ和bias变化量的残差：计算求解雅可比行列式

西协方差对应的就是IMU预积分迭代得到的IMU增量误差的协方差

二、代码分析：

integration_base.h部分：

从这里开始正式进入代码的分析。

//这里的意思是预积分类，首先对于各个需要的变量给出了声明，包括：
//预积分类：加速度计、陀螺仪、线性加速度计ba、陀螺仪bg、雅克比矩阵初始化、协方差矩阵15*15、dt、PVQ
class IntegrationBase
{
   
  public:

    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;
    Eigen::Matrix<double, 15, 15> covariance;
    Eigen::Matrix<double, 18, 18> noise;

    double sum_dt;
    Eigen::Vector3d delta_p;
    Eigen::Quaterniond delta_q;
    Eigen::Vector3d delta_v;

    // saves all the IMU measurements and time difference between two image frames
    std::vector<double> dt_buf;
    std::vector<Eigen::Vector3d> acc_buf;
    std::vector<Eigen::Vector3d> gyr_buf;

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

    // after optimization, repropagate pre-integration using the updated bias
    //利用得到的更新的bias来重新计算预积分
    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();
        for (int i = 0; i < static_cast<int>(dt_buf.size()); i++)
            propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);
    }

这个函数很重要，需要

预积分传播方程，在预积分传播方程propagate中使用中点积分方法midPointIntegration计算预积分的测量值，中点积分法中主要包含两个部分，分别是得到状态变化量result_delta_q，result_delta_p，result_delta_v，result_linearized_ba，result_linearized_bg和得到跟新协方差矩阵和Jacobian矩阵（注意，虽然得到了雅各比矩阵和协方差矩阵，但是还没有求残差和修正偏置一阶项的状态变量），由于使用的是中点积分，所以需要上一个时刻的IMU数据，包括测量值加速度和角速度以及状态变化量，初始值由构造函数提供。需要注意的是这里定义的delta_p等是累积的变化量，也就是说是从i时刻到当前时刻的变化量，这个才是最终要求的结果（为修正偏置一阶项），而result_delta_q等只是一个暂时的变量，最后残差和雅可比矩阵、协方差矩阵保存在pre_integrations中，还有一个函数这里暂时还没有用到，是在优化的时候才被调用的，但是其属于预积分的内容，evaluate函数在这个函数里面进行了状态变化量的偏置一阶修正以及残差的计算。
步骤2预积分公式（3）未考虑误差，提供imu计算的当前旋转，位置，速度，作为优化的初值。求状态向量对bias的Jacobian，当bias变化较小时，使用Jacobian去更新状态；否则需要以当前imu为参考系，重新预积分，对应repropagation()。同时，需要计算error state model中误差传播方程的系数矩阵F和V：

翻译成已于理解的含义就是：
需要利用中点积分方法来计算预积分的测量值，有五个状态变化量和两个矩阵，新协方差矩阵和雅克比矩阵。
在使用中点积分的过程中，需要利用前一个时刻的IMU积分，这部分包括了测量值加速度和角速度以及状态变化量。

这里的evaluate函数没有用到，因为这是在优化时才会使用的函数。不过这个函数也是属于预积分的部分，所以在这里介绍了。

对于更新的bias进行更新IMU预积分值时，分为两种情况，当bias变化较小时，直接用雅克比矩阵去更新；当bias变化较大时，需要重新以当前的imu为参考系进行重新的预积分，对应了下文的repropagation函数。

    /**
    * @brief   IMU预积分传播方程
    * @Description  积分计算两个关键帧之间IMU测量的变化量： 
    *               旋转delta_q 速度delta_v 位移delta_p
    *               加速度的biaslinearized_ba 陀螺仪的Bias linearized_bg
    *               同时维护更新预积分的Jacobian和Covariance,计算优化时必要的参数
    * @param[in]   _dt 时间间隔
    * @param[in]   _acc_1 线加速度
    * @param[in]   _gyr_1 角速度
    * @return  void
    */
    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);

        delta_p = result_delta_p;
        delta_q = result_delta_q;
        delta_v = result_delta_v;
        linearized_ba = result_linearized_ba;
        linearized_bg