System.cc是整个系统的主接口
通过构造函数system()对SLAM系统初始化,传入ORB字典文件路径、配置文件路径、传感器类型
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)
//首先判断传感器类型是单目、双目还是RGB-D 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 fsSetting()传入配置文件路径的读取配置文件,并由isOpened()判断文件是否存在。 cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ); if(!fsSettings.isOpened()) { cerr << "Failed to open settings file at: " << strSettingsFile << endl; exit(-1); } //若存在,则通过ORBVocabulary()加载ORB字典到mpVocabulary变量,然后通过loadFromTextFile()判断加载是否成功 //Load ORB Vocabulary cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl; 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); //初始化tracking对象 //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); //初始化local mapping对象,并开启线程运行 //Initialize the Local Mapping thread and launch mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR); mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper); //初始化loop closing对象,并开启线程运行 //Initialize the Loop Closing thread and launch mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR); mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser); //初始化显示线程Viewer(),开启线程显示图像和地图点 //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); }
通过TrackStereo(),TrackRGBD(),TrackMonocular()开启对应的跟踪线程
以TrackMonocular()为例:
//track 单目 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); } //检查模型 // Check mode change { unique_lock<mutex> lock(mMutexMode); //如果是mbActivateLocalizationMode则休眠1000ms直到停止局部建图 if(mbActivateLocalizationMode) { mpLocalMapper->RequestStop(); // Wait until Local Mapping has effectively stopped while(!mpLocalMapper->isStopped()) { usleep(1000); } mpTracker->InformOnlyTracking(true); mbActivateLocalizationMode = false; } //如果是mbDeactivateLocalizationMode则重新开启局部建图的线程 if(mbDeactivateLocalizationMode) { mpTracker->InformOnlyTracking(false); mpLocalMapper->Release(); mbDeactivateLocalizationMode = false; } } // Check reset { unique_lock<mutex> lock(mMutexReset); if(mbReset) { mpTracker->Reset(); mbReset = false; } } //开启tracking线程 cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp); unique_lock<mutex> lock2(mMutexState); mTrackingState = mpTracker->mState; mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn; return Tcw; }