ORB-SLAM3代码详解 - 第 01 篇 · 系统总览与三线程架构

ORB-SLAM3代码详解 - 第 01 篇 · 系统总览与三线程架构

本篇建立 ORB-SLAM3 的全局结构视图：数据从何处进入、在哪些线程之间如何流动、

最终存储到何处。后续 17 篇均是对本篇所述各模块的展开。

源码基于 UZ-SLAMLab/ORB_SLAM3 commit 4452a3c，引用行号以此为准。

---

1. ORB-SLAM3 求解的问题

ORB-SLAM3 是一套实时的视觉/视觉惯性 SLAM 系统。给定一段相机图像序列（可选地附加 IMU 测量），它在线完成四项任务：

1. 定位（Localization） ：估计相机每一帧相对世界坐标系的 6 自由度位姿 Tcw∈SE(3)T_{cw}\in SE(3)Tcw∈SE(3)，其中 TcwT_{cw}Tcw 把世界系下的点变换到相机系：Xc=RcwXw+tcwX_c = R_{cw}X_w + t_{cw}Xc=RcwXw+tcw。
2. 建图（Mapping）：以稀疏 3D 地图点（MapPoint）的形式重建场景结构。
3. 回环与重定位（Loop Closing & Relocalization）：消除长时间累积的轨迹漂移；在跟踪丢失后重新确定相机在已有地图中的位置。
4. 多地图（Multi-Map）：跟踪彻底丢失时不丢弃已有成果，而是新建一张子地图继续工作，待检测到与旧地图的重叠区域时再行融合。

相对前作 ORB-SLAM2，ORB-SLAM3 的两项主要扩展：

• 紧耦合视觉惯性（Visual-Inertial）：将 IMU 预积分量作为因子图中的边，与视觉重投影约束在同一次优化中联合求解，而非松耦合地事后融合。
• Atlas 多地图系统：用一组子地图（Map）替代单张全局地图，并配合改进的位置识别（Place Recognition），显著提升丢失恢复与重访场景下的鲁棒性。

论文出处：Campos et al., ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual--Inertial and Multi-Map SLAM, IEEE Transactions on Robotics, 2021。

---

2. 系统骨架：一个入口对象，三套工作线程

System 类是系统对外的唯一入口。应用层代码只需构造 System、逐帧送入图像、最后关闭，典型用法见 Examples/Monocular/mono_euroc.cc：

// 构造：词典路径、配置文件、传感器类型、是否启用可视化
ORB_SLAM3::System SLAM(vocab_file, settings_file, ORB_SLAM3::System::MONOCULAR, true);

for (/* 每一帧 */) {
    Sophus::SE3f Tcw = SLAM.TrackMonocular(image, timestamp);  // 返回当前帧位姿
}

SLAM.Shutdown();                       // 结束所有线程
SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");

System 构造函数（src/System.cc）在初始化阶段完成四件事：

// src/System.cc，节选，commit 4452a3c
// ① 加载预训练 ORB 词典（约 117-127 行）
mpVocabulary = new ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
if (!bVocLoad) { cerr << "Wrong path to vocabulary."; exit(-1); }

// ② 基于词典创建关键帧数据库（约 128 行）
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

// ③ 创建 Atlas，参数 0 表示初始地图编号（约 132 行）
mpAtlas = new Atlas(0);

// ④ 创建三个工作对象，并为其中两个启动独立线程
mpTracker     = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpAtlas, mpKeyFrameDatabase, ...);            // 191 行
mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR ...);  // 195 行
mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run, mpLocalMapper); // 197 行
mpLoopCloser  = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, ...); // 213 行
mptLoopClosing  = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);   // 214 行

此外，构造阶段还会读取配置文件中的若干开关，例如 loopClosing 字段（约 101-105 行）控制是否启用回环线程，System.LoadAtlasFromFile / System.SaveAtlasToFile（约 88-98 行）控制是否从文件加载或保存地图------后者是"预建图 + 纯定位"工作流的基础，详见第 18 篇。

2.1 Tracking 不是独立线程

构造函数中以 new thread(...) 启动的线程只有三个：LocalMapping、LoopClosing、Viewer （Viewer 在 useViewer 为真时创建，约 233 行）。Tracking 没有独立线程 ，它运行在调用 TrackMonocular()／TrackStereo()／TrackRGBD() 的线程内，同步执行。

这一区分直接影响性能分析与调试：

• Tracking 处于应用层调用链上，其耗时直接决定取帧主循环的帧率；
• LocalMapping 与 LoopClosing 是后台线程，通过队列异步消费关键帧。二者变慢只会使关键帧在队列中积压，不会阻塞主循环取帧。

因此，优化实时性应优先分析 Tracking 内部耗时（特征提取、匹配、位姿优化）；而调试期添加日志时，宜放在 LocalMapping／LoopClosing，避免在 Tracking 主循环中引入 I/O 拖慢帧率。

---

3. 三套线程的职责划分

线程	职责	输入	输出	源码
Tracking	逐帧估计位姿、跟踪局部地图、判定并生成关键帧	图像（+IMU）	当前帧位姿、关键帧	Tracking.cc
LocalMapping	维护局部地图、三角化新地图点、局部 BA、剔除冗余关键帧	关键帧	优化后的局部地图	LocalMapping.cc
LoopClosing	检测回环／地图重叠、回环校正与地图融合、触发全局 BA	关键帧	全局一致的地图	LoopClosing.cc

三者构成生产者---消费者链：Tracking 产出关键帧并送入 LocalMapping 的队列；LocalMapping 处理完成后将关键帧送入 LoopClosing 的队列。队列与状态标志位（详见第 8 节）是线程间通信的全部手段------系统未引入任何消息中间件，全部基于互斥锁与布尔标志实现。

---

4. 数据流：单帧图像的处理链路

下图给出单帧图像从进入系统到产生关键帧、再到被回环线程处理的完整链路，括号内标注对应章节：

图像 ─▶ Frame 构造
        ├─ ORBextractor 提取特征点 + 描述子           (第 03 篇)
        ├─ 去畸变 / 按相机模型反投影                    (第 05 篇)
        └─ 特征分配到网格（加速区域查询）              (第 06 篇)
   │
   ▼
Tracking::Track()                                      (第 08 篇)
   ├─ 未初始化：单目用两视图 SfM，双目/RGB-D 用单帧深度 (第 07 篇)
   ├─ 估计初始位姿：恒速模型 / 参考关键帧 / 重定位      (第 08 篇)
   │     ├─ ORBmatcher 建立 2D-3D 匹配               (第 04 篇)
   │     └─ PoseOptimization 优化当前帧位姿            (第 09 篇)
   ├─ 跟踪局部地图：投影更多地图点后再次优化位姿
   └─ NeedNewKeyFrame? → CreateNewKeyFrame            (第 06 篇)
   │
   ▼ (入队)
LocalMapping::Run()                                    (第 10 篇)
   ├─ ProcessNewKeyFrame：更新共视图、计算词袋         (第 11/12 篇)
   ├─ MapPointCulling：剔除低质量地图点
   ├─ CreateNewMapPoints：与共视关键帧三角化新点
   ├─ SearchInNeighbors：融合重复地图点
   ├─ LocalBundleAdjustment：局部光束法平差           (第 09/14 篇)
   ├─ (视觉惯性模式) IMU 初始化 / 惯性优化             (第 16/17 篇)
   └─ KeyFrameCulling：剔除冗余关键帧
   │
   ▼ (入队)
LoopClosing::Run()                                     (第 13 篇)
   ├─ KeyFrameDatabase 检索候选                       (第 11 篇)
   ├─ 计算 Sim3 / SE3 并做几何验证                    (第 13 篇)
   ├─ 同一地图 → 回环校正；不同地图 → 地图融合
   └─ 触发全局 BA

该链路是贯穿全专栏的主线索，后续每一篇均可定位到其中某一段。

---

5. TrackMonocular 内部流程

System::TrackMonocular（src/System.cc 约 399 行）展示了入口函数在调用 Tracking 之前完成的若干系统级动作，值得逐段说明：

Sophus::SE3f System::TrackMonocular(const cv::Mat &im, const double &timestamp,
                                    const vector<IMU::Point>& vImuMeas, string filename)
{
    // ① 关闭检查：系统已 Shutdown 则直接返回空位姿
    { unique_lock<mutex> lock(mMutexReset);
      if (mbShutDown) return Sophus::SE3f(); }

    // ② 传感器类型校验：与构造时声明的类型必须一致
    if (mSensor != MONOCULAR && mSensor != IMU_MONOCULAR) {
        cerr << "ERROR: you called TrackMonocular but input sensor was not Monocular..."; exit(-1);
    }

    // ③ 按配置缩放输入图像（对应第 02 篇的 newWidth/newHeight）
    cv::Mat imToFeed = im.clone();
    if (settings_ && settings_->needToResize()) {
        cv::resize(im, imToFeed, settings_->newImSize());
    }

    // ④ 模式切换：进入/退出纯定位模式
    { unique_lock<mutex> lock(mMutexMode);
      if (mbActivateLocalizationMode) {
          mpLocalMapper->RequestStop();
          while (!mpLocalMapper->isStopped()) usleep(1000);  // 等建图线程停稳
          mpTracker->InformOnlyTracking(true);               // 只跟踪、不建图
          mbActivateLocalizationMode = false;
      }
      if (mbDeactivateLocalizationMode) {
          mpTracker->InformOnlyTracking(false);
          mpLocalMapper->Release();                          // 恢复建图
          mbDeactivateLocalizationMode = false;
      } }

    // ⑤ 复位检查：整体复位或仅复位当前活跃地图
    { unique_lock<mutex> lock(mMutexReset);
      if (mbReset) { mpTracker->Reset(); mbReset = false; mbResetActiveMap = false; }
      else if (mbResetActiveMap) { mpTracker->ResetActiveMap(); mbResetActiveMap = false; } }

    // ⑥ 视觉惯性模式：把本帧之前的 IMU 测量送入 Tracking 缓存
    if (mSensor == System::IMU_MONOCULAR)
        for (size_t i = 0; i < vImuMeas.size(); i++)
            mpTracker->GrabImuData(vImuMeas[i]);

    // ⑦ 真正进入跟踪
    Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed, timestamp, filename);

    // ⑧ 记录跟踪状态供外部查询
    unique_lock<mutex> lock2(mMutexState);
    mTrackingState    = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    return Tcw;
}

几点值得注意：

• 纯定位模式（步骤 ④） ：通过 ActivateLocalizationMode() 设置标志后，下一帧进入时会停止 LocalMapping，并令 Tracking 只做定位、不再扩充地图。该模式配合预加载地图（第 18 篇）即可在固定环境中长期稳定定位，避免地图无限增长。
• 复位粒度（步骤 ⑤） ：Reset 清空整个 Atlas，ResetActiveMap 仅清空当前活跃子地图------后者是多地图机制下更细的恢复手段。
• IMU 注入（步骤 ⑥） ：视觉惯性模式下，相邻两帧图像之间的 IMU 测量随当前帧一并传入，Tracking 据此完成预积分（第 15 篇）。TrackStereo／TrackRGBD（约 244／328 行）的结构与之同构，仅图像输入形式不同。

---

6. 全局共享状态：Atlas 与 Vocabulary

三套线程之间共享两个全局对象。

6.1 Atlas（多地图容器）------ src/Atlas.cc

Atlas 持有一组子地图 Map 与全部相机模型对象（针孔、鱼眼），其中始终有且仅有一张被标记为活跃地图（active map）。所有新关键帧、新地图点都加入活跃地图。

当 Tracking 判定彻底丢失且短期内无法重定位时，并不清空已有地图，而是冻结当前活跃地图、在 Atlas 中新建一张空地图并继续工作（对应 Tracking::CreateMapInAtlas，第 08 篇）。此后若 LoopClosing 检测到新地图与某张非活跃地图存在重叠区域，则通过 Sim3 变换将二者融合（第 13 篇）。这一机制使系统在反复进出同一场景或剧烈运动导致短时丢失时，不必从零重建。Atlas 的结构、序列化与重用详见第 18 篇。

6.2 ORBVocabulary（词袋词典）------ DBoW2

Vocabulary/ORBvoc.txt 是一棵离线训练的视觉词汇树。它把单帧图像的数百个 ORB 描述子量化为一个词袋向量（BoW Vector），从而将"两幅图像是否相似"转化为一次低维向量比较。回环检测、重定位、地图融合的候选筛选均依赖于此。词典结构、TF-IDF 加权与倒排索引详见第 11 篇。

---

7. 传感器配置

System 用枚举 eSensor（include/System.h 88-93 行）区分六种输入类型：

enum eSensor {
    MONOCULAR    = 0,   // 单目
    STEREO       = 1,   // 双目
    RGBD         = 2,   // RGB-D
    IMU_MONOCULAR= 3,   // 单目 + IMU
    IMU_STEREO   = 4,   // 双目 + IMU
    IMU_RGBD     = 5    // RGB-D + IMU
};

该枚举渗透到几乎每个模块的分支判断中，决定初始化方式、是否估计尺度、是否引入惯性约束等。源码中频繁出现的 mSensor==MONOCULAR || mSensor==IMU_MONOCULAR 一类判断，本质是在区分"是否具有真实尺度"与"是否含 IMU"两个维度。

配置	真实尺度	初始化方式	特性
单目	不可观（尺度自由）	两视图 SfM（第 07 篇）	硬件成本最低，需足够运动激励
双目 / RGB-D	直接可得	单帧即可恢复深度	尺度无歧义
+IMU	VI 初始化后可观	视觉先行，再估尺度／重力／零偏	抗快速运动，鲁棒性更高

尺度不可观是单目的固有性质：仅凭单目图像无法区分"小场景近距离"与"大场景远距离"。这一点在第 07 篇（单目初始化）与第 16 篇（视觉惯性初始化恢复尺度）中均有体现。

---

8. 线程间通信：锁与标志位

ORB-SLAM3 的并发模型基于互斥锁与布尔标志，未使用更高层的并发框架。理解这一点是阅读各 Run() 函数的前提。

• 数据保护 ：被多线程读写的共享数据结构各自持有互斥锁。例如关键帧位姿由 KeyFrame::mMutexPose 保护、地图整体由 Map::mMutexMap 保护、地图点位置由 MapPoint::mMutexPos 保护。二次开发中若新增对共享数据的访问而遗漏加锁，可能引发偶发崩溃或数据竞争。各数据结构对应的锁将在第 06 篇展开。
• 控制信号 ：线程的暂停、复位、终止通过布尔标志传递，典型如 LocalMapping 的 mbStopRequested／mbStopped、回环触发时用于打断局部 BA 的 mbAbortBA，以及 System 中的 mbReset／mbShutDown。阅读 Run() 时，先定位这些标志即可快速厘清线程的状态机。

---

9. 目录结构

ORB_SLAM3/
├── include/       # 头文件（类声明）
├── src/           # 实现，核心约 29900 行
│   ├── System.cc          # 系统入口（本篇）
│   ├── Tracking.cc        # 跟踪（第 08 篇）
│   ├── LocalMapping.cc    # 局部建图（第 10 篇）
│   ├── LoopClosing.cc     # 回环（第 13 篇）
│   ├── Optimizer.cc       # 各类 BA 与位姿图优化（第 09/14/16/17 篇）
│   ├── ORBextractor.cc    # 特征提取（第 03 篇）
│   ├── ORBmatcher.cc      # 特征匹配（第 04 篇）
│   ├── ImuTypes.cc        # IMU 预积分（第 15 篇）
│   └── CameraModels/      # 相机模型（第 05 篇）
├── Thirdparty/    # DBoW2 / g2o / Sophus（第 02 篇）
├── Vocabulary/    # 预训练词袋 ORBvoc.txt
└── Examples/      # 各传感器配置的运行示例（第 02 篇）

Optimizer.cc（约 5590 行）与 Tracking.cc（约 4126 行）是体量最大的两个文件，分别承载全部优化逻辑与跟踪状态机，是后续多篇的重点。

---

10. 工程提示

• 线程模型：Tracking 同步运行于调用线程，LocalMapping／LoopClosing／Viewer 为独立线程。实时性瓶颈定位优先看 Tracking，日志优先加在后台线程。
• 加锁顺序 ：多个锁嵌套获取时需保持一致的获取顺序，否则可能死锁。MapPoint、KeyFrame 中存在 static std::mutex mGlobalMutex 用于跨对象的全局同步，修改相关逻辑时需特别留意。
• 配置开关 ：loopClosing 关闭后系统退化为纯里程计 + 局部建图，可用于隔离测试回环对精度的贡献；System.LoadAtlasFromFile／SaveAtlasToFile 控制地图持久化。
• 状态查询 ：System 通过 mTrackingState（受 mMutexState 保护）对外暴露当前跟踪状态，可在应用层据此判断系统是否丢失、是否完成初始化。

---

11. 本篇小结 & 下一篇

本篇要点：

1. ORB-SLAM3 由一个入口对象 System 与三套工作逻辑（Tracking／LocalMapping／LoopClosing）构成，其中仅 LocalMapping、LoopClosing、Viewer 为独立线程，Tracking 同步运行于调用线程。
2. 单帧图像的处理链路为：Frame 构造 → Tracking 估位姿与生成关键帧 → LocalMapping 扩充并优化局部地图 → LoopClosing 全局校正与地图融合。三者以队列串联。
3. Atlas 提供多地图能力，Vocabulary 提供位置识别能力，二者为全局共享状态。
4. 系统支持六种传感器配置，差异集中在尺度是否可观与是否含惯性约束。
5. 并发完全基于互斥锁与布尔标志，阅读各线程主循环应先定位其标志位与状态机。

第 02 篇将完成环境搭建与运行验证：从源码编译、跑通 EuRoC 数据集，到对照 CMakeLists.txt 厘清依赖关系，为后续逐模块的源码分析提供可运行、可调试的实验环境。