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