orbslam：rgbd_tum源码阅读

来源：互联网发布：linux vi编辑命令编辑：程序博客网时间：2024/05/22 08:08
rgbd_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);  // 载入深度图像        }    }}
0 0