【学习SLAM】ORB SLAM2代码解析(1)

今天我们来一起读ORB SLAM2的代码，其实前几个月的时候，在博客写过一些自己的理解，但是不是很详细。然后最近再用ORB SLAM2跑一些室内的数据集，所以会更加详细一些。

我们按照github上面的说明搭建好环境，然后下载对应的代码，发现也是一个CMAKE工程，然后我们进去对应的CMakeLists.txt，发现是把对应几个.cc文件编译成可执行文件。分别是：把rgbd_tum.cc编译成rgbd_tum可执行文件；把stereo_kitti.cc编译成stereo_kitti可执行文件；把stereo_euroc.cc编译成stereo_euroc可执行文件；把mono_tum.cc编译成mono_tum可执行文件；把mono_kitti.cc编译成mono_kitti可执行文件；把mono_euroc.cc编译成mono_euroc可执行文件；

add_executable(rgbd_tum
Examples/RGB-D/rgbd_tum.cc)

add_executable(stereo_kitti
Examples/Stereo/stereo_kitti.cc)

add_executable(stereo_euroc
Examples/Stereo/stereo_euroc.cc)

add_executable(mono_tum
Examples/Monocular/mono_tum.cc)

add_executable(mono_kitti
Examples/Monocular/mono_kitti.cc)

add_executable(mono_euroc
Examples/Monocular/mono_euroc.cc)

下面我们重点看一下单目的例子，即mono_tum.cc程序。

首先判断输入是否是4个参数，按照正确的输入应该是：./mono_tum path_to_vocabulary path_to_settings path_to_sequence。这里需要注意的是传入的参数分别对应argv[0],argv[1],argv[2],argv[3]。并且argv[0]对应的是程序本身。这里总有一些小伙伴搞不清楚。

if(argc != 4)
    {
        cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" << endl;
        return 1;
    }

举个例子：

我调用程序的输入为：

./Examples/Monocular/mono_tum  Vocabulary/ORBvoc.txt /home/w/Documents/shLeft/shleft.yaml  /home/w/Documents/shLeft

cd ~/ORB_SLAM2/

　　./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml Examples/rgbd_dataset_freiburg1_xyz

mono_tum 程序

ORBvoc.txt     词典

TUM1.yaml 内参

rgbd_dataset_freiburg1_xyz 图片序列路径与时间戳

接下来，定义一个类型为vector 的变量vstrImageFilenames和类型为vector的变量vTimestamps，一个字符串类型变量strFile，用来存储/home/w/Documents/shLeft/rgb.txt这个文件。

    vector vstrImageFilenames;
    vector vTimestamps;
    string strFile = string(argv[3])+"/rgb.txt";

接下来进入一个子函数：

LoadImages(strFile, vstrImageFilenames, vTimestamps)

进入这个子函数，我们发现这个函数的功能就是将strFile文件中的时间戳信息存到vTimestamps中去，将png图片路径存在vstrImageFilenames中去。

void LoadImages(const string &strFile, vector &vstrImageFilenames, vector &vTimestamps)
{
    ifstream f;
    f.open(strFile.c_str());
    cout << "already open the file:" << strFile << endl;

    // skip first three lines
    string s0;
    getline(f,s0);
    getline(f,s0);
    getline(f,s0);

    while(!f.eof())
    {
        string s;
        getline(f,s); //读取每一行的内容到s里面去
        if(!s.empty())
        {
            stringstream ss;double t;string sRGB;

            ss << s;
            ss >> t;
            //将strFile文件中的时间戳信息存到vTimestamps中去
            vTimestamps.push_back(t);
            ss >> sRGB;
            //将png图片路径存在vstrImageFilenames中去。
            vstrImageFilenames.push_back(sRGB);
        }
    }
}

然后把控制权返回到主函数，继续往下执行。获取图片的个数，这里是在对应的文件中得到的。如果我们需要跑自己采集的数据集，那么就需要在数据集文件中预处理对应的信息。

int nImages = vstrImageFilenames.size();//图片的个数

下面进入函数的重要部分，初始化SLAM系统，(这里是进入了system.cc文件中)这里会初始化三个线程：Tracking,local Mapping和Loop Closing。

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

以我的输入为例子，这里的参数是：词典;相机参数文件；单目相机；true;

Vocabulary/ORBvoc.txt /home/w/Documents/shLeft/shleft.yaml

我们先暂停进去system.cc里面看看程序是如何进行初始化这三个线程的吧。先看一下传入的参数：

System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,const bool bUseViewer):
                        mSensor(sensor), mpViewer(static_cast(NULL)), mbReset(false),mbActivateLocalizationMode(false),mbDeactivateLocalizationMode(false)

首先是词典文件，以我的例子这里是Vocabulary/ORBvoc.txt 第二个参数是相机参数文件，以我的例子是/home/w/Documents/shLeft/shleft.yaml。第三个参数是用ORB_SLAM2::System::MONOCULAR初始化mSensor。其余的参数不用太在意，已经用默认的参数进行初始化了。

输出ORB SLAM2的欢迎信息，然后判断传感器的类型是单目、立体相机或者RGB-D相机。

    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);//strSettingsFile:相机参数文件
    if(!fsSettings.isOpened())
    {
       cerr << "Failed to open settings file at: " << strSettingsFile << endl;
       exit(-1);
    }

然后加载ORB词典文件。

    //加载 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);

创建地图

     mpMap = new Map();

创建两个显示窗口FrameDrawer，MapDrawer

    //Create Drawers. These are used by the Viewer 创建两个显示窗口FrameDrawer，MapDrawer
    mpFrameDrawer = new FrameDrawer(mpMap);
    mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

初始化Tracking线程。

   //Initialize the Tracking thread 初始化Tracking线程。
    //(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. 初始化Local Mapping线程并开启线程运行。
    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);

初始化loop closing对象，并开启线程运行

    //Initialize the Loop Closing thread and launch 初始化loop closing对象，并开启线程运行
    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);

初始化显示线程Viewer()，开启线程显示图像和地图点

    //Initialize the Viewer thread and launch. 初始化显示线程Viewer()，开启线程显示图像和地图点
    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);

下面我们把注意力收回来主函数，接着下面定义一个vector类型的容器vTimesTrack来存放追踪的时间，并把容器的大小设置为图片数量的大小。

 // Vector for tracking time statistics
    vector vTimesTrack;//记录每张图片的跟踪时间的容器，下面循环中使用。
    vTimesTrack.resize(nImages);

下面进入主循环。主循环主要完成下面几件事情，循环读取每一张图片，对图片中的特征点进行跟踪，记录跟踪的时间。代码的注释已经写在对应的代码附近啦。

// Main loop
    cv::Mat im;
    for(int ni=0; nitframe变量中。

        if(im.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << string(argv[3]) << "/" << vstrImageFilenames[ni] << endl;
            return 1;
        }
        cout<<" Pass the image to the SLAM system-----ni = "< >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;

        // Wait to load the next frame
        double T=0;                      //上一帧、当前帧、下一帧
        if(ni0)
            T = tframe-vTimestamps[ni-1];//（当前帧-上一帧）的时间戳的差

        if(ttrack

循环处理完每一张图片后，就需要关闭这个SLAM系统了。关闭的时候会判断每个线程都结束了才会关闭，不然就处于等待状态。

    // Stop all threads
    SLAM.Shutdown();

然后程序在结束后会对跟踪的时间进行排序，然后计算跟踪时间的中位数和跟踪时间的平均值。之后就把程序中的关键帧的信息保存在文件

KeyFrameTrajectory.txt中。保存的格式是：timestamp,tx,ty,tz,qx,q,qz,qw。

    // Tracking time statistics
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni