Skip to content

Algorithm_Theory

Jump to bottom
ZUXTUO edited this page May 21, 2026 · 5 revisions

核心算法原理 (Core Algorithm Theory)

本文档深入阐述 Android_ORB-SLAM2s 系统的数学基础与核心算法流程。内容涵盖视觉里程计、局部建图、回环检测、平面检测以及系统异步重定位的算法原理。

1. 符号约定

符号 含义
$\mathcal{F}_w$ 世界坐标系
$\mathcal{F}_c$ 相机坐标系
$\mathbf{K}$ 相机内参矩阵
$\mathbf{T}_{cw} \in SE(3)$ 从世界坐标系到相机坐标系的变换
$\mathbf{R} \in SO(3)$ 旋转矩阵
$\mathbf{t} \in \mathbb{R}^3$ 平移向量
$\mathbf{X}_w \in \mathbb{R}^3$ 世界坐标系下的 3D 点
$\boldsymbol{\xi} \in \mathfrak{se}(3)$ 李代数表示的位姿

2. 视觉里程计与位姿跟踪 (Visual Odometry & Tracking)

2.1 针孔相机投影模型

对于世界坐标系中的一点 $\mathbf{P}_w = [X_w, Y_w, Z_w]^T$，其在当前相机坐标系下的坐标 $\mathbf{P}_c = [X_c, Y_c, Z_c]^T$ 由刚体变换给出：

$$ \mathbf{P}_c = \mathbf{R}_{cw} \mathbf{P}_w + \mathbf{t}_{cw} $$

投影到像素平面：

$$ \mathbf{u} = \pi(\mathbf{P}_c) = \begin{bmatrix} f_x \frac{X_c}{Z_c} + c_x \ f_y \frac{Y_c}{Z_c} + c_y \end{bmatrix} $$

2.2 基于李代数的位姿优化

跟踪线程通过最小化重投影误差（Reprojection Error）来优化当前帧的位姿 $\mathbf{T}_{cw}$：

$$ \mathbf{e} = \mathbf{u}_{obs} - \pi(\mathbf{T}_{cw} \mathbf{P}_w) $$

对李代数 $\boldsymbol{\xi} \in \mathfrak{se}(3)$ 线性化。设扰动量为 $\delta \boldsymbol{\xi}$，误差函数的一阶泰勒展开为：

$$ \mathbf{e}(\boldsymbol{\xi} \oplus \delta \boldsymbol{\xi}) \approx \mathbf{e}(\boldsymbol{\xi}) + \mathbf{J} \delta \boldsymbol{\xi} $$

其中雅可比矩阵 $\mathbf{J} = \frac{\partial \mathbf{e}}{\partial \delta \boldsymbol{\xi}}$ 为 $2 \times 6$ 矩阵：

$$ \mathbf{J} = -\begin{bmatrix} \frac{f_x}{Z'} & 0 & -\frac{f_x X'}{Z'^2} & -\frac{f_x X' Y'}{Z'^2} & f_x (1 + \frac{X'^2}{Z'^2}) & -\frac{f_x Y'}{Z'} \\ 0 & \frac{f_y}{Z'} & -\frac{f_y Y'}{Z'^2} & -f_y (1 + \frac{Y'^2}{Z'^2}) & \frac{f_y X' Y'}{Z'^2} & \frac{f_y X'}{Z'} \end{bmatrix} $$

其中 $[X', Y', Z']^T = \mathbf{R} \mathbf{X}_j + \mathbf{t}$。系统使用 Levenberg-Marquardt 算法迭代求解：

$$ (\mathbf{H} + \lambda \mathbf{I}) \delta \boldsymbol{\xi} = -\mathbf{b} $$

2.3 恒速运动模型 (Constant Velocity Motion Model)

假设短时间内相机速度恒定，利用上一帧的速度 $\mathbf{V}_{k-1}$ 预测当前帧位姿：

$$ \tilde{\mathbf{T}}_{cw, k} = \mathbf{V}_{k-1} \mathbf{T}_{cw, k-1} $$

若跟踪成功，则更新速度 $\mathbf{V}{k} = \mathbf{T}{cw, k} \mathbf{T}_{cw, k-1}^{-1}$。

2.4 参考关键帧跟踪 (Reference KeyFrame Tracking)

若恒速模型失效（如运动骤变或跟丢后重找回），改为与最近的关键帧进行 BoW 加速匹配，通过 PnP 求解位姿。

2.5 重定位 (Relocalization)

当跟踪丢失时，系统进入重定位模式：

  1. 候选检索: 利用 DBoW2 词袋库检索与当前帧相似的历史关键帧
  2. EPnP + RANSAC: 对每个候选关键帧，使用 Efficient PnP 算法结合 RANSAC 求解位姿
  3. 位姿优化: 对内点进行非线性优化，若内点数足够（>50），则重定位成功

2.6 EPnP 算法

EPnP 将 $n$ 个 3D 点表示为 4 个控制点 $\mathbf{C}_j^w$ 的加权和：

$$ \mathbf{P}_i^w = \sum_{j=1}^4 \alpha_{ij} \mathbf{C}_j^w, \quad \sum_{j=1}^4 \alpha_{ij} = 1 $$

在相机坐标系下该关系保持不变，结合投影约束构建线性方程组 $\mathbf{M}\mathbf{x} = \mathbf{0}$，通过 SVD 求解控制点在相机坐标系下的坐标，进而恢复 $\mathbf{R}$ 和 $\mathbf{t}$。

2.7 单目初始化 (Monocular Initialization)

系统并行计算单应性矩阵 $\mathbf{H}{21}$ 和基础矩阵 $\mathbf{F}{21}$，通过评分选择最优模型：

$$ S_M = \sum_i (\rho(d^2(\mathbf{x}_{2i}, M \mathbf{x}_{1i})) + \rho(d^2(\mathbf{x}_{1i}, M^{-1} \mathbf{x}_{2i}))) $$

若 $S_H / (S_H + S_F) > 0.40$ 选择 Homography，否则选择 Fundamental。

2.8 仅定位模式 (Localization Only)

通过 ActivateLocalizationMode() 可停用 LocalMapping 线程，系统仅利用现有地图进行位姿估计，不插入新关键帧。适用于地图已构建完成的 AR 导航场景。

3. 局部建图 (Local Mapping)

3.1 共视关系图 (Covisibility Graph)

  • 节点: 关键帧 (KeyFrame)
  • 边: 两个关键帧观测到超过 15 个相同的地图点则存在边，权重 = 共享点数
  • 所有局部操作限制在共视关系最强的邻域内，保证复杂度与地图规模无关 $O(1)$

3.2 局部光束法平差 (Local Bundle Adjustment)

优化局部关键帧集合 $\mathcal{K}_L$ 和局部地图点集合 $\mathcal{P}_L$ 的重投影误差：

$$ E = \sum_{i \in \mathcal{K}_L} \sum_{j \in \mathcal{P}_L} \rho \left( | \mathbf{x}_{ij} - \pi(\mathbf{T}_{cw,i} \mathbf{X}_{w,j}) |_{\Sigma_{ij}}^{2} \right) $$

  • $\rho(\cdot)$: Huber 鲁棒核函数

3.3 关键帧剔除策略

严格的冗余检测机制：如果一个关键帧中 90% 以上的地图点能被至少 3 个其他关键帧（在相同或更精细尺度上）观测到，则标记为冗余并剔除。

3.4 地图资源上限管理

系统通过 Config.h 中的硬限制防止内存无限增长：

参数 值 说明
MAX_KEYFRAMES 2000 最大关键帧数
MAX_MAPPOINTS 10000 最大地图点数
TRACKING_MAX_LOCAL_MAP_POINTS 5000 局部地图点上限

超限时从最早的数据开始批量剔除。

4. 回环检测与 Sim3 优化 (Loop Closing)

4.1 回环检测

  1. BoW 检索: 查询数据库，寻找与当前关键帧相似度高的候选
  2. 共视一致性验证: 要求候选帧及其共视组的时间连续性和几何一致性至少 3 级
  3. Sim3 几何验证: 对满足条件的候选帧进行 Sim3 求解

4.2 Sim3 优化

单目 SLAM 存在尺度漂移，需要使用 7 自由度相似变换：

$$ \mathbf{S} = \begin{bmatrix} s\mathbf{R} & \mathbf{t} \ \mathbf{0} & 1 \end{bmatrix} \in Sim(3) $$

使用 Horn 方法（四元数闭式解）进行初始估计：

  1. 计算质心，去质心化
  2. 构造协方差矩阵 $\mathbf{H} = \sum \mathbf{P}'_i (\mathbf{Q}'_i)^T$
  3. SVD 分解获取旋转矩阵 $\mathbf{R}$
  4. 计算尺度因子 $s$
  5. 计算平移向量 $\mathbf{t}$

4.3 全局位姿图优化 (Essential Graph Optimization)

闭环成功后，对本质图进行全局优化。本质图 = 生成树 + 强共视边（权重 > 100）+ 闭环边。仅优化关键帧位姿（不优化地图点），快速修正整个地图的尺度与轨迹。

5. 平面检测 (Plane Detection)

专为 AR 虚拟物体放置设计，基于 SVD 分解的平面拟合算法。

5.1 算法流程

  1. 点云筛选: 获取当前帧可见的地图点集合
  2. RANSAC 拟合: 随机采样 3 点确定平面，统计内点数，迭代 50 次寻找最优模型 $ax+by+cz+d=0$
  3. SVD 精炼: 对内点集构建矩阵 $\mathbf{A} \in \mathbb{R}^{N \times 4}$，进行 SVD 分解 $\mathbf{A} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^T$，最优平面参数为 $\mathbf{V}$ 中最小奇异值对应的列向量
  4. 坐标系构建: 利用 Rodrigues 公式（$\mathfrak{so}(3)$ 到 $SO(3)$ 的指数映射）构建从世界坐标系到平面坐标系的变换矩阵

5.2 坐标系对齐

构建平面坐标系 $\mathbf{T}_{pw}$，使法向量 $\vec{n}$ 对齐到 Y 轴：

  1. 计算旋转轴: $\mathbf{v} = \vec{n} \times \vec{up}$
  2. 计算旋转角: $\theta = \arctan2(|\mathbf{v}|, \vec{n} \cdot \vec{up})$
  3. 使用 Rodrigues 公式计算旋转矩阵: $\mathbf{R} = \mathbf{I} + \sin\theta [\mathbf{k}]{\times} + (1-\cos\theta) [\mathbf{k}]{\times}^2$

5.3 平面追踪

  • 每次检测到新平面时，设置 planeLoadedFromMap = false（手动检测标记）
  • 当从地图加载平面时，planeLoadedFromMap = true，此时需要对齐成功才能显示 AR
  • Recompute() 方法可在 SLAM 地图更新后重新计算平面参数

6. 异步全局重定位 (Async Global Relocalization)

这是本项目的核心创新之一，通过在后台独立线程持续运行全局重定位，实现当前 SLAM 坐标系与加载地图坐标系的无缝对齐。

6.1 参考缓存构建

加载地图后，系统需要构建高效的索引结构以支持快速重定位：

倒排索引 (mRefInverted):

WordID → [MapPoint_Index_1, MapPoint_Index_2, ...]

基于视觉词汇的哈希表，将每个 DBoW2 词汇映射到拥有该词汇的地图点。

空间网格索引 (mRefGrid):

3D Grid Cells (默认 10m/格)
→ [Center, Radius] 包围盒内的候选点列表

空间划分索引，支持圆形范围的精确过滤。

参考快照 (mRefSnapshots):

struct RefMPSnapshot {
    cv::Point3f Pw;     // 世界坐标
    float minD, maxD;   // 深度范围
    int mapId;          // 地图 ID
};

不可变快照，避免后台线程与 SLAM 主线程的竞态条件。

6.2 PnP 求解优化 (SolvePnPSafe)

封装 OpenCV 的 solvePnPRansac，增加额外鲁棒性检查：

  • NaN/Inf 检查: 严格检查 3D 点和 2D 点数值有效性
  • 最小点数限制: 至少 6 个点才进行 RANSAC 求解
  • 精度提升: 强制使用 CV_64F (double) 内部计算
  • 策略分级: $N &lt; 4$ 直接返回失败；$N < 6$ 跳过 RANSAC 仅做最小二乘；$N \ge 6$ 启用 RANSAC

6.3 快照与绑定机制

  • 生产: 主线程将帧描述子、关键点、位姿打包为快照（原子序列号 mSnapSeqProduced）
  • 消费: 后台线程获取最新快照进行匹配求解
  • 绑定: 对齐成功后，主线程将加载地图点投影到当前帧（BindLoadedMapPointsUsingSnapshots），每帧最多绑定 50 点

7. 安全地图重置 (Safe Map Reset)

Reset(bool bKeepMap) 支持两种重置模式：

模式 行为
Reset(false) 完全重置：清空所有地图数据和跟踪状态
Reset(true) 轻量重置：仅清除跟踪状态（位姿、局部地图），保留所有加载的地图点

轻量重置适用于跟踪丢失后快速恢复，JNI 层结合 LOST_RESET_TIMEOUT（3 秒）自动触发。

8. 参数配置体系

项目使用 Config.h 头文件统一管理所有运行时参数，替代了原始 ORB-SLAM2 的 YAML 文件方式。主要参数分组：

分组 关键参数 默认值
相机内参 fx, fy, cx, cy 640, 640, 320, 180
ORB 特征 nFeatures, scaleFactor, nLevels 1000, 1.2, 8
闭环检测 minFrames, minMatches 10, 20
优化器 Huber 阈值, 卡方阈值 2.448, 5.991
地图限制 maxKFs, maxMPs 2000, 10000
系统运行 FPS, 下采样因子 30, 2.0
平面检测 RANSAC 迭代 50

Clone this wiki locally