ORB SLAM2学习笔记之mono_kitti（一）

一、简要说明

ORB_SLAM2是由Raul Mur-Artal，J. M. M. Montiel和Juan D. Tardos发表的基于ORB特征点检测的视觉SLAM系统。项目主页网址为：https://github.com/raulmur/ORB_SLAM2。 该项目继承自ORB_SLAM，在此基础上拓展为支持单目（MONOCULAR），双目（STEREO），深度相机（RGBD）的SLAM系统。更详细的说明可以参考：https://blog.csdn.net/u010128736/article/details/53157605 。

本篇记录的是基于KITTI数据集的单目系统的学习历程，该系统包含了跟踪（Tracking）、建图（Mapping）、重定位（Relocalization）、闭环检测（Loop closing）。由于ORB-SLAM2系统是基于特征点的SLAM系统，故其能够实时计算出相机的轨线，并可以生成场景的稀疏三维重建结果。

二、流程—Part 1

本部分简述 mono_kitti.cc的逻辑框架，以代码为例。

1.读取文件

/*!!!!!!!!!!!!!!!读取文件目录!!!!!!!!!!*/
    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;


    //load函数，从第三个传入参数装载图像和时间戳，vstrImageFilenames是装有图像地址+名字的vector，vTimestamps是times.txt里每一行的时间组成的vector
    LoadImages(string(argv[3]), vstrImageFilenames, vTimestamps);

    //有几张图片
    int nImages = vstrImageFilenames.size();

需要说明的是，程序运行需要传入三个参数（程序中README中有详细说明，请学习之前务必仔细阅读）：① 字典词包的路径； ② 系统中装有一些如相机参数、viewer窗口的参数的配置文件，格式为YAML； ③数据集的路径。

LoadImages函数

LoadImages函数为主要读取文件函数，在 mono_kitti.cc文件中这么写道：

void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
    ifstream fTimes;
    string strPathTimeFile = strPathToSequence + "/times.txt";
    fTimes.open(strPathTimeFile.c_str());

    //循环读入timestamp文件到一个名为vTimestamps的vector
    while(!fTimes.eof())
    {
        string s;
        //从流中读取一行 到 字符串
        getline(fTimes,s);

        //如果字符串不是空的，就写进流，读成double类型放入vector
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            ss >> t;
            vTimestamps.push_back(t);
        }
    }

    //左眼为单目，读取左图像，strPrefixLeft是左图像路径
    string strPrefixLeft = strPathToSequence + "/image_0/";

    const long nTimes = vTimestamps.size(); //有多少个时间戳
    vstrImageFilenames.resize(nTimes); //有多少个时间戳就有多少个图像

    //vstrImageFilenames是装有图像地址+名字的vector
    for(int i=0; i<nTimes; i++)
    {
        stringstream ss;
        ss << setfill('0') << setw(6) << i; //填6个0,如果来了个i，则代替0的位置，也就是总共有6位，末尾是i，其他用0填充
        vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png";
    }
}

注释比较详细，大概流程为：传入三个参数，后两个参数vstrImageFilenames和vTimestamps为空，引用完从函数中获取他们的值，函数执行完前者是装有图片具体位置的vector，位置形式如xxx/xxx/xxx/000xx.png，后者为装有一系列图片时间戳的vector。与C++基础的相关参考链接可参考：流控制函数，vector的resize函数，getline函数。
函数在编写的过程中考虑了几个细节，比如在录时间戳的过程中，要注意录入的字符串是否为空，非空再录入，空的话再录入一行；又比如为了防止时间戳与图片数量不一样的情况，利用resize函数保持其维度的一致性；

2.实例化SLAM系统

系统类的定义在System.cc中，在第一步读取完文件后，需要实例化一个SLAM对象：

    /*!!!!!!!!!!!Create SLAM system. It initializes all system threads and gets ready to process frames.!!!!!!*/
    /*!!!!!!!!!!!!!!!创建 SLAM system. 初始化系统的所有线程，并准备好处理帧!!!!!!!!!!*/
    //读入词包路径，读入YAML配置文件，设置SLAM为mono状态,启用viewer的线程简要说明
    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;

3.循环tracking

 /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!循环读图并追踪!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
    cv::Mat im;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file   imread从一个string类型的地址读取图片，图片参数为CV_LOAD_IMAGE_UNCHANGED
        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系统
        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

        //track的用时
        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.Shutdown();

上述分为两步：读图、Tracking，其中有一部分代码（注释 //Wait to load the next frame 后）目的是为了模拟真实时间状况，如果tracking过快，则下一帧可能还没来，所以要“睡” T-ttrack 秒等待装载下一帧图片。

下篇博客将介绍流程中SLAM系统实例化的细节～