orb-slam2 从单目开始的简单学习（1）：总览 和 System.cc

前言

小白记录一下，讲得不正确或者是不理解的的地方可以在评论区或者是私信我，万分感谢。

1. 运行

简单的使用orb-slam2应该都会，不会的去看这篇
下面的以我的为例

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml /home/evening/slam/rgbd_dataset_freiburg1_xyz

四个参数如下：
a）./Examples/Monocular/mono_tum 可执行文件
b）Vocabulary/ORBvoc.txt 字典文件路径
c）Examples/Monocular/TUM1.yaml 相机参数文件路径
d) home/evening/slam/rgbd_dataset_freiburg1_xyz

2. mono_tum.cc

论文翻译传送门

（1）核心System SLAM

slam/ORB_SLAM2/Examples/Monocular/mono_tum.cc中真正核心的就一句

//*******************核心**************************
    // 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::MONOCULAR,true);
    //argv[1]:Vocabulary/ORBvoc.txt字典文件路径      argv[2]:Examples/Monocular/TUM1.yaml相机参数文件路径
    //true:启用可视化查看器
//*******************核心**************************  

对整个slam系统进行了初始化

（2）主循环

进入主循环通过SLAM.TrackMonocular(im,tframe)开始处理照片，将图像传递给slam系统

    // 进入循环 Main loop
    cv::Mat im;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file
        im = cv::imread(string(argv[3])+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        //argv[3]：home/evening/slam/rgbd_dataset_freiburg1_xyz
        
        double tframe = vTimestamps[ni];

        if(im.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << string(argv[3]) << "/" << vstrImageFilenames[ni] << endl;
            return 1;
        }

//计时
#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

        // 将图像传递给slam系统 Pass the image to the SLAM system
        SLAM.TrackMonocular(im,tframe);

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;

        // Wait to load the next frame
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)
            usleep((T-ttrack)*1e6);
    }

3.System.cc

核心函数为初始化函数和TrackMonocular函数

3.1 系统初始化函数

系统的初始化围绕上面这张图给的三个主要线程
（1）tracking
（2）localmapping
（3）localclosing
各自的初始化都会放在相应的章节中进行分析

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)
/*
strVocFile：词典文件 	strSettingsFile：相机配置文件  eSensor：相机类型（单/双目，RGB-D）bUseViewer：相机位姿&图片帧显示
其他为类的成员变量
*/      
       
{
    // Output welcome message
    cout << endl <<
    "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<
    "This program comes with ABSOLUTELY NO WARRANTY;" << endl  <<
    "This is free software, and you are welcome to redistribute it" << endl <<
    "under certain conditions. See LICENSE.txt." << endl << 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;

    //Check settings file
    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);//读取相机环境配置的yaml文件
    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;
    

     //ORBVocabulary.h ORBVocabulary本质是一个换名操作
     //typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB> ORBVocabulary
    mpVocabulary = new ORBVocabulary();
    bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);//读入词典文件，返回值为True
    
    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);//mpVocabulary->loadFromTextFile(strVocFile)
    			//本质是一个resize操作	
    

    //Create the Map
    mpMap = new Map();

    //Create Drawers. These are used by the Viewer
    mpFrameDrawer = new FrameDrawer(mpMap);//初始状态和一张黑img
    mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);//相机环境配置的yaml文件



    //初始化追踪线程Initialize the Tracking thread
    //(it will live in the main thread of execution, the one that called this constructor)
    
/*
Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor):
（2）ORBVocabulary* pVoc:词典（3）帧绘制器 （4）地图绘制器
*/
    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
   /*初始化Tracking实现了：
   （a）读取相机内参，畸变矩阵，fps等参数
   （b）根据单/双目 创建 ORBextractor
   （c）双目/RGB-D计算深度信息
   ...
   */                   
    //初始化局部建图 并 启动 Initialize the Local Mapping thread and launch
    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);//mpMap = new Map();创建出的map
    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);
}

3.2 TrackMonocular

从该函数开始进入追踪

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);
    }

    //检查模式改变（系统初始化时均置为False，即不会进入） Check mode change
    //主要有两种模式：（1）mbActivateLocalizationMode:激活定位模式 和 （2）mbDeactivateLocalizationMode清空局部地图
    {
        unique_lock<mutex> lock(mMutexMode);
        
    	
        if(mbActivateLocalizationMode)// mbActivateLocalizationMode:激活定位模式
        {
            mpLocalMapper->RequestStop();
            //mbStopRequested = true;
            //mbAbortBA = true;

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

            mpTracker->InformOnlyTracking(true);
            //mbOnlyTracking = flag;----->true
            //只进行追踪的线程  
            // 只计算相机的位姿，没有对局部地图进行更新

            mbActivateLocalizationMode = false;
            //使读入系统的每一帧图像都进行定位模式，可以将将该语句注释掉from《ORB_SLAM2 定位模式》
        }

        if(mbDeactivateLocalizationMode)// mbDeactivateLocalizationMode是否清空局部地图.
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            //mbStopped = false;
    	    //mbStopRequested = false;
            //清理关键帧
            
            mbDeactivateLocalizationMode = false;
        }
    }

    //检查是否重启系统 Check reset
    {
    unique_lock<mutex> lock(mMutexReset);
    if(mbReset)
    {
        mpTracker->Reset();//重置tracking线程，清空之前的东西
        mbReset = false;
    }
    }
    
    //图像预处理并开始追踪
    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;
}