视觉和imu（惯性传感器）( 一)

转：https://blog.csdn.net/qq_18661939/article/details/53574981

目前单目slam存在初始化的尺度问题和追踪的尺度漂移问题，而双目也存在精度不高和鲁棒性不好的问题。针对这些问题，提出了融合imu的想法。

那么imu的作用是什么呢？

单目

（1)解决初始化尺度问题

（2)追踪中提供较好的初始位姿。

（3)提供重力方向

（4)提供一个时间误差项以供优化

双目

（1)追踪中提供较好的初始位姿。

（2)提供重力方向

（3)提供一个时间误差项以供优化

目前做这方面融合论文很多，但开源的比较少，这里给出几个比较好的开源code和论文

开源code：

（1）imu和单目的数据融合开源代码（EKF)

https://github.com/ethz-asl/rovio

（2）imu和单目的数据融合开源代码

https://github.com/ethz-asl/okvis_ros(非线性优化）

（3）orbslam+imu（立体相机）

https://github.com/JzHuai0108/ORB_SLAM

论文：

（1)Keyframe-based visual–inertial odometry（okvis的论文）

（2) IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation（预积分）

（3)Visual-Inertial Monocular SLAM with Map Reuse （orb+imu）

（4)Robust Visual Inertial Odometry Using a Direct EKF-Based Approach（eth的rovio）

（5)On-Manifold Preintegration for Real-Time Visual-Inertial Odometry（gtsam）

由于是初学比较详细看得就是以上5篇，而且自认为还不错的论文。

本人研究的是基于非线性优化的视觉和imu融合的算法研究，那么这里先引出融合的方式：

滤波方法：

（1)紧耦合

（2)松耦合

非线性优化：

（1）紧耦合（本人研究方向）

（2)松耦合

imu'和视觉是怎样融合的呢？

仅仅视觉的时候我们优化的只是重投影误差项：

以上的公式我就不解释了。

而imu+视觉优化的是重投影误差项+imu的时间误差项：

其中imu时间误差项：

其中为：

这里：imu时间误差项要求的主要有5个变量：eR，ev，ep，eb，W。即求（R ，v，p，b，W）

这里先给出一张非线性优化视觉+imu融合的图：

下面我们就开始用与积分的方式求以上的6个变量，下面给出预积分的code

Eigen::Matrix4d Tracking::propagate(const double time_frame)
{
    bool is_meas_good=getObservation(time_frame);
    assert(is_meas_good);
    time_pair[0]=time_pair[1];
    time_pair[1]=time_frame;
    Eigen::Vector3d tempVs0inw;
    Eigen::Matrix<double, 15,15>* holder=NULL;
    if(bPredictCov)
        holder= &P_;
    predictStates(T_s1_to_w, speed_bias_1, time_pair,
                                 measurement, imu_.gwomegaw, imu_.q_n_aw_babw,
                                 &pred_T_s2_to_w, &tempVs0inw);
    pred_speed_bias_2.head<3>()=tempVs0inw;//速度偏差
    pred_speed_bias_2.tail<6>()=speed_bias_1.tail<6>();     //biases do not change in propagation
   Eigen::Matrix4d pred_Tr_delta=pred_T_s2_to_w*imu_.T_imu_from_cam;//camera-imu-world（矩阵的乘法从左开始）
   cam_to_w=pred_Tr_delta;
   pred_Tr_delta=pred_Tr_delta.inverse()*(T_s1_to_w*imu_.T_imu_from_cam);//由imu计算（预测）上一帧-》当前帧的变换关
  // T_s1_to_w=pred_T_s2_to_w;
   speed_bias_1=pred_speed_bias_2;
   return pred_Tr_delta;
}

void Tracking::predictStates(const Eigen::Matrix4d  &T_sk_to_w, const Eigen::Matrix<double, 9,1>& speed_bias_k,
                   const double * time_pair,
                   std::vector<Eigen::Matrix<double, 7,1> >& measurements, const Eigen::Matrix<double, 6,1> & gwomegaw,
                   const Eigen::Matrix<double, 12, 1>& q_n_aw_babw,
                   Eigen::Matrix4d  * pred_T_skp1_to_w, Eigen::Matrix<double, 3,1>* pred_speed_kp1,
                   Eigen::Matrix<double, 15,15> *covariance,
                   Eigen::Matrix<double, 15,15>  *jacobian)
{
    double time=time_pair[0],end = time_pair[1];
    // the eventual covariance has little to do with this param as long as it remains small
    Eigen::Matrix<double, 3,1>  r_0(T_sk_to_w.topRightCorner<3, 1>());
    Eigen::Matrix<double,3,3> C_WS_0(T_sk_to_w.topLeftCorner<3, 3>());
    Eigen::Quaternion<double>  q_WS_0(C_WS_0);

    Eigen::Quaterniond Delta_q(1,0,0,0);
    Eigen::Matrix3d C_integral = Eigen::Matrix3d::Zero();
    Eigen::Matrix3d C_doubleintegral = Eigen::Matrix3d::Zero();
    Eigen::Vector3d acc_integral = Eigen::Vector3d::Zero();
    Eigen::Vector3d acc_doubleintegral = Eigen::Vector3d::Zero();

    Eigen::Matrix3d cross = Eigen::Matrix3d::Zero();

    // sub-Jacobians
    Eigen::Matrix3d dalpha_db_g = Eigen::Matrix3d::Zero();
    Eigen::Matrix3d dv_db_g = Eigen::Matrix3d::Zero();
    Eigen::Matrix3d dp_db_g = Eigen::Matrix3d::Zero();

    // the Jacobian of the increment (w/o biases)
    Eigen::Matrix<double,15,15> P_delta = Eigen::Matrix<double,15,15>::Zero();
    double Delta_t = 0;
    bool hasStarted = false;
    int i = 0;
    for (int it=0;it<measurements.size();it++)
    {
        Eigen::Vector3d omega_S_0 =measurements[it].block<3,1>(4,0);//角速度
        Eigen::Vector3d acc_S_0 = measurements[it].block<3,1>(1,0);//线加速度
        Eigen::Vector3d omega_S_1 = measurements[it+1].block<3,1>(4,0);
        Eigen::Vector3d acc_S_1 = measurements[it+1].block<3,1>(1,0);
        ave_omega_S=ave_omega_S+omega_S_0;
        ave_omega_S=ave_omega_S/(it+1);
        // time delta
        double nexttime;
       if ((it + 1) == (measurements.size()-1)) {
          nexttime = end;
        }
        else
          nexttime =measurements [it + 1][0];
        double dt = (nexttime - time);

        if ( end < nexttime) {
          double interval = (nexttime - measurements[it][0]);
          nexttime = end;
          dt = (nexttime - time);
          const double r = dt / interval;
          omega_S_1 = ((1.0 - r) * omega_S_0 + r * omega_S_1).eval();
          acc_S_1 = ((1.0 - r) * acc_S_0 + r * acc_S_1).eval();
        }
      /* if ( it+1==measurements.size()) {
          double interval = last_dt;
          nexttime = end;
          double dt = (nexttime - time);
          const double r = dt / interval;
          omega_S_1 = ((1.0 - r) * omega_S_0 + r * omega_S_1).eval();
          acc_S_1 = ((1.0 - r) * acc_S_0 + r * acc_S_1).eval();
        }
        else
        nexttime =measurements [it + 1][0];
          double dt = (nexttime - time);*/
      if (dt <= 0.0) {
          continue;
        }
        Delta_t += dt;

    if (!hasStarted) {
      hasStarted = true;
      const double r = dt / (nexttime -measurements[it][0]);
      omega_S_0 = (r * omega_S_0 + (1.0 - r) * omega_S_1).eval();//求开始是加权的角速度和线加速度
      acc_S_0 = (r * acc_S_0 + (1.0 - r) * acc_S_1).eval();
    }
    // ensure integrity
    double sigma_g_c = q_n_aw_babw(3);//Gyroscope noise density.
    double sigma_a_c = q_n_aw_babw(1);
    // actual propagation
    // orientation:
    Eigen::Quaterniond dq;
    const Eigen::Vector3d omega_S_true = (0.5*(omega_S_0+omega_S_1) - speed_bias_k.segment<3>(3));//w
    const double theta_half = omega_S_true.norm() * 0.5 * dt;
    const double sinc_theta_half = ode(theta_half);
    const double cos_theta_half = cos(theta_half);
    dq.vec() = sinc_theta_half * omega_S_true * 0.5 * dt;
    dq.w() = cos_theta_half;
    Eigen::Quaterniond Delta_q_1 = Delta_q * dq;
    // rotation matrix integral:
    const Eigen::Matrix3d C = Delta_q.toRotationMatrix();
    const Eigen::Matrix3d C_1 = Delta_q_1.toRotationMatrix();
    const Eigen::Vector3d acc_S_true = (0.5*(acc_S_0+acc_S_1) - speed_bias_k.segment<3>(6));
    const Eigen::Matrix3d C_integral_1 = C_integral + 0.5*(C + C_1)*dt;
    const Eigen::Vector3d acc_integral_1 = acc_integral + 0.5*(C + C_1)*acc_S_true*dt;
    // rotation matrix double integral:
    C_doubleintegral += C_integral*dt + 0.25*(C + C_1)*dt*dt;
    acc_doubleintegral += acc_integral*dt + 0.25*(C + C_1)*acc_S_true*dt*dt;
    // Jacobian parts
    dalpha_db_g += dt*C_1;
    const Eigen::Matrix3d cross_1 = dq.inverse().toRotationMatrix()*cross +
    okvis::kinematics::rightJacobian(omega_S_true*dt)*dt;
    const Eigen::Matrix3d acc_S_x = crossMx(acc_S_true);
    Eigen::Matrix3d dv_db_g_1 = dv_db_g + 0.5*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);
    dp_db_g += dt*dv_db_g + 0.25*dt*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);

    // covariance propagation
    if (covariance) {
      Eigen::Matrix<double,15,15> F_delta = Eigen::Matrix<double,15,15>::Identity();
      // transform
      F_delta.block<3,3>(0,3) = -crossMx(acc_integral*dt + 0.25*(C + C_1)*acc_S_true*dt*dt);
      F_delta.block<3,3>(0,6) = Eigen::Matrix3d::Identity()*dt;
      F_delta.block<3,3>(0,9) = dt*dv_db_g + 0.25*dt*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);
      F_delta.block<3,3>(0,12) = -C_integral*dt + 0.25*(C + C_1)*dt*dt;
      F_delta.block<3,3>(3,9) = -dt*C_1;
      F_delta.block<3,3>(6,3) = -crossMx(0.5*(C + C_1)*acc_S_true*dt);
      F_delta.block<3,3>(6,9) = 0.5*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);
      F_delta.block<3,3>(6,12) = -0.5*(C + C_1)*dt;
      P_delta = F_delta*P_delta*F_delta.transpose();
      // add noise. Note that transformations with rotation matrices can be ignored, since the noise is isotropic.
      //F_tot = F_delta*F_tot;
      const double sigma2_dalpha = dt * sigma_g_c * sigma_g_c;
      P_delta(3,3) += sigma2_dalpha;
      P_delta(4,4) += sigma2_dalpha;
      P_delta(5,5) += sigma2_dalpha;
      const double sigma2_v = dt * sigma_a_c * q_n_aw_babw(1);
      P_delta(6,6) += sigma2_v;
      P_delta(7,7) += sigma2_v;
      P_delta(8,8) += sigma2_v;
      const double sigma2_p = 0.5*dt*dt*sigma2_v;
      P_delta(0,0) += sigma2_p;
      P_delta(1,1) += sigma2_p;
      P_delta(2,2) += sigma2_p;
      const double sigma2_b_g = dt * q_n_aw_babw(9) * q_n_aw_babw(9);
      P_delta(9,9)   += sigma2_b_g;
      P_delta(10,10) += sigma2_b_g;
      P_delta(11,11) += sigma2_b_g;
      const double sigma2_b_a = dt * q_n_aw_babw(6) * q_n_aw_babw(6);
      P_delta(12,12) += sigma2_b_a;
      P_delta(13,13) += sigma2_b_a;
      P_delta(14,14) += sigma2_b_a;
    }

    // memory shift
    Delta_q = Delta_q_1;
    C_integral = C_integral_1;
    acc_integral = acc_integral_1;
    cross = cross_1;
    dv_db_g = dv_db_g_1;
    time = nexttime;
    interval_time=Delta_t;
     last_dt=dt;

    ++i;

    if (nexttime == end)
      break;

  }
// actual propagation output:
const Eigen::Vector3d g_W = gwomegaw.head<3>();
const Eigen::Vector3d  t=r_0+speed_bias_k.head<3>()*Delta_t+ C_WS_0*(acc_doubleintegral)+0.5*g_W*Delta_t*Delta_t;
const  Eigen::Quaterniond q=q_WS_0*Delta_q;
(*pred_T_skp1_to_w)=rt_to_T(t,q.toRotationMatrix());

(*pred_speed_kp1)=speed_bias_k.head<3>() + C_WS_0*(acc_integral)+g_W*Delta_t;//???语法曾有错误
if (jacobian) {
  Eigen::Matrix<double,15,15> & F = *jacobian;
  F.setIdentity(); // holds for all states, including d/dalpha, d/db_g, d/db_a
  F.block<3,3>(0,3) = -okvis::kinematics::crossMx(C_WS_0*acc_doubleintegral);
  F.block<3,3>(0,6) = Eigen::Matrix3d::Identity()*Delta_t;
  F.block<3,3>(0,9) = C_WS_0*dp_db_g;
  F.block<3,3>(0,12) = -C_WS_0*C_doubleintegral;
  F.block<3,3>(3,9) = -C_WS_0*dalpha_db_g;
  F.block<3,3>(6,3) = -okvis::kinematics::crossMx(C_WS_0*acc_integral);
  F.block<3,3>(6,9) = C_WS_0*dv_db_g;
  F.block<3,3>(6,12) = -C_WS_0*C_integral;
}

// overall covariance, if requested
if (covariance) {
  Eigen::Matrix<double,15,15> & P = *covariance;
  // transform from local increments to actual states
  Eigen::Matrix<double,15,15> T = Eigen::Matrix<double,15,15>::Identity();
  T.topLeftCorner<3,3>() = C_WS_0;
  T.block<3,3>(3,3) = C_WS_0;
  T.block<3,3>(6,6) = C_WS_0;
  P = T * P_delta * T.transpose();
}
}

 

以上code来自以下公式：

其中认为角速度w和加速度a在连续两次imu测量之间是匀速的，因此上式可写成：

其中上式的角速度和加速度：

因此最终公式：

上面公式给出5个变量（R，V，P，b，W）中的3个最重要的变量：R，V，P。

而偏差变量b我们可以初始化的时候可以设为0（其实最好是要求出来的，这里就不给出推倒公式了）。

下面的们就是有关W（权重）的公式了。

其中

是有关R，P，V，b的协方差矩阵

到此为止已经把imu时间误差项求完。

下一篇将是怎样把时间误差项融合到目标函数里（主要是局部地图的优化）

--------------------- 本文来自 金木炎 的CSDN 博客 ，全文地址请点击：https://blog.csdn.net/qq_18661939/article/details/53574981?utm_source=copy