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  (University of Zaragoza)
* For more information see 
*
* 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 .
*/


#ifndef SYSTEM_H
#define SYSTEM_H

#include
#include
#include

#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 ×tamp);

    // 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 ×tamp);

    // 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 ×tamp);

    // 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  (University of Zaragoza)
* For more information see 
*
* 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 .
*/

#ifndef KEYFRAMEDATABASE_H
#define KEYFRAMEDATABASE_H

#include 
#include 
#include 

#include "KeyFrame.h"
#include "Frame.h"
#include "ORBVocabulary.h"

#include


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 DetectLoopCandidates(KeyFrame* pKF, float minScore);

    // Relocalization
    // 追踪丢失以后重定位
    std::vector DetectRelocalizationCandidates(Frame* F);

protected:

    // Associated vocabulary
    // 所需字典
    const ORBVocabulary* mpVoc;

    // Inverted file
    // 存储的关键帧
    std::vector<list > 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 &vpMPs);
    // 获取所有的关键帧、点云、参考点云
    std::vector GetAllKeyFrames();
    std::vector GetAllMapPoints();
    std::vector GetReferenceMapPoints();

    long unsigned int MapPointsInMap();
    long unsigned  KeyFramesInMap();

    long unsigned int GetMaxKFid();

    void clear();

    vector mvpKeyFrameOrigins;

    std::mutex mMutexMapUpdate;

    // This avoid that two points are created simultaneously in separate threads (id conflict)
    std::mutex mMutexPointCreation;

protected:
    std::set mspMapPoints;
    std::set mspKeyFrames;

    std::vector 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 ×tamp);
    cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp);
    cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp);
    // 设置局部地图、设置局部闭环检测、设置视图
    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 mvbPrevMatched;
    std::vector 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 mlRelativeFramePoses;
    list 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 mvpLocalKeyFrames;
    std::vector 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 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