初始化完成之后进入追踪每一帧图像：

Vec4 tres = trackNewCoarse(fh);

对像素使用旋转和位移的作用比来判断运动状态
返回的是第0层残差和三维光流信息，用来判断是否生成关键帧；
[ step 1 ]生成5+26种运动模式。
[ step 2 ]遍历5+26种运动模式，判断追踪效果：
对新来的帧进行跟踪, 优化得到位姿, 光度参数，把到目前为止最好的残差值作为每一层的阈值,粗层的能量值大, 也不继续优化了, 来节省时间。

bool trackingIsGood = coarseTracker->trackNewestCoarse(
                fh, lastF_2_fh_this, aff_g2l_this,
                pyrLevelsUsed-1,
                achievedRes);

• 使用金字塔进行跟踪, 从顶层向下开始跟踪
  [ step 1 ] 计算残差, 保证最多60%残差大于阈值, 计算正规方程
• 保证大于阈值的点小于60%
  [ step 2 ] LM迭代优化
  [ step 2.1 ] 计算增量
  [ step 2.2 ] 使用增量更新后, 重新计算能量值
  [ step 2.3 ] 接受则求正规方程, 继续迭代, 优化到增量足够小
  [ step 3 ] 记录上一次残差, 光流指示, 如果调整过阈值则重新计算这一层。如果算出来大于阈值,说明初始值不好,及时return,不浪费时间,最好的直接放弃。
  [ step 4 ] 判断优化失败情况,求解的变化太大,不可能突变特别多,说明求解错误

[ step 3 ] 如果跟踪正常, 并且0层残差比最好的还好留下位姿, 保存最好的每一层的能量值。
[ step 4 ] 小于阈值则暂停, 并且为下次设置阈值；把这次得到的最好值给下次用来当阈值。

IMU预积分

1.成员变量有:

    // delta measurements, position/velocity/rotation(matrix)
    Eigen::Vector3d _delta_P;    // P_k+1 = P_k + V_k*dt + R_k*a_k*dt*dt/2
    Eigen::Vector3d _delta_V;    // V_k+1 = V_k + R_k*a_k*dt
    Eigen::Matrix3d _delta_R;    // R_k+1 = R_k*exp(w_k*dt).     note: Rwc, Rwc'=Rwc*[w_body]x

    // jacobian of delta measurements w.r.t bias of gyro/acc
    Eigen::Matrix3d _J_P_Biasg;     // position / gyro
    Eigen::Matrix3d _J_P_Biasa;     // position / acc
    Eigen::Matrix3d _J_V_Biasg;     // velocity / gyro
    Eigen::Matrix3d _J_V_Biasa;     // velocity / acc
    Eigen::Matrix3d _J_R_Biasg;   // rotation / gyro

    // noise covariance propagation of delta measurements
    Mat99 _cov_P_V_Phi;

    double _delta_time;

2.成员函数有:

// reset to initial state
void reset();

// incrementally update 1)delta measurements, 2)jacobians, 3)covariance matrix
void update(const Vec3& omega, const Vec3& acc, const double& dt);


inline Sophus::Quaterniond normalizeRotationQ(const Sophus::Quaterniond& r)
{
    Sophus::Quaterniond _r(r);
    if (_r.w()<0)
    {
        _r.coeffs() *= -1;
    }
    return _r.normalized();
}

inline Mat33 normalizeRotationM (const Mat33& R)
{
    Sophus::Quaterniond qr(R);
    return normalizeRotationQ(qr).toRotationMatrix();
}

3.主要是IMUPreintegrator::update:

// incrementally update 1)delta measurements, 2)jacobians, 3)covariance matrix
// acc: acc_measurement - bias_a, last measurement!! not current measurement
// omega: gyro_measurement - bias_g, last measurement!! not current measurement
void IMUPreintegrator::update(const Vec3& omega, const Vec3& acc, const double& dt)
{
    double dt2 = dt*dt;

    Mat33 dR = Expmap(omega*dt);
    Mat33 Jr = JacobianR(omega*dt);

    // noise covariance propagation of delta measurements
    // err_k+1 = A*err_k + B*err_gyro + C*err_acc
    Mat33 I3x3 = Mat33::Identity();
    Mat99 A = Mat99::Identity();
    A.block<3,3>(6,6) = dR.transpose();
    A.block<3,3>(3,6) = -_delta_R*skew(acc)*dt;
    A.block<3,3>(0,6) = -0.5*_delta_R*skew(acc)*dt2;
    A.block<3,3>(0,3) = I3x3*dt;
    Mat93 Bg = Mat93::Zero();
    Bg.block<3,3>(6,0) = Jr*dt;
    Mat93 Ca = Mat93::Zero();
    Ca.block<3,3>(3,0) = _delta_R*dt;
    Ca.block<3,3>(0,0) = 0.5*_delta_R*dt2;
    _cov_P_V_Phi = A*_cov_P_V_Phi*A.transpose() +
        Bg*GyrCov*Bg.transpose() +
        Ca*AccCov*Ca.transpose();


    // jacobian of delta measurements w.r.t bias of gyro/acc
    // update P first, then V, then R
    _J_P_Biasa += _J_V_Biasa*dt - 0.5*_delta_R*dt2;
    _J_P_Biasg += _J_V_Biasg*dt - 0.5*_delta_R*skew(acc)*_J_R_Biasg*dt2;
    _J_V_Biasa += -_delta_R*dt;
    _J_V_Biasg += -_delta_R*skew(acc)*_J_R_Biasg*dt;
    _J_R_Biasg = dR.transpose()*_J_R_Biasg - Jr*dt;

    // delta measurements, position/velocity/rotation(matrix)
    // update P first, then V, then R. because P's update need V&R's previous state
    _delta_P += _delta_V*dt + 0.5*_delta_R*acc*dt2;    // P_k+1 = P_k + V_k*dt + R_k*a_k*dt*dt/2
    _delta_V += _delta_R*acc*dt;
    _delta_R = normalizeRotationM(_delta_R*dR);  // normalize rotation, in case of numerical error accumulation


//    // noise covariance propagation of delta measurements
//    // err_k+1 = A*err_k + B*err_gyro + C*err_acc
//    Mat33 I3x3 = Mat33::Identity();
//    MatrixXd A = MatrixXd::Identity(9,9);
//    A.block<3,3>(6,6) = dR.transpose();
//    A.block<3,3>(3,6) = -_delta_R*skew(acc)*dt;
//    A.block<3,3>(0,6) = -0.5*_delta_R*skew(acc)*dt2;
//    A.block<3,3>(0,3) = I3x3*dt;
//    MatrixXd Bg = MatrixXd::Zero(9,3);
//    Bg.block<3,3>(6,0) = Jr*dt;
//    MatrixXd Ca = MatrixXd::Zero(9,3);
//    Ca.block<3,3>(3,0) = _delta_R*dt;
//    Ca.block<3,3>(0,0) = 0.5*_delta_R*dt2;
//    _cov_P_V_Phi = A*_cov_P_V_Phi*A.transpose() +
//        Bg*IMUData::getGyrMeasCov*Bg.transpose() +
//        Ca*IMUData::getAccMeasCov()*Ca.transpose();

    // delta time
    _delta_time += dt;

}