VINS_Fusion IMU數據處理過程

VINS_Fusion中IMU數據從話題中訂閱得到

1.訂閱IMU話題

在rosNodeTest.cpp中

ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());

參數如下:

• IMUTOPIC:IMU話題字符串;
• 2000: (uint32t類型) 消息隊列大小
• imu_callback,回調函數
• ros::TransportHints().tcpNoDelay()

回調函數功能是把話題中的數據打包好后調用estimator的inputIMU函數進行后續處理.
代碼如下所示:

    double t = imu_msg->header.stamp.toSec();
    double dx = imu_msg->linear_acceleration.x;
    double dy = imu_msg->linear_acceleration.y;
    double dz = imu_msg->linear_acceleration.z;
    double rx = imu_msg->angular_velocity.x;
    double ry = imu_msg->angular_velocity.y;
    double rz = imu_msg->angular_velocity.z;
    Vector3d acc(dx, dy, dz);
    Vector3d gyr(rx, ry, rz);
    estimator.inputIMU(t, acc, gyr);

2.快速預測相機位姿

通過回調函數把IMU數據輸入到estimator中做的處理如下:
函數 Estimator::inputIMU()

1. 輸入到加速度buff和gyrbuf中以后以供后續調用
2. 通過IMU快速預測

void Estimator::inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity)
{
    mBuf.lock();
    accBuf.push(make_pair(t, linearAcceleration));
    gyrBuf.push(make_pair(t, angularVelocity));
    mBuf.unlock();

    if (solver_flag == NON_LINEAR)
    {
        mPropagate.lock();
        fastPredictIMU(t, linearAcceleration, angularVelocity); //僅通過IMU預測最新的P,V,acc_0,gyr_0
        pubLatestOdometry(latest_P, latest_Q, latest_V, t);
        mPropagate.unlock();
    }
}

其中:fastPredictIMU 使用上一時刻的姿態進行快速的imu預積分來預測最新P,V,Q的姿態
其中:latest_p, latest_q, latest_v, latest_acc_0, latest_gyr_0 最新時刻的姿態。這個的作用是為了刷新姿態的輸出，但是這個值的誤差相對會比較大，是未經過非線性優化獲取的初始值。 r然后發布最新的(P,Q,U)值;

3.預積分

當accbuff和gyrbuff中存儲了足夠的IMU數據時,將對IMU進行預計分;
這部分內容在函數processMeasureMent()中;
首先,對imu的時間進行判斷，將隊列里的imu數據放入到accVector和gyrVector中，完成之后返回true;
在函數getIMUInterval中:

getIMUInterval(prevTime, curTime, accVector, gyrVector);

IMU數據處理過程:

1. 如果沒有初始化,則初始化IMU位姿
2. 計算IMU量測之間的dt(時間)
3. 調用IMU

  if(USE_IMU)
  {
      if(!initFirstPoseFlag)
          initFirstIMUPose(accVector);
      for(size_t i = 0; i < accVector.size(); i++)
      {
          double dt;//計算每次imu量測之間的dt
          if(i == 0)
              dt = accVector[i].first - prevTime;
          else if (i == accVector.size() - 1)
              dt = curTime - accVector[i - 1].first;
          else
              dt = accVector[i].first - accVector[i - 1].first;

          processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
      }
  }

函數processIMU流程:

void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
    // 第一個imu處理
    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]};
    }

    //f rame_count是窗內圖像幀的計數
    // 一個窗內有是個相機幀，每個相機幀之間又有多個IMU數據
    if (frame_cobunt != 0)
    {
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        // push_back進行了重載，的時候就已經進行了預積分

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

        // 對位移速度等進行累加
        // Rs Ps Vs是frame_count這一個圖像幀開始的預積分值,是在絕對坐標系下的.
        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];//移除了偏執的gyro
        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; 
}