初始化完成之后進(jìn)入追蹤每一幀圖像：
Vec4 tres = trackNewCoarse(fh);
對(duì)像素使用旋轉(zhuǎn)和位移的作用比來判斷運(yùn)動(dòng)狀態(tài)
返回的是第0層殘差和三維光流信息，用來判斷是否生成關(guān)鍵幀；
[ step 1 ]生成5+26種運(yùn)動(dòng)模式氯檐。
[ step 2 ]遍歷5+26種運(yùn)動(dòng)模式强胰，判斷追蹤效果：
對(duì)新來的幀進(jìn)行跟蹤, 優(yōu)化得到位姿, 光度參數(shù)哼鬓，把到目前為止最好的殘差值作為每一層的閾值,粗層的能量值大, 也不繼續(xù)優(yōu)化了, 來節(jié)省時(shí)間毯辅。
bool trackingIsGood = coarseTracker->trackNewestCoarse(
fh, lastF_2_fh_this, aff_g2l_this,
pyrLevelsUsed-1,
achievedRes);
- 使用金字塔進(jìn)行跟蹤, 從頂層向下開始跟蹤
[ step 1 ] 計(jì)算殘差, 保證最多60%殘差大于閾值, 計(jì)算正規(guī)方程 - 保證大于閾值的點(diǎn)小于60%
[ step 2 ] LM迭代優(yōu)化
[ step 2.1 ] 計(jì)算增量
[ step 2.2 ] 使用增量更新后, 重新計(jì)算能量值
[ step 2.3 ] 接受則求正規(guī)方程, 繼續(xù)迭代, 優(yōu)化到增量足夠小
[ step 3 ] 記錄上一次殘差, 光流指示, 如果調(diào)整過閾值則重新計(jì)算這一層发钝。如果算出來大于閾值,說明初始值不好,及時(shí)return,不浪費(fèi)時(shí)間,最好的直接放棄菇篡。
[ step 4 ] 判斷優(yōu)化失敗情況,求解的變化太大,不可能突變特別多,說明求解錯(cuò)誤
[ step 3 ] 如果跟蹤正常, 并且0層殘差比最好的還好留下位姿, 保存最好的每一層的能量值漩符。
[ step 4 ] 小于閾值則暫停, 并且為下次設(shè)置閾值；把這次得到的最好值給下次用來當(dāng)閾值驱还。
IMU預(yù)積分
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.成員函數(shù)有:
// 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;
}