ORB-SLAM3-2：案例mono_inertial_tum_vi解读

使用MH-05数据运行案例：mono_inertial_tum_vi.cc
./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml /home/mol/study/SLAM/Datasets ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH05.txt

首先对代码进行详细的解读，并加注释：

/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.

./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml /home/mol/study/SLAM/Datasets ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH05.txt
可执行文件    。。。    相机参数    数据库    数据的时间戳
* If not, see <http://www.gnu.org/licenses/>.
*/

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

#include<opencv2/core/core.hpp>

#include<System.h>
#include "ImuTypes.h"

using namespace std;

//载入图像的函数，输入数据为图像路径strImagePath和时间戳路径strPathTimes，vstrImages存图像的文件路径，vTimeStamps存图像对应的时间戳
void LoadImages(const string &strImagePath, const string &strPathTimes,
                vector<string> &vstrImages, vector<double> &vTimeStamps);

//载入IMU惯性数据的函数，输入为数据路径strImuPath，将时间戳存入vTimeStamps，vAcc存加速度，vGyro存角速度
void LoadIMU(const string &strImuPath, vector<double> &vTimeStamps, vector<cv::Point3f> &vAcc, vector<cv::Point3f> &vGyro);

double ttrack_tot = 0;   //存储轨迹跟踪总时间
int main(int argc, char **argv)
{
    const int num_seq = (argc-3)/2;     //查看一共输入几组数据，前三个文件为必要文件，后面的是数据库和时间戳
    cout << "num_seq = " << num_seq << endl;
    bool bFileName= ((argc % 2) == 1);

    string file_name;
    if (bFileName)
        file_name = string(argv[argc-1]);

    cout << "file name: " << file_name << endl;


    if(argc < 5)
    {
        cerr << endl << "Usage: ./mono_inertial_tum_vi path_to_vocabulary path_to_settings path_to_image_folder_1 path_to_times_file_1 path_to_imu_data_1 (path_to_image_folder_2 path_to_times_file_2 path_to_imu_data_2 ... path_to_image_folder_N path_to_times_file_N path_to_imu_data_N) (trajectory_file_name)" << endl;
        return 1;
    }


    // Load all sequences:
    int seq;
    vector< vector<string> > vstrImageFilenames;  //存图像的文件路径
    vector< vector<double> > vTimestampsCam;      //存图像对应的时间戳
    vector< vector<cv::Point3f> > vAcc, vGyro;    //存加速度和角速度
    vector< vector<double> > vTimestampsImu;      //存IMU对应的时间戳
    vector<int> nImages;                          //存图像数量
    vector<int> nImu;                             //存IMU数量
    vector<int> first_imu(num_seq,0);             //存第一个可用的IMU数据的位置，全部初始化为0

    vstrImageFilenames.resize(num_seq);
    vTimestampsCam.resize(num_seq);
    vAcc.resize(num_seq);
    vGyro.resize(num_seq);
    vTimestampsImu.resize(num_seq);
    nImages.resize(num_seq);
    nImu.resize(num_seq);

    int tot_images = 0;   //图片总数
    for (seq = 0; seq<num_seq; seq++)
    {
        cout << "Loading images for sequence " << seq << "...";
        //获取图片路径
        LoadImages(string(argv[3*(seq+1)]), string(argv[3*(seq+1)+1]), vstrImageFilenames[seq], vTimestampsCam[seq]);
        cout << "LOADED!" << endl;

        cout << "Loading IMU for sequence " << seq << "...";
        //获取IMU数据
        LoadIMU(string(argv[3*(seq+1)+2]), vTimestampsImu[seq], vAcc[seq], vGyro[seq]);
        cout << "LOADED!" << endl;

        //存入图片与IMU的数量
        nImages[seq] = vstrImageFilenames[seq].size();
        tot_images += nImages[seq];
        nImu[seq] = vTimestampsImu[seq].size();

        //
        if((nImages[seq]<=0)||(nImu[seq]<=0))
        {
            cerr << "ERROR: Failed to load images or IMU for sequence" << seq << endl;
            return 1;
        }

        // Find first imu to be considered, supposing imu measurements start first
        //找到时间戳和图像第一个时间戳相等的IMU数据位置

        while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][0])
            first_imu[seq]++;
        first_imu[seq]--; // first imu measurement to be considered

    }

    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(tot_images);

    cout << endl << "-------" << endl;
    cout.precision(17);  //定义输出字符多少

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    //定义模型，输入：Vocabulary词汇树，相机参数，SLAM系统(IMU和单目)，   ，   ，数据库路径
    ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true, 0, file_name);

    int proccIm = 0;
    for (seq = 0; seq<num_seq; seq++)
    {

        // Main loop
        cv::Mat im;
        vector<ORB_SLAM3::IMU::Point> vImuMeas;
        proccIm = 0;
        //直方图均衡算法，对图像进行去雾。
        //3.0是颜色对比度的阈值
        //(8,8)是进行像素均衡化的网格大小，即在多少网格下进行直方图的均衡化操作
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
        {

            // Read image from file 获取灰度图
            im = cv::imread(vstrImageFilenames[seq][ni],cv::IMREAD_GRAYSCALE);

            // clahe处理im图像，并将处理后的数据存入im
            clahe->apply(im,im);

            double tframe = vTimestampsCam[seq][ni];

            if(im.empty())
            {
                cerr << endl << "Failed to load image at: "
                     <<  vstrImageFilenames[seq][ni] << endl;
                return 1;
            }


            // Load imu measurements from previous frame
            vImuMeas.clear();

            //将之前时间的IMU存入vImuMeas中
            if(ni>0)
            {

                while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                {
                    vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                             vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                             vTimestampsImu[seq][first_imu[seq]]));
                    first_imu[seq]++;
                }
            }

    #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系统添加数据
            SLAM.TrackMonocular(im,tframe,vImuMeas);

    #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();
            ttrack_tot += ttrack;   //累计总时间

            vTimesTrack[ni]=ttrack;   //储存时间

            // Wait to load the next frame
            double T=0;
            if(ni<nImages[seq]-1)
                T = vTimestampsCam[seq][ni+1]-tframe; //计算两帧之间的时间差
            else if(ni>0)
                T = tframe-vTimestampsCam[seq][ni-1];

            if(ttrack<T)   //等待下次数据传入
                usleep((T-ttrack)*1e6);

        }
        if(seq < num_seq - 1)
        {
            cout << "Changing the dataset" << endl;

            //更换数据库
            SLAM.ChangeDataset();
        }

    }

    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory保存跟踪路径数据

    if (bFileName)
    {
        const string kf_file =  "kf_" + string(argv[argc-1]) + ".txt";
        const string f_file =  "f_" + string(argv[argc-1]) + ".txt";
        SLAM.SaveTrajectoryEuRoC(f_file);
        SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
    }
    else
    {
        SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
        SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
    }

    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages[0]; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages[0]/2] << endl;
    cout << "mean tracking time: " << totaltime/proccIm << endl;

    return 0;
}

void LoadImages(const string &strImagePath, const string &strPathTimes,
                vector<string> &vstrImages, vector<double> &vTimeStamps)
{
    ifstream fTimes;
    cout << strImagePath << endl;
    cout << strPathTimes << endl;
    fTimes.open(strPathTimes.c_str());
    vTimeStamps.reserve(5000);
    vstrImages.reserve(5000);
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            //根据时间戳获取图片
            vstrImages.push_back(strImagePath + "/" + ss.str() + ".png");
            double t;
            ss >> t;
            vTimeStamps.push_back(t/1e9);

        }
    }
}

void LoadIMU(const string &strImuPath, vector<double> &vTimeStamps, vector<cv::Point3f> &vAcc, vector<cv::Point3f> &vGyro)
{
    ifstream fImu;
    fImu.open(strImuPath.c_str());
    vTimeStamps.reserve(5000);
    vAcc.reserve(5000);
    vGyro.reserve(5000);

    while(!fImu.eof())
    {
        string s;
        getline(fImu,s);
        if (s[0] == '#')
            continue;

        if(!s.empty())
        {
            string item;
            size_t pos = 0;
            double data[7]; //包括时间戳共7个数据
            int count = 0;
            while ((pos = s.find(',')) != string::npos) {
                item = s.substr(0, pos);
                data[count++] = stod(item);  //将字符串转变为double类型
                s.erase(0, pos + 1);  //删除已经读取的数据和","
            }
            item = s.substr(0, pos);  //只剩最后一个数据时，已经没有","，所以要手动添加。
            data[6] = stod(item);

            vTimeStamps.push_back(data[0]/1e9);
            vAcc.push_back(cv::Point3f(data[4],data[5],data[6]));
            vGyro.push_back(cv::Point3f(data[1],data[2],data[3]));
        }
    }
}

下面是SLAM初始化：

// argv[1]= path_to_vocabulary
 // argv[2]= path_to_settings 
 // file_name = trajectory_file_name
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR,true);
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true, 0, file_name);
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::RGBD,true);
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::STEREO,true);
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO, true, 0, file_name);

• path_to_vocabulary：指向词典（Bag of word)路径，因为ORB-SLAM3 是以 ORB 特征进行闭环检测等操作，在这里也就是ORB 词典路径 Vocabulary/ORBvoc.txt
• path_to_settings：是相机参数的文件路径
• file_name：是数据库的路径

SLAM数据传入与跟踪

SLAM.TrackMonocular(im,tframe);  // ORB_SLAM3::System::MONOCULAR
SLAM.TrackMonocular(im,tframe,vImuMeas); // ORB_SLAM3::System::IMU_MONOCULAR
SLAM.TrackStereo(imLeft,imRight,tframe); // ORB_SLAM3::System::STEREO
SLAM.TrackStereo(imLeft,imRight,tframe,vImuMeas); // ORB_SLAM3::System::IMU_STEREO
SLAM.TrackRGBD(imRGB,imD,tframe); // ORB_SLAM3::System::RGBD

不同的方案传入的数据也不一样：

• 单目：添加图像 im 和对应的时间 tframe
• 单目+惯性：添加当前帧图像 im 、对应的时间tframe 和 IMU数据
• 双目：需要添加左右相机的当前帧 imLeft 和 imRight，以及时间tframe
• 双目 + 惯性：需要添加左右相机的当前帧 imLeft 和 imRight、时间tframe，以及IMU数据
• RGBD相机：需要添加当前帧图像imRGB、深度数据imD 和 时间 tframe