[ORBSLAM2源码笔记(1)]Introduction
经过了高博老师《视觉SLAM14讲》的学习,从本次博客开始,准备正式开始学习ORBSLAM2源码,并在博客中进行源码笔记和记录。
源码学习过程也借鉴了不少大佬的博客、视频以及学习资源,在此不一一列举,respect!
首先和看一篇论文一样,Introduction是了解论文的入口,同样地,作为一套成熟规范的C++源码,在正式开始阅读ORBSLAM2的源码之前,需要了解一些必要的内容。
1.ORBSLAM2变量命名规则
ORBSLAM2工程量较大,代码量多,其中涉及到大量的成员变量、数据类型等等,为此作者大大对整套代码采用匈牙利命名法
进行了规范化的命名规则,大致的内容如下:
- 变量名的第一个字母为m表示该变量为某个类的成员变量
- 变量名的前两个字母表示数据类型:
p表示指针类型
n表示int类型
b表示bool类型
s表示std::set类型
v表示std::vector类型
l表示std::list类型
KF表示KeyFrame类型
mb表示布尔(bool)型类成员变量;
mv表示向量(vector)型类成员变量;
mpt表示指针(pointer)型类成员变量,并且它是一个线程(thread);
ml表示列表(list)型类成员变量;
mlp表示列表(list)型类成员变量,并且它的元素类型是指针(pointer);
mlb表示列表(list)型类成员变量,并且它的元素类型是布尔型(bool);
2.多线程机制
为何使用多线程机制:
1.在多核处理器中加快运算速度,提高三大线程运行效率;
2.由于系统的随机性,各步骤的运行顺序是不确定的
ORBSLAM2拥有三大进程Tracking、LocalMapping、LoopClosing,
Tracking线程不产生关键帧时,LocalMapping和LocalClosing线程基本处于空转的状态,
而Tracking线程产生关键帧的频率和时间是不确定的,因此需要3个线程同时进行,LocalMapping和
LocalClosing线程不断循环查询Tracking线程是否产生了关键帧,如果产生的话就开始进行处理流程
// Tracking线程主函数
void Tracking::Track()
{
...
// 进行跟踪
...
// Check if we need to insert a new keyframe
// 若跟踪成功,根据条件判定是否会产生关键帧
if(NeedNewKeyFrame())
// 产生关键帧并传给LocalMapping进程
CreateNewKeyFrame();
// 进行跟踪
}
// LocalMapping线程主函数
void LocalMapping::Run()
{
// 死循环
while(1)
{
// Check if there are keyframes in the queue
// 判断是否接收到关键帧
if(CheckNewKeyFrames())
{
// 处理关键帧
// ...
// 将关键帧传给LoopClosing线程
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
// 线程暂停3毫秒,3毫秒结束后再从while(1)首行继续运行
usleep(3000);
}
}
// LoopClosing主函数
void LoopClosing::Run()
{
// 死循环
while(1)
{
// Check if there are keyframes in the queue
// 判断是否接收到关键帧
if(CheckNewKeyFrames())
{
// 处理关键帧
// ...
}
}
// 查看是否有外部线程请求复位当前线程
ResetIfRequested();
// 线程暂停5毫秒,5毫秒结束后再从while(1)首行继续运行
usleep(5000);
}
}
3.多线程中的锁机制
为了防止多个线程同时操作同一变量造成变量调用、赋值混乱,代码中引入了锁机制
首先将成员变量本身设置为私有变量(protected或private),然后在对变量进行操作的共有函数内进行部分加锁,例如:
void MapPoint::UpdateNormalAndDepth()
{
//..............
// 对mMutexFeatures、mMutexPos加锁
{
// unique_lock语句进行加锁,锁的作用域仅限于该语句所在的{}内部,程序运行出{}后锁就被释放了
unique_lock<mutex> lock1(mMutexFeatures);
unique_lock<mutex> lock2(mMutexPos);
if(mbBad)
return;
observations=mObservations;
pRefKF=mpRefKF;
Pos = mWorldPos.clone();
}
//..............
}
需要特别说明的是,在任何代码中对变量进行加锁操作,其作用范围仅为离加锁语句最近的{}内部,可以对加锁区域自行添加{}来限制锁的作用范围。同一把锁在某个时刻只有一个线程能够拿到,然后才能继续向下执行,若其他线程不释放锁则一直等待。
4.ORBSLAM2主类System
System类是ORBSLAM2系统的主类,其构造函数注释如下:
// System类构造函数
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;
//-----------------------------------------------------------------------
// step1:初始化各成员变量
//-----------------------------------------------------------------------
//Check settings file
// step1.1:读取配置文件信息
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;
// step1.2:创建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;
//Create KeyFrame Database
// step1.3:创建关键帧数据库,主要保存ORB描述子倒排索引(即根据描述子查找拥有该描述子的关键帧)
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
//Create the Map
// step1.4:创建地图
mpMap = new Map();
//-----------------------------------------------------------------------
// step2:创建三大线程:Tracking、LocalMapping、LoopClosing
//-----------------------------------------------------------------------
//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)
// step2.1:主线程就是Tracking线程,只需要创建Tracking对象即可,剩下两个线程是Tracking的子线程
// 主线程通过持有两个子线程的指针(mptLocalMapping、mptLoopClosing)来控制子线程
// 编程上是父子关系,但是逻辑上三者并发,不存在相互控制问题
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
//Initialize the Local Mapping thread and launch
// step2.2:创建LocalMapping线程以及mpLocalMapping
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
//Initialize the Loop Closing thread and launch
// step2.3:创建LoopClosing线程以及mpLoopCloser
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);
}
//-----------------------------------------------------------------------
// step3:设置线程间通信
//-----------------------------------------------------------------------
//Set pointers between threads
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);
}