ORB_SLAM2代码逻辑分析(1) -- 代码架构（system类）

1、系统基本构成

1.1、三大线程及其功能

  ORB_SLAM系统分为三大线程(这一点可以去看论文)，每个线程有如下功能：
1）主线程：Tracking线程就是在主线程上 这一部分主要工作是从图像中提取ORB特征，根据上一帧进行姿态估计，或者进行通过全局重定位初始化位姿，然后跟踪已经重建的局部地图，优化位姿，再根据一些规则确定新的关键帧。

2）Local mappng线程 这一部分主要完成局部地图构建。包括对关键帧的插入，验证最近生成的地图点并进行筛选，然后生成新的地图点，使用局部捆集调整（Local BA），最后再对插入的关键帧进行筛选，去除多余的关键帧。

3）Loop closing线程 这一部分主要分为两个过程，分别是闭环探测和闭环校正。闭环检测先使用BOW进行探测，然后通过Sim3算法计算相似变换。闭环校正，主要是闭环融合和Essential Graph的图优化。

*4）Viewer线程 对估计的位姿和特征点进行可视化显示

1.2、关键数据类型

//三大线程
Tracking -- 跟踪线程类：	1.接受一个frame，检测其特征点并计算描述子
						2.计算帧与帧帧之间的匹配关系，计算当前帧的位姿
						3.new keyframe decision，并插入一个新的关键帧
						4.创建新的Mappoints
						5.重定位

LocalMapping -- 局部地图类：	1.管理局部地图
							2.计算局部地图的Bundle adjustment

LoopClosing -- 回环操作类：	1.对每一个新插入的keyframe进行回环检测
							2.检测回环后，进行位姿图优化，并在一个新的线程中进行全局的full BA

//可视化线程
Viewer -- 可视化工具类：负责地图和当前相机位姿的可视化

FrameDrawer -- Frame可视化工具：负责在可视化窗口中画出Frame的实体

MapDrawer -- Map可视化工具：负责画出mappoint在可视化窗口中的实体

//基本数据类型
ORBVocabulary -- orb视觉词袋模型

ORBextractor  -- orb特征点检测器

Frame -- 帧：输入的基本单位，包括当前相机的图像、内参和位姿等数据，
			以及特征点检测，词袋计算，左右目关键点match等功能

KeyFrame -- 关键帧：Map类的基本单位之一，由frame生成（并非继承），除了frame包含的数据以外（不包括方法），
				  包含与地图点、地图和关键帧数据库的关联信息，以及BA优化的相关信息。

KeyFrameDatabase -- 关键帧数据库：包含了当前地图中的关键帧序列，以及关键帧的在数据库中的
							    增加和消除、计算回环的候选和重定位候选的方法		

Mappoint -- 地图点：Map类的基本单位之一，由特征点生成，包括的该特征点的ID，在全局地图中
				  的位置，被观测的次数，是否为坏点和用于计算BA的信息，以及相关的方法

Map	--	地图：地图包含Mappoint和KeyFrame两个成员，负责管理维护全局的Mappoint和KeyFrame

//其他
Initializer -- 单目视觉初始化：仅用于单目视觉的初始化，包含初始化的方法

2、程序入口

2.1、系统初始化

首先我们以demo文件夹中的rgbd_tum.cc为例

  在代码第65行，创建类一个slam系统，输入参数分别为（词袋文件，参数设置文件，传感器类型，是否可视化）

 // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);
    //我们在运行代码时，需要输入的两个参数就是 词袋路径 和 设置文件路径

  然后在rgbd_tum.cc之后的代码中，读取数据集的文件，然后进行TrackRGBD。

// Pass the image to the SLAM system -- 将文件传入到slam系统中
        SLAM.TrackRGBD(imRGB,imD,tframe);

TrackRGBD函数在system.cc中，我们跳转进去，看一下里面做了那些工作：

cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)
{
    if(mSensor!=RGBD)
    {
        cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl;
        exit(-1);
    }    

    // Check mode change -- 先检查系统状态
    {
        unique_lock<mutex> lock(mMutexMode);
        if(mbActivateLocalizationMode)	//如果激活定位模式
        {
            mpLocalMapper->RequestStop();	//局部地图建图请求停止

            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped())	//等待局部建图完全停止
            {
                usleep(1000);
            }

            mpTracker->InformOnlyTracking(true);//Track线程模式为 OnlyTracking
            mbActivateLocalizationMode = false;	//定位跟踪模式设置完成，标志位置为false
        }
        if(mbDeactivateLocalizationMode)	//如果失活定位模式
        {
            mpTracker->InformOnlyTracking(false);	//失活OnlyTracking模式
            mpLocalMapper->Release();	//清空局部地图 TODO：这个有必要吗？好像有点暴力
            mbDeactivateLocalizationMode = false;	//标志位置为false
        }
    }

    // Check reset
    {
    unique_lock<mutex> lock(mMutexReset);
    if(mbReset)	//如果reset，就将tracker reset
    {
        mpTracker->Reset();
        mbReset = false;
    }
    }
    //获取RGBD图像到系统中
    cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);

    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;	//TrakingState为Tracker的State
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;//TrackedPoints为当前帧的Mappoints
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;	//TrackedKeyPointsUn为当前帧的去畸变的关键点
    return Tcw;	//返回当前相机的位姿
}

  我们可以看出，程序的真正入口实际是在GrabImageRGBD中，该函数在Tracking.cc中实现，我们下次再解读。这次我们先看一下一个system类包含了哪些关于SLAM系统的工作。

system.cc 解读

  之前介绍类程序的起始位置，我们不妨看一下system类对整个框架是如何初始化的。
  在system.cc的构造函数System::System()中，首先包含了我们要构建的ORB_SLAM框架。函数输入如下：

const string &strVocFile,	//词袋文件路径
const string &strSettingsFile,	//设置文件路径
const eSensor sensor,	//传感器类型：MONOCULAR,STEREO,RGBD
const bool bUseViewer	//是否可视化

  同时，还初始化了以下参数，按顺序如下：

mSensor(sensor);	//传感器类型
mpViewer(static_cast<Viewer*>(NULL));	//可视化地图和当前相机位姿类
mbReset(false);		//系统重置flag，默认false
mbActivateLocalizationMode(false);	//激活定位模式flag，默认false
mbDeactivateLocalizationMode(false);//失活定位模式flag，默认false

//Check settings file -- 将配置文件读入到内存中，存储到fsSettings变量中
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);	
//Load ORB Vocabulary -- 读取视觉词典
mpVocabulary = new ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
//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 
//初始化Tracking线程
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
if(bUseViewer)
{	//如果可视化，创建一个可视化的线程
	mpViewer = new Viewer(this,mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
	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);                     

2.2、system类成员解析

pubic:
// Input sensor	-- 传感器类型
    enum eSensor{
        MONOCULAR=0,
        STEREO=1,
        RGBD=2
    };
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;	//keyframe数据库指针

    // 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.
    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.
    LoopClosing* mpLoopCloser;

    //可视化工具
    // The viewer draws the map and the current camera pose. It uses 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.
    std::thread* mptLocalMapping;
    std::thread* mptLoopClosing;
    std::thread* mptViewer;

    // Reset flag -- Reset标志位，清空地图
    std::mutex mMutexReset;
    bool mbReset;

    // Change mode flags
    std::mutex mMutexMode;
    bool mbActivateLocalizationMode;	//为真 则 停止建图，进行重定位
    bool mbDeactivateLocalizationMode;	//为真 则 停止重定位，继续建图

    // Tracking state
    int mTrackingState;	//系统的Tracking状态
    std::vector<MapPoint*> mTrackedMapPoints;	//已经追踪到的Mappoints
    std::vector<cv::KeyPoint> mTrackedKeyPointsUn;	//已经追踪到的去畸变的特征点
    std::mutex mMutexState;
};

2.3、system类方法解析

public:

    // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads. -- 系统的构造函数
    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.
    //停止建图线程，并只进行视觉的tracking以进行定位
    void ActivateLocalizationMode();
    // This resumes local mapping thread and performs SLAM again.
    void DeactivateLocalizationMode();

    // Returns true if there have been a big map change (loop closure, global BA)
    // since last call to this function
    //如果地图有加到改动，就调用中国函数
    bool MapChanged();

    // Reset the system (clear map)	-- 将bReset置true，并在track线程中清空当前系统中的所有数据
    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.
    // Only for stereo and RGB-D. This method does not work for monocular.
    // 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.
    // This method works for all sensor input.
    // 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.
    // Only for stereo and RGB-D. This method does not work for monocular.
    // Call first Shutdown()
    // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php
    void SaveTrajectoryKITTI(const string &filename);

    // TODO: Save/Load functions
    // SaveMap(const string &filename);
    // LoadMap(const string &filename);

    // Information from most recent processed frame
    // You can call this right after TrackMonocular (or stereo or RGBD)
    int GetTrackingState();
    std::vector<MapPoint*> GetTrackedMapPoints();
    std::vector<cv::KeyPoint> GetTrackedKeyPointsUn();