ORB-SLAM2 源码笔记(0) -系统启动

1、通过ROS来启动ORBSLAM2

orbslam2中集成了一个ROS包，我们可以通过启动该ROS包中的节点来启动ORBSLAM2，因为ROS在平时做项目中是很常用的，所以就重点学习基于ROS启动ORBSLAM2的例子。
orbslam2的ROS包在Examples/ROS文件夹中，源代码ROS包的名字是ORB_SLAM2,进入到ROS包中后，先打开CMakeLists.txt文件，查看我们可以启动那些节点
可以看到，
对于单目，其节点的执行文件叫Mono，通过src/ros_mono.cc生成;
RGBD节点的执行文件叫RGBD，通过src/ros_rgbd.cc生成;
而双目的执行文件叫Stereo，通过ros_stereo.cc生成。
那么也就可以知道，如果我们要通过ROS启动ORBSLAM2的话，就可以通过rosrun去启动这几个节点来完成。
ROS启动ORBSLAM2的流程中，最主要的两个步骤是：
1、创建和初始化SLAM系统

 ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

2、在回调函数中执行Tracking线程的主函数

// 单目
mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec());
//RGBD
mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());
// 双目
mpSLAM->TrackStereo(cv_ptrLeft->image,cv_ptrRight->image,cv_ptrLeft->header.stamp.toSec());

2、 System类 - 外界应用层与SLAM底层库的桥梁

正如上一节所学习的，在ORBSLAM2的ROS节点中，执行了两个重要的步骤，首先是节点启动时，创建并初始化了System类对象SLAM，并且在图片输入后会进入的回调函数中执行System 类的成员函数，TrackStereo/TrackRGBD/TrackMonocular ，这些函数即构成Tracking线程。

下面进入System类的构造函数中看看执行了什么。

System(const string &strVocFile,            //指定ORB字典文件的路径
       const string &strSettingsFile,       //指定配置文件的路径
       const eSensor sensor,                //指定所使用的传感器类型
       const bool bUseViewer = true);       //指定是否使用可视化界面 TODO 

构造函数里主要完成如下工作：
1、加载参数文件
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
2、加载字典文件
//建立一个新的ORB字典
mpVocabulary = new ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
3、创建关键帧数据库
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
4、创建地图
mpMap = new Map();
5、创建绘图相关对象
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
6、初始化4个slam模块对象 mpTracker ，mpLocalMapper ， mpLoopCloser，mpViewer.
并且创建3个线程mptLocalMapping，mptLoopClosing，mptViewer ，并且立即启动运行：

 //运行这个局部建图线程
 mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,	//这个线程会调用的函数
    							 mpLocalMapper);				//这个调用函数的参数
 //启动回环检测线程
 mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run,	//线程的主函数
							mpLoopCloser);					//该函数的参数
 // 启动viewer线程
 mptViewer = new thread(&Viewer::Run, mpViewer);

而Tracking线程其实就是ROS的图像回调函数了，而在回调函数中又是调用如下函数-

// 跟踪双目
cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp)
// 跟踪 RGBD相机
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)
// 跟踪单目相机  
cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)

3种相机在这个函数中的执行都差不多，大概的流程是：

1、检查模式有无变化(开启了定位模式，或者开启了建图模式？)
2、检查需不需要重置Reset。
3、调用Tracking类中的函数——

// 双目
mpTracker->GrabImageStereo(imLeft,imRight,timestamp);
//单目
mpTracker->GrabImageMonocular(im,timestamp);
//RGBD
mpTracker->GrabImageRGBD(im,depthmap,timestamp);

看到这里梳理一下，以单目为例，程序运行以后，在System类构造函数中启动mptLocalMapping，mptLoopClosing，mptViewer3个线程，而tracking线程则是在图像的回调函数中运行，并且最终调用的是Tracking类中的函数(如上) ，这样一个完整的SLAM系统就执行了起来。
下一节，我们就将进入Tracking.cc中进行Tracking线程的学习啦。

3、数据集测试

EuRoC数据集
主要测试单目和双目
单目：rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/EuRoC.yaml
双目：rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml true
单目启动rosbag play --pause MH_01_easy.bag /cam0/image_raw:=/camera/image_raw 其中后面参数的意思是将数据集原本发送的topic名字改为/camera/image_raw，即节点订阅的话题名。
双目启动：rosbag play --pause MH_01_easy.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw
即可运行成功：

TUM数据集
主要测试RGBD和单目
启动RGBD：rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/RGB-D/TUM2.yaml
启动数据集：rosbag play --pause rgbd_dataset_freiburg2_pioneer_slam.bag /camera/rgb/image_color:=/camera/rgb/image_raw /camera/depth/image:=/camera/depth_registered/image_raw
然后就能看到跑起来了