ORB-SLAM2原始碼解讀（2.1）：Tracking
Tracking是SLAM的靈魂，更像是前端里程計VO，這裡Tracking的主要任務兩方面：（1）完成相機位姿估計（2）跟蹤區域性地圖
思路：TrackLocalMap（）在當前幀和區域性地圖之間找到儘可能多的對應關係，優化當前幀的位姿。對每一幀都進行跟蹤
第一次接觸這麼大的工程，發現之前接觸的真的好弱雞，總結下來是：先看基本流程圖+從頭到尾啃一遍程式碼+總結
這裡要感謝吳博和吃水的魚和蟻族的堅持的部落格，大神的部落格給了很大的啟發。
下面重點研究單目作感測器
Tracking執行緒程式碼主要有兩部分:Track()函式為主線，各部分成員函式為分支。第一部分主要講Track（）函式部分程式碼，下一講詳細介紹各分支程式碼及原理。
基本流程框架：
1、單目初始化 MonocularInitialization()
2、相機位姿跟蹤：（1）跟蹤運動模型TrackWithMotionModel()（2）跟蹤關鍵幀TrackReferenceKeyFrame()（3）重定位Relocalization()
3、跟蹤區域性地圖 TrackLocalMap()
流程圖1：
流程圖2
Tracking執行緒主要函式程式碼邏輯
MonocularInitialization()
最初的兩幀進行對極約束和全域性BA優化，計算兩幀位姿、三角化得到地圖點深度
1、建立單目初始器mpInitializer，用Frame()函式構造第一幀mInitialFrame 2、如果當前幀特徵點數大於100，則得到用於單目初始化的第二幀 3、在mInitialFrame與mCurrentFrame中找匹配的特徵點對 4、如果初始化的兩幀之間的匹配點太少，重新初始化 5、通過H模型或F模型進行單目初始化，得到兩幀間相對運動、初始MapPoints 6、CreateInitialMapMonocular()將三角化得到的3D點包裝成MapPoints
TrackWithMotionModel()
假設勻速運動，用上一幀位姿和速度估計當前幀位姿。方法：上一幀地圖點投影到當前幀，完成匹配。
1、假設：兩幀間相對運動不變，根據上一幀位姿和速度用PnP估計當前幀位姿。
2、SearchByProjection（）在將上一幀的地圖點投影到當前固定大小範圍的幀平面上，如果匹配點少，那麼擴大兩倍的採點範圍
3、PoseOptimization（）優化相機位姿，BA演算法，最小二乘法。
4、優化位姿後剔除outlier的mvpMapPointsTrackReferenceKeyFrame()
關鍵幀中查詢BOW相近的幀，進行匹配優化位姿。方法：關鍵幀BOW向量與當前幀進行匹配
1、mCurrentFrame.ComputeBoW()將當前幀的描述子轉為BOW向量，加塊匹配速度
2、matcher.SearchByBoW()通過特徵點的BoW加快當前幀與參考幀之間的特徵點匹配
3、mCurrentFrame.SetPose(mLastFrame.mTcw)將上一幀的位姿態作為當前幀位姿的初始值
4、PoseOptimization通過優化3D-2D的重投影誤差來獲得位姿
5、剔除優化後的outlier匹配點（MapPoints）Relocalization()
重定位，之前所有關鍵幀中查詢與當前幀有充足匹配點的候選幀，Ransac迭代，PnP求解位姿
1、計算當前幀特徵點的Bow對映
2、找到與當前幀相似的候選關鍵幀，mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame)
3、通過EPnP演算法估計姿態
4、通過PoseOptimization對姿態進行優化求解
5、如果內點較少，則通過投影的方式對之前未匹配的點進行匹配，再進行優化求解TrackLocalMap()
投影，從已經生成的地圖點中找到更多對應關係。
1、UpdateLocalMap()更新區域性關鍵幀mvpLocalKeyFrames和區域性地圖點mvpLocalMapPoints
2、SearchLocalPoints()在區域性地圖中查詢與當前幀匹配的MapPoints
3、PoseOptimization()更新區域性所有MapPoints後對位姿再次優化
4、更新當前幀的MapPoints被觀測程度，並統計跟蹤區域性地圖的效果NeedNewKeyFrame
判斷是否需要生成新的關鍵幀，確定關鍵幀的標準
1. 在上一次進行重定位之後，過了20幀資料，或關鍵幀數小於20個，不滿足不能生成
2. 在上一個關鍵幀插入之後，過了20幀，或 區域性建圖是空閒狀態，不滿足不能生成。
3. 當前幀跟蹤到大於若干個點，不滿足不能生成
4. 當前幀的跟蹤點數小於90%的參考關鍵幀跟蹤點數，並且當前幀跟蹤點數大於15，不滿足不能生成
具體流程：
一、Tracking的兩個模式
1、僅追蹤模式(mbOnlyTracking)：區域性地圖不工作，只追蹤地圖現有地圖點
2、同時定位與建圖( !mbOnlyTracking )：追蹤執行緒同時有區域性建圖和迴環檢測。
二、Tracking的四種狀態
NO_IMAGES_YET表示當前沒有圖片，NOT_INITIALIZED表示當前沒有初始化追蹤執行緒，OK證明當前追蹤執行緒完好，LOST證明當前追蹤執行緒丟失——注意這裡的執行緒狀態都是指當前幀處理之前的狀態。
處於NO_IMAGES_YET，當新的一幀來臨時，將執行緒狀態改變為NOT_INITIALIZED。
處於NOT_INITIALIZED，則針對單目相機和雙目相機/RGBD相機進行不同的初始化，步驟 2
處於OK，經過初始化的系統追蹤執行緒轉為OK狀態，首先CheckReplacedInLastFrame()檢測上一幀中的所有地圖點中有沒有可以代替該地圖點的其他地圖點，如果有則將兩地圖點融合為一個地圖點（用替代點替代當前幀中的地圖點）。然後用兩種方式求解相機位姿 ，步驟 3 4 5 7
處於LOST，重定位，步驟6
1、在進行追蹤執行緒之前，首先對輸入的影象進行預處理，GrabImageMonocular（）先將RGB影象化為灰度影象，之後Frame() 建立新幀，提取特徵點。資料流以Frame的形式進入Track()函式。
2、單目初始化：需要兩幀，第一幀建立初始化器，設定該幀作為初始化參考幀；第二幀作為匹配幀，進行特徵點匹配，進而通過並行地計算基礎矩陣和單應性矩陣，選取其中一個模型，恢復出最開始兩幀之間的相對姿態，三角化得到地圖點的深度資訊。注意：單目初始化得到的深度資訊同樣具有不確定性，需要將得到的地圖點深度歸一化，位移向量也歸一化，之後的追蹤執行緒計算的地圖點和相機位姿都是相對於初始化時的距離展開的。
MonocularInitialization();//單目初始化3、跟蹤運動模型：假設：勻速運動模型，兩幀相對運動相同。這裡匹配是通過投影來與上一幀看到的地圖點匹配，使用的是matcher.SearchByProjection()，並根據上一幀的位姿和速度預測當前相機的位姿，同樣將匹配地圖點和位姿加入到圖優化中得到當前幀的位姿以及匹配內點
bOK = TrackWithMotionModel();4、跟蹤參考關鍵幀（上一幀速度為0或當前幀與上一次重定位幀之間ID差大於2）：根據參考關鍵幀與當前幀匹配得到匹配地圖點，將上一參考幀位姿作為當前幀的初始位姿，然後將匹配點和位姿同時進行圖優化，得到當前幀的位姿以及匹配內點
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
bOK = TrackReferenceKeyFrame();
}5、重定位：
（1）計算當前幀的BoW對映
（2）找到與當前幀相似的候選關鍵幀
（3）匹配當前幀與找到的候選關鍵幀，計算相機位姿Tcw（RANSAC+PNP演算法）
（4）優化相機位姿Tcw
bOK = Relocalization();6、跟蹤區域性地圖：對相機位姿有個基本估計和初始特徵匹配，將區域性地圖投影到當前幀尋找更多對應的匹配地圖點MapPoints，優化相機位姿。
（1）UpdateLocalMap（）更新區域性地圖（關鍵幀+區域性地圖點）；
（2）SearchLocalPoints（）匹配區域性地圖視野範圍內的點和當前幀特徵點；
（3）PoseOptimization（）根據新的匹配點重新優化相機位姿。
bOK = TrackLocalMap();//跟蹤區域性地圖7、跟蹤成功，需要
（1）根據優化的相機位姿更新恆速運動模型TrackWithMotionModel（）
（2）並清除臨時地圖點（UpdateLastFrame()函式針對雙目和RGB-D，需要根據深度值對上一關鍵幀產生臨時的MapPoints），刪除地圖點中是外點的地圖點
（3）更新參考關鍵幀（參考關鍵幀是所有關鍵幀中公式地圖點最多的關鍵幀）。檢測此時是否需要新增關鍵幀NeedNewKeyFrame()，這裡新增關鍵幀是給區域性地圖執行緒新增關鍵幀，然後在區域性地圖中處理完成後將關鍵幀傳遞給迴環檢測。
（4）跟蹤成功記錄當前位姿資訊，跟蹤失敗則使用上一次位姿值。
void Tracking：：Track()
// track包含兩部分：估計運動、跟蹤區域性地圖
// mState為tracking的狀態機
// 如果影象復位過、或者第一次執行，則為NO_IMAGE_YET狀態
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
// mLastProcessedState儲存了Tracking最新的狀態，用於FrameDrawer中的繪製
mLastProcessedState=mState;
// 上鎖Get Map Mutex -> Map cannot be changed
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);處於NO_IMAGES_YET， 有關unique_lock<mutex>是為了上鎖，保證多執行緒不能同時修改共享資料，這裡對地圖上鎖。
// 步驟1：初始化
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
MonocularInitialization();//單目初始化
mpFrameDrawer->Update(this);
if(mState!=OK)
return;//只是return 相當於break
}
else// 步驟2：正常跟蹤OK
{
// System is initialized. Track Frame.
// 系統已初始化，開始跟蹤幀
// bOK為臨時變數，用於表示每個函式是否執行成功
bool bOK;
// 初始相機位姿估計用運動模型或重定位(跟蹤丟了)
// 在viewer中有個開關menuLocalizationMode，有它控制是否ActivateLocalizationMode，並最終管控mbOnlyTracking
// mbOnlyTracking等於false表示正常VO模式（有地圖更新），mbOnlyTracking等於true表示使用者手動選擇定位模式
if(!mbOnlyTracking)//激活了區域性地圖
{
// 正常初始化成功
if(mState==OK)
{
// 檢查並更新上一幀被替換的MapPoints
// 更新Fuse函式和SearchAndFuse函式替換的MapPoints
CheckReplacedInLastFrame();
// 步驟2.1：跟蹤上一幀或者參考幀或者重定位
// 上一幀速度為0或當前幀與上一次重定位幀之間ID差大於2，跟蹤關鍵幀
// mnLastRelocFrameId上一次重定位的那一幀
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
// 將上一幀的位姿作為當前幀的初始位姿
// 通過BoW的方式在參考幀中找當前幀特徵點的匹配點
// 優化每個特徵點都對應3D點重投影誤差即可得到位姿
bOK = TrackReferenceKeyFrame();
}
else
{
// 根據恆速模型設定當前幀的初始位姿
// 通過投影的方式在參考幀中找當前幀特徵點的匹配點
// 優化每個特徵點所對應3D點的投影誤差即可得到位姿
bOK = TrackWithMotionModel();
if(!bOK)
// TrackReferenceKeyFrame是跟蹤參考幀，不能根據固定運動速度模型預測當前幀的位姿態，通過bow加速匹配（SearchByBow）
// 最後通過優化得到優化後的位姿
bOK = TrackReferenceKeyFrame();
}
}
else//初始化失敗
{
// 重定位：BOW搜尋，PnP求解位姿
bOK = Relocalization();
}
}
else//沒有啟用區域性地圖，只進行跟蹤tracking
{
// tracking跟丟了，重定位
if(mState==LOST)
{
bOK = Relocalization();
}
else
{
// mbVO是mbOnlyTracking為true時的才有的一個變數
// mbVO為false表示此幀匹配了很多的MapPoints，跟蹤很正常，
// mbVO為true表明此幀匹配了很少的MapPoints，少於10個，要跪的節奏
if(!mbVO)//跟蹤正常, mbVO為false則表明此幀匹配了很多的3D map點，非常好
{
if(!mVelocity.empty())//上一幀有速度，跟蹤模型
{
bOK = TrackWithMotionModel();
// 這個地方是不是應該加上：
// if(!bOK)
// bOK = TrackReferenceKeyFrame();
}
else//上一幀沒速度，跟蹤關鍵幀
{
bOK = TrackReferenceKeyFrame();
}
}
else//特徵點不夠多,小於10個
{
// 先使用運動模型和重定位計算兩種相機位姿，如果重定位失敗，保持VO結果
// mbVO為1，則表明此幀匹配了很少的3D map點，少於10個，要跪的節奏，既做跟蹤又做定位
bool 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)
{
// 這段程式碼是不是有點多餘？應該放到TrackLocalMap函式中統一做
// 更新當前幀的MapPoints被觀測程度
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;
}
}
}
// 將最新的關鍵幀作為reference frame
mCurrentFrame.mpReferenceKF = mpReferenceKF;處於NOT_INITIALIZED，進行單目初始化即可。
處於OK，按照兩種相機位姿追蹤模式得到初始位姿估計，
1、區域性地圖啟用(!mbOnlyTracking)：如果(mState==OK)，CheckReplacedInLastFrame()首先更新上一幀被替換的MapPoints，然後如果特徵點匹配太少，需要匹配參考關鍵幀bOK = TrackReferenceKeyFrame()，否則根據勻速運動模型匹配bOK = TrackWithMotionModel()。特殊情況初始化跟蹤失敗需要重定位bOK = Relocalization();
2、區域性地圖不工作，只跟蹤當前地圖中地圖點：如果(mState==LOST)，需要重定位Relocalization()。
（1）正常情況下需要先看匹配的特徵點數量是否足夠多 (!mbVO），接下來上一幀有速度（!mVelocity.empty()）勻速運動模型TrackWithMotionModel()，上一幀沒速度跟蹤參考關鍵幀TrackReferenceKeyFrame()。
（2）當匹配特徵點不夠多，小於10個，使用運動模型和重定位計算兩種相機位姿，如果重定位失敗，保持VO結果，否則更相信重定位結果。
if(!mbOnlyTracking)//不是隻跟蹤還插入關鍵幀，區域性地圖工作
{
if(bOK)
bOK = TrackLocalMap();//跟蹤區域性地圖
}
else
{
// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
// the camera we will use the local map again.
// 區域性地圖不工作，特徵點足夠且重定位成功
if(bOK && !mbVO)
bOK = TrackLocalMap();
}
if(bOK)
mState = OK;
else
mState=LOST;
// Update drawer
mpFrameDrawer->Update(this);
// If tracking were good, check if we insert a keyframe
// 跟蹤成功，檢查是否插入了關鍵幀
if(bOK)
{
// 先更新運動模型
if(!mLastFrame.mTcw.empty())
{
// 步驟2.3：更新恆速運動模型TrackWithMotionModel中的mVelocity
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
mVelocity = mCurrentFrame.mTcw*LastTwc; // Tcl
}
else
mVelocity = cv::Mat();
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
// 清除 VO matches
// 步驟2.4：清除UpdateLastFrame中為當前幀臨時新增的MapPoints
for(int i=0; i<mCurrentFrame.N; i++)
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(pMP)
// 排除UpdateLastFrame函式中為了跟蹤增加的MapPoints
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// 清除臨時地圖點
// 步驟2.5：清除臨時的MapPoints，這些MapPoints在TrackWithMotionModel的UpdateLastFrame函式裡生成（僅雙目和rgbd）
// 步驟2.4中只是在當前幀中將這些MapPoints剔除，這裡從MapPoints資料庫中刪除
// 這裡生成的僅僅是為了提高雙目或rgbd攝像頭的幀間跟蹤效果，用完以後就扔了，沒有新增到地圖中
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
// 這裡不僅僅是清除mlpTemporalPoints，通過delete pMP還刪除了指標指向的MapPoint
mlpTemporalPoints.clear();
// Check if we need to insert a new keyframe
// 步驟2.6：檢測並插入關鍵幀，對於雙目會產生新的MapPoints
if(NeedNewKeyFrame())
CreateNewKeyFrame();
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
// if they are outliers or not. We don't want next frame to estimate its position
// with those points so we discard them in the frame.
// 刪除那些在bundle adjustment中檢測為outlier的3D map點
for(int i=0; i<mCurrentFrame.N;i++)
{
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Reset if the camera get lost soon after initialization
// 跟蹤失敗，並且relocation也沒有搞定，只能重新Reset
if(mState==LOST)
{
if(mpMap->KeyFramesInMap()<=5)
{
cout << "Track lost soon after initialisation, reseting..." << endl;
mpSystem->Reset();
return;
}
}
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// 儲存上一幀的資料
mLastFrame = Frame(mCurrentFrame);
}處於OK，跟蹤區域性地圖。上一步得到初始位姿，這一步將區域性MapPoints和當前幀進行投影匹配以尋找更多匹配點，優化相機位姿。
1、如果區域性地圖啟用(!mbOnlyTracking)跟蹤區域性地圖bOK = TrackLocalMap()，否則在重定位後特徵點足夠(bOK && !mbVO)也會跟蹤區域性地圖bOK = TrackLocalMap()
2、更新運動模型中的速度mVelocity，mVelocity = mCurrentFrame.mTcw*LastTwc
3、清除臨時的MapPoints，這些MapPoints在TrackWithMotionModel的UpdateLastFrame函式裡生成（僅雙目和rgbd）
4、檢測（NeedNewKeyFrame()）並插入關鍵幀CreateNewKeyFrame()
5、儲存最新一幀的資料mLastFrame = Frame(mCurrentFrame);
// 步驟3：記錄位姿資訊，用於軌跡復現
if(!mCurrentFrame.mTcw.empty())
{
// 計算相對姿態T_currentFrame_referenceKeyFrame
cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
mlRelativeFramePoses.push_back(Tcr);
mlpReferences.push_back(mpReferenceKF);
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
mlbLost.push_back(mState==LOST);
}
else
{
// This can happen if tracking is lost
// 如果跟蹤失敗，則相對位姿使用上一次值
mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
mlpReferences.push_back(mlpReferences.back());
mlFrameTimes.push_back(mlFrameTimes.back());
mlbLost.push_back(mState==LOST);
}當前幀的相對位姿Tcr=mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();