orb_tum源码阅读

#include<iostream>   // 输入输出
#include<algorithm>  // 泛型算法头文件
#include<fstream>     // 文件读写
#include<chrono>      // C++11 std::chrono库：时间库 三个重要概念（duration、time_point、clock）

#include<opencv2/core/core.hpp>

#include<System.h>

using namespace std;
// 函数声明：载入图像
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps);

int main(int argc, char **argv)
{
    if(argc != 5)
    {   // 输入参数满足5个，节点+4个路径
        cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;
        return 1;
    }

    //  检索图像路径
    vector<string> vstrImageFilenamesRGB;
    vector<string> vstrImageFilenamesD;
    vector<double> vTimestamps;
    string strAssociationFilename = string(argv[4]);
    // 载入图像
    // 文件位置、彩色图、深度图、时间戳
    LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);

    // 检查彩色深度图编号是否一致
    int nImages = vstrImageFilenamesRGB.size();  // 个数
    if(vstrImageFilenamesRGB.empty())
    {
        cerr << endl << "No images found in provided path." << endl;
        return 1;
    }
    else if(vstrImageFilenamesD.size()!=vstrImageFilenamesRGB.size())
    {
        cerr << endl << "Different number of images for rgb and depth." << endl;
        return 1;
    }

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    // 创建SLAM系统，初始化所有线程，准备获取帧
    // System.h：ORB_SLAM2命名空间，System类初始化
    // 词袋/ yaml参数文件/ emum枚举：RGBD传感器/ use Viewer
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);

    // 跟踪模块时间统计
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);   // 调整容器大小

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

    // 主循环，读入图像进行处理
    cv::Mat imRGB, imD;
    for(int ni=0; ni<nImages; ni++)
    {
        // 从文件中读取图像
        // 增量的载入数据集里图片
        imRGB = cv::imread(string(argv[3])+"/"+vstrImageFilenamesRGB[ni],CV_LOAD_IMAGE_UNCHANGED);
        imD = cv::imread(string(argv[3])+"/"+vstrImageFilenamesD[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];

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

#ifdef COMPILEDWITHC11    // 获取当前时间t1
        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系统处理（彩色图/深度图/时间戳）
        SLAM.TrackRGBD(imRGB,imD,tframe);

#ifdef COMPILEDWITHC11    // 获取当前时间t2
        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;    // 记录下每帧用的时间

        // 等待下一帧的载入
        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);
    }

    // 所有图像处理完，关闭所有线程
    SLAM.Shutdown();

    // 时间统计追踪
    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;

    // 保持相机轨迹/关键帧轨迹
    SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");   

    return 0;
}
// 载入图像函数定义
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps)
{
    ifstream fAssociation;  // 输入流
    fAssociation.open(strAssociationFilename.c_str());  // 打开指定文件
    while(!fAssociation.eof())    // 检测末尾
    {
        string s;
        getline(fAssociation,s);    // 将fAssociation流中1行读取内容到string对象s中去
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            // 关联文件中的1行：1305031453.359684【t】 rgb/1305031453.359684.png【sRGB】 
            // 1305031453.374112【t】 depth/1305031453.374112.png【sD】

            double t;
            string sRGB, sD;
            ss >> t;    // 写到t
            vTimestamps.push_back(t);   // 载入时间戳
            ss >> sRGB;
            vstrImageFilenamesRGB.push_back(sRGB);  // 载入彩色图像
            ss >> t;
            ss >> sD;
            vstrImageFilenamesD.push_back(sD);  // 载入深度图像

        }
    }
}

转自https://blog.youkuaiyun.com/LIZHONGPING00/article/details/64132677