初始化完成之后进入追踪每一帧图像：
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;
}