K2E-B-S2-2 · Paper Note

ORB-SLAM2

Created 2021-07-06Updated 2026-06-11slam / papers / s-slam-systems / s2-orb-slam
  • Description：ORB-SLAM2 论文与源码笔记 — 特征点法 SLAM 标杆，单目/双目/RGB-D 统一框架;三线程架构、近/远点分类、motion-only/local/full BA 残差体系、共视图/本质图、DBoW2 回环;含 KITTI/EuRoC/TUM 完整结果与源码解析
  • My Notion Note ID：K2E-B-S2-2
  • Created：2021-07-06
  • Updated：2026-06-11
  • License：转载欢迎：转载请注明作者 Yu Zhang 并附原文出处（yuzhang.io）

Table of Contents

1. Summary

Title: ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras Authors: Raúl Mur-Artal, Juan D. Tardós Paper: arXiv:1610.06475 (IEEE T-RO 33(5), 2017) Github: raulmur/ORB_SLAM2

ORB-SLAM2 把初代单目 ORB-SLAM 扩展为单目 / 双目 / RGB-D 统一框架:输入预处理层把三种传感器统一成"立体关键点 + 单目关键点"两类观测，之后整个系统(跟踪、建图、回环)对传感器类型完全透明。全程稀疏特征 + BA，CPU 实时，不需要 GPU。

核心设计:三个并行线程(跟踪 / 局部建图 / 闭环)+ 闭环后第四线程做全局 BA;近点/远点分类(深度 < 40 倍基线为近点)决定一帧能否直接三角化;双目/RGB-D 下尺度可观，回环几何校验与位姿图从单目的 Sim(3) 退化为 SE(3) 刚体变换，没有尺度漂移。

结果上:KITTI 双目大多数序列相对平移误差 < 1%，作者称当时 KITTI 上最佳双目方案;EuRoC 双目 RMSE 厘米级;TUM RGB-D 上以纯稀疏 BA 胜过 ElasticFusion / Kintinuous / DVO-SLAM 等稠密/直接法 — 论文的论点:BA 比直接法或 ICP 更准，而且更省算力。另有轻量仅定位模式(已知地图下纯跟踪)支持长期复用地图。

2. Key Contributions

  • 首个面向单目/双目/RGB-D 的开源 SLAM 系统，含回环、重定位、地图复用
  • RGB-D 用 BA 而非 ICP/光度深度误差，精度更高且 CPU 即可实时
  • 近/远双目点分类 + 利用单目观测:远点多视角三角化只约束旋转，近点单帧三角化给出尺度/平移
  • 轻量仅定位模式:利用 VO 匹配 + 地图点匹配，在已建图区域零漂移定位(建图/回环线程停用)
  • 双目/RGB-D 下回环用 SE(3) 位姿图(尺度可观，不再需要 Sim(3))

3. Method

3.1 三线程 + 第四线程架构

ORB-SLAM2 系统架构 — 跟踪 / 局部建图 / 闭环三线程，闭环后启动第四线程做全局 BA (论文 Fig. 2a)

  • 跟踪线程:每帧提 ORB → 双目/深度匹配 → motion-only BA 求位姿 → 局部地图跟踪 → 关键帧判定;跟丢则 DBoW2 重定位
  • 局部建图线程:关键帧插入(共视图 + 生成树)→ 地图点剔除 → 三角化新点 → 邻域融合 → local BA → 冗余关键帧剔除
  • 闭环线程:DBoW2 查询候选 → 几何校验(双目/RGB-D 用 SE(3))→ 闭环融合 → 本质图位姿图优化 → 启动第四线程
  • 全局 BA 线程:闭环后异步全量优化;若期间检测到新回环则中止重启;结束后沿生成树把修正传播到 BA 期间新插入的关键帧

3.2 输入预处理 — 三种传感器统一

输入预处理 — 双目做左右 ORB 匹配，RGB-D 由深度合成虚拟右目坐标，统一输出立体/单目关键点 (论文 Fig. 2b)

  • 双目:左右校正图各提 ORB，左目特征沿水平极线在右目找匹配(块相关亚像素精确定位)→ 立体关键点 xs=(uL,vL,uR)x_s = (u_L, v_L, u_R)
  • RGB-D:RGB 图提 ORB，由深度 dd 合成虚拟右目坐标:
uR=uL−fx bd(1)u_R = u_L - \frac{f_x\, b}{d} \tag{1}

符号:uLu_L 为特征在 RGB(左)图的横坐标，uRu_R 为合成的虚拟右目横坐标;fxf_x 为水平焦距，dd 为传感器深度值;bb 为结构光投射器与红外相机的基线(Kinect/Asus Xtion 约取 8 cm)。之后 RGB-D 与双目在系统内完全等价。

  • 单目关键点 xm=(uL,vL)x_m = (u_L, v_L):无立体匹配或深度无效的特征 — 只能多视角三角化，不提供尺度

近点 / 远点:深度 < 40× 基线 → 近点(单帧即可三角化，约束尺度/平移/旋转);否则远点(需多视角，主要约束旋转)。阈值 40 来自 Paz et al. 的经验结论。

KITTI 01 高速场景的近点 (绿) 与远点 (蓝)：远处特征只约束旋转，平移要靠足够的近点 (论文 Fig. 3)

系统自举:双目/RGB-D 首帧直接建关键帧 + 由全部立体点出图 — 不需要单目那套两帧 H/F 初始化。

3.3 BA 残差体系

三个层级共用同一重投影残差，只在优化变量范围上不同。

Motion-only BA(跟踪线程，每帧):只优化当前帧 {R,t}\{R, t\}，地图点固定:

{R,t}=argmin⁡R,t∑i∈Xρ ⁣(∥x(⋅)i−π(⋅) ⁣(RXi+t)∥Σ2)(2)\{R, t\} = \operatorname*{argmin}_{R, t} \sum_{i \in \mathcal{X}} \rho\!\left(\left\| x^{i}_{(\cdot)} - \pi_{(\cdot)}\!\left(R X^{i} + t\right)\right\|^{2}_{\Sigma}\right) \tag{2}

符号:R∈SO(3),t∈R3R \in SO(3), t \in \mathbb{R}^3 为相机位姿;XiX^i 为世界系 3D 地图点，xix^i 为其观测(单目 2 维 / 立体 3 维);X\mathcal{X} 为本帧全部 3D-2D 匹配;ρ\rho 为 Huber 鲁棒核，Σ\Sigma 按关键点金字塔层级取协方差。投影 π\pi 按单目/立体二选一:

πm ⁣(XYZ)=(fxXZ+cxfyYZ+cy),πs ⁣(XYZ)=(fxXZ+cxfyYZ+cyfxX−bZ+cx)(3)\pi_m\!\begin{pmatrix}X\\Y\\Z\end{pmatrix} = \begin{pmatrix} f_x \frac{X}{Z} + c_x \\ f_y \frac{Y}{Z} + c_y \end{pmatrix}, \qquad \pi_s\!\begin{pmatrix}X\\Y\\Z\end{pmatrix} = \begin{pmatrix} f_x \frac{X}{Z} + c_x \\ f_y \frac{Y}{Z} + c_y \\ f_x \frac{X - b}{Z} + c_x \end{pmatrix} \tag{3}

符号:(X,Y,Z)(X, Y, Z) 为相机系 3D 点;fx,fyf_x, f_y 为焦距，(cx,cy)(c_x, c_y) 为主点，bb 为双目基线;πm\pi_m 输出 2 维像素坐标，πs\pi_s 额外输出虚拟右目横坐标(第 3 维)。立体观测是 3 维(uL,vL,uRu_L, v_L, u_R)— 这也是后面卡方阈值单目 5.991(2 自由度)、立体 7.815(3 自由度)的来源。

Local BA(局部建图线程):优化共视窗口 KL\mathcal{K}_L 内关键帧与其全部地图点 PL\mathcal{P}_L;能观测这些点但不在窗口内的关键帧 KF\mathcal{K}_F 参与残差但固定不动:

{Xi,Rl,tl}=argmin⁡∑k∈KL∪KF∑j∈Xkρ ⁣(Ekj),Ekj=∥x(⋅)j−π(⋅) ⁣(RkXj+tk)∥Σ2(4)\{X^i, R_l, t_l\} = \operatorname*{argmin} \sum_{k \in \mathcal{K}_L \cup \mathcal{K}_F} \sum_{j \in \mathcal{X}_k} \rho\!\left(E_{kj}\right), \quad E_{kj} = \left\| x^{j}_{(\cdot)} - \pi_{(\cdot)}\!\left(R_k X^j + t_k\right)\right\|^{2}_{\Sigma} \tag{4}

符号:KL\mathcal{K}_L 为当前关键帧的共视窗口(待优化)，PL\mathcal{P}_L 为窗口内观测到的全部地图点(待优化);KF\mathcal{K}_F 为窗口外但能观测 PL\mathcal{P}_L 的关键帧(固定);Xk\mathcal{X}_k 为关键帧 kk 与 PL\mathcal{P}_L 的匹配集;EkjE_{kj} 与式 (2) 同形。

Full BA(闭环后):式 (4) 的特例 — 所有关键帧和点都优化，仅固定原点关键帧消除规范自由度。

3.4 共视图 / 生成树 / 本质图 + DBoW2

  • 共视图:关键帧为节点，共视地图点 ≥ 15 连边，权重 = 共视点数 — 局部窗口与局部地图都从这取
  • 生成树:每帧只连共视最高的父关键帧 — 全局 BA 后修正沿它传播
  • 本质图:生成树边 + 闭环边 + 强共视边(θmin=100\theta_{min} = 100 共视点)— 位姿图优化在这上面做，比全共视图稀疏得多
  • DBoW2 词袋(ORB 二进制描述子 + TF-IDF):重定位 + 闭环检测两用;闭环候选需连续 3 次共视一致性确认

3.5 闭环 — SE(3) 替代 Sim(3)

单目 ORB-SLAM 闭环要解 Sim(3)(7 DoF，吸收尺度漂移)。ORB-SLAM2 双目/RGB-D 下尺度可观，几何校验与位姿图退化为 SE(3) 刚体变换 — 论文原话:不再需要处理尺度漂移。单目模式下仍走 Sim(3)(公式体系在初代论文中，相似变换见 几何变换层级 §3):

S=[sRt01],S−1=[1sRT−1sRTt01]S = \begin{bmatrix} sR & t \\ 0 & 1 \end{bmatrix}, \qquad S^{-1} = \begin{bmatrix} \tfrac{1}{s}R^T & -\tfrac{1}{s}R^T t \\ 0 & 1 \end{bmatrix}

符号:ss 为尺度因子，RR 为旋转，tt 为平移 — Sim(3) 比 SE(3) 多出的正是尺度这 1 个自由度。

本质图位姿图优化（最小化所有边的相对位姿误差）：

min⁡{Tk}∑(i,j)∈E∥log⁡ ⁣(T^ij⋅Tj−1Ti)∥2\min_{\{T_k\}} \sum_{(i,j)\in\mathcal{E}} \bigl\|\log\!\bigl(\hat{T}_{ij}\cdot T_j^{-1} T_i\bigr)\bigr\|^2

T^ij\hat{T}_{ij}：约束的帧间相对变换（闭环边或共视边测量，相机坐标系 j→ij\to i）；Ti,TjT_i, T_j：待优化关键帧相机到世界位姿；log⁡(⋅)\log(\cdot)：SE(3)/Sim(3) 群到李代数的对数映射；双目/RGB-D 用 SE(3)（6 维），单目用 Sim(3)（7 维，含尺度）；g2o 求解。

闭环流程:候选检测(BoW 得分 > 共视帧最低分 smins_{min}，连续性检查)→ BoW 粗匹配 ≥ 20 对才进 Sim3/SE3 RANSAC → 引导投影匹配加边(总匹配 ≥ 40 接受)→ 闭环融合(重复地图点合并)→ 本质图优化 → 全局 BA。

3.6 关键帧策略

  • 插入(比初代更激进，"插得快、删得狠"):除常规条件外新增双目/RGB-D 条件 — 跟踪的近点 < τt=100\tau_t = 100 且本帧可新建近点 ≥ τc=70\tau_c = 70 → 立即插入。KITTI 01 这类高速远景序列全靠它保住平移精度
  • 剔除:某关键帧 ≥ 90% 的地图点能被至少 3 个其他关键帧在相同或更精细金字塔层级观测时，判为冗余并删除

3.7 仅定位模式

建图与闭环线程停用，跟踪线程混用两类匹配:

  • VO 匹配:当前 ORB ↔ 上一帧由立体/深度新建的临时 3D 点 — 未建图区域也能跟，但会漂移
  • 地图点匹配:当前 ORB ↔ 既有地图 — 零漂移定位

环境无显著变化时可长期复用地图，跟丢用 DBoW2 重定位。

4. Experiments & Results

评测设置:Intel i7-4790 + 16GB，每序列跑 5 次取中位数(补偿多线程不确定性)。

4.1 KITTI 双目(论文 Table I，节选)

指标:相对平移 trelt_{rel} (%) / 相对旋转 rrelr_{rel} (°/100m) / 绝对平移 RMSE tabst_{abs} (m)，对比 Stereo LSD-SLAM（直接法半稠密双目 SLAM，最小化光度误差，Engel 2015）:

Seq ORB-SLAM2 trelt_{rel} rrelr_{rel} tabst_{abs} LSD trelt_{rel} LSD tabst_{abs}
00 0.70 0.25 1.3 0.63 1.0
01 (高速) 1.39 0.21 10.4 2.36 9.0
05 0.40 0.16 0.8 0.64 1.5
08 (无回环) 1.05 0.32 3.6 1.11 3.9
09 0.87 0.27 3.2 1.14 5.6
  • 多数序列 trel<1%t_{rel} < 1\%，11 条序列大面积优于 Stereo LSD-SLAM
  • 01 高速公路:近点稀缺 → 平移 1.39% 偏弱但旋转 0.21°/100m 仍准(远点撑住旋转);单目版在此序列直接失败，双目版能跑：双目消除尺度漂移的直接证据(KITTI 08 对比图见论文 Fig. 5)
  • 回环检出:00/02/05/06/07;09 的回环只在末尾几帧，漏检

4.2 EuRoC 双目(论文 Table II，节选)

绝对平移 RMSE (m):

Sequence ORB-SLAM2 Stereo LSD-SLAM
V1_01_easy 0.035 0.066
V1_02_medium 0.020 0.074
V1_03_difficult 0.048 0.089
MH_04_difficult 0.119 —
V2_03_difficult ✗ 跟丢 —

厘米级精度;V2_03 因剧烈运动模糊跟丢(论文注:需 IMU，即后来 ORB-SLAM3 的方向)。

4.3 TUM RGB-D(论文 Table III，节选)

绝对平移 RMSE (m)，对比稠密/直接法（ElasticFusion：GPU 稠密面元地图 + 无回环 ICP；Kintinuous：KinectFusion 滚动体素稠密重建；DVO-SLAM：直接视觉里程计 + 图优化）:

Sequence ORB-SLAM2 ElasticFusion Kintinuous DVO-SLAM
fr1/desk 0.016 0.020 0.037 0.021
fr2/desk 0.009 0.071 0.034 0.017
fr2/xyz 0.004 0.011 0.029 0.018
fr3/office 0.010 0.017 0.030 0.035

稀疏 BA 全面胜过稠密方案且不需要 GPU。fr2 序列深度图有 4% 尺度偏差，已补偿。ORB-SLAM2 本身不做深度融合 — 稠密点云仅由关键帧位姿反投影深度图可视化:

由估计位姿反投影深度图得到的稠密点云 — fr3/office、fr1/room、fr2/desk、fr3/nst (论文 Fig. 7)

4.4 耗时(论文 Table IV，节选)

线程/任务 EuRoC V2_02 (双目 20Hz， 1200 特征) KITTI 07 (双目 10Hz， 2000 特征) TUM fr3 (RGB-D 30Hz， 1000 特征)
跟踪总耗时 41.7 ms 49.5 ms 25.6 ms
其中 ORB 提取 13.5 24.8 11.5
Local BA 138 ms 69 ms 197 ms
本质图优化 (单次) 73 ms 178 ms 282 ms
Full BA (单次) 349 ms 1144 ms 1641 ms

跟踪均满足实时(< 帧间隔);Local BA 在独立线程不阻塞跟踪;Full BA 秒级但异步。注意 fr3_office 共视图比 KITTI 07 更稠密(室内反复观测)，回环融合/BA 反而更贵 — 耗时由图的连接密度而非关键帧数决定。

5. Ablation & Discussion

论文没有正式消融实验：无"去掉近/远点分类/共视图"的对照表。可讨论的设计依据:

  • 为什么 BA 赢了直接法/ICP:作者论点 — 稀疏特征 + 多视角 BA 在精度上优于光度/深度稠密对齐，且计算便宜得多(全 CPU);TUM Table III 是直接证据
  • 近/远点分类为什么重要:KITTI 01 显示远点只约束旋转;没有 τt/τc\tau_t/\tau_c 激进插帧，高速场景平移会崩
  • SE(3) vs Sim(3):尺度可观时少 1 个自由度，位姿图更稳 — 但这是结构性结论，论文没有定量对比
  • 局限场景:剧烈运动模糊(V2_03)、纹理缺失场景 — 特征点法的固有边界

6. Strengths / Limitations / Future Work

Strengths:

  • 单目/双目/RGB-D 一套架构，输入预处理后传感器透明
  • CPU 实时 + 高精度，三大数据集当时 SOTA
  • 完整系统:回环、重定位、地图复用、仅定位模式 — 工程参考价值极高

Limitations:

  • 特征点法边界:运动模糊 / 弱纹理跟丢(V2_03 失败)
  • 仅稀疏地图，无稠密融合(ElasticFusion 在个别结构化场景反超，如 fr3/nst)
  • 仅定位模式要求环境无显著变化
  • 无地图序列化 — 地图只活在内存里(官方代码无 Save/Load)

Future Work(论文原文):非重叠多相机、鱼眼/全景支持、大规模稠密融合、协同建图、运动模糊鲁棒性：其中 IMU 与多地图由 ORB-SLAM3(2021)实现。

7. Code Walkthrough

官方 C++ 实现，依赖 OpenCV / Eigen3 / Pangolin / g2o（通用图优化库，用稀疏非线性最小二乘求解位姿/地图点）/ DBoW2(后两者 vendored 在 Thirdparty/)。

7.1 结构与线程

文件 职责
System.cc 构造并接线全部子系统;暴露 TrackStereo/RGBD/Monocular
Tracking.cc 每帧位姿估计状态机(调用线程内运行)
LocalMapping.cc 关键帧处理、三角化、Local BA(独立线程)
LoopClosing.cc 回环检测、Sim3/SE3、位姿图(独立线程)
ORBextractor.cc 金字塔 + FAST + 四叉树均匀化
Optimizer.cc g2o 封装:PoseOptimization / LocalBA / EssentialGraph / Sim3 / GlobalBA
KeyFrame.cc / MapPoint.cc / Map.cc 共视图、生成树、观测管理

System 构造:加载词典 → 建 Tracking(主线程)→ new thread(&LocalMapping::Run, ...) → new thread(&LoopClosing::Run, ...);线程间用互斥队列传关键帧，后台线程 usleep 自旋。

7.2 跟踪状态机

// Tracking::Track() 核心分发
if(mState==OK) {
    CheckReplacedInLastFrame();
    if(mVelocity.empty() || mCurrentFrame.mnId < mnLastRelocFrameId+2)
        bOK = TrackReferenceKeyFrame();   // BoW 匹配参考关键帧
    else {
        bOK = TrackWithMotionModel();     // 恒速模型投影匹配 (首选)
        if(!bOK) bOK = TrackReferenceKeyFrame();
    }
} else {
    bOK = Relocalization();               // DBoW2 候选 + EPnP RANSAC
}
if(bOK) bOK = TrackLocalMap();            // 局部地图投影 + 再优化
  • 恒速模型优先;重定位后两帧内速度不可信，退回 BoW 匹配
  • Relocalization:候选帧 ≥ 15 BoW 匹配后进 PnPsolver(EPnP RANSAC，算法见 PnP §4)，位姿优化 ≥ 50 内点确认
  • TrackLocalMap:共视关键帧 + 父子帧组成局部地图，投影筛点(视深/视角/尺度)后再做一次 motion-only BA

7.3 PoseOptimization — 4 轮卡方筛选

const float chi2Mono[4]   = {5.991, 5.991, 5.991, 5.991};   // χ²(2) 95%
const float chi2Stereo[4] = {7.815, 7.815, 7.815, 7.815};   // χ²(3) 95%
for(size_t it=0; it<4; it++) {
    optimizer.optimize(10);
    for (每条边) {
        if(chi2 > th) { e->setLevel(1); outlier = true; }   // 踢出但不删除
        else          { e->setLevel(0); outlier = false; }  // 回到阈内可复活
        if(it==2) e->setRobustKernel(0);                    // 第 3 轮后撤掉 Huber
    }
}

工程要点:外点用 setLevel(1) 非破坏性禁用，后续轮次若回到阈值内会被重新启用 — 论文只写"鲁棒优化"，这套 4×10 轮 + 复活机制全在代码里。

7.4 ORB 提取 — 四叉树均匀化

  • 金字塔 8 层 × 1.2 倍，各层特征数按几何级数分配
  • 每层 30×30 px 网格跑 FAST(iniThFAST=20，失败降到 minThFAST=7)
  • DistributeOctTree:不断四分裂节点直到节点数达标，每叶保留响应最大的点 → 空间均匀分布(名字叫 OctTree，实为 2D 四叉树)
  • 单目初始化阶段用 2× 特征数的专用提取器提高配对成功率(论文未提)

7.5 立体深度与近/远阈值

// Frame::ComputeStereoMatches —— 视差转深度
float disparity = uL - bestuR;
mvDepth[iL] = mbf / disparity;        // depth = fx·b / d
// RGB-D:虚拟右目  mvuRight[i] = u - mbf/d   (即式 (1))

ThDepth 从 yaml 读入，近/远边界 = mbf/fx × ThDepth(≈ ThDepth 倍基线)。

7.6 关键 yaml 参数

参数 KITTI00-02 EuRoC (双目) TUM1 (RGB-D)
nFeatures 2000 1200 1000
scaleFactor / nLevels 1.2 / 8 1.2 / 8 1.2 / 8
iniThFAST / minThFAST 20 / 7 20 / 7 20 / 7
ThDepth 35 35 40
Camera.fps 10 20 30
DepthMapFactor — — 5000 (uint16 → m)

7.7 Paper-vs-Code 差异

# 差异 说明
1 近/远阈值 论文写 40× 基线;KITTI yaml 实际 ThDepth=35
2 4 轮卡方 + 外点复活 论文只说鲁棒优化，机制全在代码
3 四叉树均匀化 论文只说"均匀分布"，DistributeOctTree 未描述
4 单目初始化 2× 特征 论文未提
5 仅定位模式实现 mbOnlyTracking + mbVO 回退逻辑，论文只有一段描述
6 无地图保存 论文讲"地图复用"，官方代码无序列化(社区 fork 补)
7 GlobalBundleAdjustemnt 原版代码函数名拼写错误(少个 e)，非 fork 引入

References

资源 链接
ORB-SLAM2 论文 arXiv:1610.06475, IEEE T-RO 2017
官方代码 raulmur/ORB_SLAM2
初代 ORB-SLAM (单目， Sim3/共视图体系) Mur-Artal, Montiel & Tardós, IEEE T-RO 2015, arXiv:1502.00956
ORB-SLAM3 (+IMU/多地图) Campos et al., IEEE T-RO 2021
DBoW2 Gálvez-López & Tardós, IEEE T-RO 2012

相关笔记:ORB 特征见 特征描述子;EPnP 见 PnP;三角化见 三角化;Sim3 与尺度漂移见 几何变换层级;与 VINS-Mono(加 IMU 的优化派 VIO)、DSO(直接法)对照阅读。