ORB_SLAM2程序入口(System.cc)
程序入口
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 framedouble 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-Denum eSensor{MONOCULAR=0,STEREO=1,RGBD=2};
(4)bUseViewer:是否使用可视化界面,即运行ORB_SLAM2中的运行界面。
界面的声明信息
// Output welcome messagecout << 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 filecv::FileStorage fsSettings(strSettingsFile.c_str(), //将配置文件名转换成为字符串cv::FileStorage::READ); //只读//如果打开失败,就输出调试信息if(!fsSettings.isOpened()){cerr << "Failed to open settings file at: " << strSettingsFile << endl;//然后退出exit(-1);}//Load ORB Vocabularycout << 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 launchmpLocalMapper = new LocalMapping(mpMap, //指定使iomanipmSensor==MONOCULAR); // TODO 为什么这个要设置成为MONOCULAR???//运行这个局部建图线程mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, //这个线程会调用的函数mpLocalMapper); //这个调用函数的参数//Initialize the Loop Closing thread and launchiomanipmpLoopCloser = new LoopClosing(mpMap, //地图mpKeyFrameDatabase, //关键帧数据库mpVocabulary, //ORB字典mSensor!=MONOCULAR); //当前的传感器是否是单目//创建回环检测线程mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, //线程的主函数mpLoopCloser); //该函数的参数if(bUseViewer){//如果指定了,程序的运行过程中需要运行可视化部分//新建viewermpViewer = 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 ×tamp)
主循环中的函数System::TrackMonocular。
(1)判断系统的模式是否发生了变化。即激活定位模式和关闭定位模式。
(2)如果开启定位模式,则关闭局部建图线程,且标志位 置为true。
(3)如果关闭定位模式,则开启局部建图线程,且标志位 置为false。
if(mbActivateLocalizationMode){mpLocalMapper->RequestStop();// Wait until Local Mapping has effectively stoppedwhile(!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)
ORB_SLAM2程序入口(System.cc)相关推荐
- 有一个包含主函数的java类_重新认识Java中的程序入口即主函数各组成部分
主函数的定义: public:代表着该函数访问权限是最大的 static:代表主函数随着类的加载就已经存在了 void:主函数没有具体的返回值 main:不是关键字,但是一个特殊的单词,能够被JVM识 ...
- 微擎框架小程序 入口
微擎框架小程序 入口 App({onLaunch: function () {},onShow: function () {}, onHide: function () {}, onError: fu ...
- Java基础笔记(2) 程序入口 关键字 标识符 常量 变量
提醒:关于那些和我一样新鸟来看资料的,能看懂多少看多少,看不懂的就是不重要,重要的你想我自己学习肯定要标注的,这些信息明白每个知识点实际作用就好了,其他的比如等会讲的常量内存,常量池这些都是我找的资料 ...
- 学习Direct3D(五)应用程序入口
// // // File: WinMainApp.cpp // Author :hsg (c) All Rights Reserved // System: // 功能:启动应用程序入口 类 // ...
- 利用多态特性,创建一个手机类Phones,定义打电话的方式call,创建三个子类,并在各自的方法中重写方法,编写程序入口main()方法,实现多种手机打电话...
|--需求说明 利用多态特性,创建一个手机类Phones,定义打电话的方式call,创建三个子类,并在各自的方法中重写方法,编写程序入口main()方法,实现两种手机打电话 |--解题思路 采用简单工 ...
- 利用多态特性,编程创建一个手机类Phones,定义打电话方法call()。创建两个子类:苹果手机类IPhone和安卓手机类APhone,并在各自类中重写方法call(),编写程序入口main()方法
利用多态特性,编程创建一个手机类Phones,定义打电话方法call().创建两个子类:苹果手机类IPhone和安卓手机类APhone,并在各自类中重写方法call(),编写程序入口main()方法, ...
- Java程序入口+输出语句+数据类型
一(Java程序入口):要想执行一个java程序,就必须有一个程序入口,且该入口为固定格式:public static void main(String[ ] args){}.注意:是main而不是m ...
- Cartographer源码阅读(1):程序入口
带着几个思考问题: (1)IMU数据的使用,如何融合,Kalman滤波? (2)图优化的具体实现,闭环检测的策略? (3)3D激光的接入和闭环策略? 1. 安装Kdevelop工具: http://b ...
- X509证书 指定了无效的提供程序类型 System.Security.Cryptography.CryptographicException 错误解决方法
X509证书 指定了无效的提供程序类型 System.Security.Cryptography.CryptographicException 错误解决方法 参考文章: (1)X509证书 指定了无效 ...
最新文章
- 【leetcode】1032. Stream of Characters
- mysql dba系统学习(21)mysql存储引擎InnoDB
- SMGP3.0协议的概念知识
- JavaScript---Ajax和函数回调,异步编程
- 第04讲: 基础探究,Session 与 Cookies
- 【.NET架构】BIM软件架构02:Web管控平台后台架构
- pg加密扩展的安装_postgresql的加密扩展插件pgcrypto
- java多线程区别_Java中实现多线程的两种方式之间的区别
- 巧用HashSet装载非重数据(洛谷P2250题题解,Java语言描述)
- Javaoop_继承
- python最大迭代次数_python scipy eigs:无论收敛容差如何,在最大迭代次数后返回特征向量...
- [课堂实践与项目]IOS优先级的计算器
- 8客户端安装后无法启动_QQ下载几天后无法启动、无法运行——可能与火绒有关...
- QueryRunner类 的應用,以及ResultSetHandler 接口的实现类
- C# 引用访问权限,很多老手都不懂
- ACL访问控制 getfacl 、setfacl 命令
- 腾讯云播放器隐藏音频播放的按钮
- 成功制作海报的十个简单法则
- Zabbix实现企业微信报警
- 2017年全国大学生电子设计竞赛 单相用电器分析监测装置(k题)