一起学ORBSLAM2（3）system框架搭建

转载请注明原创地址：https://blog.csdn.net/qq_30356613/article/category/6897125

接着上一期的一起学ORBSLAM2继续，只不过上一期已经是几个月前了，由于这一段时间忙或者毕业答辩的事情，所以没有时间更新博客，望各位见谅，现在到暑假了，没什么事情，接着之前的工作继续，坚持过好每一天，共勉！ 

ORB-SLAM2是萨拉格萨大学做到一个开源SLAM方案，源代码上传到github上边：https://github.com/raulmur/ORB_SLAM2
相关效果视频网站为：http://webdiis.unizar.es/~raulmur/orbslam/
ORB-SLAM2包含了用三种方案进行同步追踪与地图构建，分别为以kinect为视觉传感器的RGBD-SLAM方案、以单目摄像机为视觉传感器的mono-slam方案、以双目摄像机为视觉传感器的stereo-slam方案。三种方案有均已ORB特征点作为图像构建SLAM的基础，因此此工程名ORB-SLAM。
接下来我们将以RGB-D SLAM作为例子来介绍ORB-SLAM开源方案中所用到的方法
RGB-D SLAM分为以下三个线程，第一个是图像追踪线程（主线程）作为视觉里程计的Tracking线程，第二个为临时地图构建LocalMapping线程，第三个为回环检测LoopClosing线程，第四个为观测器线程Viewer（根据参数而定，可有可无）在接下来的几个章节中，我们将分别就这些分线程进行介绍ORB-SLAM。
此讲中我们主要分析ORB-SLAM2如何与Kinect进行数据传输和系统线程构建的方法及其代码分析。
ORB-SLAM2与Kinect的数据传输有许多中，我们在这里讲述其在ROS操作系统上进行数据传输的实例。该工程中开发者在example中ROS文件夹下举例说明了如何将该工程运行在机器人操作系统中（ROS），我们将从这里的main开始讲起。即ros_rgbd.cc文件
代码如下：

int main(int argc, char **argv)
{
//初始化ROS系统 
ros::init(argc, argv, "RGBD");
//ROS启动函数
ros::start();
//检测给定参数数量是否完整
if(argc != 3)
{
cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl; 
ros::shutdown();
return 1;
} 
// Create SLAM system. It initializes all system threads and gets ready to process frames.
// 创建SLAM系统，初始化所有系统线程并准备进程帧
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);
//定义捕获图像的对象
ImageGrabber igb(&SLAM);
//建立ROS节点
ros::NodeHandle nh;
//Subscriber滤波器作为ROS消息的最顶层，不能够将其他滤波器的输出作为其输入
message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/kinect2/hd/image_color_rect", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/kinect2/hd/image_depth_rect", 1);
//The message_filters::sync_policies::ApproximateTime policy uses an adaptive algorithm to match messages based on their timestamp.
//下边两条指令是为了将rgb_sub和depth_sub进行数据对齐，滤掉不对齐的帧数
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
//采用registerCallback()方法，注册回调函数ImageGrabber::GrabRGBD。
sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));
//此函数用于ROS不断回调节点mageGrabber::GrabRGBD函数
ros::spin();
// Stop all threads 停止所有线程
SLAM.Shutdown();
// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
ros::shutdown();
return 0;
}

主函数中使用ROS编程的风格将该SLAM系统作为ROS上的一个节点来运行在ROS上面，通过订阅Kinect发布的话题，来获取Kinect上采集的图像信息。
具体代码含义已经在代码中给出，后面通过不断的回调ImageGrabber类中的成员函数GrabRGBD()来读取Kinect数据，从而完成整个系统的运行。

class ImageGrabber
{
public:
ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}
void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);
ORB_SLAM2::System* mpSLAM;
};
void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
// Copy the ros image message to cv::Mat.
cv_bridge::CvImageConstPtr cv_ptrRGB;
try
{
cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv_bridge::CvImageConstPtr cv_ptrD;
try
{
cv_ptrD = cv_bridge::toCvShare(msgD);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec()); //该成员函数的核心代码
}

以上类和代码主要就是将采集到的数据检测是否有异常，然后将采集到的数据给system，供整个系统使用。
system构建及使用在system.cc中实现，如下：

//strVocFile指向字典的路径，strSettingsFile指向参数配置文件的路径，sensor所使用传感器的种类，bUseViewer是否打开实时观测
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
const bool bUseViewer):mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false),mbActivateLocalizationMode(false),
mbDeactivateLocalizationMode(false)
{
// 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 将配置文件读入到内存中，存储到fsSettings变量中
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;
mpVocabulary = new ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); //将字典加载到mpVocabulary中
if(!bVocLoad)
{
cerr << "Wrong path to vocabulary. " << endl;
cerr << "Falied to open at: " << strVocFile << endl;
exit(-1);
}
cout << "Vocabulary loaded!" << endl << endl;
//Create KeyFrame Database 创建关键帧数据集
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
//Create the Map 创建地图
mpMap = new Map();
//Create Drawers. These are used by the Viewer 创建观测器 帧的观测器和地图观测器
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
//Initialize the Tracking thread 初始化跟踪线程器
//(it will live in the main thread of execution, the one that called this constructor) 它运行在主线程中，这个线程被称作constructor
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
//Initialize the Local Mapping thread and launch 初始化当地地图线程并启动
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
//Initialize the Loop Closing thread and launch 初始化回环检测线程并启动
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
//Initialize the Viewer thread and launch 初始化观测器线程并启动
if(bUseViewer)
{
mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
mptViewer = new thread(&Viewer::Run, mpViewer);
mpTracker->SetViewer(mpViewer);
}
//Set pointers between threads 各个线程之间建立联系
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);
}

此函数为system的构造函数，构造函数中主要做了以一下工作
（1）检测传感器的种类，
（2）将配置文件和词典读入内存中，
（3）建立线程，分为四个，一个是主线程（tracking线程），第二个为局部建图线程（mptLocalMapping线程），第三个为回环检测线程（LoopClosing线程），第四个为实时显示线程（根据bUseViewer而定）
（4）各线程之间建立联系及通信
在main函数中节点的主要工作是不断回调成员函数ImageGrabber::GrabRGBD，而在该成员函数中除了验证一下传输数据是否正常并将ROS图片转变为cv::Mat格式外主要的工作量在不断调用SLAM->TrackRGBD(parameter)函数，那么该函数的具体工作在什么呢?如下所示函数：

cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp)
{
if(mSensor!=RGBD)
{
cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl;
exit(-1);
} 
// Check mode change
//核查运行模式是否改变
{
unique_lock<mutex> lock(mMutexMode); //使用mMutexMode锁
//停用局部建图模式
if(mbActivateLocalizationMode)
{
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped 等待直到局部地图构建线程被暂停
while(!mpLocalMapper->isStopped())
{
usleep(1000);
}
mpTracker->InformOnlyTracking(true); //仅追踪线程开启
mbActivateLocalizationMode = false;
}
//重新激活局部建图模式
if(mbDeactivateLocalizationMode)
{
mpTracker->InformOnlyTracking(false); //仅追踪线程关闭
mpLocalMapper->Release(); //局部地图构建线程释放
mbDeactivateLocalizationMode = false;
}
}
// Check reset
// 检查复位
{
unique_lock<mutex> lock(mMutexReset); //使用mMutexReset锁
if(mbReset)
{
mpTracker->Reset();
mbReset = false;
}
}
cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;
}

该函数主要的工作：
（1）检测输入传感器
（2）核查运行模式是否改变（主要就是核查mbActivateLocalizationMode和mbDeactivateLocalizationMode）
（3）检查是否需要复位
（4）计算变换矩阵T(GrabImageRGBD() 函数来计算)这一步将是主要的工作量所在
至于GrabImageRGBD()函数的具体实现就是线程Tracking中的内容了，我们将在下一节中详细介绍线程Tracking，希望大家多多关注。