详谈ORB-SLAM2代码运行流程

ORB-SLAM2可谓是经典中的经典，知道代码里有哪些函数，这些函数做了什么还不够，这只能让你读懂代码，只有搞懂这些函数合适被调用，类和类之间如何发生关系才能会改的动代码。

一、ORB-SLAM2代码运行流程

1、项目文件简介

重点关注的文件夹：Examples、include、src

（1）src文件夹是整个ORB-SLAM2的源文件，include是头文件，在src中每个头文件就写了一个类，类的定义在头文件中

在.cc的把头文件引入

#include "xxx.h"

一个头文件对应一个源文件
头文件定义，源码是实现

（2）Examples文件夹是写好的文件，有单目Monocular、RGB-D双目，暂时先不考虑ROS

以RGB-D为例，运行之后生成一个rgbd-tum文件，这个文件可以针对tum数据集，tum中的rgbd数据集，.yaml是配置文件

asspciations是左右目配置文件，因为左右两个相机的帧位是不同的，很多时候不同步，所以需要一个配置文件把这两个大致配准一下，哪个时刻的深度大致对应哪个时刻的RGB，只要帧率足够，他们之间有些许的误差也无关紧要

2、运行官方Demo

以TUM数据集为例，运行Demo的命令：

./Examples/RGB-D/ r gbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml
PATH_TO_SEQUENCE_FOLDER ASSOCIATIONS_FILE

• ./Examples/RGB-D/可执行文件的路径
• 第一个参数：Vocabulary/ORBvoc.txt 磁带：特征点描述值聚类之后的结果，主要在后文加速匹配，文本文件读取较慢，描述值是经过近邻数聚类之后的，在后文里能看到它的作用
• 第二个参数：Examples/RGB-D/TUM1.yamlTUM1.yaml配置文件
• 第三个参数：PATH_TO_SEQUENCE_FOLDER数据集的路径
• 第四个参数：ASSOCIATIONS_FILE左右目配置文件
  

int main(int argc, char **argv) {
    // 判断输入参数个数
    if (argc != 5) {
        cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;
        return 1;
    }

	// step1. 读取图片及左右目关联信息
    vector<string> vstrImageFilenamesRGB;
    vector<string> vstrImageFilenamesD;
    vector<double> vTimestamps;
    string strAssociationFilename = string(argv[4]);
    LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);	
	
    // step2. 检查图片文件及输入文件的一致性
    int nImages = vstrImageFilenamesRGB.size();
    if (vstrImageFilenamesRGB.empty()) {
        cerr << endl << "No images found in provided path." << endl;
        return 1;
    } else if (vstrImageFilenamesD.size() != vstrImageFilenamesRGB.size()) {
        cerr << endl << "Different number of images for rgb and depth." << endl;
        return 1;
    }

	// step3. 创建SLAM对象,它是一个 ORB_SLAM2::System 类型变量
    ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, true);
    
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);
    cv::Mat imRGB, imD;
    // step4. 遍历图片,进行SLAM
    for (int ni = 0; ni < nImages; ni++) {
        // step4.1. 读取图片
        imRGB = cv::imread(string(argv[3]) + "/" + vstrImageFilenamesRGB[ni], CV_LOAD_IMAGE_UNCHANGED);
        imD = cv::imread(string(argv[3]) + "/" + vstrImageFilenamesD[ni], CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];
        // step4.2. 进行SLAM
        SLAM.TrackRGBD(imRGB, imD, tframe);
        // step4.3. 加载下一张图片
        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);
    }

    // step5. 停止SLAM
    SLAM.Shutdown();
}

3、变量命名规则

ORB-SLAM2中的变量遵循一套命名规则：

（1）变量名的首字母为m，表示该变量为某类的成员变量

（2）变量名的首字母非m，表示该变量为函数的局部变量或全局变量，或者是通过define定义的宏

变量名的第一、二个字母表示数据类型

• p表示指针类型
• n表示int类型
• b表示bool类型
• s表示std::set类型，集合
• v表示std::vector类型，动态数组
• l表示std::list类型，列表
• KF表示KeyFrame类型，关键帧（ORB-SLAM 2重要类）

4、多线程

SLAM中大量运用了多线程，多线程可以：

（1）加快运行速度

bool Initializer::Initialize(const Frame &CurrentFrame) {
 	// ...
    thread threadH(&Initializer::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H));
    thread threadF(&Initializer::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F));
    // ...
}

在初始化类中，初始化函数同时开了两个线程，计算两个矩阵，因为在单目相机初始化有可能单应矩阵或基础矩阵来初始化，具体是哪种需要把两个矩阵算出来，看哪个矩阵的表现更好再用哪个矩阵初始化，所以运算上要解决一个超定方程，奇异值分解(SVD)，像是运算密集型的任务，开一个单独的线程，这样在多核处理器上会加快速度

（2）系统的随机性，各步骤的运行顺序是不确定的

ORB-SLAM中有三个线程：Tracking线程、Local Mapping局部建图、Loop Closing回环检测

三大线程之间的关系就是：随机读入某帧，一帧就代表每个时刻的图片，对于双目或者RBG-D就有两张图片， 提取特征点，跟踪运动，所有帧都会参与和跟踪，因为每过一帧SLAM系统就要知道当前的传感器的位姿，但是并不是所有帧都会参与建图，也不是所有帧都会参与回环检测

建图和回环检测是属于比较重的任务，所以只有关键帧（KeyFrame）才能参与，关键帧就是在帧里面比较好的比较有代表性的帧，他们才会参与建图和回环检测，关键帧是从普通帧选取出来的，并不是所有的普通帧都可以成为关键帧，在Tracking线程里的New KeyFrame Decision函数来判断当前帧是不是关键帧， 这个事情本身就有一定的随机性，一个帧能否作用成为关键帧，是否能够参与其他的两个线程，并不仅有帧本身的品质来决定，可能还和当前系统的状态，包括当前系统的跟踪情况来决定。

所以Local Mapping和Loop Closing这两个线程，他们什么时候运行都是不确定的事儿，只有上游的Tracking线程传递的参考帧（KeyFrame关键帧）才能运行

以前的RGB-D都是30帧取一个关键帧或者50帧取一个关键帧，这种定时取关键帧，那可能整个系统就不用多线程了，可能就是Tracking线程里循环30次，之久调用LocalMapping，但是ORB-SLAM不是这样的情况，不知道什么时候就会出现关键帧

代码实现：
①Tracking线程主函数

// Tracking线程主函数
void Tracking::Track() {
	// 进行跟踪
    // ...
	
    // 若跟踪成功,根据条件判定是否产生关键帧
    if (NeedNewKeyFrame())
        // 产生关键帧并将关键帧传给LocalMapping线程
        KeyFrame *pKF = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
        mpLocalMapper->InsertKeyFrame(pKF);	
}

每次来一帧都进行跟踪，跟踪之后决定是否产生关键帧。

②局部建图LocalMapping线程主函数

// LocalMapping线程主函数
void LocalMapping::Run() {
	// 死循环
    while (1) {
        // 判断是否接收到关键帧
        if (CheckNewKeyFrames()) {
            // 处理关键帧
            // ...
            
            // 将关键帧传给LoopClosing线程
            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
        }
        
        // 线程暂停3毫秒,3毫秒结束后再从while(1)循环首部运行
        std::this_thread::sleep_for(std::chrono::milliseconds(3));
    }
}

每次循环判断tracking线程有没有给它关键帧的情况，如果没有直接线程暂停3毫秒，3毫秒之后继续查一遍，如果有关键帧就进行建图等后续操作，如果没有就进入下一轮循环
③回环检测LoopClosing线程主函数

// LoopClosing线程主函数
void LoopClosing::Run() {
    // 死循环
    while (1) {
        // 判断是否接收到关键帧
        if (CheckNewKeyFrames()) {
            // 处理关键帧
            // ...
        }

        // 查看是否有外部线程请求复位当前线程
        ResetIfRequested();

        // 线程暂停5毫秒,5毫秒结束后再从while(1)循环首部运行
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

死循环，每5毫秒检测一次上游线程是否给它关键帧，如果没检测到关键帧就停5毫秒再检测，如果检测到了就进行后面的一系列操作，

这个就是应付这个系统的随机性，这种情况下这三个线程：Tracking、LocalMapping、LoopClosing调用的顺序是完全不可预料的，所以必须使用多线程才能实现，而这几个线程之间沟通的桥梁就是关键帧（KeyFrame）

5、多线程中的锁

关于多个线程要加锁的理解：多个线程运行同一资源，操作同一个变量，这样会造成混乱，就好比地下管道有煤气、下水、电力等，一个部门要对管道维修的时候，它开了那个管道，其他部门就不能动了，因为两个部门同时维修管道容易发生事故。

多线程的原理类似，多个线程操作用一个变量的时候，可能在某个线程这个时刻期望操作的变量暂时值是不变的，如果其他线程改变了这个变量的值，那程序就很有可能会报错或出Bug，所以当一些变量有可能在被多个线程同时操作的时候，这个时候我们都会给它加锁

（1）范例

class KeyFrame {
protected:
	KeyFrame* mpParent;
    
public:
    void KeyFrame::ChangeParent(KeyFrame *pKF) {
        unique_lock<mutex> lockCon(mMutexConnections);		// 加锁
        mpParent = pKF;
        pKF->AddChild(this);
    }

    KeyFrame *KeyFrame::GetParent() {
        unique_lock<mutex> lockCon(mMutexConnections);		// 加锁
        return mpParent;
    }
}

以KeyFrame类为例，这一类里有成员变量mpParent，生成树的头结点，mpParent变量有两个函数同时操作，一个是ChangeParent()，一个是GetParent()。

ChangeParent()给mpParent变量赋值
GetParent()读取mpParent变量

在读取变量的时候肯定是希望mpParent变量不变，读完再改变。所以在两个函数里面加锁，一个线程读，一个线程改，加锁之后只有抢到锁的线程才能执行，没想到锁的线程只能等到抢到锁的线程执行完毕释放锁才能执行，这样就保证了不会造成混乱。

一把锁在某个时刻只有一个线程能够拿到，如果程序执行到某个需要锁的位置，但是锁被别的线程拿着不释放的话，当前线程就会暂停下来；直到其它线程释放了这个锁，当前线程才能拿走锁并继续向下执行

（2）编程范式

需要被很多线程同时使用的变量，本身都会设置成私有变量（private或protected）,在类外部调用不到，只有公有函数使用接口来操作，而这两个接口都是加锁的，这样保证了其他线程一旦想改变变量的值，碰到的都是加锁的情况，不会绕过锁直接改变。

在ORB-SLAM中，很多以mMutex开头的变量，它们都是锁，不同的锁锁住不同的变量。

（3）什么时候加锁和释放锁?

unique_lock<mutex> lockCon(mMutexConnections);

以上代码就是加锁，锁的有效性只在定义了它的大括号{}之内

void KeyFrame::EraseConnection(KeyFrame *pKF) {
    // 第一部分加锁
    {
        unique_lock<mutex> lock(mMutexConnections);
        if (mConnectedKeyFrameWeights.count(pKF)) {
            mConnectedKeyFrameWeights.erase(pKF);
            bUpdate = true;
        }
    }// 程序运行到这里就释放锁,后面的操作不需要抢到锁就能执行
	//第二部分不需要加锁
    UpdateBestCovisibles();
}

以上代码表示，第一部分的大括号为了限制锁的作用域，EraseConnection()函数中有两部分，第一部分是需要加锁的函数{unique_locl ...}，第二部分 UpdateBestCovisibles() ，这样第一部分加大括号，出了大括号锁消失所以第二部分就不需要加锁了，这样设计的目的在于锁的部分越小越好

二、SLAM主类System

eSensor类表示传感器类型：单目相机/双目相机/RBB-D深度相机；

1、三个线程

在局部建图的时候有专门定义表示线程的thread变量，来表示这个局部建图线程，局部建图和回环检测线程都有thread，而追踪器线程是主线程没有用thread变量表示，线程通过持有两个子线程的指针(mptLocalMapping和mptLoopClosing)控制子线程

虽然在编程实现上三大主要线程构成父子关系，SLAM中虽然有代码处理三大主要线程的父子关系，但逻辑上我们认为这三者是并发的，不存在谁控制谁的问题

2、三个主要的公有函数

（1）TrackMonocular()函数：跟踪单目相机,返回相机位姿

（2） TrackStereo()函数：跟踪双目相机,返回相机位姿

（3） TrackRGBD()函数：跟踪RGBD相机,返回相机位姿

每当循环读到一张图片的时候，根据传感器类型调用三个函数，函数内部再调用内部的track和各种函数，可以当做整个SLAM系统入口函数

3、部分重要成员变量

成员变量/函数	访问控制	意义
eSensor mSensor	private	传感器类型单目相机/双目相机/RBB-D深度相机
ORBVocabulary* mpVocabulary	private	ORB字典,保存ORB描述子聚类结果
KeyFrameDatabase* mpKeyFrameDatabase	private	关键帧数据库,保存ORB描述子倒排索引
Map* mpMap	private	地图
Tracking* mpTracker	private	追踪器
LocalMapping* mpLocalMapper	private	局部建图器
std::thread* mptLocalMapping	private	局部建图线程
LoopClosing* mpLoopCloser	private	回环检测器
std::thread* mptLoopClosing	private	回环检测线程
Viewer* mpViewer	private	查看器
FrameDrawer* mpFrameDrawer	private	帧绘制器
MapDrawer* mpMapDrawer	private	地图绘制器
std::thread* mptViewer	private	查看器线程
System(const string &strVocFile, string &strSettingsFile, const eSensor	public	构造函数
cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp)	private	跟踪单目相机,返回相机位姿
cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp)	public	跟踪双目相机,返回相机位姿
cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp)	public	跟踪RGBD相机,返回相机位姿
int mTrackingState	private	追踪状态
std::mutex mMutexState	private	追踪状态锁
bool mbActivateLocalizationMode bool mbDeactivateLocalizationMode std::mutex mMutexMode void ActivateLocalizationMode() void DeactivateLocalizationMode()	private private private public public public	开启/关闭纯定位模式
bool mbReset std::mutex mMutexReset void Reset()	private private public	系统复位
void Shutdown()	public	系统关闭
void SaveTrajectoryTUM(const string &filename) void SaveKeyFrameTrajectoryTUM(const string &filename) void SaveTrajectoryKITTI(const string &filename)	public public public	以TUM/KITTI格式保存相机运动轨迹和关键帧位姿

三、构造函数

每个变量要创建的时候构造的函数，为成员变量、成员函数赋值

1、初始化一系列变量

2、创建三大线程

3、设置线程间通信

通过关键帧进行通信，是以存储的关键帧，KeyFrame的队列的数据结构来通信

本质上就是一个线程在它对应的队列中入队，另一个线程查看，这样就实现了通信

代码实现：

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) {
	
	// step1. 初始化各成员变量
	// step1.1. 读取配置文件信息
    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
	// step1.2. 创建ORB词袋
    mpVocabulary = new ORBVocabulary();
    // step1.3. 创建关键帧数据库,主要保存ORB描述子倒排索引(即根据描述子查找拥有该描述子的关键帧)
	mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
	// step1.4. 创建地图
    mpMap = new Map();

	// step2. 创建3大线程: Tracking、LocalMapping和LoopClosing
    // step2.1. 主线程就是Tracking线程,只需创建Tracking对象即可
	mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
	// step2.2. 创建LocalMapping线程及mpLocalMapper
    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper);
	// step2.3. 创建LoopClosing线程及mpLoopCloser
    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
            
	// step3. 设置线程间通信
	mpTracker->SetLocalMapper(mpLocalMapper);
    mpTracker->SetLoopClosing(mpLoopCloser);
    mpLocalMapper->SetTracker(mpTracker);
    mpLocalMapper->SetLoopCloser(mpLoopCloser);
    mpLoopCloser->SetTracker(mpTracker);
    mpLoopCloser->SetLocalMapper(mpLocalMapper);
}

四、跟踪函数

SLAM系统中的主函数GrabImageMonocular()/GrabImageStereo/GrabImageRGBD函数，
主函数中调用了Tracker类的主函数，通知Tracker对象来抓取单目图像进行分析，Tracker对象会进行跟踪和位姿估计，跟踪一下追踪的状态，追踪的关键帧和地图点。

传感器类型	用于跟踪的成员函数
MONOCULAR	cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp)
STEREO	cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp)
RGBD	cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp)

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp) {
    cv::Mat Tcw = mpTracker->GrabImageMonocular(im, timestamp);
    unique_lock<mutex> lock(mMutexState);
    mTrackingState = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
    return Tcw;
}

此博客参考ncepu_Chen的《5小时让你假装大概看懂ORB-SLAM2源码》