ORB-SLAM 代碼筆記（四）tracking代碼結構

首先要清楚ORB-SLAM視覺跟蹤的原理，然后對tracking.cc中的函數逐個講解

 

代碼的前面部分是從配置文件中讀取校准好的相機參數（內參和畸變參數，以及雙目的深度測量設定），並且加載ORB特征點提取的參數（特征點數，金字塔層數，變化尺度，以及提取Fast關鍵點的閾值）；以及四個線程之間鎖的代碼。

接下來是將從攝像頭或者數據集讀入的圖像封裝成Frame類型對象：

這里以單目為例，無論圖片是RGB，BGR， 還是RGBA，BGRA，均轉化為灰度圖，放棄彩色信息。然后將當前讀入幀封裝為Frame類型的mCurrentFrame對象；為了讓單目成功初始化（單目的初始化需要通過平移運動歸一化尺度因子），初始化時mpIniORBextractor提取的特征點數量設定為普通幀的2倍。

mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

有了mCurrentFrame對象，就可以進入跟蹤模塊（tracking()函數）， 經過一系列跟蹤處理（重點），可以返回當前幀的位姿。

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)
{
    mImGray = im;

    if(mImGray.channels()==3)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGB2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGR2GRAY);
    }
    else if(mImGray.channels()==4)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
    }

    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
        mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
    else
        mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

    Track();

    return mCurrentFrame.mTcw.clone();
}

具體的tracking步驟如下：

（1）初始化

首先是初始化，分為雙目、RGBD，以及單目兩種不同的初始化過程，單目的初始化需要注意的較多，可以先看void Tracking::StereoInitialization()函數：

如果檢測到的特征點數 N>500，則將當前幀構建為第一個關鍵幀，關鍵幀通過當前幀，地圖點，以及BoW構建；同時，mpMap地圖中也需要加入該關鍵幀（mpMap包含圖像幀以及相應的地圖點）， 對每一個特征點，通過反投影得出3D地圖點，地圖點中通過3D位置信息，對應的關鍵幀，以及mpMap構建。

這其中的關系有點讓人混亂，其實直觀地去理解很容易：

mpMap就是我們整個位姿與地圖（可以想象成ORB-SLAM運行時的那個界面世界），MapPoint和KeyFrame都被包含在這個mpMap中。因此創建這三者對象（地圖，地圖點，關鍵幀）時，三者之間的關系在構造函數中不能缺少。另外，由於一個關鍵幀提取出的特征點對應一個地圖點集，因此需要記下每個地圖點的在該幀中的編號；同理，一個地圖點會被多幀關鍵幀觀測到，也需要幾下每個關鍵幀在該點中的編號。

地圖點，還需要完成兩個運算，第一個是在觀測到該地圖點的多個特征點中（對應多個關鍵幀），挑選出區分度最高的描述子，作為這個地圖點的描述子；

pNewMP->ComputeDistinctiveDescriptors();

第二個是更新該地圖點平均觀測方向與觀測距離的范圍，這些都是為了后面做描述子融合做准備。

pNewMP->UpdateNormalAndDepth();

最后，還需要在局部地圖中（為了實現局部Bundle Adjustment）插入當前關鍵幀，還需要初始化局部關鍵幀和局部地圖點。將上一幀，上一關鍵幀，以及上一關鍵幀Id號初始化為當前幀。參考關鍵幀，當前幀的參考關鍵幀，當前地圖中的參考地圖點（集），並初始化當前相機姿態。

void Tracking::StereoInitialization()
{
    if(mCurrentFrame.N>500)
    {
        // Set Frame pose to the origin
        mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));

        // Create KeyFrame
        KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

        // Insert KeyFrame in the map
        mpMap->AddKeyFrame(pKFini);

        // Create MapPoints and asscoiate to KeyFrame
        for(int i=0; i<mCurrentFrame.N;i++)
        {
            float z = mCurrentFrame.mvDepth[i];
            if(z>0)
            {
                cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
                MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
                pNewMP->AddObservation(pKFini,i);
                pKFini->AddMapPoint(pNewMP,i);
                pNewMP->ComputeDistinctiveDescriptors();
                pNewMP->UpdateNormalAndDepth();
                mpMap->AddMapPoint(pNewMP);

                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;
    }
}

 單目的初始化則相對負責，放在下一篇筆記中去講。