0 引言

ORB-SLAM2是一种基于特征的视觉SLAM（Simultaneous Localization and Mapping）系统，它能够从单个、双目或RBGD相机的输入中实时地同时定位相机的位置，并构建环境的三维地图。ORB-SLAM2是在ORB-SLAM的基础上进行改进和扩展的版本。

本文主要对ORB-SLAM2的整体框架，System主类和多线程进行学习和总结，如有理解错误，欢迎指正交流。

1 整体框架

1.1 整体流程

ORB-SLAM2整体框架如下图，主要流程可以概括为以下几个步骤：

请添加图片描述

特征提取和匹配：ORB-SLAM2首先对输入的图像进行特征提取，通常使用Oriented FAST and Rotated BRIEF (ORB)算法来检测和描述图像中的特征点。然后，它使用特征描述子进行特征匹配，以在连续帧之间建立对应关系。
初始化：初始化阶段是在初始帧上建立初始地图并估计相机的初始位姿。ORB-SLAM2使用基于单目、双目或RGB-D输入的不同方法来进行初始化。在单目或双目情况下，可以使用基于运动的方法或基于平面的方法来估计相机的初始位姿。在RGB-D情况下，可以通过三角测量来估计初始位姿。
跟踪：跟踪阶段是ORB-SLAM2的核心部分，它通过连续图像帧之间的特征匹配和运动估计来实时定位相机。通过追踪特征点的运动，ORB-SLAM2可以估计相机的位姿变化，并通过优化方法来减小累积误差。
局部地图更新：ORB-SLAM2通过局部地图来表示环境的三维结构。在跟踪过程中，它会不断地更新和扩展局部地图，包括添加新的地图点和关键帧。同时，ORB-SLAM2还会执行一些优化步骤，如相机位姿优化、地图点优化等，以提高地图的一致性和准确性。
回环检测：回环检测是为了解决定位漂移和累积误差问题的关键步骤。ORB-SLAM2会在跟踪过程中检测可能的回环，并使用回环检测算法来识别和纠正回环。一旦回环被检测到，ORB-SLAM2会进行全局优化来提高整体的一致性。
闭环优化：闭环优化是在回环检测之后执行的步骤，通过全局优化来进一步提高地图的一致性和准确性。ORB-SLAM2会使用所有的关键帧和地图点进行非线性优化，以减小累积误差并提高整体的位姿和地图质量。
地图管理：ORB-SLAM2会维护一个稠密的局部地图和一个稀疏的全局地图，用于表示环境的三维结构。地图管理模块负责管理和更新地图，包括删除冗余地图点、关键帧的选择和插入、地图点的筛选等。

以上是ORB-SLAM2的主要流程和步骤。通过不断的特征提取、跟踪、地图更新、回环检测和优化，ORB-SLAM2能够实现实时的定位和地图构建，并在大范围和长时间的场景中表现出较好的性能。

也有大佬绘制了更详细的流程图（以mono_tum.cc的运行流程为例，建议下载学习）：
👉 https://www.jianguoyun.com/p/Dc1MEhMQ-9KLBxjM3uED

请添加图片描述
此外，还有大佬已经中文注释了ORB_SLAM2可以参考理解代码：
👉 https://github.com/electech6/ORB_SLAM2_detailed_comments/tree/master

但是在学习以上的核心的主要流程之前，需要先熟悉ORB-SLAM2中的System主类和多线程…

2 System主类

System类是ORB-SLAM2系统的主类，主要代码是头文件ORB_SLAM2/include/System.h和源文件ORB_SLAM2/src/System.cc，分析其主要的成员函数和成员变量。

2.1 成员函数

vscode打开System.cc文件，如下，可以看到成员函数的大纲：

请添加图片描述
具体成员函数的类型和定义如下：

成员函数	类型	定义
`System(const string &strVocFile, string &strSettingsFile, const eSensor sensor, const bool bUseViewer=true)`	`public`	构造`System`函数
`cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp)`	`public`	跟踪双目相机,返回相机位姿
`cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)`	`public`	跟踪RGBD相机,返回相机位姿
`cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp)`	`public`	跟踪单目相机,返回相机位姿
`void ActivateLocalizationMode()`	`public`	开启纯定位模式
`void DeactivateLocalizationMode()`	`public`	关闭纯定位模式
`bool System::MapChanged()`	`public`	检测地图是否有较大变化
`void System::Reset()`	`public`	系统复位
`void System::Shutdown()`	`public`	系统关闭
`void System::SaveTrajectoryTUM(const string &filename)`	`public`	以`TUM`格式保存相机运动轨迹
`void System::SaveKeyFrameTrajectoryTUM(const string &filename)`	`public`	以`TUM`格式保存关键帧位姿
`void System::SaveTrajectoryKITTI(const string &filename)`	`public`	以`KITTI`格式保存相机运动轨迹
`int System::GetTrackingState()`	`public`	获取追踪器状态
`vector<MapPoint*> System::GetTrackedMapPoints()`	`public`	获取追踪到的地图点
`vector<cv::KeyPoint> System::GetTrackedKeyPointsUn()`	`public`	获取追踪到的关键帧的点

2.2 成员变量

主要的成员变量及其定义如下：

成员变量	类型	定义
`eSensor mSensor`	`private`	传感器类型单目相机`MONOCULAR`,双目相机`STEREO`,彩色深度相机`RGBD`
`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`	查看器线程
`int mTrackingState`	`private`	追踪状态
`std::mutex mMutexState`	`private`	追踪状态加锁
`bool mbActivateLocalizationMode`	`private`	开启纯定位模式
`bool mbDeactivateLocalizationMode`	`private`	关闭纯定位模式
`std::mutex mMutexMode`	`private`	纯定位模式加锁
`bool mbReset`	`private`	系统复位
`std::mutex mMutexReset`	`private`	系统复位加锁

都说ORB-SLAM2有三大线程Tracking，LocalMapping和LoopClosing线程，可从成员变量中只定义了LocalMapping和LoopClosing线程，其实Tracking线程就是Syetem类的主线程，构成三大线程，虽然Tracking线程在代码实现上是主线程，但三者的关系其实是并发的。

3 多线程

刚刚学习到ORB-SLAM2中主要有三大线程，其实SLAM项目中一般都会使用多线程，由于某个节点可能同时订阅多个消息，或多个线程函数共享数据，为了防止在多个消息被订阅时发生处理时间过长或阻塞，而导致其他回调函数无法正常使用，也为了防止共享数据时在存储或调用时发生错乱，一般都会使用std::mutex(互斥锁)和std::thread(多线程管理)。

3.1 ORB-SLAM2中的多线程

ORB-SLAM2中三大线程中的Tracking线程产生关键帧的频率和时机不是固定的，三个线程同时运行，方便LocalMapping和LoopClosing线程查询Tracking线程是否产生关键帧。

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

// 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));
    }
}

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

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

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

3.2 加锁

多线程一般都是和锁一起使用，ORB-SLAM2中多线程和互斥锁一起使用，而互斥锁是有范围的，锁的有效性仅限于大括号{}之内，程序运行出大括号之后就释放锁。另外，一把锁一般在某个时刻只有一个线程能够拿到，比如程序执行到某个需要锁的范围，但是锁正在另一个线程，那当前线程就会先停下来，直到其他线程释放这个锁，当前线程才能继续向下运行。

void KeyFrame::EraseConnection(KeyFrame *pKF) {
    // 以下大括号中的代码部分加锁
    {
        unique_lock<mutex> lock(mMutexConnections);
        if (mConnectedKeyFrameWeights.count(pKF)) {
            mConnectedKeyFrameWeights.erase(pKF);
            bUpdate = true;
        }
    }// 程序运行到这里就释放锁,比如下行代码未在加锁范围
	
    UpdateBestCovisibles();
}