1、參考關鍵幀追蹤模式

bool Tracking::TrackReferenceKeyFrame()

??對參考關鍵幀中的路標點進行跟蹤。在Tracking線程中，每傳入一幀，都會進行位姿優化。
??以上一幀的位姿為當前位姿進行優化。
（1）計算當前幀的詞袋

mCurrentFrame.ComputeBoW();

（2）通過詞袋加速算法，獲得當前幀的特征點與參考幀的路標點間的聯系。當匹配數小于15時，退出。

    // Step 2：通過特征點的BoW加快當前幀與參考幀之間的特征點匹配// 特征點的匹配關系由MapPoints進行維護int nmatches = matcher.SearchByBoW(mpReferenceKF,          //參考關鍵幀mCurrentFrame,          //當前幀vpMapPointMatches);     //存儲匹配關系//這里的匹配數超過15才繼續if(nmatches<15)return false;

（3）以上一幀的位姿為當前位姿，通過重投影誤差進行優化

mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw設置初值，在PoseOptimization可以收斂快一些
// Step 4:通過優化3D-2D的重投影誤差來獲得位姿Optimizer::PoseOptimization(&mCurrentFrame);

（4）刪除外點
??根據BA優化的結果，判斷那些點是外點。如果是外點的話，就刪除其痕跡：路標點的指針置為NULL等。如果是內點的話，計數器加一。

	int nmatchesMap = 0;for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i]){//如果對應到的某個特征點是外點if(mCurrentFrame.mvbOutlier[i]){//清除它在當前幀中存在過的痕跡MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);mCurrentFrame.mvbOutlier[i]=false;pMP->mbTrackInView = false;pMP->mnLastFrameSeen = mCurrentFrame.mnId;nmatches--;}else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)//匹配的內點計數++nmatchesMap++;}}

（4）如果內點的數目大于10的話，返回追蹤成功。此時當前幀的位姿已經被優化。

2、恒速運動模型

bool Tracking::TrackWithMotionModel

?&emps;根據前兩幀的運動估計當前幀的位姿，作為優化初值。

（1）更新上一幀位姿。如果是雙目、RGB-D相機的話，還會將未創建為地圖點的三維點（雙目相機可獲得深度信息）作為臨時地圖點加入到地圖中。

UpdateLastFrame();

（2）根據恒速運動模型獲得當前幀的位姿。mVelocity為估計的上一幀到當前幀的位姿變換Tcl。Tcl*Tlw=Tcw

mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);

（3）根據上一幀特征點對應地圖點進行投影匹配，返回匹配上的個數。
（4）如果匹配個數不夠的話，擴大搜索半徑。如果匹配個數（特征點-特征點的聯系、特征點-路標點的聯系含義是一樣的）還不夠，直接返回。
（4）僅優化位姿。
（5）去除外點，根據剩余的匹配數量，返回是否匹配成功。追蹤模式的要求更加嚴格（個人認為是追蹤模式只有位姿約束，因此需要更多的位姿進行約束。）。
?
?

重定位（最后的拯救措施）

??以PNP估計結果作為優化初值。

（1）使用詞袋模型找到當前幀的相似關鍵幀集合

    mCurrentFrame.ComputeBoW();// Relocalization is performed when tracking is lost// Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation// Step 2：找到與當前幀相似的候選關鍵幀組vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);// 如果沒有候選關鍵幀，則退出if(vpCandidateKFs.empty())return false;

（2）使用ORB特征點檢測候選關鍵幀，對合適的關鍵幀進行優化
vvpMapPointMatches：存放當前幀對每個關鍵幀成功匹配上的路標點的數量。
vbDiscarded：某個候選關鍵幀是否要放棄。
1）使用詞袋快速匹配，匹配成功數量小于15的話，直接放棄

            int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);// 如果和當前幀的匹配數小于15,那么只能放棄這個關鍵幀if(nmatches<15){vbDiscarded[i] = true;continue;}

2）對匹配的路標點數目大于15的每個候選關鍵幀，設置pnp求解參數

				// 參數為當前幀、當前幀的特征點索引---->路標點PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);pSolver->SetRansacParameters(0.99,   //用于計算RANSAC迭代次數理論值的概率10,     //最小內點數, 但是要注意在程序中實際上是min(給定最小內點數,最小集,內點數理論值),不一定使用這個300,    //最大迭代次數4,      //最小集(求解這個問題在一次采樣中所需要采樣的最少的點的個數,對于Sim3是3,EPnP是4),參與到最小內點數的確定過程中0.5,    //這個是表示(最小內點數/樣本總數);實際上的RANSAC正常退出的時候所需要的最小內點數其實是根據這個量來計算得到的5.991); // 自由度為2的卡方檢驗的閾值,程序中還會根據特征點所在的圖層對這個閾值進行縮放vpPnPsolvers[i] = pSolver;nCandidates++;

（3）遍歷候選關鍵幀，進行RANSAC+EPNP迭代估計相機位姿。

            // Step 4.1：通過EPnP算法估計姿態，迭代5次PnPsolver* pSolver = vpPnPsolvers[i];// 獲得初步估計的相機位姿cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

（4）對每個候選關鍵幀的位姿進行優化

int nGood = Optimizer::PoseOptimization(&mCurrentFrame);

（5）如果匹配到的內點數目不夠，則將參考關鍵幀中的地圖點投影至當前幀中，形成新的匹配。如果夠了的話，直接結束循環。

??之前是通過詞袋加速算法獲得了當前幀中的匹配到的路標點，會有遺漏。這里將參考關鍵幀中的所有路標點再次進行投影。
（6）再次進行優化

nGood = Optimizer::PoseOptimization(&mCurrentFrame);

（7）如果不行的話，再用嚴格的標準投影一遍

                        if(nGood>30 && nGood<50){// 用更小窗口、更嚴格的描述子閾值，重新進行投影搜索匹配sFound.clear();for(int ip =0; ip<mCurrentFrame.N; ip++)if(mCurrentFrame.mvpMapPoints[ip])sFound.insert(mCurrentFrame.mvpMapPoints[ip]);nadditional =matcher2.SearchByProjection(mCurrentFrame,          //當前幀vpCandidateKFs[i],      //候選的關鍵幀sFound,                 //已經找到的地圖點，不會用于PNP3,                      //新的窗口閾值，會乘以金字塔尺度64);                    //匹配的ORB描述子距離應該小于這個閾值// Final optimization// 如果成功挽救回來，匹配數目達到要求，最后BA優化一下if(nGood+nadditional>=50){nGood = Optimizer::PoseOptimization(&mCurrentFrame);//更新地圖點for(int io =0; io<mCurrentFrame.N; io++)if(mCurrentFrame.mvbOutlier[io])mCurrentFrame.mvpMapPoints[io]=NULL;}//如果還是不能夠滿足就放棄了}

（8）如果還行，只能放棄

總結

以上是生活随笔為你收集整理的ORB_SLAM2中Tracking线程的三种追踪方式的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。