调试 ORB_SLAM2 非ROS版本

背景:下面为 "mono_tum.cc" 示例的运行步骤。采用的数据集为 rgbd_dataset_freiburg1_xyz

数据简介

复制代码
rgbd_dataset_freiburg1_xyz/
│
├── rgb/
│   ├── 1305031102.175304.png
│   ├── 1305031102.211214.png
│   └── ...
│
├── depth/
│   ├── 1305031102.160407.png
│   ├── 1305031102.226738.png
│   └── ...
│
├── rgb.txt
├── depth.txt
├── groundtruth.txt
├── accelerometer.txt
└── associate.txt   (需要自己生成)

rgb文件夹:保存 Kinect RGB 摄像头采集的彩色图像,每个文件的文件名就是时间戳

depth文件夹:保存 Kinect 深度相机测量的距离信息,每个文件的文件名就是时间戳

rgb.txt: 记录时间戳+图片路径

depth.txt:Depth时间戳+ 深度图片路径

groundtruth.txt:(真实轨迹)

字段 含义
timestamp 时间
tx ty tz 相机位置
qx qy qz qw 旋转四元数

accelerometer.txt: Kinect 内置 IMU 加速度数据 timestamp ax ay az

代码调试

官方给出的步骤直接运行,但是我想调试,所以对main函数进行了修改

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

    // ================================
    // 1. 设置文件路径
    // ================================

    string strVocFile ="Vocabulary/ORBvoc.txt";  //ORB特征词典文件

    string strSettingsFile ="Examples/Monocular/TUM1.yaml";

    string strSequencePath ="dataset/rgbd_dataset_freiburg1_xyz";

    string strFile = strSequencePath+"/rgb.txt";

    // Retrieve paths to images
    vector<string> vstrImageFilenames;
    vector<double> vTimestamps;
  //  string strFile = string(argv[3])+"/rgb.txt";
  /*
    将rgb.txt文件中的数据读入到vstrImageFilenames和vTimestamps中
    strFile: rgb.txt文件路径
    vstrImageFilenames: 图片文件名向量
    vTimestamps: 时间戳向量
  */
    LoadImages(strFile, vstrImageFilenames, vTimestamps);

    int nImages = vstrImageFilenames.size();

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
   // ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
   //初始化ORB-SLAM2系统
    ORB_SLAM2::System SLAM(strVocFile,strSettingsFile,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;

    // Main loop
    cv::Mat im;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file
        string imagePath =strSequencePath + "/" +vstrImageFilenames[ni];
        im = cv::imread(imagePath,CV_LOAD_IMAGE_UNCHANGED);  //im: 读取的图片
        double tframe = vTimestamps[ni];
        
        if(im.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << imagePath << 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(im,tframe);  //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.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.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

    return 0;
}
相关推荐
wangxiaoxuan2173 天前
地图表示怎么选:栅格、点云、体素、TSDF 与 ESDF
机器人·slam·机器人导航·科沃斯八界
米糕闯编程9 天前
鱼香ros2(二十五)导航
机器人·slam·ros2·launch·navigation2
蓝悦无人机12 天前
《概率机器人》读书笔记-第六章-机器人感知模型:波束、似然场与地标
机器人·slam·测距仪·观测模型·波束模型·似然场模型·地标模型
feasibility.15 天前
世界模型WSM技术解读:SLAM内化为具身智能的状态记忆,纯RGB机器人导航反超RGB-D
人工智能·机器人·aigc·slam·导航·具身智能·世界模型
大江东去浪淘尽千古风流人物15 天前
【HexVIO】DSP卸载双目惯性VIO:Hexagon向量化前端与单帧融合调度深度解析
前端·状态模式·slam·dsp·vio·hexagon·移动端加速
Felaim20 天前
【SLAM】SLAM-Former论文与代码导读
transformer·slam·dense slam·代码导读
Felaim21 天前
【SLAM】SLAM数据集相机规格说明
数据集·vslam·slam·相机标定
Felaim21 天前
【SLAM】SLAM-Former
transformer·slam·dense slam·几何基础模型
Felaim22 天前
【SLAM】人形机器人 VLN 相机与传感器配置调研
slam·传感器·人形机器人·vln·相机选型
火星机器人life24 天前
深度学习如何重塑移动机器人SLAM?一篇综述讲透视觉里程计、闭环检测与语义建图
深度学习·视觉里程计·slam·语义建图