本文不满足于”SLAM 分前端后端回环五步”的科普框架，而是讲清楚每个模块在数学上到底在解什么：SLAM 是最大后验估计 → 前端是帧间几何 → 后端是稀疏最小二乘 → 回环是全局一致性 → KD 树是搜索加速。结合 RoboMaster 视觉（HSV 灯条识别、深度相机选型）和 FAST-LIO2 的实际工程经验。
SLAM 的数学本质：一个状态估计问题
SLAM 所有模块都可以统一到同一个框架——带噪声的状态估计。定义两个方程：
运动方程（状态怎么变）：$x_k = f(x_{k-1}, u_k) + w_k$
观测方程（传感器看到什么）：$z_{k,j} = h(x_k, l_j) + v_{k,j}$
- $x_k$：k 时刻位姿；$u_k$：控制输入（IMU/轮速）；$l_j$：第 j 个路标（地图点）
- $w_k, v_{k,j}$：过程噪声与观测噪声，假设零均值高斯 $N(0, \Sigma)$
要求的是：给定所有观测 Z 和输入 U，估计位姿 X 和路标 L。这是典型的贝叶斯推断——最大后验估计（MAP）：
$$X^*, L^* = \arg\max_{X,L} P(X, L | Z, U) \propto \arg\max P(Z|X,L) \cdot P(X|U)$$
高斯假设下，MAP 等价于最小二乘：对高斯分布取负对数，指数项变成二次型：
$$X^*, L^* = \arg\min_{X,L} \sum_k |x_k - f(x_{k-1},u_k)|^2_{\Sigma_w} + \sum_{k,j} |z_{k,j} - h(x_k, l_j)|^2_{\Sigma_v}$$
这就是为什么所有 SLAM 后端最后都变成一个最小二乘问题——前端负责构造约束（观测 z），后端负责最小化残差。这个视角统一了视觉 SLAM（ORB-SLAM）、激光 SLAM（LOAM）和图优化。
前端里程计：帧与帧之间的几何
前端回答”这两帧之间我动了多少”。视觉方案分特征点法和直接法，激光方案是 ICP。
对极几何：为什么两帧能恢复运动
设空间点 P 在帧 1 归一化坐标 $p_1$、帧 2 坐标 $p_2$，两帧之间运动为 $R, t$（相机从 1 到 2）。由 $p_2 = R p_1 + t$（深度被归一化吸收），两边叉乘 t 再点乘 $p_2$，消掉含深度项，得到对极约束：
$$p_2^T \underbrace{t^\wedge R}_{E} p_1 = 0$$
本质矩阵 $E = t^\wedge R$。给定 ≥8 对匹配点（8 点法），线性解出 E；再对 E 做 SVD 分解 $E = U\Sigma V^T$，恢复出 4 组可能的 $(R, t)$——其中 3 组是”伪解”（三角化后点在相机后方），用三角化验证剔除。
为什么单目没有绝对尺度？ 对极约束对 $E$ 的缩放不敏感（$E$ 乘任意常数约束仍成立），所以单目恢复的 $t$ 只有方向没有大小——尺度漂移是单目的本质缺陷，需要 IMU（VINS-Mono）或已知尺寸物体来”锚定”尺度。
三角化：深度从哪来
已知两帧位姿和匹配点，求空间点深度：$s_2 p_2 = s_1 R p_1 + t$，两个方程两个未知数（$s_1, s_2$），最小二乘解。精度：基线越长、视差越大，深度越准；纯平移时精度最高，纯旋转时对极约束退化（$E$ 的信息量不足）——这就是”纯旋转无法三角化”的原因，也是单目 SLAM 初始化要求”有平移”的原因。
ICP：激光/点云配准的 SVD 闭式解
两帧点云 ${p_i}, {q_i}$ 已配对，求 $R, t$ 使 $\sum|q_i - (Rp_i + t)|^2$ 最小。推导：对 t 求导得 $t^* = \bar q - R\bar p$（质心关系），代入后目标变成只含 R 的迹最大化问题：
$$R^* = \arg\max_R \operatorname{tr}(R^T W), \quad W = \sum_i (p_i - \bar p)(q_i - \bar q)^T$$
对 $W$ 做 SVD：$W = U\Sigma V^T$，则 $R^* = UV^T$，$t^* = \bar q - R^*\bar p$。整个 ICP 就是一次 SVD——这就是 PCL 里 pcl::registration::TransformationEstimationSVD 的实现原理。注意：ICP 需要初始位姿足够近（否则陷入局部最优），工程上常用 IMU/里程计提供初值，或先用特征匹配粗对齐。
后端优化：稀疏最小二乘
后端把前端攒下的所有约束（帧间约束 + 回环约束 + 路标观测）一起优化。写成图：顶点 = 位姿/路标，边 = 约束。目标函数是所有边残差的平方和：
$$F(x) = \sum_{k} |r_k(x)|^2_{\Sigma_k}$$
高斯牛顿与 LM
对残差一阶展开 $r(x + \Delta x) \approx r(x) + J\Delta x$，最小化 $|r + J\Delta x|^2$，求导为零得到法方程：
$$J^T J \Delta x = -J^T r$$
高斯牛顿直接解这个方程；LM 在其上加阻尼：
$$(J^T J + \lambda I)\Delta x = -J^T r$$
为什么必须加 $\lambda$？ $J^TJ$ 半正定，接近奇异时 $\Delta x$ 爆炸；加 $\lambda I$ 保证可逆，且 $\lambda$ 大时退化为梯度下降（稳）、$\lambda$ 小时退化为高斯牛顿（快）——自适应调整 $\lambda$（Levenberg-Marquardt）是工程标配，ceres/g2o 默认就是它。SLAM 里 $J$ 通常列满秩但病态（弱约束方向），没有 $\lambda$ 的纯 GN 几乎必然发散。
H 矩阵的稀疏性与 Schur 消元
$H = J^TJ$ 是箭头状稀疏矩阵：位姿-位姿块沿对角线、位姿-路标块在边缘、路标-路标块是块对角。直接求逆 $O(n^3)$ 不可行，利用结构做 Schur 消元（边缘化路标）：
$$\begin{bmatrix} B & E \ E^T & C \end{bmatrix} \begin{bmatrix} \Delta x_c \ \Delta x_p \end{bmatrix} = \begin{bmatrix} v \ w \end{bmatrix} \Rightarrow (B - EC^{-1}E^T)\Delta x_c = v - EC^{-1}w$$
先解位姿增量（维度小得多），再回代解路标。这就是 ORB-SLAM / g2o 能实时跑几千个路标的原因——**复杂度从 $O((n+m)^3)$ 降到 $O(n^3 + nm)$**。
回环检测：为什么必须有
里程计误差是累积的——每一步的误差都是随机量，轨迹估计的方差随时间线性增长（随机游走）。跑 100 米可能偏几厘米，跑 10 圈可能偏几米。回环检测的任务：**认出”我回到过这里”**，把首尾约束加进图优化，一次性拉平全局漂移。
词袋模型（BoW）
视觉回环用词袋：把特征描述子聚类成”视觉单词”（离线训练，如 ORB-SLAM 的 DBoW2），每帧图像变成单词的直方图，用 TF-IDF 加权后算相似度：
$$s(v_a, v_b) = 1 - \frac{1}{2}\left|\frac{v_a}{|v_a|} - \frac{v_b}{|v_b|}\right|$$
超过阈值 → 候选回环，再做几何验证（对极约束 + 一致性检查）排除误检。坑：纯靠外观相似会误检（相似场景、光照变化），必须加几何验证和时序一致性（连续多帧确认）。
FAST-LIO2 与 IEKF：为什么”迭代”卡尔曼
FAST-LIO2 是 LiDAR-惯性里程计，核心是迭代扩展卡尔曼滤波（IEKF）。标准 EKF 在观测非线性时把 $h$ 一阶线性化——线性化点不准，估计就偏。IEKF 的做法：在当前估计点迭代重线性化，每步重新算 $h$ 的雅可比，直到收敛：
1 | # IEKF 观测更新（伪代码）：反复线性化直到收敛 |
为什么 LIO 必须用 IEKF 而不是 EKF？ 激光点云观测是强非线性的（点到面残差），且每帧几千个点——EKF 一次线性化误差大，IEKF 迭代收敛到 MAP 解，精度接近图优化但计算量小得多（这正是 FAST-LIO 系列”实时 + 高精度”的卖点）。FAST-LIO2 另一个关键贡献是 ikd-Tree：增量式 KD 树，支持动态插入/删除点，避免每帧重建整棵树——把 KD 树的构建从 $O(n \log n)$ 摊薄到增量 $O(\log n)$。
KD 树：最近邻搜索的工程实现
KD 树是 ICP、特征匹配、回环检测里”找最近邻”的基础设施。暴力搜索 $O(n)$，KD 树平均 $O(\log n)$。
构建：为什么选方差最大的维度
1 | // 生产级 KD 树：构建 + 最近邻搜索（带剪枝） |
为什么选中位数分裂？ 保证左右子树点数均衡 → 树高 $O(\log n)$ → 搜索深度可控。为什么选方差最大维度？ 数据在该维度越分散，超平面切得越”有效”，剪枝概率越高。两者都是为了让树更平衡、搜索更快。
最近邻搜索：剪枝的几何条件
1 | void nearest(Node* n, const Eigen::Vector3f& q, |
**剪枝条件就是 diff² < best_d**——查询点到分割超平面的距离如果已经大于当前最优距离，另一侧子树里不可能有更近的点，直接剪掉。这个条件让搜索跳过大部分子树，平均 $O(\log n)$。
复杂度与维数灾难
| 方法 | 构建 | 单次查询 | 适用 |
|---|---|---|---|
| 暴力 | $O(1)$ | $O(n)$ | n 小 |
| KD 树 | $O(n\log n)$ | 平均 $O(\log n)$，最坏 $O(n)$ | d ≤ 20（点云、描述子低维） |
| ikd-Tree | 增量 | $O(\log n)$ | 激光 SLAM 动态地图 |
| FLANN (随机KD森林) | $O(n\log n)$ | 亚线性 | 高维描述子（ORB 256 维） |
维数灾难：维度升高时，KD 树剪枝效率急剧下降——高维空间里”最近邻”几乎处处等距，超平面剪不掉任何东西，查询退化到 $O(n)$。所以：点云（3 维）用 KD 树，ORB 描述子（256 维）用 FLANN 的近似最近邻。工程上还有个常见优化：对查询点做半径搜索（只找 r 以内的点），用 diff > r 提前剪枝，ICP 匹配就是这么干的。
视觉处理实战：RoboMaster 灯条识别
为什么用 HSV 而不是 RGB
RGB 三个通道都含亮度信息，光照一变三个值一起变；HSV 把色相 H 和亮度 V 分开——识别颜色只比 H（和少量 S），光照变化基本不影响 H：
1 | // 灯条颜色阈值（红方示例）：H 集中在 0° 附近（红色在色相环两端） |
坑 1：实际比赛灯光下灯条饱和度 S 会掉，纯阈值会漏检——用自适应阈值或加形态学补偿。坑 2：白色装甲板反光会把 V 拉满、S 拉低，误检为灯条——按”灯条长宽比（>3:1）+ 面积 + 左右灯条配对（等高、同 y、间距合理）”过滤。
形态学：为什么先腐蚀后膨胀
- 膨胀：亮区域扩张，填小洞、连断线
- 腐蚀：亮区域收缩，去小噪点
- 开运算 = 先腐蚀后膨胀：去掉小噪点（先腐蚀把噪点消掉，再膨胀恢复主体尺寸）
- 闭运算 = 先膨胀后腐蚀：填小孔洞（先膨胀把洞填上，再腐蚀恢复）
数学定义：膨胀是”结构元素内取最大”（$\max$ 滤波），腐蚀是”取最小”（$\min$ 滤波）。灯条识别流水线：inRange 阈值 → 开运算去噪 → 闭运算填洞 → findContours → 按长宽比/面积过滤 → 左右灯条配对 → 计算装甲板中心与距离。这套流程在 720p 上单帧 < 1ms，是 RoboMaster 视觉的经典 baseline。
深度相机选型
| 方案 | 原理 | 优点 | 缺点 |
|---|---|---|---|
| 结构光 (Kinect v1) | 投射编码图案，图案形变解深度 | 室内精度高、成本低 | 室外阳光干扰严重、近距有盲区 |
| ToF (Kinect v2) | 测量光飞行时间 | 帧率高、范围大 | 分辨率低、边缘飞点 |
| 双目立体 | 视差三角化 | 室内外都能用、无主动光 | 弱纹理失效、算力高、基线限制近距离 |
选型结论：RoboMaster 赛场上高速运动 + 强光环境，主动光方案（结构光/ToF）直接被淘汰——双目是唯一可靠选项，但需要在 ROS 里用 depth_image_proc 把左右目变成视差图再转点云，且”弱纹理地面测距会失败”必须接受。SLAM 里同理：室外大场景用激光/双目，室内小场景 RGB-D 最省事。
踩坑清单
- 单目尺度漂移：单目 SLAM 轨迹没有绝对尺度，要接 IMU 或已知尺寸物体锚定；别拿单目轨迹长度当真值
- 纯旋转不能初始化/三角化：对极约束退化，初始化阶段必须保证有平移
- ICP 初值敏感：无初值直接 ICP 会陷局部最优，务必用 IMU/里程计给初值
- GN 发散：$J^TJ$ 病态时法方程爆炸，用 LM 并限制单步最大增量
- 回环误检：只靠外观相似度会误检，必须几何验证 + 时序一致性
- 高维用 FLANN：256 维描述子别用 KD 树，退化到 $O(n)$ 不如近似最近邻
- HSV 红色双段：红色在 H 环两端，阈值要两段取或；反光误检靠灯条几何约束过滤
- 深度相机怕阳光：室外/强光场景选双目或激光，别用结构光/ToF