ORBSLAM2 rgbd_tum.cc程序解析

rgbd_tum.cc相关步骤及对应代码如下所示：

1.输入图像信息

定义的变量如下

 vector<string> vstrImageFilenamesRGB;//RGB 名字
 vector<string> vstrImageFilenamesD;//depth 名字
 vector<double> vTimestamps;//时间戳名字
 string strAssociationFilename = string(associate);//rgb,depth与时间戳对齐后的文件名（assoiate.txt文件名）

图像信息载入函数

LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);

/*功能函数*/
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps)
{
    ifstream fAssociation;
    fAssociation.open(strAssociationFilename.c_str());//读取associate.txt文件
    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;//读取rgb图片名
            vstrImageFilenamesRGB.push_back(sRGB);//存取rgb图片名
            ss >> t;//深度图像时间戳只读取，不存储
            ss >> sD;//深度图像名读取
            vstrImageFilenamesD.push_back(sD);//深度图像名存储
        }
    }
}

int nImages = vstrImageFilenamesRGB.size();//图像数量

ORBSLAM2系统初始化

//1.系统初始化主要载入ORBvoc.txt词袋信息，配置文件TUM1.yaml信息 和对应的相机类型（单目相机，双目相机，深度相机）
//2.初始化跟踪线程，初始化局部地图构建并开启线程，初始化回环检测并开启线程，初始化显示功能并开启线程
 ORB_SLAM2::System SLAM("ORBvoc.txt","TUM1.yaml",ORB_SLAM2::System::RGBD,true);

//载入图像，开启追踪线程

    {
        // Read image and depthmap from file
        imRGB = cv::imread(string(rgbd_image)+"/"+vstrImageFilenamesRGB[ni],CV_LOAD_IMAGE_UNCHANGED);
        imD = cv::imread(string(rgbd_image)+"/"+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
        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

        // 将图片信息传到ORBSLAM2系统，开启追踪线程
        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

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

        if(ttrack<T)
            usleep((T-ttrack)*1e6);
    }

待所有图像遍历完，关闭所有线程

 SLAM.Shutdown();

最后保存相机位姿和关键帧位姿

SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");