系统对象——System
在上一篇文章中， 我们了解到ORB-SLAM2创建了一个ORB_SLAM2::System类型的对象， 然后不断的把数据喂给该对象就可以驱动算法完成稀疏三维重建的工作。可以说这个System类型是ORB-SLAM2最外层的封装，本文我们将来分析该数据结构的成员变量和构造函数，从总体上了解这个类型。
1. 成员变量
下面四个字段是系统运行过程中维护的核心数据。枚举变量 mSensor 记录了相机类型，支持单目、双目、RGB-D三种形式的相机。mpMap 是整个 ORB-SLAM 系统维护的对象， 它完整记录了所有的关键帧和定位的地图点(MapPoint)。mpVocabulary 和 mpKeyFrameDatabase 主要用于快速检索关键帧，支持词袋模型进行重定位和闭环检测。
eSensor mSensor; //! 传感器类型，内部嵌套定义的枚举类型，MONOCULAR=0, STEREO=1, RGBD=2
ORBVocabulary* mpVocabulary; //! DBoW2离线字典，通过词袋模型进行重定位和闭环检测
KeyFrameDatabase* mpKeyFrameDatabase; //! 用于重定位和闭环检测的关键帧数据库
Map* mpMap; //! 系统的地图数据对象，保存所有关键帧和地图点
 |
 |
| (a). ORB-SLAM的系统框图 | (b). ORB-SLAM2系统框图 |
上面我们把 ORB-SLAM 的系统框图又贴了一遍，它有三个并行的线程:TRACKING、LOCAL MAPPING、LOOP CLOSING，分别用来完成轨迹跟踪、更新地图、闭环检测。
在下面左侧的代码片段中分别定义了三个对象，它们是这三个线程运行的主体框架。轨迹跟踪器，mpTracker，通过输入的图像估计相机位姿，生成关键帧并创建新的地图点。 如果相机位姿跟丢了，将由它触发重定位。局部地图管理器，mpLocalMapper，管理一段时间内的局部地图并进行局部的BA。 闭环检测器，mpLoopCloser，新增关键帧时搜索闭环。如果检测到闭环就触发一次位姿图优化，然后创建一个新的线程进行完整的BA。
下面右侧的代码片段中列举了三个线程对象 mptLocalMapping, mptLoopClosing 和 mptViewer，分别用于更新地图、闭环检测和可视化，
好像少了跟踪相机运动轨迹的 TRACKING 线程，实际上，例程中 main 函数运行的时候就已经创建了一个线程，
这个线程在一个for循环中通过TrackMonocular 或者 TrackRGBD 完成相机的位姿跟踪任务。
|
|
如下的这些成员变量主要是在记录当前系统的跟踪状态，当前观测到的特征点和地图点，由于 mTrackingState 可能在多个线程中被访问所以为之提供了一个互斥信号量进行保护。
int mTrackingState; //! 跟踪状态
std::vector<MapPoint*> mTrackedMapPoints; //! 当前帧参与估计相机位姿的地图点
std::vector<cv::KeyPoint> mTrackedKeyPointsUn; //! 当前帧中矫畸变(Undistorted)后的特征点
std::mutex mMutexState; //! 保护跟踪状态的互斥信号量
结构体 System 还有其它一些成员变量，主要用于可视化、管理系统工作模式。命名也很规范，根据字面意思就可以大致猜到变量的作用，这里不再详细展开了。
Viewer* mpViewer; //! 基于 Pangolin 的可视化对象，用于绘制地图和当前的相机位姿
FrameDrawer* mpFrameDrawer; //! 关键帧渲染器，用于绘制关键帧(或者说是相机位姿)
MapDrawer* mpMapDrawer; //! 地图渲染器，用于绘制地图点
std::mutex mMutexReset; //! 保护成员变量 mbReset 的互斥信号量
bool mbReset; //! 重置系统标志
std::mutex mMutexMode; //! 保护工作模式标志的互斥信号量
bool mbActivateLocalizationMode; //! 是否以纯定位模式运行，不理解为啥要弄两个这样的标志
bool mbDeactivateLocalizationMode;
2. 成员函数
结构体 System 中定义了不少成员函数，我们主要关注如下的四个。其中构造函数负责初始化 SLAM 系统，开启 LOCAL MAPPING 和 LOOP CLOSING 线程。 TrackStereo, TrackRGBD, TrackMonocular分别是双目、深度、单目相机的跟踪接口。
System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer); //! 构造函数
cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp); //! 跟踪双目相机
cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp); //! 跟踪深度相机
cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp); //! 跟踪单目相机
原作者还提供了一对儿接口，用于进入和退出纯定位模式。在该模式下 ORB-SLAM2 系统将以视觉里程计的形式运行，停止更新地图。
void ActivateLocalizationMode(); //! 停止更新地图只进行相机位姿跟踪
void DeactivateLocalizationMode(); //! 恢复地图更新进行完整的 SLAM
我们还可以通过接口 Reset 清空地图，重置 SLAM 系统。接口 Shutdown 可以关闭整个 SLAM 系统，等待所有线程退出。
void Reset(); //! 重置 SLAM 系统，清空地图
void Shutdown(); //! 关闭 SLAM 系统，等待所有线程退出
此外，System 还定义了一些接口函数可以在系统运行的过程中查看系统的状态，保存相机轨迹。
bool MapChanged(); //! 距离上次调用该接口，地图是否因为闭环、全局BA发生了大的改变
int GetTrackingState(); //! 获取跟踪状态
std::vector<MapPoint*> GetTrackedMapPoints(); //! 获取跟踪的地图点
std::vector<cv::KeyPoint> GetTrackedKeyPointsUn(); //! 获取校畸变后的特征点
void SaveTrajectoryTUM(const string & filename); //! 按照 TUM RGB-D 数据集的格式保存相机轨迹
void SaveKeyFrameTrajectoryTUM(const string & filename); //! 按照 TUM RGB-D 数据集的格式保存关键帧的相机位姿
void SaveTrajectoryKITTI(const string & filename); //! 按照 KITTI 数据集的格式保存相机轨迹
3. 构造函数
下面是System构造函数的代码片段，它有四个输入参数，其中 strVocFile 和 strSettingsFile 分别指示了词袋模型所需的字典文件和 SLAM 系统配置文件的路径， sensor则具体描述了建图所用的传感器类型 MONOCULAR=0, STEREO=1, RGBD=2。bUseViewer用于设置是否开启可视化程序，头文件的声明中说明了在缺省的情况下是开启的。 第2,3行冒号之后的那些是成员变量的初始化列表，可以在构造对象的时候给列表中的成员变量赋予初值。
System::System(const string & strVocFile, const string & strSettingsFile, const eSensor sensor, const bool bUseViewer)
: mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false),
mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false) {
在下面的代码片段中，我们省略了一些输出日志的语句。在输出一些日志之后，System 通过 OpenCV 的 FileStorage 打开了配置文件，并实例化了词汇表对象mpVocabulary。
// 省略日志输出
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
// 省略 if(!fsSettings.isOpened())
mpVocabulary = new ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
// 省略 if (!bVocLoad)
接着，实例化关键帧数据库和地图对象，以及地图和关键帧的渲染器。
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
mpMap = new Map();
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
然后，构建了用于三个基本任务的对象mpTracker, mpLocalMapper, mpLoopCloser，同时为LOCAL MAPPING、LOOP CLOSING分别创建了两个任务线程。 在例程中，TRACKING 任务由运行main函数时的主线程，通过调用 TrackXXXX 的接口驱动的。
// (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);
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
如果输入参数bUseViewer要求开启可视化界面，则实例化对象 mpViewer 和 mptViewer。同时初始化轨迹跟踪器 mpTracker 的可视化对象。
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);
}
4. 数据驱动接口
System通过TrackStereo、TrackMonocular、TrackRGBD三个接口分别来处理双目、单目、深度相机的数据。这三个接口的原型如下：
cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp);
cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp);
cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp);
他们有一个共同类型的返回值，cv::Mat，该值以齐次矩阵的形式记录了相机的位姿估计Tcw。如果跟丢了，那么这些接口将返回一个空的cv::Mat。 TrackStereo的前两个参数分别是左目和右目采集到的图像数据，它们可以是3通道(CV_8UC3)的彩色图， 也可以是单通道(CV_8U)的灰度图。彩色图也将在该接口中先转换为灰度图后在进行计算。类似的TrackRGBD的前两个参数分别是相机图像和深度图，而TrackMonocular则只需要提供一个图像即可。 这些接口的最后一个参数则是采集到图像的时间戳。
这三个接口的实现也大同小异，下图的代码片段是用来跟踪单目相机位姿的 TrackMonocular 函数。 它在函数之初先检查一下传感器类型，若不是单目相机则报错退出。接着对系统的工作状态和复位状态进行检查并做相应的处理，因为篇幅原因这部分代码被省略了。 关键是第7行，进一步的将图像和时间戳数据提供给轨迹跟踪器 mpTracker 进行处理。mpTracker 完成系统状态和成员变量的更新之后返回退出，并返回相机位姿的齐次矩阵赋值给局部变量 Tcw。
cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) {
if(mSensor!=MONOCULAR) {
cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;
exit(-1);
}
// 省略对工作模式和复位状态检查和处理
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);
// 省略状态和成员变量更新
return Tcw;
}
双目相机的跟踪接口TrackStereo的实现与单目的没有什么太大的差别， 只是一开始判定mSensor是否是STEREO，然后在检查并处理了工作模式和复位状态后，通过 GrabImageStereo 接口来具体处理双目的数据。
if(mSensor!=STEREO)
cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);
深度相机的跟踪接口TrackRGBD则判定mSensor是否是RGBD， 并通过 GrabImageRGBD 接口来具体处理深度相机的数据。
if(mSensor!=RGBD)
cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);
5. 完
本文中，我们简单查看了System类型内部定义的成员变量和成员函数。详细分析了它的构造函数和三种相机的位姿跟踪接口。 在分析的过程中，我们可以看到 ORB-SLAM2 通过三个任务对象mpTracker、mpLocalMapper、mpLoopCloser分别完成了轨迹跟踪、更新地图、闭环检测。我们将在后续的文章中详细分析它们。