ORB-SLAM2代码阅读笔记（一）：从mono_kitti单目运行开始

目录

 

1.写在前边的话

2.mono_kitti单目数据集运行效果

3.系统框架简要了解

4.最开始的代码

5.mono_kitti代码总结

---

1.写在前边的话

        最近开始学习SLAM，除了学习SLAM相关理论知识外，自己还开始学习ORB-SLAM2开源系统代码，毕竟ORB-SLAM2是比较完整的SLAM系统，里边包括了前端VO，后端优化和闭环检测这三大主要模块。所以，结合SLAM的理论学习和ORB-SLAM2代码完整梳理一遍相信一定会对SLAM相关理论有更深入的认识。

        ORB-SLAM2的github代码路径为https://github.com/raulmur/ORB_SLAM2 下载后按照github上的相关编译提示来进行编译就可以了。对了，需要预先安装的软件也都要照着要求一一安装好。

2.mono_kitti单目数据集运行效果

ORB-SLAM2支持单目、双目和RGBD数据，我自己选择mono_kitti的单目数据集来运行和调试。如下所示来运行系统：

运行命令格式为：./mono_kitti path_to_vocabulary path_to_settings path_to_sequence

该命令共有4个参数：

mono_kitti为ORB-SLAM2编译生成的针对MONO_KITTI数据集的可执行文件，一般路径为ORB-SLAM2工程目录下Examples/Monocular/mono_kitti；

path_to_vocabulary为所选词典的路径，路径为ORB-SLAM2/Vocabulary/ORBvoc.txt；

path_to_settings为配置文件，存放在ORB-SLAM2/Examples/Monocular下，我运行时选择了KITTI00-02.yaml，该配置文件中存放的相机的内参和外参，提取特征时候图像金字塔相关参数，可视化过程中用到的参数；

path_to_sequence为数据集路径，这个根据自己下载的kitti数据集路径来指定， http://www.cvlibs.net/datasets/kitti/eval_odometry.php为kitti数据集的下载路径。

xxx@xxx-TM1705:~/Desktop/SLAM/ORB_SLAM2$ ./Examples/Monocular/mono_kitti Vocabulary/ORBvoc.txt Examples/Monocular/KITTI00-02.yaml ../dataset/sequences/00/

ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza.
This program comes with ABSOLUTELY NO WARRANTY;
This is free software, and you are welcome to redistribute it
under certain conditions. See LICENSE.txt.

Input sensor was set to: Monocular

Loading ORB Vocabulary. This could take a while...
Vocabulary loaded!

//以下是从KITTI00-02.yaml中读取的相机参数，其中fx fy cx cy为相机内参，k1 k2 p1 p2为相机外参而且都为0，可以看出该数据集图片是没有畸变的
Camera Parameters: 
- fx: 718.856
- fy: 718.856
- cx: 607.193
- cy: 185.216
- k1: 0
- k2: 0
- p1: 0
- p2: 0
- fps: 10
- color order: RGB (ignored if grayscale)
//ORB特征提取器的参数
ORB Extractor Parameters: 
- Number of Features: 2000
- Scale Levels: 8
- Scale Factor: 1.2
- Initial Fast Threshold: 20
- Minimum Fast Threshold: 7

-------
Start processing sequence ...
Images in the sequence: 4541

New Map created with 115 points
Local Mapping STOP
Local Mapping RELEASE
System Reseting
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
New Map created with 84 points
Wrong initialization, reseting...
System Reseting
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
New Map created with 147 points
Local Mapping STOP
Local Mapping RELEASE
System Reseting
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
New Map created with 153 points
virtual int g2o::SparseOptimizer::optimize(int, bool): 0 vertices to optimize, maybe forgot to call initializeOptimization()
virtual int g2o::SparseOptimizer::optimize(int, bool): 0 vertices to optimize, maybe forgot to call initializeOptimization()
virtual int g2o::SparseOptimizer::optimize(int, bool): 0 vertices to optimize, maybe forgot to call initializeOptimization()
Local Mapping STOP
Local Mapping RELEASE
System Reseting
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
New Map created with 128 points
Track lost soon after initialisation, reseting...
System Reseting
Reseting Local Mapper... done
Reseting Loop Closing... done
Reseting Database... done
New Map created with 109 points

Loop detected!
Local Mapping STOP
Local Mapping RELEASE
Starting Global Bundle Adjustment
Global Bundle Adjustment finished
Updating map ...
Local Mapping STOP
Local Mapping RELEASE
Map updated!
-------

median tracking time: 0.0313816
mean tracking time: 0.0334428
//这行日志出现后，表示讲相机位子存储在了KeyFrameTrajectory.txt文件中了
Saving keyframe trajectory to KeyFrameTrajectory.txt ...

trajectory saved!
QObject::~QObject: Timers cannot be stopped from another thread

运行效果如下：

       可以看到，此时桌面出现了两个窗口，上边的图像一帧一帧变化，下边的相机路径也同步变化。其实这个在代码中对应两个窗口类，FrameDrawer类为上边的显示图像帧的窗口，MapDrawer为下边的显示地图路径的窗口。上图中的绿色点为每一帧图像中识别出来的特征关键点，下图中所有的倒三角行连在一起构成了相机的运动轨迹，红色的点云为图像中的特征点。

3.系统框架简要了解

一般情况下，要了解一个系统的代码实现，总是要先从熟悉系统的框架开始，ORB-SLAM2的系统框架如下图所示：

        在只对SLAM的正常框架有所了解的前提下，我们可以看到TRACKING线程中做了提取ORB特征、初始位姿估计等操作，那么这个线程应该属于SLAM中的“前端VO”；LOCAL MAPPING线程里有Local BA，我们可以知道这个线程属于后端，主要做后端优化和建图工作；LOOP CLOSING线程从字面意思就能判断出是做回环检测的，分为回环检测和回环矫正。

       再需要知道的就是TRACKING线程接收了一帧一帧的图片后开始工作的，而LOCAL MAPPING线程接收的KeyFrame来自TRACKING线程“加工提炼”之后，LOOP CLOSING线程也是接收LOCAL MAPPING线程“进一步加工提炼”后的KeyFrame来进行工作。

       可以看出，Frame为整个系统运行起来的“原料”。这样三个核心的线程就串起来了，至于PLACE RECOGNITION和MAP是什么鬼？留到以后需要了解的时候再去研究吧。

4.最开始的代码

看过了系统框架的真面目，此时开始分析代码。本篇笔记作为笔者开始记录ORB-SLAM2的第一篇笔记，本篇只会对ORB-SLAM2最开始的代码进行分析。

从mono_kitti开始运行的，那么自然应该先从寻找mono_kitti的main函数开始。mono_kitti.cc文件主要的函数如下。

int main(int argc, char **argv)
{
    if(argc != 4)
    {
        cerr << endl << "Usage: ./mono_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl;
        return 1;
    }

    // Retrieve paths to images
    vector<string> vstrImageFilenames;
    vector<double> vTimestamps;
    //讲单目数据集图片（也就是Frames）load进来，argv[3]是Frame的路径
    LoadImages(string(argv[3]), vstrImageFilenames, vTimestamps);

    int nImages = vstrImageFilenames.size();

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    //argv[1]为vocfile 里边存储的是词汇
    //argv[2]为settingfile 里边存储摄像机校准和畸变参数和ORB相关参数
    //这里创建了System类型的SLAM对象，SLAM构造函数中初始化了系统所有线程和相关参数，代码留待后边详细分析
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

    cv::Mat im;
    // Main loop 主循环代码，循环读取一帧一帧的图片
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file 读取图片
        im = cv::imread(vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        //读取时间戳
        double tframe = vTimestamps[ni];

        if(im.empty())
        {
            cerr << endl << "Failed to load image at: " << 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

        // Pass the image to the SLAM system 
        //将图片传给SLAM系统。TrackMonocular讲图片传给Tracking，其实Tracking就是系统的主线程。这里传入的im引起了系统的一系列操作
        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);
    }

    // Stop all threads 关闭SLAM系统，也就关闭了系统中的几个线程
    SLAM.Shutdown();

    // Tracking time statistics
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;

    // Save camera trajectory 
    // 整个SLAM系统运行完后，计算出来的是相机的运行轨迹，系统退出时保存相机轨迹
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");    

    return 0;
}
/**
 * 读入一帧帧的图像数据和时间戳数据
 * strPathToSequence 为入参，表示image文件存储的路径
 * vstrImageFilenames 为出参，存储获取image文件列表的具体路径
 * vTimestamps 为出参， 存储和每一个image文件对应的时间戳
 * 
*/
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
    ifstream fTimes;
    string strPathTimeFile = strPathToSequence + "/times.txt";
    fTimes.open(strPathTimeFile.c_str());
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            ss >> t;
            //vTimestamps中存入了所有的时间戳
            vTimestamps.push_back(t);
        }
    }
    //组装mono_kitti数据集中image_0目录的路径
    string strPrefixLeft = strPathToSequence + "/image_0/";

    const int nTimes = vTimestamps.size();
    vstrImageFilenames.resize(nTimes);

    for(int i=0; i<nTimes; i++)
    {
        stringstream ss;
        //std::setw ：需要填充多少个字符,默认填充的字符为' '空格
        //std::setfill：设置std::setw将填充什么样的字符，如:std::setfill('*')
        
        //ss总共为6位，i之外的前边几位用0来填充，得到的结果为000001 000099之类
        ss << setfill('0') << setw(6) << i;
        vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png";
    }
}

5.mono_kitti代码总结

mono_kitti的main函数主要步骤如下，可以看清楚看到和SLAM系统关联的函数为TrackMonocular，这个函数接收了Frame(或者说将Frame传给了TRACKING线程)。  

main函数中调用的System相关函数见ORB-SLAM2代码阅读笔记（二）：System.cc相关函数