ORBSLAM3 --- 双目惯导执行ORBSLAM3（一）：Stereo_intertail_euroc.cc文件解析

1.执行双目例程的参数

        在Clion中，我们输入以下参数：

/home/liuhongwei/Desktop/slam/ORB_SLAM3_detailed_comments-master/Vocabulary/ORBvoc.txt
/home/liuhongwei/Desktop/slam/ORB_SLAM3_detailed_comments-master/Examples_old/Stereo-Inertial/EuRoC.yaml
/home/liuhongwei/Desktop/slam/ORB_SLAM3_detailed_comments-master/data/03
/home/liuhongwei/Desktop/slam/ORB_SLAM3_detailed_comments-master/Examples_old/Stereo-Inertial/EuRoC_TimeStamps/MH03.txt

        分别对应着ORB词典的位置、配置文件的地址、图像序列的地址、时间戳的地址。

2. Stereo_intertail_euroc.cc文件解析

2.1 标注代码

int main(int argc, char **argv)
{
    // ORBSLAM3支持多序列建图(多地图建图)
    //
    if(argc < 5)
    {
        cerr << endl << "Usage: ./stereo_inertial_euroc path_to_vocabulary path_to_settings path_to_sequence_folder_1 path_to_times_file_1 (path_to_image_folder_2 path_to_times_file_2 ... path_to_image_folder_N path_to_times_file_N) " << endl;
        return 1;
    }

    const int num_seq = (argc-3)/2;
    cout << "num_seq = " << num_seq << endl;
    bool bFileName= (((argc-3) % 2) == 1);
    string file_name;
    if (bFileName)
    {
        file_name = string(argv[argc-1]);
        cout << "file name: " << file_name << endl;
    }

    // Load all sequences:
    int seq;
    vector< vector > vstrImageLeft;
    vector< vector > vstrImageRight;
    vector< vector > vTimestampsCam;
    vector< vector > vAcc, vGyro;
    vector< vector > vTimestampsImu;
    vector nImages;
    vector nImu;
    vector first_imu(num_seq,0);

    vstrImageLeft.resize(num_seq);
    vstrImageRight.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> K_l;
    fsSettings["RIGHT.K"] >> K_r;

    fsSettings["LEFT.P"] >> P_l;
    fsSettings["RIGHT.P"] >> P_r;

    fsSettings["LEFT.R"] >> R_l;
    fsSettings["RIGHT.R"] >> R_r;

    fsSettings["LEFT.D"] >> D_l;
    fsSettings["RIGHT.D"] >> D_r;

    int rows_l = fsSettings["LEFT.height"];
    int cols_l = fsSettings["LEFT.width"];
    int rows_r = fsSettings["RIGHT.height"];
    int cols_r = fsSettings["RIGHT.width"];

    if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
            rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
    {
        cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
        return -1;
    }

    cv::Mat M1l,M2l,M1r,M2r;
    cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
    cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);


    // Vector for tracking time statistics
    vector 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.
    ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO, true);
    float imageScale = SLAM.GetImageScale();

    cv::Mat imLeft, imRight, imLeftRect, imRightRect;
    for (seq = 0; seq vImuMeas;
        double t_rect = 0.f;
        double t_resize = 0.f;
        double t_track = 0.f;
        int num_rect = 0;
        int proccIm = 0;
        for(int ni=0; ni >(t_End_Rect - t_Start_Rect).count();
            SLAM.InsertRectTime(t_rect);
            t_rect = std::chrono::duration_cast >(t_End_Rect - t_Start_Rect).count();
#endif

            if(imageScale != 1.f)
            {
#ifdef REGISTER_TIMES
    #ifdef COMPILEDWITHC11
                std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now();
    #else
                std::chrono::monotonic_clock::time_point t_Start_Resize = std::chrono::monotonic_clock::now();
    #endif
#endif
                int width = imLeftRect.cols * imageScale;
                int height = imLeftRect.rows * imageScale;
                cv::resize(imLeftRect, imLeftRect, cv::Size(width, height));
                cv::resize(imRightRect, imRightRect, cv::Size(width, height));
#ifdef REGISTER_TIMES
    #ifdef COMPILEDWITHC11
                std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
    #else
                std::chrono::monotonic_clock::time_point t_End_Resize = std::chrono::monotonic_clock::now();
    #endif
                t_resize = std::chrono::duration_cast >(t_End_Resize - t_Start_Resize).count();
                SLAM.InsertResizeTime(t_resize);
#endif
            }

            double tframe = vTimestampsCam[seq][ni];

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

            if(ni>0)
                while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) // while(vTimestampsImu[first_imu]<=vTimestampsCam[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 images to the SLAM system
            SLAM.TrackStereo(imLeftRect,imRightRect,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

#ifdef REGISTER_TIMES
            t_track = t_rect + t_resize + std::chrono::duration_cast >(t2 - t1).count();
            SLAM.InsertTrackTime(t_track);
#endif

            double ttrack= std::chrono::duration_cast >(t2 - t1).count();

            vTimesTrack[ni]=ttrack;

            // Wait to load the next frame
            double T=0;
            if(ni0)
                T = tframe-vTimestampsCam[seq][ni-1];

            if(ttrack

2.2 代码解析----读取图片、IMU信息

        由于ORBSLAM3支持多序列建图，因此在main函数中我们可以输入多个图像序列和时间戳：

    if(argc < 5)
    {
        cerr << endl << "Usage: ./stereo_inertial_euroc path_to_vocabulary path_to_settings path_to_sequence_folder_1 path_to_times_file_1 (path_to_image_folder_2 path_to_times_file_2 ... path_to_image_folder_N path_to_times_file_N) " << endl;
        return 1;
    }

        并且如果输入有错误的话输出一行字并退出SLAM系统。

        首先我们读取图像：我们看这几个路径

        // argv[3] = /home/liuhongwei/Desktop/slam/ORB_SLAM3_detailed_comments-master/data/03
        string pathSeq(argv[(2*seq) + 3]);
        // argv[4] = /home/liuhongwei/Desktop/slam/ORB_SLAM3_detailed_comments-master/Examples_old/Stereo-Inertial/EuRoC_TimeStamps/MH03.txt
        string pathTimeStamps(argv[(2*seq) + 4]);

        // /home/liuhongwei/Desktop/slam/ORB_SLAM3_detailed_comments-master/data/03/mav0/cam0/data
        string pathCam0 = pathSeq + "/mav0/cam0/data";
        // /home/liuhongwei/Desktop/slam/ORB_SLAM3_detailed_comments-master/data/03/mav0/cam1/data
        string pathCam1 = pathSeq + "/mav0/cam1/data";
        // /home/liuhongwei/Desktop/slam/ORB_SLAM3_detailed_comments-master/data/03/mav0/imu0/data.csv
        string pathImu = pathSeq + "/mav0/imu0/data.csv";

        我们看读取图片的函数LoadImage：

// /mav0/cam0/data  /mav0/cam1/data
// /home/liuhongwei/Desktop/slam/ORB_SLAM3_detailed_comments-master/Examples_old/Stereo-Inertial/EuRoC_TimeStamps/MH03.txt
// vector< vector > vstrImageLeft
// vector< vector > vstrImageRight
// vector< vector > vTimestampsCam
void LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes,
                vector &vstrImageLeft, vector &vstrImageRight, vector &vTimeStamps)
{
    ifstream fTimes;
    fTimes.open(strPathTimes.c_str());
    vTimeStamps.reserve(5000);
    vstrImageLeft.reserve(5000);
    vstrImageRight.reserve(5000);
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            vstrImageLeft.push_back(strPathLeft + "/" + ss.str() + ".png");
            vstrImageRight.push_back(strPathRight + "/" + ss.str() + ".png");
            double t;
            ss >> t;
            vTimeStamps.push_back(t/1e9);

        }
    }
}

        我们先看看strPathTimes里面的内容：里面存放所有图像的时间戳

        vstrImageLeft里面存放着/mav0/cam0/data/1403637xxxxx104.png，很显然，这里面存放着时间戳对应的图像，vstrImageRight同理。vTimeStamps存放着这个时间戳。

        即vstrImageLeft存放图像的路径，vTimeStamps存放图像对应的时间戳。

        我们看读取IMU的函数：

// pathImu : /home/liuhongwei/Desktop/slam/ORB_SLAM3_detailed_comments-master/data/03/mav0/imu0/data.csv
// vTimestampsImu vector< vector > vTimestampsImu
// vAcc  vector< vector > vAcc, vGyro
// vGyro  vector< vector > vAcc, vGyro
void LoadIMU(const string &strImuPath, vector &vTimeStamps, vector &vAcc, vector &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];
            int count = 0;
            while ((pos = s.find(',')) != string::npos) {
                item = s.substr(0, pos);
                data[count++] = stod(item);
                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]));
        }
    }
}

        vAcc存放加速度计的信息。

        vGyro存放角速度的信息。

        vTimestampsImu存放IMU的时间戳。

        对于多序列数据集来说，nImages存放着每一个序列的图片数量，tot_images存放所有序列的图像数量，nImu存放每一个序列的IMU数据的数量。

        为了对齐两者的数据（我的理解是可能IMU可能初始化需要一些时间.....）：

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

        如果vTimestampsImu[0][first_imu[0]] = vTimestampsImu[0][0] <= vTimestampsCam[0][0]的话我们把IMU的时间戳向上增加。即对齐时间戳。

        接下来我们看看读取配置文件的部分：

LEFT.D: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0]
LEFT.K: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0]
LEFT.R:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176]
LEFT.Rf:  !!opencv-matrix
   rows: 3
   cols: 3
   dt: f
   data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176]
LEFT.P:  !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [435.2046959714599, 0, 367.4517211914062, 0,  0, 435.2046959714599, 252.2008514404297, 0,  0, 0, 1, 0]

        主要读取的有几部分：

        D：畸变系数

        P：这个矩阵是一个 3*4 的相机投影矩阵(P)，通常用于将三维世界坐标系中的点投影到相机图像平面上，得到其在二维图像上的坐标。

         其中，fx 和 fy 是相机的内参矩阵，分别表示相机在 x 和 y 方向上的焦距，cx 和 cy 是相机的光心在图像平面上的坐标。在这个矩阵中，fx = fy = 435.2046959714599，cx = 367.4517211914062，cy = 252.2008514404297，表示了这个相机的内参信息。

这个矩阵中最后一列的值都是0，通常用于齐次坐标的变换。在这个矩阵中，最后一列的值表示图像点在相机坐标系中的 z 轴坐标，因为这是一个投影矩阵，所以z轴坐标始终为0。

总的来说，这个矩阵描述了一个内参已知的相机在三维空间中的位置和方向，可以用于将三维点投影到相机坐标系中，并进一步投影到相机图像平面上。

        K：这个矩阵是一个 3*3 的相机内参矩阵(K)，也称为相机矩阵。它描述了相机的内部参数，包括焦距和光心在像素坐标系中的位置。

其中，fx 和 fy 分别表示相机在 x 和 y 方向上的焦距，cx 和 cy 表示相机的光心在像素坐标系中的坐标。在这个矩阵中，fx = 458.654，fy = 457.296，cx = 367.215，cy = 248.375，表示了这个相机的内参信息。

        这个矩阵常用于相机标定和相机几何变换中。相机标定是指通过多次拍摄已知的空间点并测量它们在图像中的位置来确定相机的内部参数，而相机几何变换是指将图像中的点从像素坐标系转换为相机坐标系或世界坐标系。         

        LEFT.P 和 LEFT.K 表示的是相机内参矩阵和相机投影矩阵，它们不同的地方在于是否包含了相机的外参信息。

具体来说，相机内参矩阵 LEFT.K 只包含了相机的内部参数，即相机在水平和垂直方向上的焦距和光心在像素坐标系中的位置，而不包含相机在世界坐标系中的位置和方向。

相机投影矩阵 LEFT.P 则包含了相机的内部参数和外部参数，即相机在世界坐标系中的位置和方向。它可以将三维世界坐标系中的点投影到相机坐标系中，再进一步投影到相机图像平面上，得到其在二维图像上的坐标。

        因此，LEFT.P 和 LEFT.K 的具体含义和使用场景不同。在相机标定和相机几何变换中，常常需要用到相机内参矩阵，而在三维重建和机器人视觉等应用中，则需要用到相机投影矩阵。

        再详细解释一下：

        假设你拍摄了一个相机移动的视频，现在你需要使用这个相机的参数来进行三维重建。在这种情况下，你需要使用相机的内参矩阵 LEFT.K 和外参矩阵 LEFT.R 来计算相机的旋转和平移，以及畸变参数 LEFT.D 来校正图像畸变。

接着，你需要将相机的内参矩阵 LEFT.K 和外参矩阵 LEFT.P 结合起来，得到一个新的投影矩阵 P'。这个新的投影矩阵 P' 将会被用来将图像坐标转换为相机坐标，然后再进行三维重建。

        具体地，你可以使用以下公式计算新的投影矩阵 P'：

        其中 * 表示矩阵乘法，K 是相机的内参矩阵 LEFT.K，P 是相机的外参矩阵 LEFT.P。这个公式将外参矩阵 LEFT.P 中的平移向量和旋转矩阵都结合在了一起，得到了一个新的投影矩阵 P'。

        有了新的投影矩阵 P'，你就可以使用它来将图像坐标转换为相机坐标，然后再进行三维重建。

2.3 代码解析---图像去畸变部分

cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);

      这是 OpenCV 中用于图像去畸变和校正的函数 cv::initUndistortRectifyMap()。以下是每个参数的含义：

        K_l：左相机内参矩阵，为 3x3 浮点型矩阵。
        D_l：左相机畸变参数，为 1xN 或 Nx1 浮点型向量，其中 N 是畸变系数的数量（通常为 4 或 5）。
        R_l：左相机旋转矩阵，为 3x3 浮点型矩阵。
        P_l.rowRange(0,3).colRange(0,3)：左相机投影矩阵（3x3），它包含左相机的内参矩阵和旋转矩阵，用于计算校正后的图像。
        cv::Size(cols_l,rows_l)：输出映射的图像大小（宽度 x 高度）。
        CV_32F：输出映射数据的类型，这里使用单精度浮点型。
        M1l 和 M2l：可选参数，输出的映射数据，是两个矩阵，每个矩阵的大小是 cv::Size(cols_l, rows_l)。

        R_l 和 P_l 是摄像机标定时的两个重要参数，它们分别表示左相机的旋转矩阵和投影矩阵，用于计算校正后的图像。

        旋转矩阵 R_l：表示将左相机的坐标系旋转到与右相机坐标系相同的旋转矩阵。在立体视觉中，我们需要保持左右相机的坐标系一致，才能进行深度的计算和匹配。通过标定得到的 R_l，我们可以将左相机的图像校正到与右相机相同的视角下。
        投影矩阵 P_l：是左相机的投影矩阵，它包含左相机的内参矩阵和旋转矩阵，用于将校正后的图像投影到三维坐标系。通过标定得到的 P_l，我们可以将校正后的图像转换为三维点云，然后与右相机的点云进行匹配，计算两个相机之间的距离和深度信息。
        总之，R_l 和 P_l 是计算立体视觉中校正后的图像和深度信息所必需的参数。

        该函数计算左相机图像去畸变和校正后的映射，以便校正后的图像具有更好的几何性质。这个函数将计算从畸变图像坐标到校正后的图像坐标的映射，以便在校正图像中重新投影畸变图像的像素。使用返回的映射和 cv::remap() 函数，可以将畸变的左相机图像转换为校正后的图像。

cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);

        这两行代码使用了 OpenCV 库中的 cv::remap() 函数，对输入的图像进行重映射操作。

具体来说，cv::remap() 函数通过输入的像素映射数据 M1l、M2l 和 M1r、M2r，将 imLeft 和 imRight 两个原始图像进行畸变校正和图像矫正。其中，imLeft 和 imRight 分别是左右相机采集的原始图像，imLeftRect 和 imRightRect 分别是经过校正和矫正后的图像。

        cv::remap() 函数的第一个参数是输入图像，第二个参数是输出图像，第三个参数和第四个参数分别是横向和纵向的像素映射数据。第五个参数是插值方法，可以选择不同的插值方法，比如 cv::INTER_LINEAR 表示双线性插值。

        这两行代码的作用是根据畸变校正和图像矫正的映射数据，将左右相机采集的原始图像进行处理，得到经过畸变校正和图像矫正后的左右相机图像。这样做的目的是为了减小图像畸变和视差对立体匹配和三维重建的影响。

        将去畸变后的图像输入到追踪线程。