3. 数学原理

完整算法对比与工程背景见 09_survey.md；Atlas 实现细节见 06_atlas.md、05_architecture.md。

3.1 问题形式化

机器人在世界坐标系 \(\mathcal{W}\) 中的位姿为 \(T_{wb} \in SE(3)\)（或 2D 情况下 \(T_{wb} \in SE(2)\)）。定位即估计：

\[ \hat{T}_{wb}(t) = \arg\max_{T} \; p(T \mid z_{1:t}, u_{1:t}) \]

其中 \(z_t\) 为传感器观测（图像特征、激光扫描等），\(u_t\) 为控制/里程计输入。

Atlas（视觉 SLAM） 同时维护：

相机位姿序列 \(\{T_{cw}^{(i)}\}\)（关键帧）
稀疏 3D 路标集 \(\mathcal{L} = \{\mathbf{X}_j \in \mathbb{R}^3\}\)
共视图 \(\mathcal{G} = (\mathcal{K}, \mathcal{E})\)（关键帧为节点，共视边为边）

AMCL（粒子滤波） 用粒子集 \(\{(\mathbf{x}^{(m)}, w^{(m)})\}_{m=1}^M\) 近似后验，详见 §3.10。

3.2 相机投影模型

3.2.1 针孔模型

3D 点 \(\mathbf{P}_c = (X, Y, Z)^\top\)（相机系）投影到像素 \((u, v)\)：

\[\begin{split} \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \frac{1}{Z} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} = \frac{1}{Z} \mathbf{K} \mathbf{P}_c \end{split}\]

内参矩阵（camera::perspective）：

\[\begin{split} \mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \end{split}\]

3.2.2 畸变模型（Brown-Conrady）

归一化平面坐标 \((x, y) = (X/Z, Y/Z)\)，径向 + 切向畸变：

\[\begin{split} \begin{aligned} r^2 &= x^2 + y^2 \\ x_d &= x(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + 2 p_1 xy + p_2(r^2 + 2x^2) \\ y_d &= y(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + p_1(r^2 + 2y^2) + 2 p_2 xy \end{aligned} \end{split}\]

像素坐标：\(u = f_x x_d + c_x\)，\(v = f_y y_d + c_y\)。

3.2.3 Bearing Vector（Atlas 内部表示）

Atlas 将特征点转为单位 bearing vector \(\mathbf{b} \in S^2\)：

\[ \mathbf{b} = \frac{\mathbf{K}^{-1} \tilde{\mathbf{p}}}{\|\mathbf{K}^{-1} \tilde{\mathbf{p}}\|}, \quad \tilde{\mathbf{p}} = (u, v, 1)^\top \]

重投影误差在 bearing 空间计算，对尺度变化更鲁棒。

3.2.4 世界坐标变换

路标 \(\mathbf{X}_w\) 在相机系下：

\[\begin{split} \mathbf{P}_c = \mathbf{R}_{cw} \mathbf{X}_w + \mathbf{t}_{cw} = \mathbf{T}_{cw} \begin{bmatrix} \mathbf{X}_w \\ 1 \end{bmatrix} \end{split}\]

其中 \(\mathbf{T}_{cw} = \begin{bmatrix} \mathbf{R}_{cw} & \mathbf{t}_{cw} \\ \mathbf{0}^\top & 1 \end{bmatrix}\) 为 Atlas 存储的 pose_cw。

3.3 ORB 特征与金字塔

3.3.1 图像金字塔

尺度因子 \(s = 1.2\)（scale_factor_），第 \(\ell\) 层尺度：

\[ \sigma_\ell = s^\ell, \quad \ell = 0, 1, \ldots, L-1 \]

第 \(\ell\) 层图像尺寸约为原图的 \(s^{-\ell}\)。orb_params 预计算：

\[ \sigma[\ell] = s^\ell, \quad \sigma^{-1}[\ell] = s^{-\ell} \]

对应实现中的 scale_factors、inv_scale_factors 数组。

3.3.2 FAST 角点 + BRIEF 描述子

FAST-9 检测角点，阈值 ini_fast_threshold（初始化）/ min_fast_threshold（跟踪）
Orientation：强度质心法计算主方向 \(\theta\)
rBRIEF：旋转 BRIEF 描述子，256 bit 二进制向量 \(\mathbf{d} \in \{0,1\}^{256}\)

3.3.3 Hamming 距离

两描述子 \(\mathbf{d}_1, \mathbf{d}_2\) 的匹配代价：

\[ d_H(\mathbf{d}_1, \mathbf{d}_2) = \operatorname{popcount}(\mathbf{d}_1 \oplus \mathbf{d}_2) \]

ORB 匹配采用 ratio test（Lowe）：

\[ \frac{d_H(\mathbf{d}_q, \mathbf{d}_n)}{d_H(\mathbf{d}_q, \mathbf{d}_b)} < \rho, \quad \rho \approx 0.8 \]

其中 \(\mathbf{d}_n\) 为最近邻，\(\mathbf{d}_b\) 为次近邻。

3.4 两视图几何

3.2.1 对极约束与本质矩阵

两帧 bearing \(\mathbf{b}_1, \mathbf{b}_2\) 对应同一 3D 点，满足：

\[ \mathbf{b}_2^\top \mathbf{E}_{21} \mathbf{b}_1 = 0 \]

本质矩阵 \(\mathbf{E}_{21} = [\mathbf{t}_{21}]_\times \mathbf{R}_{21}\)，其中 \([\mathbf{v}]_\times\) 为反对称矩阵：

\[\begin{split} [\mathbf{v}]_\times = \begin{bmatrix} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0 \end{bmatrix} \end{split}\]

5 点法（essential_5pt）：5 对匹配 → 最多 10 个 \(\mathbf{E}\) 候选，RANSAC 选最优。

8 点法（compute_E_21_nonminimal）：SVD 分解求解，RANSAC 后精化。

3.4.2 单应矩阵（平面场景）

共面场景用单应 \(\mathbf{H}_{21}\)：

\[ \mathbf{b}_2 \sim \mathbf{H}_{21} \mathbf{b}_1 \]

初始化时同时估计 \(\mathbf{E}\) 与 \(\mathbf{H}\)，选内点更多、视差更大者（initialize::perspective）。

3.4.3 从 \(\mathbf{E}\) 恢复 \(R, t\)

SVD：\(\mathbf{E} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^\top\)，得到 4 组 \((\mathbf{R}, \mathbf{t})\) 候选，通过 cheirality check（3D 点在两相机前方）确定唯一解。

3.4.4 三角化

两帧位姿 \(\mathbf{T}_{c1w}, \mathbf{T}_{c2w}\)，匹配 bearing \(\mathbf{b}_1, \mathbf{b}_2\)，线性三角化（DLT）：

\[\begin{split} \mathbf{A} \mathbf{X}_w = \mathbf{0}, \quad \mathbf{A} = \begin{bmatrix} \mathbf{b}_1^\top \mathbf{P}_1 \\ \mathbf{b}_2^\top \mathbf{P}_2 \end{bmatrix} \end{split}\]

\(\mathbf{P}_i = \mathbf{K}_i [\mathbf{R}_{ciw} \mid \mathbf{t}_{ciw}]\) 为投影矩阵。SVD 取 \(\mathbf{A}\) 最小奇异值对应向量，归一化得 \(\mathbf{X}_w\)。

深度比检验（mapping_module::create_new_landmarks）：

\[ \frac{\|\mathbf{t}_1 - \mathbf{X}_w\|}{\|\mathbf{t}_2 - \mathbf{X}_w\|} \in [\rho_{\min}, \rho_{\max}] \]

基线距离阈值：

\[ \|\mathbf{t}_1 - \mathbf{t}_2\| > \max\left\{ \tau_b, \; \eta \cdot d_{\mathrm{med}} \right\} \]

默认 \(\eta = 0.02\)（baseline_dist_thr_ratio_）。

3.5 PnP 位姿估计

已知 3D–2D 对应 \(\{(\mathbf{X}_w^{(j)}, \mathbf{b}^{(j)})\}\)，求 \(\mathbf{T}_{cw}\)。

3.5.1 EPnP + RANSAC

pnp_solver::find_via_ransac 流程：

随机采样最小集（4 点）
EPnP 求解候选位姿
重投影检验内点，Huber 加权代价
迭代至收敛，Gauss-Newton 精化（gauss_newton_num_iter = 10）

3.5.2 重投影误差（bearing 空间）

\[ e_j = \arccos\left( \mathbf{b}_j^\top \hat{\mathbf{b}}_j \right) \]

其中 \(\hat{\mathbf{b}}_j\) 为将 \(\mathbf{X}_w^{(j)}\) 用当前 \(\mathbf{T}_{cw}\) 投影后的 bearing。内点判定：\(e_j < \tau_{\text{rad}}\)（默认 0.2°）。

3.6 位姿图优化（g2o）

Atlas 后端统一使用 g2o，优化目标为 非线性最小二乘：

\[ \mathbf{T}^*, \mathbf{X}^* = \arg\min_{\mathbf{T}, \mathbf{X}} \sum_k \rho\left( \| \mathbf{r}_k(\mathbf{T}, \mathbf{X}) \|_{\mathbf{\Omega}_k}^2 \right) \]

其中 \(\rho\) 为鲁棒核（Huber / Tukey），\(\mathbf{\Omega}_k\) 为信息矩阵。

3.6.1 重投影边（SE3）

对观测 \((\mathbf{b}_{obs}, \mathbf{X}_w, \mathbf{T}_{cw})\)：

\[ \mathbf{r}_{\text{reproj}} = \mathbf{b}_{obs} - \pi(\mathbf{T}_{cw}, \mathbf{X}_w) \]

信息矩阵与特征金字塔层相关：

\[ \mathbf{\Omega} = \frac{1}{\sigma_\ell^2} \mathbf{I}_2, \quad \sigma_\ell = s^\ell \]

3.6.2 位姿优化（Tracking 每帧）

pose_optimizer::optimize 仅优化当前帧 \(\mathbf{T}_{cw}\)，路标固定：

\[ \min_{\mathbf{T}_{cw}} \sum_{j \in \mathcal{O}} \rho\left( \|\mathbf{r}_j\|^2 \right) \]

Levenberg-Marquardt 迭代，外点剔除后重优化。

3.6.3 局部 Bundle Adjustment（LBA）

local_bundle_adjuster::optimize 优化局部窗口 \(\mathcal{W}\) 内关键帧位姿与路标：

\[ \min_{\{\mathbf{T}_i\}, \{\mathbf{X}_j\}} \sum_{(i,j) \in \mathcal{E}} \rho\left( \|\mathbf{r}_{ij}\|_{\mathbf{\Omega}_{ij}}^2 \right) \]

局部窗口：当前关键帧 + 共视关键帧（默认最多 60 帧 max_num_local_keyfrms_）。

3.6.4 全局 Bundle Adjustment（Loop BA）

回环闭合后在独立线程执行 GBA，优化全部（或子集）关键帧与路标，消除累积漂移。

3.7 回环检测与 Sim3 校正

3.7.1 Bag-of-Words（BoW）

ORB 描述子量化到视觉词袋，关键帧 \(i\) 的 BoW 向量：

\[ \mathbf{v}_i = (w_1, w_2, \ldots, w_V)^\top, \quad w_k = \mathrm{tfidf}(k) \]

其中 \(\mathrm{tfidf}(k)\) 为词 \(k\) 的 TF-IDF 权重。 loop_detector::detect_loop_candidates 用 BoW 数据库检索相似关键帧，再几何验证。

3.7.2 Sim3 变换（单目回环）

单目 SLAM 存在尺度漂移，回环估计 Sim(3)：

\[\begin{split} \mathbf{S} = \begin{bmatrix} s \mathbf{R} & \mathbf{t} \\ \mathbf{0}^\top & 1 \end{bmatrix}, \quad s > 0 \end{split}\]

transform_optimizer 优化 Sim3 使当前关键帧与候选关键帧的 3D–3D 匹配对齐。

3.7.3 回环校正

检测到回环后（global_optimization_module::correct_loop）：

计算校正前/后 Sim3：\(\mathbf{S}_{nw}^{\text{before}}, \mathbf{S}_{nw}^{\text{after}}\)
传播校正到共视关键帧与路标
融合重复路标，更新共视图
异步启动 Loop BA

共视关键帧 \(n\) 的路标校正：

\[ \mathbf{X}_w' = s \mathbf{R}_{\Delta} \mathbf{X}_w + \mathbf{t}_{\Delta} \]

3.8 运动模型与跟踪状态机

3.8.1 恒速运动模型

相邻帧位姿增量（twist_）：

\[ \mathbf{T}_{c_{k}w} = \Delta \mathbf{T} \cdot \mathbf{T}_{c_{k-1}w} \]

其中 \(\Delta \mathbf{T} = \mathbf{T}_{c_{k-1}w} \mathbf{T}_{c_{k-2}w}^{-1}\)（update_motion_model）。

3.8.2 跟踪状态

状态	条件	行为
`Initializing`	地图未建立	两帧初始化 / 5 点法
`Tracking`	足够内点	正常跟踪 + 局部地图优化
`Lost`	内点不足	BoW 重定位 / 用户 `relocalize_by_pose`

丢失判定：跟踪路标数 \(N_{\text{track}} < N_{\min}\) 且重定位失败。

3.9 立体与 RGB-D 深度

3.9.1 立体视差

校正后对应点 \((u_L, v)\) 与 \((u_R, v)\)，视差 \(d = u_L - u_R\)，深度：

\[ Z = \frac{f_x \cdot b}{d} \]

其中 \(b\) 为基线，focal_x_baseline_ = f_x \cdot b（EuRoC 配置 47.91）。

3.9.2 RGB-D

深度图值 \(d_{\text{pix}}\) 转真实深度：

\[ Z = \frac{d_{\text{pix}}}{f_{\mathrm{dmap}}} \]

其中 \(f_{\mathrm{dmap}}\) 为 depthmap_factor_。

TUM RGB-D 典型 depthmap_factor_ = 5000（mm → m）。

3.10 AMCL 粒子滤波

AMCL 配置见 config/localization/amcl/amcl.lua，算法实现待集成。以下为 nav2_amcl 对齐的数学模型。

3.10.1 贝叶斯滤波

\[ p(\mathbf{x}_t \mid z_{1:t}, u_{1:t}) \propto p(z_t \mid \mathbf{x}_t) \int p(\mathbf{x}_t \mid u_t, \mathbf{x}_{t-1}) p(\mathbf{x}_{t-1} \mid z_{1:t-1}, u_{1:t-1}) \, d\mathbf{x}_{t-1} \]

3.10.2 运动模型（差速驱动）

里程计增量 \((\delta_{\text{rot1}}, \delta_{\text{trans}}, \delta_{\text{rot2}})\)，噪声参数 \(\alpha_1 \ldots \alpha_4\)：

\[\begin{split} \begin{aligned} \delta_{\text{rot1}}' &\sim \mathcal{N}(\delta_{\text{rot1}}, \alpha_1 |\delta_{\text{rot1}}| + \alpha_2 \|\delta_{\text{trans}}\|) \\ \delta_{\text{trans}}' &\sim \mathcal{N}(\delta_{\text{trans}}, \alpha_3 \|\delta_{\text{trans}}\| + \alpha_4(|\delta_{\text{rot1}}| + |\delta_{\text{rot2}}|)) \\ \delta_{\text{rot2}}' &\sim \mathcal{N}(\delta_{\text{rot2}}, \alpha_1 |\delta_{\text{rot2}}| + \alpha_2 \|\delta_{\text{trans}}\|) \end{aligned} \end{split}\]

3.10.3 激光观测模型（Likelihood Field）

预计算距离场 \(\mathrm{dist}(x, y)\)（到最近占据栅格），光束端点 \((x_e, y_e)\) 似然：

\[ p(z_t \mid \mathbf{x}_t) \propto \sum_i \left( w_{\text{hit}} \mathcal{N}(d_i; 0, \sigma_{\text{hit}}^2) + w_{\text{rand}} \frac{1}{r_{\max}} \right) \]

默认 z_hit=0.95, z_rand=0.05, sigma_hit=0.2。

3.10.4 粒子更新

预测：按运动模型采样 \(M\) 个粒子
更新：激光似然加权 \(w^{(m)} \leftarrow w^{(m)} \cdot p(z_t \mid \mathbf{x}^{(m)})\)
重采样：\(N_{\text{eff}} = 1 / \sum (w^{(m)})^2 < N_{\text{th}}\) 时系统重采样
输出：加权均值或 MAP 粒子作为 \(\hat{\mathbf{x}}_t\)，广播 map→odom TF

3.11 算法流水线总览

\[ \mathrm{Image} \xrightarrow{\mathrm{ORB}} \mathrm{Features} \xrightarrow{\mathrm{Matching}} \mathrm{Match}_{2\mathrm{D}/3\mathrm{D}} \xrightarrow{\mathrm{PnP/BA}} \hat{\mathbf{T}}_{cw} \xrightarrow{\mathrm{Keyframe}} \mathrm{LBA} \xrightarrow{\mathrm{BoW}} \mathrm{Loop\ Closure} \xrightarrow{\mathrm{Sim3+GBA}} \mathrm{Globally\ Consistent\ Map} \]

阶段	核心公式	Atlas 模块
特征	FAST + rBRIEF	`feature/orb_extractor`
初始化	\(\mathbf{E}, \mathbf{H}\) + 三角化	`initialize/`
跟踪	PnP + 位姿优化	`tracking_module`
建图	三角化 + LBA	`mapping_module`
回环	BoW + Sim3 + GBA	`global_optimization_module`

3.12 Cartographer 子图与位姿图

使用指南见 cartographer/guide.md。

3.12.1 局部 SLAM（子图）

每个子图 \(S_i\) 维护局部占据栅格 \(M_i\)，激光扫描 \(\mathcal{Z}_t\) 经扫描匹配估计位姿增量：

\[ \xi_t^* = \arg\min_{\xi} \sum_{\mathbf{p} \in \mathcal{Z}_t} \big( 1 - M_i(T_\xi \mathbf{p}) \big)^2 + \lambda_{\mathrm{trans}} \|\Delta t\|^2 + \lambda_{\mathrm{rot}} \Delta\theta^2 \]

其中 \(T_\xi \in SE(2)\) 为候选位姿，\(M_i(\cdot)\) 为占据概率查询（Ceres 扫描匹配器实现）。

子图完成条件：累积 num_range_data 帧扫描后冻结，开启新子图。

3.12.2 占据概率更新

对数几率（log-odds）更新，命中/空闲概率 \(p_{\mathrm{hit}}, p_{\mathrm{miss}}\)（默认 0.55 / 0.49）：

\[ \mathrm{logit}(p') = \mathrm{logit}(p) + \mathrm{logit}(p_{\mathrm{obs}}) - \mathrm{logit}(p_{\mathrm{prior}}) \]

3.12.3 位姿图优化

节点为子图位姿 \(\{\xi_i\}\)，边为：

局部约束：相邻子图间扫描匹配 \(\mathbf{z}_{ij}\)
回环约束：Fast Correlative Scan Matcher 检测到的远距离匹配

全局目标：

\[ \min_{\{\xi_i\}} \sum_{(i,j) \in \mathcal{C}} \rho\Big( \| \mathbf{r}_{ij}(\xi_i, \xi_j) \|_{\mathbf{\Omega}_{ij}}^2 \Big) \]

\(\rho\) 为 Huber 鲁棒核（huber_scale），每 optimize_every_n_nodes 个节点触发一次优化。

3.12.4 纯定位

加载冻结子图 \(\{S_k^{\mathrm{frozen}}\}\)，仅优化当前轨迹位姿；pure_localization_trimmer 限制活跃子图数为 max_submaps_to_keep，丢弃旧子图以节省内存。