在這篇文章里我們主要開始對跟蹤線程進行介紹：
在orb_slam整體編程思路及代碼解析(1)中我們發現，Tracking線程的入口是TrackMonocular，其中GrabImageMonocular返回位姿。
FUNCTION1:Tracking的構造函數
默認把跟蹤狀態設為NO_IMAGES_YET，定位跟蹤模式，默認的其他參數，諸如字典，圖像畫布，地圖畫布，地圖，關鍵幀數據庫等都是system.cc類里定義的對象。也從配置文件中傳入了相機的內參，圖像校正系數，幀率，圖像金字塔和角點提取的基本參數，這些參數都是這個類的元素等。
tracking過程都會用到mpORBextractorLeft作為特征點提取器， 在單目初始化的時候，會用mpIniORBextractor來作為特征點提取器，兩者的區別是后者比前者最多提出的點數多一倍。
FUNCTION1.1:ORBextractor的構造函數
是構造函數，傳入features_num最多提取的特征點的數量，scale_factor金字塔圖像之間的尺度參數，levels_num金字塔的層數，default_fast_threshold默認fast 角點檢測的時候的閾值，為了防止用默認閾值fast角點檢測檢測的特征數過少，添加設置min_fast_threshold最小的fast特征檢測閾值，以保證檢測的特征數目。每一層都有一些屬性參數，比如mvScaleFactor、mvLevelSigma2、mvInvScaleFactor、mvInvLevelSigma2等，以及給每層分配待提取的特征數，具體通過等比數列求和的方式，求出每一層應該提取的特征數，把每一層的特征點數都放在mnFeaturesPerLevel中，值得注意的是第零層的特征點數是nfeatures×(1-1/scaleFactor)/(1-(1/scaleFactor)^nlevels)，然后下一層是上一層點數的1/scaleFactor倍。以此類推，最后一層兜底。然后復制訓練的模板，在計算描述子的時候會用到。最后通過求x坐標對應在半徑為HALF_PATCH_SIZE的圓上的y坐標，標出了一個圓形區域用來求特征點方向。相關內容可以參考Oriented FAST and Rotated BRIEF。
FUNCTION2:GrabImageMonocular
這個函數先把圖片轉換成了灰度圖像，然后跟據跟蹤的狀態構造關鍵幀，再是進行跟蹤得到當前幀的位姿。
FUNCTION2.1:Frame的構造函數
傳入圖像，時間戳，特征點提取器，字典，內參矩陣等參數來構造關鍵幀，首先把要構造金字塔的相關參數給Frame類中的跟金字塔相關的元素。然后提取ORB特征， 這一步調用了重載了函數調用操作符operator()。
FUNCTION2.1.1:operator()
傳入的圖像必須是灰度圖像，然后構造圖像金字塔。相關內容可以參考：
ORB_SLAM2 源碼閱讀 ORB_SLAM2::ORBextractor。
FUNCTION2.1.1.1:ComputePyramid(image)
這個函數通過傳入的圖像來構造nlevel層金字塔，level層是level-1層用resize函數得到大小為level-1層大小的scale倍的線性插值后的圖像，為了方便做一些卷積計算，所以用copyMakeBorder函數來做邊界填充。填充類型是BORDER_REFLECT_101(反射101)，例如：gfedcb|abcdefgh|gfedcba。
FUNCTION2.1.1.2:ComputeKeyPointsOctTree(allKeypoints)
這個函數為了計算金字塔每一層的興趣點，找到FAST關鍵點，在操作上是依次針對圖像金字塔的每一層圖像進行的，首先在圖像四周去掉長度為EDGE_THRESHOLD-3個單位的像素點的邊界，對去掉邊界后的圖像網格化，每個窗口的大小為w個像素點的大小，然后依次在划分好的窗口中提取FAST關鍵點，這樣做的目的是為了使得每個網格都有特征，從而使得特征點在圖像上的分布相對均勻點。如果存在有的窗口中提取的特征點數為0，則降低閾值繼續提取，然后對提取出了的關鍵點換算出其位於（level層的被裁掉邊界圖像）的位置，並每個窗口中的關鍵點存入vToDistributeKeys容器中，vToDistributeKeys容器就暫時保存着level層圖像的關鍵點。然后將這些特征點送入DistributeOctTree函數，剔除一些關鍵點。將剔除后留下的關鍵點存入allKeypoints[level]容器中。
FUNCTION2.1.1.2.1:DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX, minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);
先用(maxX-minX)/(maxY-minY)來確定四叉數有幾個初始節點，這里有 bug，如果輸入的是一張 寬高比 小於 0.5 的圖像，nIni 計算得到 0，下一步計算 hX 會報錯，例如round(640/480)=1,所以只有一個初始節點，(UL,UR,BL,BR)就會分布到被裁掉邊界后的圖像的四個角。把所有的關鍵點分配給屬於它的節點，當節點所分配到的關鍵點的個數為1時就不再進行分裂，當節點沒有分配到關鍵點時就刪除此節點。再根據興趣點分布,利用四叉樹方法對圖像進行划分區域，當bFinish的值為true時就不再進行區域划分，首先對目前的區域進行划分，把每次划分得到的有關鍵點的子區域設為新的節點，將nToExpand參數加一，並插入到節點列表的前邊，刪除掉其父節點。只要新節點中的關鍵點的個數超過一個，就繼續划分，繼續插入列表前面，繼續刪除父節點，直到划分的子區域中的關鍵點的個數是一個，然后迭代器加以移動到下一個節點，繼續划分區域。當划分的區域即節點的個數大於關鍵點的個數或者分裂過程沒有增加節點的個數時就將bFinish的值設為true，不再進行划分。如果以上條件沒有滿足，但是滿足((int)lNodes.size()+nToExpand*3)>N，表示再分一次即將結束，所以開始按照特征點的數量對節點進行排序，特征點數多的節點優先划分，知道節點數量滿足。vSizeAndPointerToNode 是前面分裂出來的子節點（n1, n2, n3, n4）中可以分裂的節點。按照它們特征點的排序，先從特征點多的開始分裂，分裂的結果繼續存儲在 lNodes 中。每分裂一個節點都會進行一次判斷，如果 lNodes 中的節點數量大於所需要的特征點數量，退出整個 while(!bFinish) 循環，如果進行了一次分裂，並沒有增加節點數量，不玩了，退出整個 while(!bFinish) 循環。取出每一個節點(每個區域)對應的最大響應點，即我們確定的特征點。NOTED:因為經過FAST提取出的關鍵點有很多，當划分的子區域一旦大於mnFeaturesPerLevel[level](根據nfeatures算出的每一個level層最多的特征點數)的時候就不再進行區域划分了，所以每個區域內(節點)的關鍵點數會很多，取出響應值最大的那個就是我們想要的特征點。這個函數的意義就是根據mnFeaturesPerLevel,即該層的興趣點數,對特征點進行剔除，根據Harris角點的score進行排序，保留正確的。
經過以上步驟，我們提出來level層在無邊界圖像中的特征點，並給特征點條件邊界補償及尺度信息。
FUNCTION2.1.1.2.2:computeOrientation(mvImagePyramid[level], allKeypoints[level], umax)->IC_Angle(image, keypoint->pt, umax);
結合以上的論文我們知道要計算特征點的角度，我們要在一個圓域中算出m10和m01，計算步驟是先算出中間紅線的m10，然后在平行於x軸算出m10和m01，一次計算相當於圖像中的同個顏色的兩個line。
建立一個MAT容器descriptors，並關聯於mDescriptors，其大小為所有層特征點的個數×32，循環計算各尺度圖像中的特征點的描述子，依次順序存放於mDescriptors中。
FUNTIION2.1.1.3:computeDescriptors(workingMat, keypoints, desc, pattern)->computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i));
ORB選擇了BRIEF作為特征描述方法，但是我們知道BRIEF是沒有旋轉不變性的，所以我們需要給BRIEF加上旋轉不變性，把這種方法稱為“Steer BREIF”。之前我們在計算特征點的時候已經計算出了每個特征點的角度，這個角度對應着一個旋轉矩陣：
bit_pattern_31_是個一維數組，里面放了512個（偏差）點（256個點對）。並把這些點給類的point類型的pattern數組。並把這些點都經過相應的旋轉。即Sx=RS。
const uchar* center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x)); const int step = (int)img.step; #define GET_VALUE(idx) \ center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \ cvRound(pattern[idx].x*a - pattern[idx].y*b)]
center[0]給出了特征點的坐標，偏差點的表示是(y,x)T，乘以旋轉矩陣后就是上式中的GET_VALUE獲取相應的點。描述子矩陣中的縱坐標的個數是特征點的個數，橫坐標表示的是一個特征點的描述子總共有32×8（256）位。
最后把得到的各個尺度下的特征點都換算成在0層圖像下的坐標，並存放在mvKeys中。到這就完成了ORB特征點和描述子的提取。
現在回到Frame的構造函數上來，我們調用OpenCV的矯正函數矯正orb提取的特征點，進行畸變矯正，找到關鍵點實際應該在普通攝像頭中的位置，如果矯正矩陣等於0的話，就不進行矯正，否則將進行矯正，並把矯正后的特征點的位置存儲在mvKeysUn里。並處置好未用到的立體信息等。當傳入第一幀圖像或者標定矩陣發生變化時，我們需要計算矯正后的邊界就是把未矯正圖像的四個角點矯正后找到他們在矯正后圖像中的點的位置就可以確認邊界，最后把傳進來的圖片分割成48×64的網格，根據特征點的位置分在不同的網格里。每個特征點在mvKeysUn里都有編號，然后用mGrid[nGridPosX][nGridPosY]來存儲在各個各自里的特征點的編號。
這樣，我們就完成了從圖像到信息幀(Frame)的構造，主要就是提取圖像的特征點和特征點的描述子，把圖像初始化mCurrentFrame。
現在回到GrabImageMonocular，進行tracking過程。
FUCTION2.2:Track()
當系統第一次運行，或者被復位，就的進行初始化。
FUCTION2.2.1:MonocularInitialization()
初始化需要兩幀，第一幀為參考幀，這兩幀的特征點數都得大於100，跟蹤器用mvbPrevMatched來接管參考幀的特征點，並用參考幀、sigma:1.0 、iterations:200注冊了初始器，如果當前幀特征點太少，重新構造初始器，因此只有連續兩幀的特征點個數都大於100時，才能繼續進行初始化過程。隨后我們注冊了一個ORB匹配器，這個匹配器的初始化參數包括最佳得分和第二得分的比例(0.9)，以及是否執行角度檢測(true)。首先執行這個匹配器的SearchForInitialization用來尋找參考幀與第二幀之間的匹配點的數量。
FUCTION2.2.1.1:SearchForInitialization
取出金字塔0層的圖像的特征點，亦只對原圖像處理。
FUNCTION2.2.1.1.1:GetFeaturesInArea
這個函數是找到以x,y為中心,邊長為2r的方形內且在[minLevel, maxLevel]的特征點，返回滿足條件的特征點的序號，在具體執行上，找到這個區域占的所有grid，然后把這些grid的特征點都取出來再與x,y做差跟邊長r做比較，如果小的話就證明在邊長為2r的范圍內，返回該特征點的序號，調用是通過幀F2進行的，傳入的x,y是F1中的特征點的坐標，這樣就找到了F1的0層特征點x,y在F2的0層中的以x，y為中心的windowsize×windowsize范圍內的特征點的序號。
再找到F1中特征點的描述子與上面找到的F2中所有的序號對應的特征點的描述子對比，找到最佳匹配，和次佳匹配。然后根據閾值(在注冊匹配器時直接被賦值)和角度投票剔除誤匹配。最佳匹配點要明顯優於次佳匹配才是好的匹配，如果滿足上述閾值要求就更行匹配向量(如:traching里的mvIniMatches[i1]=bestIdx2;，其中i1為F1的特征點的序號，為F2中和特征點i1相匹配特征點的序號)，然后進行角度投票，將F1特征點的角度和F2特征點的角度做差，存入角度列表。最后找到3個得票數最高的角度，將其他的角度對應的匹配關系從匹配向量中剔除。最后將真正有匹配關系的更新到mvbPrevMatched中。並返回匹配對的數量。
如果這兩幀之間的匹配對數小於100，那么就得刪除初始化器重新初始化，函數返回。否則，通過H模型或F模型進行單目初始化，得到兩幀間相對運動、初始MapPoints。
FUNCTION2.2.1.2:Initialize
Initializer里的mvMatches12是一個元素為Match的向量，Match的數據結構是pair,mvMatches12只記錄Reference到Current匹配上的特征點對，mvbMatched1記錄Reference Frame的每個特征點在Current Frame是否有匹配的特征點，先用traching里的mvIniMatches更新上述兩個容器的值。再新建一個容器vAllIndices，生成0到N-1的數作為特征點的索引，在所有匹配特征點對中隨機選擇8對匹配特征點為一組，共選擇mMaxIterations組，結果放在mvSets中。用於FindHomography和FindFundamental求解，在選擇的過程中被選擇過的索引都會在容器中被刪除，確保同一個點不會被重復選到，做一次迭代之后，vAvailableIndices的值會被vAllIndices更新。再調用多線程分別用於計算fundamental matrix和homography。在線程調用成員函數時，需要同時傳成員函數的對象，和這個函數的傳參。
FUNCTION2.2.1.2.1:FindHomography
假設場景為平面情況下通過前兩幀求取Homography矩陣(current frame 2 到 reference frame 1),並得到該模型的評分,將mvKeys1和mvKey2歸一化到均值為0，一階絕對矩為1，歸一化矩陣分別為T1、T2。即[x',y']T=T1[x,y]T。
然后路利用RANSAC算法，找出得分最高的Homography矩陣。
FUNCTION2.2.1.2.1.1:ComputeH21
這里的單應矩陣HN是利用歸一化后的像素坐標進行操作的。在程序中vt是v的轉置，所以最小的特征值對應的特征向量是vt的最后一行。
恢復到原始的均值和尺度。
FUNCTION2.2.1.2.1.2:CheckHomography
利用重投影誤差為當次RANSAC的結果評分,把內點保存在vbMatchesInliers，得分所有點是閾值與歸一化誤差的差的和。很好理解，閾值是固定值，那么歸一化誤差越小，差值就越大，得分就越高。
好了，然后循環mMaxIterations迭代找出最高的的得分對應的單應矩陣即為所求。
FUNCTION2.2.1.2.2:FindFundamental
假設場景為非平面情況下通過前兩幀求取Fundamental矩陣(current frame 2 到 reference frame 1),並得到該模型的評分，程序步驟與求解單應矩陣一致，這里給出算法：
同樣的將F回復到原來的均值和尺度。通過得分求出最佳的基礎矩陣。
計算得分比例，選取某個模型，RH = SH/(SH+SF)，當RH>0.40，從H矩陣中恢復R,t，否則出F矩陣中恢復R,t。
FUNCTION2.2.1.2.3:ReconstructH
d'=d2和d'=-d2分別對應8組(R t)，現在需要找到最合適的解。
FUNCTION2.2.1.2.3.1:CheckRT
首先要得到投影矩陣(直接從世界坐標系到圖像坐標系)，以第一個坐標為世界坐標系，我們得到的投影矩陣P1[k|0]，第二坐標的投影矩陣為P2=k[R|t]，第二個相機的光心在世界坐標系下的坐標，O2=-RT×t。這個公式在這里解釋一下，在ORB_SLAM里，位移向量tcw的方向是從左下標到右下標的，並且位於左下標坐標系下，Rcw是從世界坐標系到相機坐標系的旋轉，RT表示R的逆旋轉，首先我們把世界t變換到世界坐標系下的平移，然后再加一個符號表示世界坐標系到相機坐標系的平移，就是相機光心的位置。
FUNCTION2.2.1.2.3.1.1:Triangulate
ReconstructH函數中對t有歸一化，這里三角化過程中恢復的3D點深度取決於 t 的尺度，但是這里恢復的3D點並沒有決定單目整個SLAM過程的尺度，因為CreateInitialMapMonocular函數對3D點深度會縮放，然后反過來對 t 有改變。
再通過3D點的深度不能為負，計算重投影誤差不能過大，淘汰掉不符合條件的R，t。在同一個R,t下的各個匹配點之間的視差角會不一樣，計算各視差角，最后到一個較大的視差角，並返回成功還原出3D點的數量。
最后，把還原出來的各個特征點對應3D點給mvIniP3D，並把各個特征點是否被成功三角化給vbTriangulated標志位容器。
FUNCTION2.2.1.2.4:ReconstructF
設置幀的位姿，將初始化的第一幀作為世界坐標系，因此第一幀變換矩陣為單位矩陣，由Rcw和tcw構造Tcw,並賦值給mTcw，mTcw為世界坐標系到當前幀的變換矩陣。
FUNCTION2.2.1.3:CreateInitialMapMonocular
將三角化得到的3D點包裝成MapPoints( Initialize函數會得到mvIniP3D，mvIniP3D是cv::Point3f類型的一個容器，是個存放3D點的臨時變量，CreateInitialMapMonocular將3D點包裝成MapPoint類型存入KeyFrame和Map中)。
FUNCTION2.2.1.3.1:KeyFrame
首先將初始參考幀和當前幀構造成關鍵幀，該構造過程就是用幀，3D點，關鍵幀數據庫在關鍵幀里注冊。
FUNCTION2.2.1.3.2:ComputeBoW
將初始關鍵幀的描述子轉為BoW,mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);根據特征點算出mBowVec，mFeatVec。mBowVec的結構是map<WordId, WordValue>表示的是反向索引，key為wordId，value為tf-idf中的tf，(idf在建立字典的時候計算一次就行)。mFeatVec的結構是map<NodeId, std::vector<unsigned int>表示的是正向索引，需要指定第m層，每幅圖像對應一個正向索引，儲存該圖像生成BoW向量時曾經到達過的第m層上節點的編號，以及路過這個節點的那些特征的編號，程序中的層數指定的是4。更多關於DBow的內容請參見：DBow2庫介紹。
FUNCTION2.2.1.3.3:AddKeyFrame
然后把這兩個關鍵幀插入地圖。
將3D點包裝成MapPoints
FUNCTION2.2.1.3.4.1:MapPoint
用3D點構造MapPoint
FUNCTION2.2.1.3.4.2:AddMapPoint
Add MapPoint to KeyFrame,表示該KeyFrame的哪個特征點可以觀測到哪個3D點
FUNCTION2.2.1.3.4.3:AddObservation
表示該MapPoint可以被哪個KeyFrame的哪個特征點觀測到
FUNCTION2.2.1.3.4.4:ComputeDistinctiveDescriptors
從眾多觀測到該MapPoint的特征點中挑選區分讀最高的描述子,由於一個MapPoint會被許多相機觀測到，因此在插入關鍵幀后，需要判斷是否更新當前點的最適合的描述子,先獲得當前點的所有描述子，然后計算描述子之間的兩兩距離，最好的描述子與其他描述子應該具有最小的距離中值,遍歷觀測到3d點的所有關鍵幀，獲得orb描述子，並插入到vDescriptors中,獲得這些描述子兩兩之間的距離,依次找到各個描述子到其它所有所有描述子之間的距離，每一組都獲得中值，尋找最小的中值，最好的描述子，該描述子相對於其他描述子有最小的距離中值，簡化來講，中值代表了這個描述子到其它描述子的平均距離，最好的描述子就是和其它描述子的平均距離最小。最后返回此描述子。
FUNCTION2.2.1.34.5:UpdateNormalAndDepth
更新平均觀測方向以及觀測距離范圍，由於一個MapPoint會被許多相機觀測到，因此在插入關鍵幀后，需要更新相應變量，獲得觀測到該3d點的所有關鍵幀，對所有關鍵幀對該點的觀測方向歸一化為單位向量進行求和，除以所有關鍵幀數就是獲得的平均觀測方向。獲得觀測到該點的參考關鍵幀和3d點在世界坐標系中的位置，得到該點到參考關鍵幀相機的距離，預測其在金字塔中的層數，就可以獲得其其距離范圍。
用3D點填補當前幀的結構。
FUNCTION2.2.1.3.4.6:AddMapPoint
在地圖中添加該MapPoint。
FUNCTION2.2.1.3.5:UpdateConnections
首先獲得該關鍵幀的所有MapPoint點，統計觀測到這些3d點的每個關鍵幀與其它所有關鍵幀之間的共視程度, 對每一個找到的關鍵幀，建立一條邊，邊的權重是該關鍵幀與當前關鍵幀公共3d點的個數。在沒有執行這個函數前，關鍵幀只和MapPoints之間有連接關系，這個函數可以更新關鍵幀之間的連接關系，KFcounter是map<KeyFrame*,int>代表關鍵幀-權重，權重為其它關鍵幀與當前關鍵幀共視3d點的個數，在執行上獲得關鍵幀對應的每一個3D點的能觀測到該3D點的所有關鍵幀，並整理到KFcounter。新建一個容器vPairs，pair<int,KeyFrame*>將關鍵幀的權重寫在前面，關鍵幀寫在后面方便后面排序，vPairs記錄與其它關鍵幀共視幀數大於th的關鍵幀，設定閾值為15，如果對應權大於閾值，對這些關鍵幀建立連接，並更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它關鍵幀與當前幀的連接權重，如果沒有超過閾值的權重，則對權重最大的關鍵幀建立連接，vPairs里存的都是相互共視程度比較高的關鍵幀和共視權重，由大到小。更新該KeyFrame的mConnectedKeyFrameWeights，更新當前幀與其它關鍵幀的連接權重，並更新關鍵幀和權重。
FUNCTION2.2.1.3.6:GlobalBundleAdjustemnt
3D-2D 最小化重投影誤差 e = (u,v) - project(Tcw*Pw)，迭代20次。得到優化后的結果，把優化后的位姿傳遞給幀參與優化的幀，更新優化后的空間點。
將MapPoints的中值深度歸一化到1，並歸一化兩幀之間變換， x/z ，y/z， 將z歸一化到1 。
在把初始關鍵幀和當前關鍵幀給局部地圖線程。 把當前關鍵幀更新為最新關鍵幀，初始關鍵幀和當前關鍵幀給跟蹤器的局部地圖關鍵幀。把所有的地圖點給跟蹤器的局部關鍵點，把當前幀設定為參考關鍵幀，把當前關鍵幀更新為最新幀，更新Map線程相關信息。
到這，初始化完成，開始進行跟蹤。
在viewer中有個開關menuLocalizationMode，有它控制是否ActivateLocalizationMode，並最終管控mbOnlyTracking，mbOnlyTracking等於false表示正常VO模式（有地圖更新），mbOnlyTracking等於true表示用戶手動選擇定位模式。
2.2.2.PART1:得到初始位姿
- 2.2.2.1.mbOnlyTracking=false
如果初始化成功，因為Local Mapping線程可能會將關鍵幀中某些MapPoints進行替換，而tracking中需要用到mLastFrame，這里檢查並更新上一幀中被替換的MapPoints，更新的是Fuse函數和SearchAndFuse函數替換的MapPoints，如果運動模型是空的或剛完成重定位。
FUNCTION2.2.2.1.1:TrackReferenceKeyFrame
將上一幀的位姿作為當前幀的初始位姿，通過BoW的方式在參考幀中找當前幀特征點的匹配點，優化每個特征點都對應3D點重投影誤差即可得到位姿。在執行上：我們先將當前幀的描述子轉化為BOW向量，再注冊一個匹配器，通過特征點的BoW加快當前幀與參考幀之間的特征點匹配。
FUNCTION2.2.2.1.1.1:SearchByBoW
這個函數是在同一個node下先從關鍵幀里面找到一個特征點，在依次遍歷當前幀這個節點下的特征點，計算其描述子的距離，如果滿足要求，就把關鍵幀里的特征點對應的3d點賦給當前幀里的特征點里對應的3d點，vpMapPointMatches[i],i表示當前幀特征點的idex，值為對應的3d點。其中，函數lower_bound()在first和last中的前閉后開區間進行二分查找，返回大於或等於val的第一個元素位置。
當匹配的點達到15個以上的時候，更新匹配到的3D點，將上一幀的位姿態作為當前幀位姿的初始值，用上一次的Tcw設置初值，在PoseOptimization可以收斂快一些。
FUNCTION2.2.2.1.1.2:PoseOptimization
3D-2D 最小化重投影誤差 e = (u,v) - project(Tcw*Pw) ，只優化Frame的Tcw，不優化MapPoints的坐標。開始優化，總共優化四次，每次優化后，將觀測分為outlier和inlier，outlier不參與下次優化， 由於每次優化后是對所有的觀測進行outlier和inlier判別，因此之前被判別為outlier有可能變成inlier，反之亦然，其中外點的誤差是需要另行計算的，因為g2o只會計算active edge的誤差，除了前兩次優化需要RobustKernel以外, 其余的優化都不需要。
剔除優化后的outlier匹配點（MapPoints）。如果inlier匹配的點超過10個則為成功，bOK即為true。
FUNCTION2.2.2.1.2:TrackWithMotionModel
根據恆速模型設定當前幀的初始位姿，通過投影的方式在參考幀中找當前幀特征點的匹配點，優化每個特征點所對應3D點的投影誤差即可得到位姿。mVelocity，這個兩表示的是從參考幀到當前幀的位姿，我們要先乘上參考幀的相對於世界坐標系位姿，表示得到當前幀相對於世界坐標系位姿的初始位姿估計值。初始化當前幀對應的mvpMapPoints為NULL。
FUNCTION2.2.2.1.2.1:SearchByProjection
通過投影，對上一幀的特征點進行跟蹤，將上一幀的MapPoints投影到當前幀(根據速度模型可以估計當前幀的Tcw)，在執行上：依次遍歷參考幀的pMapPoints，計算出該3D點在當前幀的投影位置，設定一個以該點為中心的正方形區域內的所有特征點，獲得該3D點的描述子和這些特征點的描述子之間的距離，找到距離最小的那個特征點，就是該3D點在當前幀匹配到的特征點。返回成功匹配的數量。
如果跟蹤的點少，則擴大搜索半徑再來一次。
FUNCTION2.2.2.1.2.2:PoseOptimization
剔除優化后的outlier匹配點（MapPoints）。如果inlier匹配的點超過10個則為成功，bOK即為true。
如果恆速模型不成功，那么就利用跟蹤參考幀的模式。
如果初始化成功的話，但是mState的狀態不是OK的話，那么就需要重定位。
FUNCTION2.2.2.1.3:Relocalization
計算當前幀特征點的Bow映射。
FUNCTION2.2.2.1.3.1:DetectRelocalizationCandidates
在重定位中找到與該幀相似的關鍵幀， 1. 找出和當前幀具有公共單詞的所有關鍵幀，2. 只和具有共同單詞較多的關鍵幀進行相似度計算，3. 將與關鍵幀相連（權值最高）的前十個關鍵幀歸為一組，計算累計得分，4. 只返回累計得分較高的組中分數最高的關鍵幀。在執行上：第一步：words是檢測圖像是否匹配的樞紐，遍歷當前幀的每一個word，提取所有包含該word的KeyFrame，如果找到的關鍵幀還沒有標記為當前幀的候選幀，那么就標記上，並且存入候選幀容器，該候選幀與當前幀共有的單詞數累加。第二步：統計所有閉環候選幀中與當前幀具有共同單詞最多的單詞數，並以此決定閾值。第三步：遍歷所有閉環候選幀，挑選出共有單詞數大於0.8倍共同單詞最多的單詞數的候選幀且將單詞匹配度存入和該候選幀存入lScoreAndMatch。找到與lScoreAndMatch中的候選幀連接的前10個關鍵幀，計算其組內最高得分和該組的累計得分，將改組里的累計得分和最佳得分的對應幀放入lAccScoreAndMatch。第五步，找到滿足組累計得分大於0.75倍的組中的分數最高的關鍵幀(不一定是之前選出的候選幀)，將此存入返回的vpRelocCandidates中。
然后對上式確定的候選幀，依次通過BoW進行匹配，當匹配點超過15時就開始初始化PnPsolver，設置每個PnPsolver的RANSAC迭代的參數，並記錄符合條件的候選幀的幀數。只要候選幀數大於0，就不斷得通過RANSAC，找到合適的位姿。
FUNCTION2.2.2.1.3.2:pSolver->iterate
以下內容可參考：PnP算法簡介與代碼解析
FUNCTION2.2.2.1.3.2.1:set_maximum_number_of_correspondences(mRansacMinSet)
mRansacMinSet為每次RANSAC需要的特征點數，默認為4組3D-2D對應點,為每次迭代的世界坐標系下的3D點pws，對應的圖像坐標us，
匹配的2D點的個數不能小於RANSAC迭代過程中最少的inlier數(mRansacMinInliers=10)
開始RANSAC迭代：
FUNCTION2.2.2.1.3.2.2:add_correspondence
將隨機選取的4組對應的3D-2D壓入到pws和us
FUNCTION2.2.2.1.3.2.3:compute_pose(mRi, mti)
FUNCTION2.2.2.1.3.2.3.1:choose_control_points
第一個控制點選取為四個世界坐標系下的3D點的質心，然后將這四個點減去質心，做svd變換，另外三個控制點是PCA降維之后的三個主軸上的單位向量加上質心。
FUNCTION2.2.2.1.3.2.3.2:compute_barycentric_coordinates()
求解四個控制點的系數alphas，(a2 a3 a4)' = inverse(cws2-cws1 cws3-cws1 cws4-cws1)*(pws-cws1)，a1 = 1-a2-a3-a4，每一個3D參考點，都有一組alphas與之對應，cws1 cws2 cws3 cws4為四個控制點的坐標，pws為3D參考點的坐標。
FUNCTION2.2.2.1.3.2.3.3:fill_M
然后對MTM做SVD分解，得到M的12個解向量，如下圖綠色部分表示。下面的限定條件是因為在空間中兩個點之間的距離不會因為他們所處的坐標系而改變，所以四個控制點就有6個距離。
對於投影相機模型,N等於1,因為只有一個尺度變量;對於正交相機模型,N等於4,因為每個參考點的深度變化后仍滿足約束;因此,當相機焦距比較小時,N為1。當相機焦距更大,相機接近於正交相機時,
將有4個接近於0的特征值。
FUNCTION2.2.2.1.3.2.3.4:compute_L_6x10
當N=4時，化解結果如下，N=1，2，3的結果是可以在下面的式子的一部分得到，所以上來直接構造L6x10，后面的其他的L陣都可以從L6x10抽取。
FUNCTION2.2.2.1.3.2.3.5:compute_rho
計算4個控制點的兩兩之間的距離。
FUNCTION2.2.2.1.3.2.3.6:find_betas_approx_1
利用L×betas=rho，用子集的近似約束來求解，具體方法可見代碼，大概的方法就是用包含部分自項(betas11\betas22\betas33\betas44)和交叉項的betas(betas12...)和相應的L以及rho來構成等式並用SVD求出。
FUNCTION2.2.2.1.3.2.3.7:gauss_newton
下面公式中的betas0是上面N=1，2，3，4算出的，在這為高斯牛頓優化提供初始值。
FUNCTION2.2.2.1.3.2.3.7.1:compute_A_and_b_gauss_newton
計算出誤差矩陣對betas1，betas2，betas3，betas4的偏導。上式中betas11=betas1×betas1。
FUNCTION2.2.2.1.3.2.3.7.2:qr_solve
通過QR分解求出x。
根據求出的x值，這個變量加在初始值上，迭代iterations_number次。求得betas。這樣我們就可以得到相機坐標系下的控制點的坐標。對於每個3D點，在世界坐標系下，我們可以找到四個algha，使得四個控制點可以表達這個3D點，且這四個algha的和為1。對於同樣的algha，在相機左邊系下也可以滿足四個控制點線性表示這個3D參考點。這四個控制點可以通過3D點的投影以及在不同坐標系下的兩個點之間距離的不變性求出，結果由上面的betas和M的幾個特征向量表示，再加上algha即可以表示3D點在相機坐標系下的參考點。
FUNCTION2.2.2.1.3.2.3.8:compute_R_and_t
到這，我們就求出了不同的N對應的3D點投影的坐標和3D點匹配的2D點的坐標的誤差，選出誤差最小的那組[R|t] 。
FUNCTION2.2.2.1.3.2.4:CheckInliers
在上一步中我們求出了[R|t]，據此我們可以算出檢查哪些3D-2D點對屬於inliers，這是根據重投影誤差和閾值比較來確定是否是內點，並計算出內點的數量mnInliersi。
找到內點數量值最大的組對應的位姿作為最佳位姿的候選，同時利用這一組的所有點數再進行EPnP計算位姿，通過位姿再次計算內點數量，如果內點數量還是超過了Ransac要求的最少的內點，那么就返回此位姿，否則到達了最大的Ransac的迭代次數，就置bNoMore為true，返回之前的最佳位姿的候選。
如果迭代達到最大值，我們就把相應的候選關鍵幀刪除。
FUNCTION2.2.2.1.3.3:PoseOptimization
通過重投影優化位姿，得到內點的數量
如果，內點的數量少於10個，就跳過本次循環，如果內點(10<nGood<50)，則通過投影的方式對之前當前幀和候選幀未匹配的點進行匹配(搜索邊長20，ORB距離100)找到一些新的匹配點，若此時好的匹配的數量和新找到的匹配點的數量大於50，則進行優化求解，如果找到好的內點的匹配數量還沒有超過50，那么就再次通過投影的方式對之前當前幀和候選幀未匹配的點進行匹配(搜索邊長6，ORB距離64)找一些新的匹配關系，若此時好的匹配的數量和新找到的匹配點的數量大於50，則進行優化求解，得到當前幀的優化后的位姿。當前幀就是最新的重定位幀。
- 2.2.2.2.mbOnlyTracking=ture
只進行跟蹤tracking，局部地圖不工作，如果系統跟丟了，那么就進行重定位，否則判斷變量mbVO，如果 mbVO為false表示此幀匹配了很多的MapPoints，跟蹤很正常，再根據變量mVelocity是否為空，判斷是使用跟蹤參考幀模型還是采用運動跟蹤模型。如果mbVO為true表明此幀匹配了很少的MapPoints，少於10個，要跪的節奏，這時候既做跟蹤又做定位，定位和跟蹤的結果分別用bOKMM和bOKReloc表示，只要mVelocity不為空就做基於恆速模型跟蹤，結果如果是跟蹤成功定位失敗，那么結果借用跟蹤的結果，但只要是重定位成功，那么整個跟蹤過程就正常進行(定位與跟蹤，更相信重定位)，最后只要是跟蹤和重定位只要一個成功，那么結果就正常。將最新的關鍵幀作為reference frame。
總結：mbOnlyTracking=false，系統主要做跟蹤(恆速模型和參考幀模型)，當系統跟蹤失敗做重定位。mbOnlyTracking=ture，系統一邊跟蹤(恆速模型)一邊定位，但是更相信定位的結果，同樣跟蹤失敗后做重定位。
2.2.3.PART2:在幀間匹配得到初始的姿態后，現在對local map進行跟蹤得到更多的匹配，並優化當前位姿（這是回來再看這段程序的一點點補充：這里的局部地圖是在跟蹤的過程中進行的，他是在現在處理的當前幀的地圖點確立地圖關鍵幀和這些地圖關鍵幀確立的地圖點構成的局部地圖的基礎上，完成位姿優化的目的。這些地圖會在下一幀進行局部地圖更新時被清空，以下一幀為參考再構建一個新的局部地圖）
- 2.2.3.1mbOnlyTracking=false
FUCTION2.2.3.1.1:TrackLocalMap
local map:當前幀、當前幀的MapPoints、當前關鍵幀與其它關鍵幀共視關系，1. 更新局部地圖，包括局部關鍵幀和關鍵點、2. 對局部MapPoints進行投影匹配、3. 根據匹配對估計當前幀的姿態、4. 根據姿態剔除誤匹配。
FUCTION2.2.3.1.1.1:UpdateLocalMap
更新mpMap中的地圖點，為了可視化。這行程序放在UpdateLocalPoints函數后面是不是好一些?
FUCTION2.2.3.1.1.1.1:UpdateLocalKeyFrames
更新局部關鍵幀，遍歷當前幀的MapPoints，將觀測到這些MapPoints的關鍵幀和相鄰的關鍵幀取出，更新mvpLocalKeyFrames。 每個當前幀的優化的地圖都是新建的，所以要先清空局部關鍵幀，依據一下3個策略，所以先建立一個3倍keyframeCounter的大小。步驟1：遍歷當前幀的MapPoints，記錄所有能觀測到當前幀MapPoints的關鍵幀。步驟2：更新局部關鍵幀（mvpLocalKeyFrames），添加局部關鍵幀有三個策略(策略1：能觀測到當前幀MapPoints的關鍵幀作為局部關鍵幀，策略2：與策略1得到的局部關鍵幀共視程度很高的關鍵幀作為局部關鍵幀(當局部關鍵幀超過80時就不在添加，策略2.1:最佳共視的10幀，策略2.2:自己的子關鍵幀，策略2.3:自己的父關鍵幀)，步驟3：更新當前幀的參考關鍵幀，與自己共視程度最高的關鍵幀作為參考關鍵幀，在策略1的時候計算了與自己共視程度最高的關鍵幀pKFmax)。在程序中的pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId是為了防止重復添加局部關鍵幀，表明了我要添加的這一幀已經是mCurrentFrame的局部關鍵幀了。
FUCTION2.2.3.1.1.1.2:UpdateLocalPoints
更新局部關鍵點，步驟1：清空局部MapPoints，步驟2：遍歷局部關鍵幀mvpLocalKeyFrames，步驟3：將局部關鍵幀的MapPoints添加到mvpLocalMapPoints。
在局部地圖中查找與當前幀匹配的MapPoints
FUCTION2.2.3.1.1.2:SearchLocalPoints
在局部地圖中查找在當前幀視野范圍內的點，將視野范圍內的點和當前幀的特征點進行投影匹配。步驟1：遍歷當前幀的mvpMapPoints，標記這些MapPoints不參與之后的搜索，因為當前的mvpMapPoints一定在當前幀的視野中，取出該MapPoint后，更新能觀測到該點的幀數加1，標記該點被當前幀觀測到，標記該點將來不被投影，因為已經匹配過。步驟2：將所有局部MapPoints投影到當前幀，判斷是否在視野范圍內，然后進行投影匹配，已經被當前幀觀測到MapPoint不再判斷是否能被當前幀觀測到，步驟2.1：判斷LocalMapPoints中的點是否在在視野內。
FUCTION2.2.3.1.1.2.1:isInFrustum
判斷一個點是否在視野內。策略1：將MapPoint投影到當前幀, 並判斷是否在圖像內。策略2：計算MapPoint到相機中心的距離, 並判斷是否在尺度變化的距離內。策略3：計算當前視角和平均視角夾角(CreateInitialMapMonocular的UpdateNormalAndDepth的函數計算得到)的余弦值, 若小於cos(60), 即夾角大於60度則返回。最后根據深度預測尺度（對應特征點在一層），並標記該點將來要被投影(在函數SearchByProjection中被使用)。如果以上條件滿足就代表當前的地圖點在視野里。
如果當前的地圖點在視野里，那么觀測到該點的幀數加1，該MapPoint在某些幀的視野范圍內，nToMatch計數器+1。步驟2.2：對視野范圍內的MapPoints通過投影進行特征點匹配。
FUCTION2.2.3.1.1.2.2:SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th)
通過投影，對Local MapPoint進行跟蹤。將Local MapPoint投影到當前幀中, 由此增加當前幀的MapPoints，在SearchLocalPoints()的isInFrustum()中已經將Local MapPoints重投影到當前幀，isInFrustum()還標記了這些點是否在當前幀的視野中，即mbTrackInView，對這些MapPoints，在其投影點附近根據描述子距離選取匹配，以及最終的方向投票機制進行剔除。
更新局部所有MapPoints后對位姿再次優化。
FUCTION2.2.3.1.1.3:PoseOptimization(&mCurrentFrame)
更新當前幀的MapPoints被觀測程度，並統計跟蹤局部地圖的效果，如果當前幀的MapPoints有被觀測到，mnMatchesInliers就+1，過來統計該MapPoint被其它關鍵幀觀測到過，最后得到當前幀有多少MapPoints被能其他幀觀測到。如果這個數在剛剛重定位后的情況下還小於30的話認為跟蹤失敗，或者這個數在跟蹤的狀態下小於30，則表明跟蹤失敗。
- 2.2.3.2.mbOnlyTracking=true
在狀態跟蹤正常(重定位模式下)可進行地圖跟蹤。
2.2.4.PART3:收尾工作
依據bOK的狀態更新mState的狀態，把當前幀傳遞給mpMapDrawer線程。如果bOK的狀態是true，表示上面的跟蹤是正常的，那么就根據當前幀和上一幀的狀態更新運動模型的mVelocity。mVelocity表示的是上一幀到當前幀的位姿。如果bOK為false，則mVelocity為空。把當前幀的位姿傳遞給mpMapDrawer線程。
FUCTION2.2.4.1NeedNewKeyFrame
判斷當前幀是否為關鍵幀。步驟1：如果用戶在界面上選擇重定位，因為不需要地圖更新，那么將不插入關鍵幀。如果局部地圖（mpLocalMapper）線程被閉環檢測線程使用，則不插入關鍵幀。如果距離上一次重定位不超過1s不插入或者Map中的關鍵幀超過了mMaxFrames不插入。步驟3：得到參考關鍵幀跟蹤到的MapPoints數量，在執行上判斷如果參考幀的MapPoints點被觀測到的次數大於minObs，則認為該點被跟蹤到，並遞增計數器。步驟4：查詢局部地圖管理器是否繁忙。決策是否需要插入關鍵幀，1.很長時間沒有插入關鍵幀(1s)，2. localMapper處於空閑狀態，3.與之前參考幀（最近的一個關鍵幀）重復度不是太高，匹配的內點的數量要小於跟蹤到的MapPoints的一個比例值，但也不能太小。當（1||2&&3）時，如果localMapper處於空閑狀態就插入，否則中斷BA，判斷隊列里的關鍵幀是否小於3，如果是就插入關鍵幀。（tracking插入關鍵幀不是直接插入，而且先插入到mlNewKeyFrames中，然后localmapper再逐個pop出來插入到mspKeyFrames）。
FUCTION2.2.4.:CreateNewKeyFrame
步驟1：將當前幀構造成關鍵幀，將當前關鍵幀設置為當前幀的參考關鍵幀，將關鍵幀插入mlNewKeyFrames中。把當前幀的信息更新到最新關鍵幀中。
刪除那些在bundle adjustment中檢測為outlier的3D map點。如果跟蹤失敗，且地圖線程里的關鍵幀的數量小於5，那么就重置系統。更新當前幀的參考幀，保存上一幀的數據。最后記錄位姿信息(記錄的是當前幀與其參考幀的相對位姿)，用於軌跡復現。
這里的MAP比較多，我們來稍微區分下，Map* mpMap是用於顯示的地圖對象；TrackLocalMap我們只是保留離相機當前位置較近的特征點，把遠的或者視野外的特征點丟掉，這次特征點是用來和當前幀匹配球相機位置的，所以我們希望他能夠做得快一點；mpLocalMapper是全局地圖，記錄了SLAM運行以來的所有特征點，顯然它的規模要大一些，主要用來表達整個環境，但是直接在全局地圖上定位ie對計算機的負擔太大了，主要用於回環檢測和地圖表達。
至此，tracking線程就介紹完了。