ORBSLAM3 --- 双目惯导执行ORBSLAM3(一):Stereo_intertail_euroc.cc文件解析

这篇具有很好参考价值的文章主要介绍了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词典的位置、配置文件的地址、图像序列的地址、时间戳的地址。

ORBSLAM3 --- 双目惯导执行ORBSLAM3(一):Stereo_intertail_euroc.cc文件解析,ORB-SLAM3代码解析,c++,开发语言,计算机视觉,算法,图像处理

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<string> > vstrImageLeft;
    vector< vector<string> > vstrImageRight;
    vector< vector<double> > vTimestampsCam;
    vector< vector<cv::Point3f> > vAcc, vGyro;
    vector< vector<double> > vTimestampsImu;
    vector<int> nImages;
    vector<int> nImu;
    vector<int> 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<num_seq; seq++)
    {
        cout << "Loading images for sequence " << seq << "...";

        string pathSeq(argv[(2*seq) + 3]);
        string pathTimeStamps(argv[(2*seq) + 4]);

        string pathCam0 = pathSeq + "/mav0/cam0/data";
        string pathCam1 = pathSeq + "/mav0/cam1/data";
        string pathImu = pathSeq + "/mav0/imu0/data.csv";

        LoadImages(pathCam0, pathCam1, pathTimeStamps, vstrImageLeft[seq], vstrImageRight[seq], vTimestampsCam[seq]);
        cout << "LOADED!" << endl;

        cout << "Loading IMU for sequence " << seq << "...";
        LoadIMU(pathImu, vTimestampsImu[seq], vAcc[seq], vGyro[seq]);
        cout << "LOADED!" << endl;

        nImages[seq] = vstrImageLeft[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

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

    // Read rectification parameters
    cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        cerr << "ERROR: Wrong path to settings" << endl;
        return -1;
    }

    cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
    fsSettings["LEFT.K"] >> 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<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.
    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<num_seq; seq++)
    {
        // Seq loop
        vector<ORB_SLAM3::IMU::Point> 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<nImages[seq]; ni++, proccIm++)
        {
            // Read left and right images from file
            imLeft = cv::imread(vstrImageLeft[seq][ni],cv::IMREAD_UNCHANGED);
            imRight = cv::imread(vstrImageRight[seq][ni],cv::IMREAD_UNCHANGED);

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

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


#ifdef REGISTER_TIMES
    #ifdef COMPILEDWITHC11
            std::chrono::steady_clock::time_point t_Start_Rect = std::chrono::steady_clock::now();
    #else
            std::chrono::monotonic_clock::time_point t_Start_Rect = std::chrono::monotonic_clock::now();
    #endif
#endif
            cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
            cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);

#ifdef REGISTER_TIMES
    #ifdef COMPILEDWITHC11
            std::chrono::steady_clock::time_point t_End_Rect = std::chrono::steady_clock::now();
    #else
            std::chrono::monotonic_clock::time_point t_End_Rect = std::chrono::monotonic_clock::now();
    #endif
            t_rect = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Rect - t_Start_Rect).count();
            SLAM.InsertRectTime(t_rect);
            t_rect = std::chrono::duration_cast<std::chrono::duration<double> >(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<std::chrono::duration<double,std::milli> >(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<std::chrono::duration<double,std::milli> >(t2 - t1).count();
            SLAM.InsertTrackTime(t_track);
#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[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); // 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);
        SLAM.SaveMap("patcheuroc.txt",imLeftRect.size);
    }
    else
    {
        SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
        SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
        SLAM.SaveMap("patcheuroc.txt",imLeftRect.size);

    }

    return 0;
}

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";

ORBSLAM3 --- 双目惯导执行ORBSLAM3(一):Stereo_intertail_euroc.cc文件解析,ORB-SLAM3代码解析,c++,开发语言,计算机视觉,算法,图像处理

ORBSLAM3 --- 双目惯导执行ORBSLAM3(一):Stereo_intertail_euroc.cc文件解析,ORB-SLAM3代码解析,c++,开发语言,计算机视觉,算法,图像处理

ORBSLAM3 --- 双目惯导执行ORBSLAM3(一):Stereo_intertail_euroc.cc文件解析,ORB-SLAM3代码解析,c++,开发语言,计算机视觉,算法,图像处理

        我们看读取图片的函数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<string> > vstrImageLeft
// vector< vector<string> > vstrImageRight
// vector< vector<double> > vTimestampsCam
void LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes,
                vector<string> &vstrImageLeft, vector<string> &vstrImageRight, vector<double> &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里面的内容:里面存放所有图像的时间戳

ORBSLAM3 --- 双目惯导执行ORBSLAM3(一):Stereo_intertail_euroc.cc文件解析,ORB-SLAM3代码解析,c++,开发语言,计算机视觉,算法,图像处理

        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<double> > vTimestampsImu
// vAcc  vector< vector<cv::Point3f> > vAcc, vGyro
// vGyro  vector< vector<cv::Point3f> > vAcc, vGyro
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];
            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]));
        }
    }
}

ORBSLAM3 --- 双目惯导执行ORBSLAM3(一):Stereo_intertail_euroc.cc文件解析,ORB-SLAM3代码解析,c++,开发语言,计算机视觉,算法,图像处理

        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 表示双线性插值。

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

        将去畸变后的图像输入到追踪线程。文章来源地址https://www.toymoban.com/news/detail-723814.html

到了这里,关于ORBSLAM3 --- 双目惯导执行ORBSLAM3(一):Stereo_intertail_euroc.cc文件解析的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 手把手带你死磕ORBSLAM3源代码(三十)Tracking.cc PreintegrateIMU介绍

    目录 一.前言 二.代码 2.1 完整代码 2.2 预积分技术     Tracking::PreintegrateIMU() 是 Tracking 类中的一个成员函数,用于对从IMU(Inertial Measurement Unit,惯性测量单元)获取的数据进行预积分处理。预积分是视觉-惯性里程计(VIO)中的一个关键技术,它允许将多个IMU测量值整合到一

    2024年02月03日
    浏览(64)
  • orbslam3 生成标定板rosrun kalibr kalibr_create_target_pdf --type

    小师妹要做相机视觉标定,需要制作棋盘格,无奈其电脑有些卡,对此毫无经验的博主从头开始安装(此前博主已经安装了ROS环境),如果没有安装ROS环境,请参照以下链接: 安装Kalibr请参考,按照次教程安装过程build一次过。 之后cd到Kalibr的目录下进行测试时,主要遇到的

    2024年02月16日
    浏览(46)
  • 手把手带你死磕ORBSLAM3源代码(二十九)Tracking.cc GrabImageMonocular介绍

    目录 一.前言 二.代码 2.1 完整代码 2.2 单目相机估计深度结构     Tracking::GrabImageMonocular 是 Tracking 类中的另一个成员函数,用于从单目相机(Monocular Camera)捕获的图像中提取信息,创建一个新的帧ÿ

    2024年02月02日
    浏览(44)
  • 手把手带你死磕ORBSLAM3源代码(三十四)Tracking.cc MonocularInitialization编辑

    目录 一.前言 二.代码 2.1完整代码 2.2 单目视觉跟踪初始化     这段代码是一个名为 MonocularInitialization 的函数,它属于 Tracking 类。从函数名称和代码内容来看,这个函数主要用于单目视觉跟踪的初始化过程。以下是代码的详细解读:     首先,函数检查一个名为 m

    2024年02月02日
    浏览(38)
  • 5.如何利用ORBSLAM3生成可用于机器人/无人机导航的二维/三维栅格地图--以octomap为例

            这里我们用ROS自带的安装方式即可:         如上图就是安装成功了:         如果安装失败了,尝试用小鱼ROS换一下源再去安装:         一些官方的文档如下,大家感兴趣可以学习一下: https://octomap.github.io/octomap/doc/index.html#gettingstarted_sec https://oct

    2024年02月03日
    浏览(39)
  • ORBSLAM3 --- 优化(一):g2o优化中的节点与边的定义-G2oTypes.h、G2oTypes.cc解析

    目录 1.节点类 1.1 ImuCamPose类 1.1.1 类的定义 1.1.2  ImuCamPose::ImuCamPose 1.1.3  ImuCamPose::SetParam 1.1.4  ImuCamPose::isDepthPositive 1.2 VertexPose类 1.2.1 节点定义 1.2.2 ImuCamPose::Update函数解析 1.3 VertexPose4DoF类 1.3.1 节点定义 1.3.2  ImuCamPose::UpdateW函数解析 1.4 速度节点VertexVelocity 1.4.1 节点定义 1.5

    2024年02月03日
    浏览(34)
  • DSGN: Deep Stereo Geometry Network for 3D Object Detection---基于双目视觉的3D目标检测(1)

    为了弥合2D图像和3D空间之间的差距,在平面扫描体中建立立体对应关系,然后将其转换为3DGV(3D geometric volume),以便能够对3D几何体和语义线索进行编码,并能在世界坐标系中进行目标检测。 设计了一条端到端的pipeline,用于提取像素级特征以进行立体匹配,并提取高级特

    2024年02月12日
    浏览(38)
  • 捷联惯导算法(三)姿态角和姿态矩阵

    文中算法公式摘自《捷联惯导算法与组合导航原理》(严恭敏、翁浚 编著)、《惯性导航》(秦永元 编著),其他理解仅代表个人观点。本文是对姿态角和姿态矩阵之间转化的理解。 机体坐标系b系定义: o飞机质心 x轴指向机头方向 z轴处在飞机对称面垂直x轴向下 y轴垂直

    2023年04月16日
    浏览(34)
  • 捷联惯导数值更新算法-姿态更新+速度更新+位置更新

    捷联惯导数值更新算法通常可划分为姿态、速度和位置更新三部分﹐ 姿态更新算法 是 核心 ,其求解精度对整个捷联惯导的精度起着决定性的作用。目前主流的姿态更新求解方法是,先使用陀螺角增量的 多子样采样 计算等效旋转矢量,补偿转动不可交换误差,再使用等效旋转矢

    2023年04月08日
    浏览(27)
  • ORBSLAM2环境搭建

    此处安装的是Eigen3.2.1版本,一开始安装Eigen3.4.0,在最后所有环境配置完成后,运行ORBSLAM2时,运行一小段时间就自动退出。不同版本可以在该网站找到 0.6版本 安装时报错: 参考该链接在Pangolin/include/video/drivers/ffmpeg.h开头加上 编译安装成功,会有很多的警告,这里可以忽略

    2024年02月17日
    浏览(33)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包