[游戏开发]4.vinsfusion数据流解读 ---processImu函数

回忆：
通过对惯导数据执行vins_estimator/src/estimator/estimator.cpp中inputIMU函数将原始数据存放到队列accBuf(t 、acc ）、gyrBuf（t、gyr)再通过fastPredictIMU得到latest_time，latest_Q，latest_P，latest_V，latest_acc_0，latest_gyr_0变量。

接下来进入processMesurements函数

   processMeasurements();//执行processMeasurement,处理全部量测的线程,IMU的预积分,特征点的处理

1.数据流

1.1.
// 判断输入的时间t时候的imu是否可用
  if ((!USE_IMU  || IMUAvailable(feature.first + td)))
  1.2
 //对imu的时间进行判断，将accbuf里的imu数据放入到accVector和gyrVector中。
 getIMUInterval(prevTime, curTime, accVector, gyrVector);
 1.3
   // !用初始时刻加速度方向对齐重力加速度方向，得到一个旋转，使得初始IMU的z轴指向重力加速度方向
  if(!initFirstPoseFlag)
        initFirstIMUPose(accVector);
 1.4
 // 用前一图像帧位姿，前一图像帧与当前图像帧之间的IMU数据，积分计算得到当前图像帧位姿
 //.second表示vector容器中的第二个也就是vector3d形式的陀螺仪原始数据
 /**
 * 处理一帧IMU，积分
 * 用前一图像帧位姿，前一图像帧与当前图像帧之间的IMU数据，积分计算得到当前图像帧位姿
 * Rs，Ps，Vs
 * @param t                     当前时刻
 * @param dt                    与前一历元的时间间隔
 * @param linear_acceleration   当前历元加速度
 * @param angular_velocity      当前历元角速度
 *  void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
 */
 processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);

存在的问题初始的惯导偏置是怎么来的，初始设置为零，后不断优化得到。
2.processimu详解：

2.1  
  //初始化预积分类
  pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
2.2  
//放入数据进行预积分
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
2.2.1
propagate(dt, acc, gyr);—— midPointIntegration函数
         * 中值积分
         * 1、前一时刻状态计算当前时刻状态PVQ，其中Ba，Bg保持不变
         * 2、计算当前时刻的误差Jacobian，误差协方差 todo
         * @param dt历元之间时间间隔
         * @param acc_0上一帧的对应的imu数组
         * @param acc_1当前帧imu数据
         * @param delta_p ，delta_q, delta_v上一时刻的 pvq 预积分结果，linearized_ba, linearized_bg上一时刻偏置
         * @param result_预积分结果
         */
        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);
2.2.1.1更新pvq
  // 注:以下计算PVQ都是每时刻世界坐标系下(第一帧IMU系)的量，加速度、角速度都是IMU系下的量
        // 前一时刻加速度
        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
        // 前一时刻与当前时刻角速度中值
        Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
        // 当前时刻旋转位姿Q
        result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);//Quaterniond旋转向量变化为四元数形式
        // 当前时刻加速度
        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
        // 前一时刻与当前时刻加速度中值
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        // 更新当前时刻P, V, 其中Ba, Bg保持不变
        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
        result_delta_v = delta_v + un_acc * _dt;
        result_linearized_ba = linearized_ba;
        result_linearized_bg = linearized_bg;         
  2.2.1.2更新雅克比和协方差阵
  //初始的雅克比矩阵是单位阵
           jacobian = F * jacobian;
            // 当前时刻误差协方差
            // 相邻时刻误差线性传递方程：误差k = Fk-1*误差k-1 + Vk-1*测量噪声k-1
            // 对应协方差关系：协方差k = Fk-1*协方差k-1*Fk-1转置 + Vk-1*测量噪声*Vk-1转置，认为测量噪声每时刻都是一样的，测量噪声包括高斯白噪声、bias随机游走噪声
            //协方差阵初始为0表示完全信任IMU
            covariance = F * covariance * F.transpose() + V * noise * V.transpose();
   2.3更新绝对坐标系下的位置
          //计算对应绝对坐标系下的位置
        int j = frame_count;  
        // 前一时刻加速度 
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
        // 中值积分，用前一时刻与当前时刻角速度平均值，对时间积分
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
        // !当前时刻姿态Q,感觉是相对于上一时刻的姿态
        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);
        // 当前时刻位置P
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
        // 当前时刻速度V
        Vs[j] += dt * un_acc;

疑问？frame_count是怎样更新的还是不需要更新是滑动窗口的定值。
已解决：estimator.cpp初始化过程中，同时在两帧之间的frame_count是相同的。

 if(frame_count < WINDOW_SIZE)
        {
            frame_count++;
            int prev_frame = frame_count - 1;
            Ps[frame_count] = Ps[prev_frame];
            Vs[frame_count] = Vs[prev_frame];
            Rs[frame_count] = Rs[prev_frame];
            Bas[frame_count] = Bas[prev_frame];
            Bgs[frame_count] = Bgs[prev_frame];
        }