ORB_SLAM2程序入口（System.cc）

最新推荐文章于 2024-10-04 23:30:18 发布

嚣张的叉烧包

最新推荐文章于 2024-10-04 23:30:18 发布

阅读量533

点赞数

分类专栏： # ORB_SLAM2源码理解

本文链接：https://blog.csdn.net/weixin_43828675/article/details/113828896

版权

ORB_SLAM2源码理解专栏收录该内容

15 篇文章 6 订阅

订阅专栏

程序入口

ORB_SLAM2的程序入口为src/System.cc。在CMakeList.txt中可知，ORB_SLAM2的可执行程序为：
Examples/Stereo/stereo_kitti.cc等。

add_executable(stereo_kitti
Examples/Stereo/stereo_kitti.cc)
target_link_libraries(stereo_kitti ${PROJECT_NAME})

add_executable(stereo_euroc
Examples/Stereo/stereo_euroc.cc)
target_link_libraries(stereo_euroc ${PROJECT_NAME})


set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular)

add_executable(mono_tum
Examples/Monocular/mono_tum.cc)
target_link_libraries(mono_tum ${PROJECT_NAME})

add_executable(mono_kitti
Examples/Monocular/mono_kitti.cc)
target_link_libraries(mono_kitti ${PROJECT_NAME})

add_executable(mono_euroc
Examples/Monocular/mono_euroc.cc)
target_link_libraries(mono_euroc ${PROJECT_NAME})

在Examples/Stereo/stereo_kitti.cc程序中可见：argc===4，即在命令行中输入的四部分：1）可执行程序。2）词典。3）配置文件。4）数据集。

./Examples/Stereo/stereo_kitti Vocabulary/ORBvoc.txt ./Examples/Stereo/KITTI00-02.yaml /home/dk/桌面/kitti/00

接下来，笔者以./Examples/Monocular/EuRoC_TImeStamps/mono_kitti.cc为例。因为其包含单目初始化，比较经典。

读取图片的路径+名称、时间戳

首先使用LoadImages函数获得左右目图像以及时间戳。

初始化整个系统

而后调用ORB_SLAM2::System的构造函数初始化整个系统。

主循环

调用SLAM.TrackMonocular处理每一帧输入的图片。如果处理一帧所用时间小于两帧之间实际拍摄所间隔时间，则用usleep函数停顿相应时间再进行下一轮循环。

        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();

    // 统计时间
    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;

    // 保存关键帧位姿信息
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

System.cc

主程序的实现文件。在其中声明了界面信息，导入了关键帧数据库，开启了三个线程（追踪线程、局部建图线程、回环检测线程）。

构造函数

//系统的构造函数，将会启动其他的线程
System::System(const string &strVocFile,					//词典文件路径
			   const string &strSettingsFile,				//配置文件路径
			   const eSensor sensor,						//传感器类型
               const bool bUseViewer):						//是否使用可视化界面
					 mSensor(sensor), 							//初始化传感器类型
					 mpViewer(static_cast<Viewer*>(NULL)),		//空。。。对象指针？  TODO 
					 mbReset(false),							//无复位标志
					 mbActivateLocalizationMode(false),			//没有这个模式转换标志
        			 mbDeactivateLocalizationMode(false)		//没有这个模式转换标志

（1）strVocFile为词典路径：Vocabulary路径下的ORBvoc.txt。
（2）strSettingsFile：配置文件路径，即Examples目录下的yaml文件。其中包含一些配置信息，如相机的内参，特征点的提取数量，图像金字塔的信息等。
（3）sensor：传感器的类型，在system.h中有设置。

	// 单目、双目、RGB-D
    enum eSensor{
        MONOCULAR=0,
        STEREO=1,
        RGBD=2
    };

（4）bUseViewer：是否使用可视化界面，即运行ORB_SLAM2中的运行界面。

界面的声明信息

    // Output welcome message
    cout << endl <<
    "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<
    "This program comes with ABSOLUTELY NO WARRANTY;" << endl  <<
    "This is free software, and you are welcome to redistribute it" << endl <<
    "under certain conditions. See LICENSE.txt." << endl << endl;

    // 输出当前传感器类型
    cout << "Input sensor was set to: ";

    if(mSensor==MONOCULAR)
        cout << "Monocular" << endl;
    else if(mSensor==STEREO)
        cout << "Stereo" << endl;
    else if(mSensor==RGBD)
        cout << "RGB-D" << endl;

    //Check settings file
    cv::FileStorage fsSettings(strSettingsFile.c_str(), 	//将配置文件名转换成为字符串
    						   cv::FileStorage::READ);		//只读
    //如果打开失败，就输出调试信息
    if(!fsSettings.isOpened())
    {
       cerr << "Failed to open settings file at: " << strSettingsFile << endl;
       //然后退出
       exit(-1);
    }

    //Load ORB Vocabulary
    cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;

    //建立一个新的ORB字典
    mpVocabulary = new ORBVocabulary();
    //获取字典加载状态
    bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
    //如果加载失败，就输出调试信息
    if(!bVocLoad)
    {
        cerr << "Wrong path to vocabulary. " << endl;
        cerr << "Falied to open at: " << strVocFile << endl;
        //然后退出
        exit(-1);
    }
    //否则则说明加载成功
    cout << "Vocabulary loaded!" << endl << endl;

构建关键帧数据库

ORB_SLAM2是基于关键帧优化的SLAM系统，因此需要一个关键帧数据库。
关键帧数据库类在KeyFrameDatabase.h中定义，主要包含训练好的词典，以及单词的出现情况与关键帧索引间的联系。
该库在重定位和回环检测时被使用，用于检测与当前帧最相似的关键帧。

mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

初始化地图与绘制器

初始化一个地图类，主要包含该地图中的关键帧和路标点。
构造绘图变量，用于绘制可视化界面（包括关键帧与路标点）。

mpMap = new Map();
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

开启三个线程

开启三个线程：Tracking、LocalMapping、LoopClosing。（三者之间应该有个沟通，否则数据会紊乱）
其中Tracking为主线程（初始化追踪线程），LocalMapping、LoopClosing为子线程。子线程的运行函数分别为：&ORB_SLAM2::LocalMapping::Run、&ORB_SLAM2::LoopClosing::Run。使用thread加入到子线程中。根据输入的参数bUseViewer决定是否启动可视化线程。

mpTracker = new Tracking(this,						//现在还不是很明白为什么这里还需要一个this指针  TODO  
    						 mpVocabulary,				//字典
    						 mpFrameDrawer, 			//帧绘制器
    						 mpMapDrawer,				//地图绘制器
                             mpMap, 					//地图
                             mpKeyFrameDatabase, 		//关键帧地图
                             strSettingsFile, 			//设置文件路径
                             mSensor);					//传感器类型iomanip

    //初始化局部建图线程并运行
    //Initialize the Local Mapping thread and launch
    mpLocalMapper = new LocalMapping(mpMap, 				//指定使iomanip
    								 mSensor==MONOCULAR);	// TODO 为什么这个要设置成为MONOCULAR？？？
    //运行这个局部建图线程
    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,	//这个线程会调用的函数
    							 mpLocalMapper);				//这个调用函数的参数

    //Initialize the Loop Closing thread and launchiomanip
    mpLoopCloser = new LoopClosing(mpMap, 						//地图
    							   mpKeyFrameDatabase, 			//关键帧数据库
    							   mpVocabulary, 				//ORB字典
    							   mSensor!=MONOCULAR);			//当前的传感器是否是单目
    //创建回环检测线程
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run,	//线程的主函数
    							mpLoopCloser);					//该函数的参数
    if(bUseViewer)
    {
    	//如果指定了，程序的运行过程中需要运行可视化部分
    	//新建viewer
        mpViewer = new Viewer(this, 			//又是这个
        					  mpFrameDrawer,	//帧绘制器
        					  mpMapDrawer,		//地图绘制器
        					  mpTracker,		//追踪器
        					  strSettingsFile);	//配置文件的访问路径
        //新建viewer线程
        mptViewer = new thread(&Viewer::Run, mpViewer);
        //给运动追踪器设置其查看器
        mpTracker->SetViewer(mpViewer);
    }

建立各个线程间的联系

    //设置进程间的指针
    mpTracker->SetLocalMapper(mpLocalMapper);
    mpTracker->SetLoopClosing(mpLoopCloser);

    mpLocalMapper->SetTracker(mpTracker);
    mpLocalMapper->SetLoopCloser(mpLoopCloser);

    mpLoopCloser->SetTracker(mpTracker);
    mpLoopCloser->SetLocalMapper(mpLocalMapper);

处理单目相机传入的每一帧图片

判断系统的模式是否发生了改变

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)

主循环中的函数System::TrackMonocular。
（1）判断系统的模式是否发生了变化。即激活定位模式和关闭定位模式。
（2）如果开启定位模式，则关闭局部建图线程，且标志位置为true。
（3）如果关闭定位模式，则开启局部建图线程，且标志位置为false。

if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped())
            {
                usleep(1000);
            }

            // 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新
            // 设置mbOnlyTracking为真
            mpTracker->InformOnlyTracking(true);
            // 关闭线程可以使得别的线程得到更多的资源
            mbActivateLocalizationMode = false;
        }
        // 如果mbDeactivateLocalizationMode是true，局部地图线程就被释放, 关键帧从局部地图中删除.
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            mbDeactivateLocalizationMode = false;
        }

判断是否需要重置

当重定位失败之后，系统无法定位，此时需要重置。

unique_lock<mutex> lock(mMutexReset);
    if(mbReset)
    {
        mpTracker->Reset();
        mbReset = false;
    }

计算位姿

计算相机位姿（相对于世界坐标的位姿变换），

cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

更新系统变量

包括更新系统的跟踪状态（mTrackingState），当前帧的路标点（mTrackedMapPoints），当前帧的特征点（mTrackedKeyPointsUn）。

保存相机轨迹

以数据集的格式保存相机轨迹。使用当前帧相对于参考关键帧的转换+参考关键帧相对于第一个关键帧的转换计算出当前帧相对于第一个关键帧（世界坐标）的转换。

void System::SaveTrajectoryTUM(const string &filename)