這裡是Tracking部分的第二部分，詳細講述各分支的程式碼及其實現原理。

單目初始化

MonocularInitialization()

目標：從初始的兩幀單目影象中，對SLAM系統進行初始化（得到初始兩幀的匹配，相機初始位姿，初始MapPoints），以便之後進行跟蹤。

方式：並行得計算基礎矩陣E和單應矩陣H，恢復出最開始兩幀相機位姿；三角化得到MapPoints的深度，獲得點雲地圖。

尋找匹配特徵點

單目的初始化有專門的初始化器，只有連續的兩幀特徵點均>100個才能夠成功構建初始化器。

完成前兩幀特徵點的匹配

ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
 

mvIniMatches儲存mInitialFrame,mCurrentFrame之間匹配的特徵點。

這裡的ORBmatcher類後面有機會再詳細介紹。功能就是ORB特徵點的匹配。

從匹配點中恢復初始相機位姿

在得到超過100個匹配點對後，運用RANSAC 8點法同時計算單應矩陣H和基礎矩陣E，並選擇最合適的結果作為相機的初始位姿。

mpInitializer =  new Initializer(mCurrentFrame,1.0,200);
mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)
 

Initializer類是初始化類。下面要詳細講一下類成員函式 intilize（）,能夠根據當前幀和兩幀匹配點計算出相機位姿R|t ，以及三角化得到 3D特徵點。

Initializer-> intilize（）

（1）呼叫多執行緒分別用於計算fundamental matrix和homography

    // 計算homograpy並打分
    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    // 計算fundamental matrix並打分
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

    // Wait until both threads have finished
    threadH.join();
    threadF.join();
 

（2）計算得分比例，選取某個模型進行恢復R|t

    float RH = SH/(SH+SF);

    // 步驟5：從H矩陣或F矩陣中恢復R,t
    if(RH>0.40)
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    else //if(pF_HF>0.6)
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

具體的：

FindHomography或FindFundamental

（1） 計算單應矩陣cv::Mat Hn = ComputeH21(vPn1i,vPn2i);，並進行評分currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);，分別通過單應矩陣將第一幀的特徵點投影到第二幀，以及將第二幀的特徵點投影到第一幀，計算兩次重投影誤差，疊加作為評分SH。

（2） 計算基礎矩陣cv::Mat Fn = ComputeF21(vPn1i,vPn2i);，並進行評分currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);，兩次重投影誤差疊加作為評分SF。

建立初始地圖

如果恢復相機初始位姿成功，那麼我們能得到前兩幀影象的位姿，以及三角測量後的3維地圖點。之後進行如下操作：

            // Set Frame Poses
            // 將初始化的第一幀作為世界座標系，因此第一幀變換矩陣為單位矩陣
            mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
            // 由Rcw和tcw構造Tcw,並賦值給mTcw，mTcw為世界座標系到該幀的變換矩陣
            cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
            Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
            tcw.copyTo(Tcw.rowRange(0,3).col(3));
            mCurrentFrame.SetPose(Tcw);

            // 步驟6：將三角化得到的3D點包裝成MapPoints
            // Initialize函式會得到mvIniP3D，
            // mvIniP3D是cv::Point3f型別的一個容器，是個存放3D點的臨時變數，
            // CreateInitialMapMonocular將3D點包裝成MapPoint型別存入KeyFrame和Map中
            CreateInitialMapMonocular();

 先將當前幀位姿Tcw設定好，之後CreateInitialMapMonocular () 建立初始地圖，並進行相應的3D點資料關聯操作：

CreateInitialMapMonocular ()

（1）建立關鍵幀

因為只有前兩幀，所以將這兩幀都設定為關鍵幀，並計算對應的BoW，並在地圖中插入這兩個關鍵幀。

（2）建立地圖點+資料關聯

將3D點包裝成地圖點，將地圖點與關鍵幀，地圖進行資料關聯。

• 關鍵幀與地圖點關聯

一個地圖點可被多個關鍵幀觀測到，將觀測到這個地圖點的關鍵幀與這個地圖點進行關聯；關鍵幀能夠觀測到很多地圖點，將這些地圖點與該關鍵幀關聯。

        //關鍵幀上加地圖點
        pKFini->AddMapPoint(pMP,i);
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
        //地圖點的屬性：能夠觀測到的關鍵幀
        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);

地圖點與關鍵幀上的特徵點關聯後，計算最合適的描述子來描述該地圖點，用於之後跟蹤的匹配。

        // b.從眾多觀測到該MapPoint的特徵點中挑選區分讀最高的描述子
        pMP->ComputeDistinctiveDescriptors();
        // c.更新該MapPoint平均觀測方向以及觀測距離的範圍
        pMP->UpdateNormalAndDepth();

• 關鍵幀的特徵點與地圖點關聯

一個關鍵幀上特徵點由多個地圖點投影而成，將關鍵幀與地圖點關聯。

        //Fill Current Frame structure
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

• 關鍵幀與關鍵幀關聯

關鍵幀之間會共視一個地圖點，如果共視的地圖點個數越多，說明這兩個關鍵幀之間的聯絡越緊密。對於某個關鍵幀，統計其與其他關鍵幀共視的特徵點個數，如果大於某個閾值，那麼將這兩個關鍵幀進行關聯。

    pKFini->UpdateConnections();
    pKFcur->UpdateConnections();

（3）Global BA

​ 對上述只有兩個關鍵幀和地圖點的地圖進行全域性BA。

    // 步驟5：BA優化
    Optimizer::GlobalBundleAdjustemnt(mpMap,20);

（4） 地圖尺寸初始化（中值深度為1）

​ 由於單目SLAM的的地圖點座標尺寸不是真實的，比如x=1可能是1m也可能是1cm。選擇地圖點深度的中位數作為單位尺寸1當作基準來進行地圖的尺寸初始化

跟蹤勻速運動模型

TrackWithMotionModel()