ORB-SLAM2源码学习：rgbd_tum.cc源文件

前言

1.标注好的源码放在最后边了，可直接看标注好的源码，前边的内容只是对编写代码做的简单梳理，如果能够对大家学习代码时有帮助那就希望大家能够点赞+收藏吧！（嘿嘿）

2.如果大家在学习的过程中发现我存在错误，请大家在评论区指出来，我会及时修改并回复。（谢谢）

一、代码逻辑

1.包含必要的标准库和第三方库头文件：

#include <iostream>
#include <vector>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <System.h>

2.声明函数原型：

a.函数原型一般放在main函数前边确保在调用函数时，编译器已经知道该函数的声明和参数类型。

b.由于ORB-SLAM2提供了三种模式（单目、双目、RGBD），故这个加载图像函数LoadImages在每个程序中不同（参数列表里参数的不同）。

void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps);

3. 编写main 函数：

a.参数验证：检查传入参数的数量并给出提示。

if (argc != 5) {
    cerr << "Usage: ..." << endl;
    return 1;
}

b.初始化数据结构：定义并初始化保存图像文件名、深度图文件名和时间戳的向量。

vector<string> vstrImageFilenamesRGB;
vector<string> vstrImageFilenamesD;
vector<double> vTimestamps;

c.调用 LoadImages 函数：读取关联文件数据。

LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);

d.检查图像和深度图的数量一致性。

if (vstrImageFilenamesRGB.empty() || vstrImageFilenamesD.size() != vstrImageFilenamesRGB.size()) {
    cerr << "Error: ..." << endl;
    return 1;
}

e.创建 SLAM 系统实例，配置参数。

ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, true);

f.主处理循环，逐帧处理图像并记录处理时间。

for (int ni = 0; ni < nImages; ni++) {
    // 读取图像和深度图
    imRGB = cv::imread(...);
    imD = cv::imread(...);

    // 处理图像
    SLAM.TrackRGBD(imRGB, imD, tframe);
    
    // 记录时间
    ...
}

g.停止 SLAM 系统，清理资源。

SLAM.Shutdown();

h.输出处理时间统计信息。

4. 自定义函数的实现：

void LoadImages(...) {
    ifstream fAssociation(strAssociationFilename);
    while (!fAssociation.eof()) {
        string s;
        getline(fAssociation, s);
        // 解析并存储数据
    }
}

二、已标注的源码

1.这个文件我大概学了一天，我认为学习ORB-SLAM2的代码还是要慢慢来，不要急于求成。

2.代码标注的风格可能有点糟糕，但也不妨碍学习。（嘻嘻）

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<opencv2/core/core.hpp>
#include<System.h>

using namespace std;

/*
 1. strAssociationFilename 常量字符串，关联文件路径
 2. vstrImageFilenamesRGB  字符串像向量，储存RGBD彩色图的文件名
 3. vstrImageFilenamesD 字符串向量，储存深度图的文件名
 4. vTimestamps  字符串向量，储存时间戳
*/

//LoadImages函数的原型，确保在调用函数时，编译器已经知道该函数的声明和参数类型。
//具体的定义在代码的最后。
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps);
//参数列表中的参数可以直接访问原始变量（即在main函数中定义的变量），而不是其副本。

/*
  argc表示传入的参数个数，包括程序名在内。
  argv是一个字符指针数组（char* 类型），每个元素都指向命令行输入的一个参数。
 */
int main(int argc, char **argv)
{
    if(argc != 5)
    {

      /*
         cerr 标准错误输出流，专门用于显示错误或警告信息，通常是非缓冲的，这意味着错误信息会立即显示在终端中，不会因为缓冲延迟。
         *cout 标准输出流，是缓冲的，可能会等到输出缓冲区满了或程序结束时才输出到终端。
      */
        cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;
        //./rgbd_tum 是运行程序的命令，代表程序的名字。
        //path_to_vocabulary 词汇表文件的路径，词汇表是一个预训练的字典，用于图像特征的匹配，在 ORB-SLAM2 中用于视觉词袋模型。
        //path_to_settings 配置文件的路径（通常是 settings.yaml），配置文件包含一些相机的内参、图像的分辨率、深度摄像头的设置等重要的参数。
        //path_to_sequence 图像和深度数据的文件夹路径，里面包含程序要处理的 RGB 图像和深度图像。
        //path_to_association 关联文件的路径，包含了 RGB 图像和深度图像的对应关系（即每一对 RGB 图像和深度图像的时间戳），程序通过这个文件来找到每一帧图像的对应关系。
        return 1;
    }

    // Retrieve paths to images
    vector<string> vstrImageFilenamesRGB;
    vector<string> vstrImageFilenamesD;
    vector<double> vTimestamps;
    string strAssociationFilename = string(argv[4]);//即输入参数的最后的一个路径。
    //将关联文件中的数据加载到后三个参数中去。
    LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);


    // Check consistency in the number of images and depthmaps 检查图像和深度图的数量一致性
    int nImages = vstrImageFilenamesRGB.size();
    //.size() 是一个成员函数，通常用于 STL 容器（如 std::vector, std::string, std::map 等），用于返回容器中元素的数量。
    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系统的实例，并初始化所有的系统线程，以准备处理输入的帧数据。
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);//需要词汇表文件以及配置文件路径。
    //ORB_SLAM2::System::RGBD 是一种配置选项，使 SLAM 系统能够处理 RGB-D 数据
    //如果是 true，系统将尽量快速处理输入的每一帧图像，以实现实时效果。
    //如果是 false，系统可能会以较慢的速度处理图像，可能会在计算上更为集中，但不一定能达到实时的速度。


    // Vector for tracking time statistics
    vector<float> vTimesTrack;//一个动态数组（向量），用于存储每一帧处理的时间。
    vTimesTrack.resize(nImages);
    //resize 是 std::vector 类的一个成员函数。这个函数的作用是调整向量的大小，使其能够容纳指定数量的元素。
    //有多少图像就需要多少储存每一帧处理的时间的容量。
    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;//sequence序列。
    cout << "Images in the sequence: " << nImages << endl << endl;


    // Main loop逐帧读取图像和深度图，传递给 SLAM 系统进行处理，并记录每帧的处理时间。
    cv::Mat imRGB, imD;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image and depthmap from file
        /*cv::imread 是 OpenCV 中用于读取图像的函数。
         将文件夹路径和文件名组合成一个完整的文件路径，argv[3]即为path_to_sequence。
        */
        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];//ni的时间戳，后边计算时间差需要用到。

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


/*这段代码使用条件编译来确定是否启用了 C++11 标准。如果代码是用支持 C++11 的编译器编译的，
*那么就会使用 std::chrono::steady_clock，否则使用 std::chrono::monotonic_clock。
* 条件编译常用于确保代码能够在不同的编译环境下运行。
*/
#ifdef COMPILEDWITHC11//用11进行编译

        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.TrackRGBD(imRGB,imD,tframe);//SLAM系统处理图像，更新状态。

#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
        //计算每一帧处理的时间，储存在vTimesTrack中。
        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;//ttrack 是当前帧处理的实际时间


        // Wait to load the next frame，根据时间戳加载下一张图片。
        double T=0;
        if(ni<nImages-1)//这里判断当前帧 ni 是否不是最后一帧（nImages-1）。如果不是最后一帧，则计算下一帧的时间差。
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)//如果当前帧 ni 不是第一帧（也就是说，ni 大于 0），则计算与前一帧的时间间隔。
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)//这里判断处理时间 ttrack 是否小于预期的时间间隔 T。如果是，说明处理速度较快，还有时间可以等待。
            usleep((T-ttrack)*1e6);//等待时间计算
    }

    // Stop all threads
    //停止 SLAM 系统的所有线程并进行清理的函数。
    SLAM.Shutdown();//System.h中的成员函数

    // Tracking time statistics
    //这部分代码主要用来分析 SLAM 系统在处理每帧图像时的性能表现。
    //程序统计并输出了每一帧处理的时间统计信息，包括中位数和平均处理时间。
    sort(vTimesTrack.begin(),vTimesTrack.end());//sort 是 C++ 标准库中的一个函数，用于对容器中的元素进行排序。
    //begin() 和 end() 是 STL（标准模板库）容器的成员函数，通常用于获取容器中第一个和最后一个元素的迭代器。
    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保存相机的轨迹
    //轨迹数据正在以与 TUM 数据集标准兼容的方式格式化或保存，这使得与该领域其他研究或数据集的结果进行比较变得更容易。
    SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");//Trajectory轨迹。
    //保存所有关键帧的轨迹
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");   

    return 0;
}


//定义LoadImages函数
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps)
{
    ifstream fAssociation;
    //ifstream 是 C++ 标准库中定义的一个类，表示输入文件流（input file stream）。它用于从文件中读取数据。
    fAssociation.open(strAssociationFilename.c_str());
    //open() 是 ifstream 类的一个成员函数，用于打开指定路径的文件。
    //c_str() 方法将 std::string 对象转换为 C 风格的字符串（const char*），这是因为 open() 函数接受 C 风格字符串作为参数。
    while(!fAssociation.eof())
    //eof() 是 ifstream 类的一个成员函数，返回一个布尔值，指示文件流是否到达文件末尾（EOF, End of File）。
    {
        string s;
        getline(fAssociation,s);
        //getline() 是一个标准库函数，用于从输入流中读取一行文本，直到遇到换行符为止。
        if(!s.empty())
        {
           //提取流程：
           /*读取一行数据（getline）。
           将这行数据放入 stringstream 流。
           按顺序提取时间戳、RGB 图像文件名、深度图像文件名。
           每提取一个字段，就将其存储到相应的向量中。*/
            stringstream ss;
            ss << s;
            double t;
            string sRGB, sD;
            ss >> t;
            vTimestamps.push_back(t);
            //.push_back 是 C++ 中 std::vector 类的一个成员函数，用于在向量的末尾添加一个新元素。
            ss >> sRGB;
            vstrImageFilenamesRGB.push_back(sRGB);
            ss >> t;
            ss >> sD;
            vstrImageFilenamesD.push_back(sD);

        }
    }
}