ＹＧＺ算法中关于 Tracking::TrackWithSparseAlignment(bool bTrackLastKF)

事先声明：本文只专注与vo，imu部分的代码请自己补充

bool Tracking::TrackWithSparseAlignment(bool bTrackLastKF)

具体流程：

if (!mbUseIMU || !mpLocalMapper->GetVINSInited())

{

// 不使用IMU时，利用速度模型估计当前帧位姿　和orb的tackingwithmotionmodel函数类型是一样的

mCurrentFrame.SetPose(mVelocity * mLastFrame.mTcw);

 

 

//判断函数，确保lastframe中的有效点的数量超过３０个

size_t inliers_in_last_frame = 0;

for (int i = 0; i < mLastFrame.N; i++)

if (mLastFrame.mvpMapPoints[i] && mLastFrame.mvpMapPoints[i]->isBad() == false &&

mLastFrame.mvbOutlier[i] == false)

inliers_in_last_frame++;

if (inliers_in_last_frame < 30)

{

LOG(WARNING) << "Last frame have less observations: " << inliers_in_last_frame

<< ", sparse alignment may have a erroneous result, return back to feature method." << endl;

return false;

}

//关键点的函数就是mpAlign->run()

SE3f TCR;

size_t ret = mpAlign->run(&mLastFrame, &mCurrentFrame, TCR);

//优化后的便计算出mcurrentFrame的ＳＥ３

mCurrentFrame.SetPose(TCR * mLastFrame.mTcw);

return true;

整个函数结束，和orb直接跟踪相机运动模型其实区别也不是很大　只是在方法中替换成了光流法，以下对光流法计算进行详细的解释

 

 

ret = mpAlign->run(&mLastFrame, &mCurrentFrame, TCR); 利用稀疏对齐是为了计算出参考帧到当前帧的SE3

函数的具体实现：

主要实现的功能是构造图像的雅克比矩阵，然后对图像的每一个金字塔层都进行一次优化，优化是采用父类的优化函数，目前先只考虑采用其中一个高斯牛顿的优化方式，优化部分见本段代码之后的另一份

 

size_t SparseImgAlign::run(Frame *ref_frame, Frame *cur_frame, SE3f &TCR)

{

reset();

if (ref_frame->mvKeys.empty())

{

LOG(WARNING) << "SparseImgAlign: no features to track!" << endl;

return 0;

}

 

ref_frame_ = ref_frame;

cur_frame_ = cur_frame;

 

ref_patch_cache_ = cv::Mat(ref_frame->N, patch_area_, CV_32F);//Ｎ×１６

jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows * patch_area_);//为上述矩阵构建一个６×()的雅克比矩阵

visible_fts_ = vector<bool>(ref_patch_cache_.rows, false);

//对参考帧到当前帧的SE3赋初值

SE3f T_cur_from_ref(cur_frame->mTcw * ref_frame_->mTcw.inverse());

 

int iterations[] = {10, 10, 10, 10, 10, 10};

for (level_ = max_level_; level_ >= min_level_; level_ -= 1)

{

mu_ = 0.1;

jacobian_cache_.setZero();

have_ref_patch_cache_ = false;

n_iter_ = iterations[level_];

optimize(T_cur_from_ref);

}

 

TCR = T_cur_from_ref;

return n_meas_ / patch_area_;

//返回的是一个　ｎ_meas()/patch_area_(参考块的大小，等于１６)

}

关于优化部分的代码分析：

父类优化函数：本文选择高斯牛顿的方法进行迭代

template<int D, typename T>

void NLLSSolver<D, T>::optimize(ModelType &model)

{

if (method_ == GaussNewton) {

optimizeGaussNewton(model);

} else if (method_ == LevenbergMarquardt) {

optimizeLevenbergMarquardt(model);

}

}

优化部分代码还是很长的，具体请参考源代码，只大致记录函数具体功能

for (iter_ = 0; iter_ < n_iter_; ++iter_)在这里面计算残差，正常情况下收敛就会停止优化

new_chi2 = computeResiduals(model, true, false)　

函数功能的具体功能实现是SparseImgAlign::computeResiduals

其实在计算的过程中都是赋一个Tcur_ref的一个初值，匹配的方法是利用计算描述子之间的距离来寻找到当前参考块上的匹配点，优化的时候直接优化冲投影误差就好了，光度不变性则利用参考帧投影到当前帧一整个图像块的灰度值计算差值