VINS-Mono工程笔记（五）：IMU预积分

 1.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数据的标志，并记录last_imu数据。由于使用的是中值积分，需要用到历史的一个imu。

2）对滑动窗口内每个帧创建预积分对象IntegrationBase对象存入pre_integrations数组当中。

3&#