ORB-SLAM2 从理论到代码实现（二）：System 类-平芜编程栈

1. 基本结构

System类是ORB-SLAM2的主调度类，也是ORB-SLAM2系统的入口。

把关键代码提取出来，进行标注。

mpVocabulary = new ORBVocabulary();//读取ORB词袋 mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);//创建关键帧数据库 mpMap = new Map();//创建地图对象 mpFrameDrawer = new FrameDrawer(mpMap);//创建显示关键帧的窗口 mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);//创建显示地图的窗口 mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);//初始化Tracking线程，该线程在主循环中 mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);//初始化Local Mapping线程 mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);//启动线程 mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);//初始化LoopClosing线程 mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);//启动线程 mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);//初始化窗口 mptViewer = new thread(&Viewer::Run, mpViewer);//启动，前面会有一个是否可视化的判断

从上面的代码来看构造函数system()主要对SLAM系统初始化。下面看看System.cc中的函数接口。

cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp)//追踪双目数据，返回mpTracker->GrabImageStereo(imLeft,imRight,timestamp) cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)//追踪深度相机数据，返回mpTracker->GrabImageRGBD(im,depthmap,timestamp) cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)//追踪单目相机数据返回mpTracker->GrabImageMonocular(im,timestamp) void System::ActivateLocalizationMode()//mbActivateLocalizationMode = true，激活 mbActivateLocalizationMode void System::DeactivateLocalizationMode()// mbDeactivateLocalizationMode = true，激活 mbDeactivateLocalizationMode void System::Shutdown()//判断运行是否结束，结束则关闭系统 void System::SaveTrajectoryTUM(const string &filename)//求数据集中每一帧的位姿 void System::SaveKeyFrameTrajectoryTUM(const string &filename)//求数据集中每一关键帧的位姿 void System::SaveTrajectoryKITTI(const string &filename)//求数据集中每一帧的位姿

TrackStereo()、TrackRGBD()、TrackMonocular()：

判断输入传感器类型是否正确

判断模式，如果是mbActivateLocalizationMode则休眠1000ms直到停止局部建图。如果是mbDeactivateLocalizationMode则重新开启局部建图的线程。然后调用GrabImageStereo(imRectLeft, imRectRight)、GrabImageRGBD(im, depthmap)、GrabImageMonocular(im)开启tracking线程

2. 构造函数

//构建函数 /* input: 字典， 类型（mono，rgbd，stereo）,参数，mapfile， 是否导入地图 */ System::System(const string strVocFile, const eSensor sensor, ORBParameters& parameters, const std::string & map_file, bool load_map): // map serialization addition mSensor(sensor), mbReset(false),mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), map_file(map_file), load_map(load_map)

system构造函数参数包括离线训练的词袋字典文件名，camera类型，ORB特征点提取参数，也可以增加已知地图纯定位功能。

//Load ORB Vocabulary cout << endl << "Loading ORB Vocabulary." << endl; // 创建字典 mpVocabulary = new ORBVocabulary(); //try to load from the binary file，导入2进制文件 bool bVocLoad = mpVocabulary->loadFromBinFile(strVocFile+".bin");

原始开源代码调用词典为txt文本，众所周知读入txt的文本的速度十分耗时，导致slam启动较慢。已有人将其改进为bin文件存储和读取，以此可提高启动速度。

// begin map serialization addition // load serialized map // 是否载入已有地图 if (load_map && LoadMap(map_file)) { std::cout << "Using loaded map with " << mpMap->MapPointsInMap() << " points\n" << std::endl; } else { // 新建地图，则需创建一个空的地图对象， //Create KeyFrame Database // 初始化关键帧数据对象，主要存储字典里包含的值，用于重定位和闭环 mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary); //Create the Map // 创建map数据，应该是特征点和对应的keypose mpMap = new Map(); } // end map serialization addition

orb-slam2开始slam时需创建一个新的空的map对象。同时更为重要创建一个KeyFrameDatabase对象，此对象用于存储关键帧的在词典中的词袋向量，后续的闭环和重定位也是基于此词袋数据进行搜索匹配。

//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, mpMap, mpKeyFrameDatabase, mSensor, parameters);

初始化三个线程中tracking线程；

//Initialize the Local Mapping thread and launch // 初始化建图线程 mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR); mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);

初始化三个线程中Local Mapping线程；

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

初始化三个线程中loop closing线程；

//Set pointers between threads // 三个线程间存在联系 mpTracker->SetLocalMapper(mpLocalMapper); mpTracker->SetLoopClosing(mpLoopCloser); mpLocalMapper->SetTracker(mpTracker); mpLocalMapper->SetLoopCloser(mpLoopCloser); mpLoopCloser->SetTracker(mpTracker); mpLoopCloser->SetLocalMapper(mpLocalMapper); currently_localizing_only_ = false; }

3. 输出接口

SaveTrajectoryTUM()： void System::SaveTrajectoryTUM(const string &filename) { ......//部分代码省略 vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();//获得所有关键帧vpKFs sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);//对关键帧排序，闭环检测后第一关键帧可能就不在起始位置了 cv::Mat Two = vpKFs[0]->GetPoseInverse();//获得第一帧相对于世界坐标系的位姿 //遍历所有帧 ofstream f; f.open(filename.c_str()); f << fixed; list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();//参考关键帧迭代器 list<double>::iterator lT = mpTracker->mlFrameTimes.begin();//时间戳迭代器 list<bool>::iterator lbL = mpTracker->mlbLost.begin();//标志，当追踪失败为true for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++) { if(*lbL) continue;//如果当前帧追踪失败则丢弃，继续追踪下一帧 KeyFrame* pKF = *lRit; cv::Mat Trw = cv::Mat::eye(4,4,CV_32F); // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe. while(pKF->isBad())//追踪成功但是关键帧不好，则获取当前关键帧相对于上一帧的位姿，并将上//一帧设为关键帧，依次不断的判断关键帧的质量，直到选取合适的关键帧 { Trw = Trw*pKF->mTcp; pKF = pKF->GetParent(); } Trw = Trw*pKF->GetPose()*Two;//将关键帧的位姿乘第一帧相对于世界坐标的位姿得到关键帧相//对于世界坐标的位姿 cv::Mat Tcw = (*lit)*Trw;//在将关键帧相对于世界坐标的位姿乘当前帧相对于关键帧的位姿得//到当前帧相对于世界坐标的位姿 cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();//求旋转矩阵R cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);//求t vector<float> q = Converter::toQuaternion(Rwc);//求四元数q f << setprecision(6) << *lT << " " << setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl; } f.close();//关闭文件 }

SaveKeyFrameTrajectoryTUM()与SaveTrajectoryTUM()类似。