ORB-SLAM2 代码（+理论）解析（二）： System.cc 内容

 

目录

 

一、具体代码：   

二、主要功能（总管）      

三、包含的程序

3.1    mpTracker = new Tracking( )

3.2    SLAM.TrackMonocular(im,tframe)

                       3.3 System::Shutdown函数

3.4 SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt")

---

---

 

一、具体代码：   

ORB_SLAM2::System SLAM(argv[1],argv[2], ORB_SLAM2::System::MONOCULAR, true)

/**
 * strVocFile           为词典文件
 * strSettingsFile      为设置配置文件
 * sensor               sensor类型，单目、双目和RGBD
 * bUseViewer           是否进行相机位姿和图片帧显示
*/
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)
{
       // Output welcome message
    cout << endl << "Welcome to use ORB_SLAM2 project !"  << 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;

    //配置文件读取
    cv::FileStorage fsSettings(strSettingsFile.c_str(), 	//将配置文件名转换成为字符串
    						   cv::FileStorage::READ);		//只读
    //如果打开失败，就输出调试信息
    if(!fsSettings.isOpened())
    {
       cerr << "Failed to open settings file at: " << strSettingsFile << endl;
       //然后退出
       exit(-1);
    }

    //Load ORB Vocabulary
    cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;

    //建立一个新的ORB字典
    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
    //这里的帧绘制器和地图绘制器将会被可视化的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)
 //3.初始化的Tracking线程是在main线程中运行的(其实也就是main线程)，所以不需要调用new thread来创建

/*
    	 mpVocabulary,		        //字典
    	  mpFrameDrawer, 		//帧绘制器
    	 mpMapDrawer,			//地图绘制器
         mpMap, 			//地图
         mpKeyFrameDatabase, 		//关键帧地图
         strSettingsFile, 	       //设置文件路径
         mSensor);			//传感器类型iomanip
*/

    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
 
    //Initialize the Local Mapping thread and launch
    //4.初始化LocalMapping对象，并开启LocalMapping线程
    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
 
    //Initialize the Loop Closing thread and launch 
    //5.初始化LooClosing对象，并开启LoopClosing线程
    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
 
    //Initialize the Viewer thread and launch
    //是否显示图像帧和相机位姿的窗口
    if(bUseViewer)
    {
        //6.初始化显示窗口对象，并启动线程用于显示图像和地图点
        mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
        mptViewer = new thread(&Viewer::Run, mpViewer);
        mpTracker->SetViewer(mpViewer);
    }
 
    //Set pointers between threads
    //7.给各个线程分配资源
    //以下变量设置是为了在各个线程中可以访问其他线程中的数据，方便线程之间的数据交互
    mpTracker->SetLocalMapper(mpLocalMapper);
    mpTracker->SetLoopClosing(mpLoopCloser);
 
    mpLocalMapper->SetTracker(mpTracker);
    mpLocalMapper->SetLoopCloser(mpLoopCloser);
 
    mpLoopCloser->SetTracker(mpTracker);
    mpLoopCloser->SetLocalMapper(mpLocalMapper);
}

 

二、主要功能（总管）      

1）读取ORB字典，为后期的 回环检测 做准备；

2）创建 关键帧数据库 KeyFrameDatabase，用于存放关键帧相关数据；

3）初始化Tracking线程。其实Tracking线程是在main线程中运行的，可以简单的认为main线程就是Tracking线程；

4）初始化并启动 LocalMapping线程；

5）初始化并启动 LoopClosing线程；

6）初始化并启动窗口显示线程mptViewer；

7）在各个线程之间分配资源，方便线程彼此之间的数据交互。

 

三、包含的程序

 

3.1    mpTracker = new Tracking( )

功能：在本主进程中初始化追踪线程

注意：这里初始化的Tracking线程是在main线程中运行的 (其实也就是main线程)，所以不需要调用new thread来创建

 

 

3.2    SLAM.TrackMonocular(im,tframe)

功能：
1）check mode、check reset，这个和窗口的视图显示有关，可以在显示相机位姿的窗口左侧点击按钮实际操作看看效果；
2）将图片和时间戳传给了Tracking，这里才是真正的调用了Tracking的接口GrabImageMonocular，获得视觉SLAM的信息

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

    // Check mode change 
    {
        // 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱
        unique_lock<mutex> lock(mMutexMode);
        
        // mbActivateLocalizationMode为true会关闭局部地图线程；这里的作用感觉是 单独定位准备
        if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped())
            {
                usleep(1000);
            }

            // 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新
            // 设置mbOnlyTracking为真
            mpTracker->InformOnlyTracking(true);
            
            // 关闭线程可以使得别的线程得到更多的资源
            mbActivateLocalizationMode = false;
        }
        
        // 如果mbDeactivateLocalizationMode是true，局部地图线程就被释放, 关键帧从局部地图中删除.
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            mbDeactivateLocalizationMode = false;
        }
    }

    // Check reset
    {
       unique_lock<mutex> lock(mMutexReset);
       if(mbReset)
       {
          mpTracker->Reset();
          mbReset = false;
       }
    }

    //获取相机位姿的估计结果
    cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

    //视觉SLAM获得的信息
    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;

    return Tcw;
}

  //获取相机位姿的估计结果， 在 tracking 线程里面介绍
重要的函数：      cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

 

3.3 System::Shutdown函数

这个函数也是在mono_euroc 的main函数中调度的，当SLAM系统处理完所有的图片后，系统就要退出，此时调用该接口 关闭 除Tracking线程（也就是main线程）之外的所有线程。

//退出
void System::Shutdown()
{
	//对局部建图线程和回环检测线程发送终止请求
    mpLocalMapper->RequestFinish();
    mpLoopCloser->RequestFinish();
    //如果使用了可视化窗口查看器
    if(mpViewer)
    {
    	//向查看器发送终止请求
        mpViewer->RequestFinish();
        //等到，知道真正地停止
        while(!mpViewer->isFinished())
            usleep(5000);
    }

    // Wait until all thread have effectively stopped
    while(!mpLocalMapper->isFinished() || 
    	  !mpLoopCloser->isFinished()  || 
    	   mpLoopCloser->isRunningGBA())			//说实在话不是很理解这个 TODO isRunningGBA函数是用来做什么的？
    {
        usleep(5000);
    }

    if(mpViewer)
    	//如果使用了可视化的窗口查看器执行这个
    	// TODO 但是不明白这个是做什么的。如果我注释掉了呢？
        pangolin::BindToContext("ORB-SLAM2: Map Viewer");
}

 

3.4 SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt")

保存关键帧的轨迹

//保存关键帧的轨迹
void System::SaveKeyFrameTrajectoryTUM(const string &filename)
{
    cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl;

    //获取关键帧vector并按照生成时间对其进行排序
    vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);

    //本来这里需要进行原点校正，但是实际上没有做
    // Transform all keyframes so that the first keyframe is at the origin.
    // After a loop closure the first keyframe might not be at the origin.
    //cv::Mat Two = vpKFs[0]->GetPoseInverse();

    //文件写入的准备操作
    ofstream f;
    f.open(filename.c_str());
    f << fixed;

    //对于每个关键帧
    for(size_t i=0; i<vpKFs.size(); i++)
    {
    	//获取该 关键帧
        KeyFrame* pKF = vpKFs[i];

        //原本有个原点校正，这里注释掉了
       // pKF->SetPose(pKF->GetPose()*Two);

        //如果这个关键帧是bad那么就跳过
        if(pKF->isBad())
            continue;

        //抽取旋转部分和平移部分，前者使用四元数表示
        cv::Mat R = pKF->GetRotation().t();
        vector<float> q = Converter::toQuaternion(R);
        cv::Mat t = pKF->GetCameraCenter();
        //按照给定的格式输出到文件中
        f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at<float>(0) << " " << t.at<float>(1) << " " << t.at<float>(2)
          << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;

    }

    //关闭文件
    f.close();
    cout << endl << "trajectory saved!" << endl;
}

SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt")

问题：主函数中，运行程序后，并没有发现保存的文件，感觉因该把具体的位置写全！

 

参考： https://blog.csdn.net/moyu123456789/article/details/90229783