ORB-SLAM代码详解之SLAM系统初始化

转载请注明出处：http://blog.csdn.net/c602273091/article/details/54933760

system.h

这里包含了整个SLAM系统所需要的一起，通过看这个文件可以对ORB-SLAM系统有什么有一个大概的了解。不过之前我们需要对于多线程了解一些基本的东西——信号量【1】和多线程【2】。

具体注释如下：

/*** This file is part of ORB-SLAM2.** Copyright (C) 2014-2016 Ra煤l Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)* For more information see <https://github.com/raulmur/ORB_SLAM2>** ORB-SLAM2 is free software: you can redistribute it and/or modify* it under the terms of the GNU General Public License as published by* the Free Software Foundation, either version 3 of the License, or* (at your option) any later version.** ORB-SLAM2 is distributed in the hope that it will be useful,* but WITHOUT ANY WARRANTY; without even the implied warranty of* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the* GNU General Public License for more details.** You should have received a copy of the GNU General Public License* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.*/#ifndef SYSTEM_H#define SYSTEM_H#include<string>#include<thread>#include<opencv2/core/core.hpp>#include "Tracking.h"#include "FrameDrawer.h"#include "MapDrawer.h"#include "Map.h"#include "LocalMapping.h"#include "LoopClosing.h"#include "KeyFrameDatabase.h"#include "ORBVocabulary.h"#include "Viewer.h"namespace ORB_SLAM2{class Viewer; // 画图class FrameDrawer; // 画每一帧class Map; // 对map进行操作class Tracking; // 追踪的过程class LocalMapping; // 局部地图class LoopClosing; // 闭环检测class System{public:    // Input sensor    // 输入设备：单目、立体视觉、RGBD    enum eSensor{        MONOCULAR=0,        STEREO=1,        RGBD=2    };public:    // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads.    // 对SLAM系统的初始化：包含局部地图、闭环检测、视图三个线程    System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true);    // Proccess the given stereo frame. Images must be synchronized and rectified.    // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.    // Returns the camera pose (empty if tracking fails).    cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp);    // Process the given rgbd frame. Depthmap must be registered to the RGB frame.    // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.    // Input depthmap: Float (CV_32F).    // Returns the camera pose (empty if tracking fails).    cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp);    // Proccess the given monocular frame    // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.    // Returns the camera pose (empty if tracking fails).    // 对单目的图片进行追踪    cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp);    // This stops local mapping thread (map building) and performs only camera tracking.    // 暂停局部地图的构建，只进行追踪，这个名字很有迷惑性    void ActivateLocalizationMode();    // This resumes local mapping thread and performs SLAM again.    // 重新开启局部地图的线程    // 在这里使用的是mutex信号量的多线程编程    void DeactivateLocalizationMode();    // Reset the system (clear map)    // 复位清楚地图    void Reset();    // All threads will be requested to finish.    // It waits until all threads have finished.    // This function must be called before saving the trajectory.    // 等到所有线程结束任务的时候关闭每个线程    void Shutdown();    // Save camera trajectory in the TUM RGB-D dataset format.    // Call first Shutdown()    // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset    void SaveTrajectoryTUM(const string &filename);    // Save keyframe poses in the TUM RGB-D dataset format.    // Use this function in the monocular case.    // Call first Shutdown()    // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset    void SaveKeyFrameTrajectoryTUM(const string &filename);    // Save camera trajectory in the KITTI dataset format.    // Call first Shutdown()    // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php    // kitti数据集的保存位姿的方法    void SaveTrajectoryKITTI(const string &filename);    // TODO: Save/Load functions    // SaveMap(const string &filename);    // LoadMap(const string &filename);private:    // Input sensor    // 输入的传感器类型    eSensor mSensor;    // 用于特征匹配和闭环检测的字典    // ORB vocabulary used for place recognition and feature matching.    ORBVocabulary* mpVocabulary;    // KeyFrame database for place recognition (relocalization and loop detection).    // 关键帧存储的地方    KeyFrameDatabase* mpKeyFrameDatabase;    // Map structure that stores the pointers to all KeyFrames and MapPoints.    // 所有的关键帧和点云存储的地方    Map* mpMap;    // Tracker. It receives a frame and computes the associated camera pose.    // It also decides when to insert a new keyframe, create some new MapPoints and    // performs relocalization if tracking fails.    // 追踪器。接受一帧并且计算相机位姿，并决定何时插入关键帧，关键点。    // 当追踪失败以后进行重定位    Tracking* mpTracker;    // Local Mapper. It manages the local map and performs local bundle adjustment.    // 构建局部地图并对局部地图使用BA。    LocalMapping* mpLocalMapper;    // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs    // a pose graph optimization and full bundle adjustment (in a new thread) afterwards.    // 闭环检测，每插入一个关键帧就计算是否有闭环并且进行全局的BA。    LoopClosing* mpLoopCloser;    // The viewer draws the map and the current camera pose. It uses Pangolin.    // 使用Pangolin库看地图和相机位姿。    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.    // 追踪这个线程是在main函数里面，这里另外开了局部地图、局部闭环检测、显示地图三个线程    std::thread* mptLocalMapping;    std::thread* mptLoopClosing;    std::thread* mptViewer;    // Reset flag    std::mutex mMutexReset;    bool mbReset;    // Change mode flags    std::mutex mMutexMode;    bool mbActivateLocalizationMode;    bool mbDeactivateLocalizationMode;};}// namespace ORB_SLAM#endif // SYSTEM_H

这样对于ORB-SLAM我们有了一个大致的认识。

接下来我们看System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer).

system.c的System::System

使用understand的control flow，一剑封喉，直接看到各个部分的联系。流程图出来了，感觉看起来很爽。

在这个基础上，我再对整个流程进行注释。

System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,               const bool bUseViewer):mSensor(sensor),mbReset(false),mbActivateLocalizationMode(false),        mbDeactivateLocalizationMode(false){    // Output welcome message    cout << endl <<    "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<    "This program comes with ABSOLUTELY NO WARRANTY;" << endl  <<    "This is free software, and you are welcome to redistribute it" << endl <<    "under certain conditions. See LICENSE.txt." << endl << endl;    cout << "Input sensor was set to: ";    // 判断什么类型的传感器    if(mSensor==MONOCULAR)        cout << "Monocular" << endl;    else if(mSensor==STEREO)        cout << "Stereo" << endl;    else if(mSensor==RGBD)        cout << "RGB-D" << endl;    //Check settings file    // 检查配置文件是否存在    // cv::FileStorage对XML/YML的配置文件进行操作，读取配置文件    // yml的配置文件已经读入fsSettings了    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);    if(!fsSettings.isOpened())    {       cerr << "Failed to open settings file at: " << strSettingsFile << endl;       exit(-1);    }    // 加载ORB的字典    //Load ORB Vocabulary    cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;    // 加载字典到mpVocabulary    mpVocabulary = new ORBVocabulary();    bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);    if(!bVocLoad)    {        cerr << "Wrong path to vocabulary. " << endl;        cerr << "Falied to open at: " << strVocFile << endl;        exit(-1);    }    cout << "Vocabulary loaded!" << endl << endl;    //Create KeyFrame Database    // 创建关键帧的数据库    mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);    //Create the Map    // 创建地图    mpMap = new Map();    //Create Drawers. These are used by the Viewer    // 创建视图    mpFrameDrawer = new FrameDrawer(mpMap);    //　创建画图器    mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);    //Initialize the Tracking thread    //(it will live in the main thread of execution, the one that called this constructor)    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,                             mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);    //Initialize the Local Mapping thread and launch    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);    //Initialize the Loop Closing thread and launch    // 初始化局部图线程    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);    //Initialize the Viewer thread and launch    // 初始化显示线程    mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);    if(bUseViewer)        mptViewer = new thread(&Viewer::Run, mpViewer);    mpTracker->SetViewer(mpViewer);    //Set pointers between threads    mpTracker->SetLocalMapper(mpLocalMapper);    mpTracker->SetLoopClosing(mpLoopCloser);    mpLocalMapper->SetTracker(mpTracker);    mpLocalMapper->SetLoopCloser(mpLoopCloser);    mpLoopCloser->SetTracker(mpTracker);    mpLoopCloser->SetLocalMapper(mpLocalMapper);}

接下来对每个类看一下初始化的效果。从字典类，关键帧类，地图类，局部图类等等看看它们如何进行初始化。

ORBVocabulary()：字典类

这个字典的类在Orbvocabulary.h里面定义，但是实际上是定义在DBow2这个库里面。所以目前就不再深究。

typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB> ORBVocabulary;

KeyFrameDatabase()：关键帧的数据库

这个定义在KeyFrameDatabase.h里面。

/*** This file is part of ORB-SLAM2.** Copyright (C) 2014-2016 Ra煤l Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)* For more information see <https://github.com/raulmur/ORB_SLAM2>** ORB-SLAM2 is free software: you can redistribute it and/or modify* it under the terms of the GNU General Public License as published by* the Free Software Foundation, either version 3 of the License, or* (at your option) any later version.** ORB-SLAM2 is distributed in the hope that it will be useful,* but WITHOUT ANY WARRANTY; without even the implied warranty of* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the* GNU General Public License for more details.** You should have received a copy of the GNU General Public License* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.*/#ifndef KEYFRAMEDATABASE_H#define KEYFRAMEDATABASE_H#include <vector>#include <list>#include <set>#include "KeyFrame.h"#include "Frame.h"#include "ORBVocabulary.h"#include<mutex>namespace ORB_SLAM2{    class KeyFrame;    class Frame;class KeyFrameDatabase{public:    // 初始化关键帧仓库    KeyFrameDatabase(const ORBVocabulary &voc);    // 添加关键帧    void add(KeyFrame* pKF);    // 去除关键帧    void erase(KeyFrame* pKF);    //清理关键帧    void clear();    // Loop Detection    // 闭环检测    std::vector<KeyFrame *> DetectLoopCandidates(KeyFrame* pKF, float minScore);    // Relocalization    // 追踪丢失以后重定位    std::vector<KeyFrame*> DetectRelocalizationCandidates(Frame* F);protected:    // Associated vocabulary    // 所需字典    const ORBVocabulary* mpVoc;    // Inverted file    // 存储的关键帧    std::vector<list<KeyFrame*> > mvInvertedFile;    // Mutex    std::mutex mMutex;};} //namespace ORB_SLAM#endif

在KeyFameDatabase.cc中，它的初始化为：这里的mvInvertedFile我不清楚是什么。

KeyFrameDatabase::KeyFrameDatabase (const ORBVocabulary &voc):    mpVoc(&voc){    mvInvertedFile.resize(voc.size());}

Map(): 地图的构建

在Map.h里面，对地图的各种操作。主要集中在关键帧、点云、参考点云。

class Map{public:    Map();    // 添加关键帧    void AddKeyFrame(KeyFrame* pKF);    // 添加点云    void AddMapPoint(MapPoint* pMP);    // 去掉点云    void EraseMapPoint(MapPoint* pMP);    // 去掉关键帧    void EraseKeyFrame(KeyFrame* pKF);    // 设置参考点云    void SetReferenceMapPoints(const std::vector<MapPoint*> &vpMPs);    // 获取所有的关键帧、点云、参考点云    std::vector<KeyFrame*> GetAllKeyFrames();    std::vector<MapPoint*> GetAllMapPoints();    std::vector<MapPoint*> GetReferenceMapPoints();    long unsigned int MapPointsInMap();    long unsigned  KeyFramesInMap();    long unsigned int GetMaxKFid();    void clear();    vector<KeyFrame*> mvpKeyFrameOrigins;    std::mutex mMutexMapUpdate;    // This avoid that two points are created simultaneously in separate threads (id conflict)    std::mutex mMutexPointCreation;protected:    std::set<MapPoint*> mspMapPoints;    std::set<KeyFrame*> mspKeyFrames;    std::vector<MapPoint*> mvpReferenceMapPoints;    long unsigned int mnMaxKFid;    std::mutex mMutexMap;};} //namespace ORB_SLAM

在Map.cc里，

Map::Map():mnMaxKFid(0){}

FrameDrawer(): 画图工具

这个先不用管。画关键帧的。当然还有Mapdrawer，画整个图的。

class MapDrawer{public:    MapDrawer(Map* pMap, const string &strSettingPath);    Map* mpMap;    // 画点云    void DrawMapPoints();    // 画关键帧    void DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph);    // 画当前相机采集的图片    void DrawCurrentCamera(pangolin::OpenGlMatrix &Twc);    // 设置当前相机的位姿    void SetCurrentCameraPose(const cv::Mat &Tcw);    // 设置参考帧    void SetReferenceKeyFrame(KeyFrame *pKF);    void GetCurrentOpenGLCameraMatrix(pangolin::OpenGlMatrix &M);private:    float mKeyFrameSize;    float mKeyFrameLineWidth;    float mGraphLineWidth;    float mPointSize;    float mCameraSize;    float mCameraLineWidth;    cv::Mat mCameraPose;    std::mutex mMutexCamera;};

Tracking：追踪

这一块的东西比较重要，需要重点看看。

mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,   mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);

class Tracking{  public:    Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,             KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor);    // 对输入的图片进行处理，提取特征和立体匹配    // Preprocess the input and call Track(). Extract features and performs stereo matching.    cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double &timestamp);    cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp);    cv::Mat GrabImageMonocular(const cv::Mat &im, const double &timestamp);    // 设置局部地图、设置局部闭环检测、设置视图    void SetLocalMapper(LocalMapping* pLocalMapper);    void SetLoopClosing(LoopClosing* pLoopClosing);    void SetViewer(Viewer* pViewer);    // Load new settings    // The focal lenght should be similar or scale prediction will fail when projecting points    // TODO: Modify MapPoint::PredictScale to take into account focal lenght    // 把焦距考虑进去改变MapPoint的scale    void ChangeCalibration(const string &strSettingPath);    // Use this function if you have deactivated local mapping and you only want to localize the camera.    // 设置只对摄像头的位姿进行计算    void InformOnlyTracking(const bool &flag);public:    // Tracking states    // 追踪的状态    enum eTrackingState{        SYSTEM_NOT_READY=-1,        NO_IMAGES_YET=0,        NOT_INITIALIZED=1,        OK=2,        LOST=3    };    eTrackingState mState;    eTrackingState mLastProcessedState;    // Input sensor    // 输入的传感器    int mSensor;    // Current Frame    // 当前帧和图片    Frame mCurrentFrame;    cv::Mat mImGray;    // Initialization Variables (Monocular)    std::vector<int> mvIniLastMatches;    std::vector<int> mvIniMatches;    std::vector<cv::Point2f> mvbPrevMatched;    std::vector<cv::Point3f> mvIniP3D;    Frame mInitialFrame;    // Lists used to recover the full camera trajectory at the end of the execution.    // Basically we store the reference keyframe for each frame and its relative transformation    // 最后保存下来的整个的摄像头的位姿    list<cv::Mat> mlRelativeFramePoses;    list<KeyFrame*> mlpReferences;    list<double> mlFrameTimes;    list<bool> mlbLost;    // True if local mapping is deactivated and we are performing only localization    bool mbOnlyTracking;    void Reset();protected:    // Main tracking function. It is independent of the input sensor.    // 与输入传感器无关的追踪函数    void Track();    // Map initialization for stereo and RGB-D    // 地图的初始化    void StereoInitialization();    // Map initialization for monocular    // 单目视觉的地图初始化    void MonocularInitialization();    void CreateInitialMapMonocular();    void CheckReplacedInLastFrame();    bool TrackReferenceKeyFrame();    void UpdateLastFrame();    bool TrackWithMotionModel();    bool Relocalization();    void UpdateLocalMap();    void UpdateLocalPoints();    void UpdateLocalKeyFrames();    bool TrackLocalMap();    void SearchLocalPoints();    bool NeedNewKeyFrame();    void CreateNewKeyFrame();    // In case of performing only localization, this flag is true when there are no matches to    // points in the map. Still tracking will continue if there are enough matches with temporal points.    // In that case we are doing visual odometry. The system will try to do relocalization to recover    // "zero-drift" localization to the map.    bool mbVO;    //Other Thread Pointers    LocalMapping* mpLocalMapper;    LoopClosing* mpLoopClosing;    //ORB    ORBextractor* mpORBextractorLeft, *mpORBextractorRight;    ORBextractor* mpIniORBextractor;    //BoW    ORBVocabulary* mpORBVocabulary;    KeyFrameDatabase* mpKeyFrameDB;    // Initalization (only for monocular)    Initializer* mpInitializer;    //Local Map    KeyFrame* mpReferenceKF;    std::vector<KeyFrame*> mvpLocalKeyFrames;    std::vector<MapPoint*> mvpLocalMapPoints;    // System    System* mpSystem;    //Drawers    Viewer* mpViewer;    FrameDrawer* mpFrameDrawer;    MapDrawer* mpMapDrawer;    //Map    Map* mpMap;    //Calibration matrix    cv::Mat mK;    cv::Mat mDistCoef;    float mbf;    //New KeyFrame rules (according to fps)    int mMinFrames;    int mMaxFrames;    // Threshold close/far points    // Points seen as close by the stereo/RGBD sensor are considered reliable    // and inserted from just one frame. Far points requiere a match in two keyframes.    float mThDepth;    // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled.    float mDepthMapFactor;    //Current matches in frame    int mnMatchesInliers;    //Last Frame, KeyFrame and Relocalisation Info    KeyFrame* mpLastKeyFrame;    Frame mLastFrame;    unsigned int mnLastKeyFrameId;    unsigned int mnLastRelocFrameId;    //Motion Model    cv::Mat mVelocity;    //Color order (true RGB, false BGR, ignored if grayscale)    bool mbRGB;    list<MapPoint*> mlpTemporalPoints;};} 

追踪这一块的东西呢发现涉及了基本上所有的内容。

LocalMapping(): 局部地图的类

//Initialize the Local Mapping thread and launch    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);

这个类就是对局部地图的关键点、关键帧、局部地图的闭环检测进行操作。这里还涉及多线程的线程同步问题，在看代码的时候可以忽略这个问题。

LoopClosing(): 闭环检测

//Initialize the Loop Closing thread and launch    // 初始化局部图线程    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);

这里是进行回环检测的部分，解决的就是在地图中检测回环并进行偏移校正。

其实看了这么的初始化代码，主要就是对这个项目有一个大致的了解。接下来就是对程序的主体部分进行详细的介绍。

最重要的就是： SLAM.TrackMonocular(im,tframe); 对这个部分进行非常细致的了解，就可以由这条线抽丝剥茧得到想要的东西。

在类里面有很多private、public、protected这些关键字，看【3】可以知道它们的作用域。

参考链接：
【1】mutex： http://www.cplusplus.com/reference/mutex/
【2】thread： http://www.cplusplus.com/reference/thread/thread/
【3】public、private、protected： http://www.jb51.net/article/54224.htm