ORBSLAM2解析（1）——main函数

ORB-SLAM2项目的main()函数（即可执行文件）在Examples文件夹下，可以看到有四个子文件夹。Monocular，Stereo和RGB-D文件夹是对数据集中存储的场景进行SLAM，ROS目文件夹是利用ROS和相机进行实际场景的SLAM。

以对TUM数据集进行RGBD-SLAM为例。rgb_tum.cc为项目的可执行文件，主要有四部分：

1. 载入图像 ：根据命令行输入的参数，载入在相关文件夹下图像和时间戳。LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);*注 ：该过程只是将图像的名称保存在相应的vector变量中。
2. 创建SLAM系统：根据命令行输入的参数创建SLAM系统并进行系统初始化。ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);其中：argv[1]表示词袋文件的路径，argv[2]表示配置文件的路径，ORB_SLAM2::System::RGBD表示创建SLAM系统为RGBD-SLAM系统，true表示进行可视化操作。
3. 传入图像进行追踪 ：通过一个循环过程将图像传入SLAM系统中进行追踪。SLAM.TrackRGBD(imRGB,imD,tframe);
4. 停止SLAM系统并保存轨迹：通过Shutdown()函数停止SLAM系统中的所有线程，利用SaveTrajectoryTUM()函数来保存轨迹。

参考链接：https://blog.csdn.net/u014709760/article/details/87922386

下面给出了代码以及注释：

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>

#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)
{
    
    
    // ./Examples/RGB-D/rgbd_tum  -> arg[0] -> 运行rgbd_tum.cc文件，ORB_SLAM2的main函数
    //  Vocabulary/ORBvoc.txt  -> arg[1] ->  词袋文件的路径
    // Examples/RGB-D/TUM1.yaml  -> arg[2] -> 配置文件的路径
    // /media/Files/rgbdslam_DataSet/rgbd_dataset_freiburg1_desk -> arg[3] -> 下载的数据集文件夹
    //  Examples/RGB-D/associations/fr1_desk.txt -> arg[4] -> 关联文件的路径
    if(argc != 5)
    {
    
    
        cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;
        return 1;
    }

    // Retrieve paths to images
    vector<string> vstrImageFilenamesRGB;
    vector<string> vstrImageFilenamesD;
    vector<double> vTimestamps;
    string strAssociationFilename = string(argv[4]);
    //loadImage用于获取RGB和深度图像的路径，保存于vstrImageFilenamesRGB和vstrImageFilenamesD，
    //时间戳保存于vTimestamps
    LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);

    // Check consistency in the number of images and depthmaps
    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.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);  // -> System.cc, 创建了System类的对象

    // 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 imRGB, imD;
    for(int ni=0; ni<nImages; ni++)
    {
    
    
        // Read image and depthmap from file，CV_LOAD_IMAGE_UNCHANGED->加载原图
        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 //时间点，steady_clock最短时间间隔0.1us
                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); 

        #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
        //duration还有一个成员函数count()返回Rep类型的Period数量（如0.01s，0.01->Rep，s->period)
        //Rep表示一种数值类型，用来表示Period的数量，比如int float double 
        // Period是ratio类型，用来表示时间单位，比如second milisecond
        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];  //0～ni-1，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.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; //文件读操作
    //在fstream类中，成员函数open()实现打开文件的操作，从而将数据流和文件进行关联
    fAssociation.open(strAssociationFilename.c_str());
    //C++ eof()函数可以帮助我们用来判断文件是否为空，或是判断其是否读到文件结尾。
    while(!fAssociation.eof()) 
    {
    
    
        string s;
        getline(fAssociation,s);
        if(!s.empty())
        {
    
    
            stringstream ss;
            ss << s;
            double t;
            string sRGB, sD;
            ss >> t;
            vTimestamps.push_back(t);
            ss >> sRGB;
            vstrImageFilenamesRGB.push_back(sRGB);
            ss >> t;
            ss >> sD;
            vstrImageFilenamesD.push_back(sD);

        }
    }
}