ORB_SLAM2代碼閱讀（2）——Tracking線程

• 1. 說明
• 2. 簡介
• • 2.1 Tracking 流程
  • 2.2 Tracking 線程的二三四
  • • 2.2.1 Tracking 線程的二種模式
    • 2.2.2 Tracking 線程的三種方法
    • 2.2.3 Tracking 線程的四種狀態
  • 2.3 Tracking 類的成員變量
• 3. Tracking線程的各個功能模塊
• • 3.1 初始化
  • 3.2 相機位姿跟蹤
  • • 3.2.1 運動模型跟蹤
    • 3.2.2 參考關鍵幀跟蹤
    • 3.2.3 重定位（三次匹配三次優化）
  • 3.2 局部地圖跟蹤
  • 3.3 關鍵幀處理
  • • 3.3.1 插入關鍵幀的判定依據
    • 3.3.2 判定是否插入關鍵幀的整個流程
    • 3.3.3 創建關鍵幀
  • 3.4 姿態保存
• 4.結束語

1. 說明

本文接著上一篇對于ORB-SLAM的系統介紹繼續記錄ORB-SLAM2的主線程tracking的相關內容。有很多細節部分還沒有弄清楚，暫時先將整體思路捋順。本文的內容最好參照著代碼一起閱讀，由于篇幅較長，并未在文中插入太多代碼。在將整個ORB_SLAM2系統閱讀完畢后，我在公開我注釋的代碼。

2. 簡介

2.1 Tracking 流程

ORB-SLAM的tracking線程作為系統的主線程，也是SLAM前端視覺里程計的主要內容，實現的主要內容就是確定每一幀圖像的位姿和確定關鍵幀。

由系統整體框架可知，tracking部分的主要內容有以下幾個部分：

在閱讀了代碼之后，發現將tracking部分的主要內容劃分為以下幾個模塊比較合適：

下文將對各個部分進行詳述。

2.2 Tracking 線程的二三四

2.2.1 Tracking 線程的二種模式

Tracking線程由兩種模式：1.純追蹤模式 2.同步定位與建圖模式（默認模式）

1. 純追蹤模式：不插入新的關鍵幀，不添加新的地圖點，局部地圖線程不工作，而且回環檢測線程也不會工作，只會追蹤地圖中現有的地圖點。
2. 同步定位與建圖模式：在追蹤線程的同時有局部建圖和回環檢測

2.2.2 Tracking 線程的三種方法

Tracking線程中涉及三種位姿計算方法：運動模型跟蹤、參考關鍵幀跟蹤、重定位

1. 運動模型跟蹤：匹配方式使用上一幀特征點投影到當前幀的方式進行匹配，得到的匹配地圖點作為圖優化節點，然后根據上一幀的位姿和上一幀位姿的變換速度得到當前幀的初始位姿，用位姿圖優化進行位姿優化得到當前幀位姿。
2. 參考關鍵幀跟蹤：匹配方式使用關鍵幀BOW向量與當前幀進行匹配，通過該匹配方式得到地圖點得到的地圖點作為圖優化節點，然后將上一幀的位姿作為初始位姿，用位姿圖優化進行位姿優化得到當前幀位姿。
3. 重定位：匹配方式是將局部地圖點（除去1、2已經匹配過的地圖點剩下的局部地圖點！！！）投影到到當前幀下進行匹配，得到的匹配地圖點作為圖優化節點，然后根據上面1、2得到幀位姿作為當前幀的初始位姿，用位姿圖優化進行位姿優化得到當前幀位姿。

2.2.3 Tracking 線程的四種狀態

Tracking線程有四種狀態：1.NO_IMAGES_YET 2.NOT_INITIALIZED 3.OK 4.LOST

1. NO_IMAGES_YET：表示當前沒有圖片。處于NO_IMAGES_YET，當新的一幀來臨時，將線程狀態改變為NOT_INITIALIZED。
2. NOT_INITIALIZED：表示當前沒有初始化追蹤線程。處于NOT_INITIALIZED，則針對單目相機和雙目相機/RGBD相機進行不同的初始化。
3. OK：表示當前追蹤線程完好。處于OK，經過初始化后追蹤線程就轉為OK狀態，在沒有丟幀或者是復位的情況下系統將一直處于OK狀態。處于OK狀態的系統就可以進行位姿估計，地圖點追蹤。
4. LOST：表示當前追蹤線程丟失——注意這里的線程狀態都是指當前幀處理之前的狀態。處于LOST狀態，上一幀追蹤失敗，當前幀進行重定位。

2.3 Tracking 類的成員變量

Tracking 類中由很多成員變量。直接看代碼會很難理解成員變量代表的意義，所以先簡單介紹一下成員變量。

其中較為重要的變量有：

變量名	變量類型	說明
mState	eTrackingState	跟蹤狀態標志
mbOnlyTracking	bool	跟蹤模式標志
mCurrentFrame	Frame	當前幀
mLastFrame	Frame	上一幀
mpReferenceKF	KeyFrame*	參考關鍵幀
mpLastKeyFrame	KeyFrame*	上一關鍵幀
mvpLocalKeyFrames	std::vector<KeyFrame*>	局部地圖關鍵幀
mvpLocalMapPoints	std::vector<MapPoint*>	局部地圖的地圖點
mpMap	Map*	指代整個地圖
mlRelativeFramePoses	list < cv::Mat>	圖像幀與其參考關鍵幀之間的變換關系鏈表（用于繪制軌跡）
mlpReferences	list<KeyFrame*>	每一幀的參考關鍵幀 （用于繪制軌跡）
mlFrameTimes	list< double >	每一幀的時間戳（用于繪制軌跡）

其它變量在遇到的時候在做說明！

3. Tracking線程的各個功能模塊

Tracking線程的入口是TrackStereo()，其中GrabImageStereo()返回位姿。GrabImageStereo()成員函數將輸入圖像轉換為灰度圖并構建當前幀，然后調用Track()函數。調用Track()函數表示進入了真正的跟蹤流程。

進入跟蹤流程后，下面開始介紹其中的各個功能模塊。

3.1 初始化

這里的初始化指的是追蹤過程的初始化環節，而不是Tracking類構造函數中的初始化內容。順便說一下，構造函數中根據配置文件設置了相應的參數并聲明了ORB特征點提取器。ORB特征提取的過程在構造當前幀的時候進行。

圖像傳輸正常的情況下，追蹤過程的第一步就是判斷是否已經初始化。

判斷部分相應代碼為：

 if(mState==NOT_INITIALIZED)// ———————————————未初始化{if(mSensor==System::STEREO || mSensor==System::RGBD)StereoInitialization();    //雙目和rgbd地圖的初始化elseMonocularInitialization(); // 單目初始化mpFrameDrawer->Update(this);   //更新幀的觀測器if(mState!=OK)return;}else                      // ————————————————已初始化{。。。    }

下面介紹雙目初始化過程。整個雙目初始化的流程為：

初始化過程相對簡單，邏輯并不復雜。我在代碼中進行了注釋，在此不在贅述。

/********************************************************************************     函數屬性：類Tracking的成員函數StereoInitialization()*     函數參數介紹：NULL*     備注：雙目或者RGBDSLAM的第一幀處理函數，地圖初始化******************************************************************************/
void Tracking::StereoInitialization()
{//當前幀關鍵點的數量大于500，才將此幀作為初始幀并認為其為關鍵幀if(mCurrentFrame.N>500)    {// 設置初始幀的位姿mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));// 將當前幀（第一幀）作為初始關鍵幀（調用關鍵幀的構造函數）KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);// 將關鍵幀插入地圖中.  KeyFrame中包含了地圖、反過來地圖中也包含了KeyFrame，相互包含mpMap->AddKeyFrame(pKFini);//  創建地圖點并將其與關鍵幀建立聯系for(int i=0; i<mCurrentFrame.N;i++){float z = mCurrentFrame.mvDepth[i];                       //獲取當前幀第i個關鍵點的深度值if(z>0){cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);      //將當前幀的第i個特征點反投影到3D世界坐標系下MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);   //用該特征點構造新的地圖點pNewMP->AddObservation(pKFini,i);                    //地圖點添加關鍵幀  說明該地圖點屬于哪一關鍵幀pKFini->AddMapPoint(pNewMP,i);                       //關鍵幀添加地圖點  表明在該關鍵幀下可以看到該地圖點pNewMP->ComputeDistinctiveDescriptors();             //從眾多觀測到該MapPoint的特征點中挑選區分讀最高的描述子pNewMP->UpdateNormalAndDepth();                      //更新該MapPoint平均觀測方向以及觀測距離的范圍mpMap->AddMapPoint(pNewMP);                          //將新的地圖點加入到地圖中// 將地圖點加入到當前針的mvpMapPoints中，為當前Frame的特征點與MapPoint之間建立聯系mCurrentFrame.mvpMapPoints[i]=pNewMP; }}cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;//在局部地圖中添加該初始關鍵幀mpLocalMapper->InsertKeyFrame(pKFini);                              //更新上一幀為當前幀mLastFrame = Frame(mCurrentFrame);mnLastKeyFrameId=mCurrentFrame.mnId;// 更新上一關鍵幀為當前關鍵幀mpLastKeyFrame = pKFini;mvpLocalKeyFrames.push_back(pKFini);           //將初始關鍵幀加入到局部地圖的關鍵幀mvpLocalMapPoints=mpMap->GetAllMapPoints();    //將全部地圖點加入到當前局部地圖點mpReferenceKF = pKFini;                        //將當前關鍵幀作為參考關鍵幀mCurrentFrame.mpReferenceKF = pKFini;          //將該關鍵幀作為當前幀的參考關鍵幀mpMap->SetReferenceMapPoints(mvpLocalMapPoints);//將當前局部地圖點作為整個地圖參考地圖點,用于畫圖mpMap->mvpKeyFrameOrigins.push_back(pKFini);    //將關鍵幀加入地圖的原始的關鍵幀mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); //將當前幀加入到地圖觀測器mState=OK;  //更新跟蹤狀態}
}

3.2 相機位姿跟蹤

這部分內容是整個跟蹤流程中非常重要的一部分，邏輯也較為復雜。

首先說明一下該模塊的主要目的是：對當前幀的位姿進行初步估計并優化當前幀對應的地圖點，這里之所以說是對位姿進行初步估計是因為在局部地圖跟蹤環節還要對位姿進行優化。

該部分內容的邏輯流程：

從流程圖中可以看到相機位姿跟蹤主要涉及三種跟蹤方法：運動模型跟蹤、參考關鍵幀跟蹤、重定位（跟蹤丟失時）。

值得一提的是，代碼中有一個 mbVO 變量。這個變量只在純定位模式下才被使用，mbVO為false表示此幀匹配了很多的MapPoints，跟蹤很正常；mbVO為true表明此幀匹配了很少的MapPoints，少于10個，跟蹤效果不好。

在純跟蹤模式下，當mbVO為true時，如果速度mVelocity不為空。則運動模型跟蹤與重定位同時進行。定位和跟蹤的結果分別用bOKMM和bOKReloc表示。 如果是跟蹤成功且重定位失敗，那么結果為跟蹤的結果，但只要是重定位成功，那么整個跟蹤過程就正常進行(定位與跟蹤，更相信重定位)。 最后只要是跟蹤和重定位只要一個成功，那么結果就正常。將最新的關鍵幀作為reference frame。

該部分代碼為：

 // 這時候既做跟蹤又做重定位，定位和跟蹤的結果分別用bOKMM和bOKReloc表示，只要mVelocity不為空就做基于恒速模型跟蹤。// 如果是跟蹤成功重定位失敗，那么結果借用跟蹤的結果，但只要是重定位成功，那么整個跟蹤過程就正常進行(定位與跟蹤，更相信重定位)，
// 最后只要是跟蹤和重定位只要一個成功，那么結果就正常。將最新的關鍵幀作為reference framebool bOKMM = false;bool bOKReloc = false;vector<MapPoint*> vpMPsMM;vector<bool> vbOutMM;cv::Mat TcwMM;if(!mVelocity.empty()){bOKMM = TrackWithMotionModel();    //根據運動模型進行追蹤vpMPsMM = mCurrentFrame.mvpMapPoints;vbOutMM = mCurrentFrame.mvbOutlier;TcwMM = mCurrentFrame.mTcw.clone();}bOKReloc = Relocalization();           //重定位獲取相機位姿if(bOKMM && !bOKReloc)                 // 跟蹤成功,重定位失敗{mCurrentFrame.SetPose(TcwMM);mCurrentFrame.mvpMapPoints = vpMPsMM;mCurrentFrame.mvbOutlier = vbOutMM;if(mbVO)         // 表明此幀匹配了很少的MapPoints，少于10個，跟蹤效果不好  {for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]){mCurrentFrame.mvpMapPoints[i]->IncreaseFound();  //則將當前幀的檢測到的地圖點的查找次數增加}}}}else if(bOKReloc)                      // 重定位成功  則整個跟蹤進程正常進行{mbVO = false;}bOK = bOKReloc || bOKMM;

3.2.1 運動模型跟蹤

運動模型跟蹤的主要思想是：

1. 根據上一幀的位姿和速度來計算當前幀的位姿。
2. 遍歷上一幀中所有地圖點，將上一幀的地圖點向當前幀進行投影，投影過后在當前幀中找到一個描述子距離最相近的特征點作為投影點的匹配點。
3. 如果匹配點的數量滿足要求，則對當前幀進行位姿優化。優化過后剔除外點。
4. 最后根據匹配點的數量判斷跟蹤是否成功。

具體流程如下圖所示：

需要注意的是：這里的速度（mVelocity）指的是上一幀與上上幀之間的位姿變換關系（即，當前幀的前一幀與前兩幀之間的位姿變換關系）。

具體的匹配方法和優化方法在后面章節進行闡述。

3.2.2 參考關鍵幀跟蹤

當運動模型跟蹤失敗或mVelocity為空時，則需要進行參考關鍵幀跟蹤。

參考關鍵幀跟蹤的主要思想是：

1. 首先計算當前幀的詞袋向量。
2. 根據詞袋向量進行參考關鍵幀和當前幀進行匹配 ，得到匹配點。
3. 如果匹配點的數量滿足要求，則將匹配點設為當前幀的地圖點，上一幀的位姿設為當前幀的位姿，并進行優化。
4. 剔除優化后的外點。
5. 根據剔除外點后的匹配點數量判定跟蹤是否成功。

具體流程如下圖所示：

參考關鍵幀跟蹤與運動模型跟蹤的異同點：

1. 不同點：
   (1)、運動模型跟蹤利用上一幀姿態和速度來初始化當前幀位姿，參考幀模型僅僅使用上一幀位姿來初始化當前幀位姿。
   (2)、運動模型跟蹤利用投影匹配，參考幀模型使用詞袋匹配。
   (3)、運動模型跟蹤需要考慮是否為純跟蹤模式，而參考關鍵幀跟蹤則不需要。
   (4)、運動模型跟蹤與上一幀進行匹配，而參考關鍵幀跟蹤則是與參考關鍵幀進行匹配。
2. 相同點：整體思路一致，都是通過初始化位姿、匹配、優化位姿、剔除外點四步來判定跟蹤狀態

3.2.3 重定位（三次匹配三次優化）

重定位的過程相對與運動模型跟蹤和參考關鍵幀跟蹤要復雜一些。可以將重定位的過程總結為 三次匹配三次優化。

重定位過程的主要思路為：

1. 計算當前幀的詞袋向量。
2. 根據詞袋向量在關鍵幀數據庫中挑選出候選關鍵幀。
3. 當前幀與每一個候選關鍵幀進行詞袋匹配得到匹配點的數量，并根據匹配點的數量剔除一部分不合格的候選關鍵幀（第一次匹配）。
4. 根據匹配得到的地圖點和當前幀來設置PNP求解器，PNP求解器迭代五次來初步求解當前幀的位姿。并根據是否達到最大迭代次數剔除一部分不合格的候選關鍵幀。
5. 將匹配得到的地圖點剔除外點后賦值當前幀作為當前幀的地圖點，然后進行位姿優化（第一次優化）。
6. 位姿優化之后，剔除當前幀地圖點中的外點。若此時內點數較少，不能滿足要求，則進行候選關鍵幀與當前幀之間的投影匹配，以此來增加匹配點數量。（第二次匹配）
7. 優化后的內點數與投影匹配得到的匹配點數之和 大于 閾值，則再次進行優化（第二次優化）。
8. 第二次優化后內點數若是還不滿足要求，那么就再次通過縮小窗口進行投影匹配（第三次匹配）。
9. 第二次優化后的內點數與第三次投影匹配得到的匹配點數之和 大于 閾值，則進行最終優化（第三次優化）。
10. 最后，根據優化后的內點數判定重定位是否成功。

具體流程如下圖所示：

重定位的過程中進行了三次匹配三次優化。第一次匹配為詞袋匹配，用于初步確定當前幀的地圖點。后兩次匹配均為投影匹配，目的是為了增加匹配點，為優化位姿做準備。而三次優化的過程是為了根據匹配點不斷的優化當前幀的位姿，使其滿足要求。

3.2 局部地圖跟蹤

要理解局部地圖跟蹤的原理，首先要知道ORB_SLAM2是如何定義局部地圖的。

局部地圖包括兩部分：局部地圖關鍵幀，局部地圖地圖點。

那么如何確定哪些關鍵幀應該作為局部地圖的關鍵幀呢？ORB_SLAM2采用的是這張策略：

1. 將所有能夠觀測到當前幀地圖點的關鍵幀作為局部地圖關鍵幀。
2. 如果局部地圖關鍵幀數量不夠的話將這些關鍵幀的共視關鍵幀、子關鍵幀、父關鍵幀也加入到局部地圖關鍵幀中，直到滿足局部關鍵幀的數量要求。
3. 將能看到當前幀最多地圖點的關鍵幀設為參考關鍵幀,并更新當前幀的參考關鍵幀為該幀。

局部地圖的地圖點即為局部關鍵幀的所有地圖點的集合。

在了解了局部地圖的定義之后，再來看局部地圖跟蹤的目的。

局部地圖跟蹤的目的就是：在得到當前幀的初始位姿之后，根據局部地圖中的地圖點和當前幀進行匹配，然后根據匹配結果進一步優化當前幀的位姿和當前幀的地圖點。

局部地圖跟蹤的主要思路為：

1. 更新局部地圖。將之前的局部地圖數據清空，重新構建局部地圖。構建局部地圖的過程就是重新確定局部地圖關鍵幀和局部地圖地圖點的過程。
2. 局部地圖中的地圖點與當前幀的地圖點進行匹配，然后利用匹配的地圖點來優化當前幀的位姿。
3. 根據優化后的位姿重新更新匹配點的內點和外點。
4. 根據內點數量判定跟蹤是否成功。

局部地圖跟蹤的流程如下圖所示：

相機位姿跟蹤與局部地圖跟蹤之間有什么區別和聯系？

1. 在相機位姿跟蹤過程中，不論是運動模型跟蹤還是參考關鍵幀模型，他們都是根據上一幀的位姿來初步確定當前幀的位姿，然后對位姿進行優化。在該過程中，確定相機位姿的時候只用到了當前幀之前的一幀或兩幀圖像。對之前數據的利用程度不高。
2. 在局部地圖跟蹤過程中，局部地圖更新的前提是必須知道當前幀的姿態和地圖點（盡管可能不準確）。然后局部地圖地圖點與當前幀地圖點進行匹配，然后進行當前幀的位姿優化。在該過程中，利用到了當前幀之前的多幀關鍵幀。這是與相機位姿跟蹤過程最大的區別。可以說，局部地圖跟蹤也是在相機位姿跟蹤基礎之上做的操作，此外局部地圖跟蹤比相機位姿跟蹤利用的信息更多。

3.3 關鍵幀處理

如果相機位姿跟蹤和局部地圖跟蹤都正常的話，接下來需要考慮的問題就是否需要將當前幀作為新的關鍵幀進行存儲。

3.3.1 插入關鍵幀的判定依據

在ORB_SLAM論文中，插入新關鍵幀的條件是：

1. 與上次重定位相比幀數超過了20幀。
2. 建圖線程空閑 或 距上次插入關鍵幀超過20幀。
3. 當前幀跟蹤的特征點少于50個。
4. 當前幀跟蹤的特征點少于參考關鍵幀的90%。

論文的原文如下（ORB_SLAM論文部分內容）：

代碼中插入關鍵幀的判定條件與論文稍微有點區別。

代碼如下（ORB_SLAM代碼）：

   // Condition 1a: More than "MaxFrames" have passed from last keyframe insertionconst bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;// Condition 1b: More than "MinFrames" have passed and Local Mapping is idleconst bool c1b = mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle;// Condition 2: Less than 90% of points than reference keyframe and enough inliersconst bool c2 = mnMatchesInliers<nRefMatches*0.9 && mnMatchesInliers>15;if((c1a||c1b)&&c2){// If the mapping accepts keyframes insert, otherwise send a signal to interrupt BA, but not insert yetif(bLocalMappingIdle){return true;}else{mpLocalMapper->InterruptBA();return false;}}elsereturn false;

從代碼中可以看到，插入關鍵幀的判定條件有三條：

1. 此幀距離上次插入關鍵幀是否超過了最大幀數，其中最大幀數mMaxFrames = 18*fps/30。
2. 此幀距離上次插入關鍵幀已經超過了最小幀數且此時局部地圖線程處于空閑狀態，其中最小幀數mMinFrames = 0。
3. 當前幀跟蹤的特征點少于參考關鍵幀的90% 且 當前幀地圖點中內點數大于15。

在做判定時，這三條判定條件的邏輯關系是 (c1a||c1b)&&c2 ，而并非單純的" 與 "關系。此外，當滿足判定條件時仍然需要在建圖線程空閑時才能插入關鍵幀。

以上內容是ORB_SLAM中插入關鍵幀的判定依據，作者在ORB_SLAM2中對該判定依據又進行了拓展。

ORB_SLAM2論文進行了如下表述。

可以看出，ORB_SLAM2引入了近點和原點來拓展判斷依據，從而使系統能夠應當更有挑戰性的場景。

ORNB_SLAM2中該部分內容的代碼為：

   //近點中被追蹤數量小于100   并且近點未被追蹤點數量大于70bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);     // Thresholds  設置閾值   參考比率(當前幀內點數量/參考關鍵幀中所有地圖點被觀察到的次數大于2或3次的地圖點數量)float thRefRatio = 0.75f;if(nKFs<2)//如果整個地圖中關鍵幀的數量小于2  則閾值設為0.4thRefRatio = 0.4f;if(mSensor==System::MONOCULAR)//  如果是單目相機   比例設為0.9thRefRatio = 0.9f;// Condition 1a: More than "MaxFrames" have passed from last keyframe insertion    const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId + mMaxFrames;// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle     const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+ mMinFrames && bLocalMappingIdle);//Condition  1c: tracking is weak       const bool c1c =  mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers > 15);if((c1a||c1b||c1c)&&c2){// If the mapping accepts keyframes, insert keyframe.// Otherwise send a signal to interrupt BA     if(bLocalMappingIdle)   //如果局部建圖線程處于空閑狀態則返回真{return true;}else   //否則中斷BA優化{mpLocalMapper->InterruptBA();if(mSensor!=System::MONOCULAR){if(mpLocalMapper->KeyframesInQueue()<3)// 如果關鍵幀隊列中關鍵幀數量小于3   則添加關鍵幀return true;elsereturn false;}elsereturn false;}}elsereturn false;

從代碼中可以看到，插入關鍵幀的判定條件為：

1. 此幀距離上次插入關鍵幀是否超過了最大幀數，其中最大幀數mMaxFrames = fps。
2. 此幀距離上次插入關鍵幀已經超過了最小幀數且此時局部地圖線程處于空閑狀態，其中最小幀數mMinFrames = 0。
3. 傳感器類型為非單目 且 匹配點中內點數小于參考關鍵幀中被觀測到兩到三次的地圖點數的0.25倍 且 bNeedToInsertClose標志為真。（bNeedToInsertClose標志為真時表示 近點中被追蹤數量小于100 并且近點未被追蹤點數量大于70）（ps:說起來真拗口，虧他想得出來）。
4. 內點數量大于15 且 匹配點中內點數小于參考關鍵幀中被觀測到兩到三次的地圖點數的thRefRatio倍 或 bNeedToInsertClose標志為真。

在做判定時，這三條判定條件的邏輯關系是(c1a||c1b||c1c)&&c2，當滿足判定條件時仍然考慮建圖線程是否空閑。

3.3.2 判定是否插入關鍵幀的整個流程

在了解了插入關鍵幀的判定依據后，來看判定插入關鍵幀整個流程（該流程為ORB_SLAM2中的流程，而非ORB_SLAM）。
具體流程如下圖所示。

從流程圖中可以看出，是否需要插入關鍵幀不僅僅需要考慮3.3.1中提到的判定條件還有其他前提條件。

3.3.3 創建關鍵幀

一旦當前幀滿足插入關鍵幀的條件，接下來要做的就是創建關鍵幀并插入到建圖線程中。
該過程主要由以下幾步：

1. 將當前幀構造為關鍵幀，將該關鍵幀設置參考關鍵幀和為當前幀的參考關鍵幀。
2. 在非單目的情況下向地圖中添加地圖點。
3. 將關鍵幀插入建圖線程
4. 更新mpLastKeyFrame和mpLastKeyFrameId。

3.4 姿態保存

在經過相機位姿跟蹤、局部地圖跟蹤和關鍵幀處理后，跟蹤線程的大部分工作已經完成。其中還有一些其他的處理細節在這里沒有寫出，但是那部分代碼比較好理解，屬于邊邊角角的工作。

為了在程序結束后能夠將相機的運動軌跡畫出來，跟蹤線程中提供了姿態保存的內容。

姿態保存部分分為兩種情況：跟蹤成功和跟蹤丟失（通過當前幀的位姿是否為空來判定）。

在跟蹤成功時，保存位姿的方式為：

1. 計算當前幀的參考關鍵幀 到 當前幀的位姿變換矩陣 Tcr。
2. 將變換矩陣Tcr 保存到mlRelativeFramePoses鏈表中。
3. 將參考關鍵幀mpReferenceKF保存到mlpReferences鏈表中。
4. 將當前幀的時間戳和跟蹤狀態分別保存到mlFrameTimes和mlbLost鏈表中。

在跟蹤丟失時，保存位姿的方式為：

1. 將mlRelativeFramePoses鏈表中的最后一個元素再次存入該鏈表（相當于在鏈表末端復制了最后一個元素）。
2. 同樣，將mlpReferences鏈表和mlFrameTimes鏈表的最后一個元素再次存入該鏈表。
3. 將跟蹤狀態保存到mlbLost鏈表中。

以這種方式保存姿態，在繪制運動軌跡時需要根據參考關鍵幀的位姿和Tcr來計算每一幀的位姿。另外，跟蹤丟失是運動軌跡將停止在最后一幀跟蹤成功的圖像幀的位置，直到重定位成功或系統重啟。

4.結束語

至此，已經把ORB_SLAM2的Tracking線程的整體流程梳理了一遍。內容并不是很詳細，后期如果有新的體會或感悟再來更新。

參考博客：
orb_slam代碼解析(2)Tracking線程
一起學ORBSLAM2（4）tracking主線程

若本文有錯誤或沒有表達清楚的地方，歡迎指正！
希望能夠共同學習，共同進步！!

總結

以上是生活随笔為你收集整理的ORB_SLAM2代码阅读(2)——tracking线程的全部內容，希望文章能夠幫你解決所遇到的問題。

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