ORB SLAM2学习笔记之mono_kitti(一)

ORB SLAM2 专栏收录该内容
4 篇文章 1 订阅

一、简要说明

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

注释比较详细,大概流程为:传入三个参数,后两个参数vstrImageFilenamesvTimestamps为空,引用完从函数中获取他们的值,函数执行完前者是装有图片具体位置的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系统实例化的细节~

  • 1
    点赞
  • 0
    评论
  • 12
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值