orb slam2 代碼解析1

orb slam 2 代碼解析1

orb slam 2 是一個完整的SLAM系統，包括了 VO，跟蹤，及迴環，是一種基於單目稀疏特徵點的slam系統，使用orb 特徵 作爲視覺slam的特徵點，有以下的特點。

• 使用ORB特徵，實時性好
• 使用詞袋模型進行迴環檢測，g2o圖優化
• 支持多種傳感器 單目、雙目、RGB-D
• 支持ROS

我們主要是看看其中的代碼實現。

git clone https://github.com/stevenlovegrove/Pangolin.git #下載代碼

安裝過程忽略，可自行搜索教程

打開下載代碼的路徑，主要有以下文件夾

• camke_modules #cmake 相關項，搜索需要的庫
• Examples # 使用示例，包括ros下的示例，注意orbslam2主要代碼編譯之後只是生成庫orb_slam2的動態庫
• include # 主實現代碼頭文件
• src #實現代碼
• Thirdparty #第三方依賴庫，源碼下載及編譯
• Vocabulary #詞袋模型

根據這個文件組織結構，代碼閱讀課從示例代碼開始

決定預先看單目實現代碼，我們挑選 /Examples/Monocular/mono_kitti.cc 文件作爲解析的範例代碼，看文件名就知道這個示例是使用kitti 數據集，而不需要使用真正的攝像頭。

void LoadImages(const string &strSequence, vector<string> &vstrImageFilenames,
                vector<double> &vTimestamps); //前置聲明一個圖像加載函數,實現在最後，主要實現讀取文件路徑及名稱到vstrImageFilenames 裏

接下來，我們看下main函數，看下整個系統的運行流程，（只挑選核心代碼，精簡所有主功能無關代碼）

LoadImages(string(argv[3]), vstrImageFilenames, vTimestamps); //加載文件名稱
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); //orb slam 核心管理類，所有功能都在這裏實現

cv::Mat im;
for(int ni=0; ni<nImages; ni++)  //根據路徑一張張讀圖像
{
    im = cv::imread(vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED); //讀圖

    // Pass the image to the SLAM system
    SLAM.TrackMonocular(im,tframe);  //跟蹤及解析每張圖

}
SLAM.Shutdown(); //關閉slam，釋放資源，鎖等

SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");  /存取關鍵幀，主要存儲信息爲關鍵幀的 R t矩陣 R轉換爲四元數表示 

至此，我們看完了，處理流程，但由於主要的處理均在TrackMonocular函數中，所以我們需要再去分析 TrackMonocular 函數的實現，也即去看System.cc文件。

要閱讀system文件，可先看頭文件，瞭解大體的數據結構及功能，再看詳細的代碼實現，我們閱讀代碼的目的一般有以下幾個

• 分析問題的整體解決及結構
• 關注實現過程的語言技巧及細節
• 關注算法實現思路
• 用來對原有庫進行某種修改或擴展

對於orb_slam 這樣的庫，更重要的是算法實現和整體工程解決思路，所以我們更關注1,3點，第4點也可以關注，我們也會在其中分析如果需要修改時，可以怎麼去實現相關功能

//System.h 簡化版（數據結構部分，函數下回分解）

class System
{
public:
    enum eSensor{
        MONOCULAR=0,  //傳感器類型，單目，雙目，RGBD
        STEREO=1,
        RGBD=2
    };
public:
    System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true);    
    cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp);   
    cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp);  
    cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp);   
    void ActivateLocalizationMode();
    void DeactivateLocalizationMode();
    bool MapChanged();  
    void Reset();  
    void Shutdown();
    void SaveTrajectoryTUM(const string &filename);
    void SaveKeyFrameTrajectoryTUM(const string &filename);
    void SaveTrajectoryKITTI(const string &filename);
    int GetTrackingState();
    std::vector<MapPoint*> GetTrackedMapPoints();
    std::vector<cv::KeyPoint> GetTrackedKeyPointsUn();

private:

    eSensor mSensor; //傳感器類型定義
    ORBVocabulary* mpVocabulary; //詞袋模型，用於位置識別和特徵匹配
    KeyFrameDatabase* mpKeyFrameDatabase; //關鍵幀數據結構，使用vector<keyframe*> 實現，存儲關鍵幀
    Map* mpMap; //地圖 存儲 mappoint keyframe 所有的指針
    Tracking* mpTracker; //關鍵類，跟蹤幀，並估計變換和相機位置，確定是否爲關鍵幀，創建新的mappoint 並且當跟蹤失敗時重新定位
    LocalMapping* mpLocalMapper; //局部地圖，管理局部地圖並執行局部BA（優化）
    LoopClosing* mpLoopCloser; //迴環檢測，對每一個新的關鍵幀進行迴環搜索，如果發現有迴環則在新線程執行圖優化和全局BA
    Viewer* mpViewer;//繪製地圖和當前相機位置

    FrameDrawer* mpFrameDrawer; //幀繪製
    MapDrawer* mpMapDrawer;//地圖繪製

    // System threads: Local Mapping, Loop Closing, Viewer.
    // The Tracking thread "lives" in the main execution thread that creates the System object.
    std::thread* mptLocalMapping; //關鍵實現，orb採用三線程並行處理，提高實時性能，此爲局部繪圖線程
    std::thread* mptLoopClosing; //迴環檢測及處理線程
    std::thread* mptViewer; //展示線程，繪製相關交互界面線程

    // 使用三個鎖和一些標誌位實現 資源分配及調度 重置/ 模式改變/跟蹤

    std::mutex mMutexReset;  
    bool mbReset;

    std::mutex mMutexMode;
    bool mbActivateLocalizationMode;
    bool mbDeactivateLocalizationMode;

    int mTrackingState;
    std::vector<MapPoint*> mTrackedMapPoints;
    std::vector<cv::KeyPoint> mTrackedKeyPointsUn;
    std::mutex mMutexState;
};

至此，本次簡單分析就到這裏啦。