3. 视觉SLAM入门
视觉SLAM（Simultaneous Localization and Mapping，同时定位与建图）要回答的核心问题是：一个搭载相机的移动平台，在未知环境中运动时，能否一边估计自己的位姿轨迹，一边构建环境的地图？这个问题的难点在于“鸡生蛋、蛋生鸡”——定位需要地图作为参照，建图又需要准确的位姿作为基准，两者互为条件、必须同时求解。
一、SLAM 问题的数学定义
设离散时刻 t = 1, 2, …, K，记 xk 为 k 时刻相机位姿，yj 为第 j 个地图路标，uk 为 k-1 到 k 时刻的运动输入，zk,j 为 k 时刻对路标 j 的观测。SLAM 的运动方程与观测方程可写为：
x_k = f(x_{k-1}, u_k) + w_k // 运动模型：位姿如何随运动变化
z_{k,j} = h(y_j, x_k) + v_{k,j} // 观测模型：路标在图像中如何成像
// w_k, v_{k,j} 分别为运动噪声与观测噪声，通常假设为零均值高斯这两个方程把 SLAM 变成了一个状态估计问题：已知 u 和 z，估计 x 与 y。若把全部待估量记为状态向量，问题就转化为大规模非线性最小二乘或滤波推断。
二、前端与后端：SLAM 的两大支柱
现代视觉SLAM普遍采用“前端 + 后端”的分工架构。前端负责从图像中提取可用的几何与运动信息，后端负责在概率框架下做全局一致的优化。
- 前端（视觉里程计）：提取特征点、做帧间匹配、估计相邻帧的相对位姿，并三角化出初始地图点。它决定系统的实时性与短期精度，但会累积漂移。
- 后端（优化/滤波）：接收前端给出的位姿与路标估计，结合回环检测结果，做全局或滑动窗口的位姿图优化、BA（Bundle Adjustment），消除累积误差。
- 回环检测：判断相机是否回到曾经到过的位置，为后端提供“长程约束”，是消除漂移的关键。
- 建图：把优化后的位姿与观测融合，输出稀疏点云、稠密地图或语义地图。
三、地图表示：从稀疏到稠密，从几何到语义
地图是 SLAM 的输出，也是定位的参照。不同任务对地图的需求差异很大，常见表示形式如下表：
| 地图类型 | 数据结构 | 典型用途 | 优点 | 局限 |
|---|---|---|---|---|
| 稀疏特征点地图 | 三维路标点集合 | 定位、BA优化 | 轻量、实时性好 | 无法直接用于导航避障 |
| 稠密点云/网格地图 | 点云、TSDF、Mesh | 重建、避障 | 几何完整 | 存储与计算开销大 |
| 栅格占据地图 | 二维/三维栅格 | 路径规划 | 便于规划算法使用 | 分辨率与内存矛盾 |
| 语义地图 | 带标签的对象/区域 | 场景理解、人机交互 | 高层语义信息 | 依赖语义分割精度 |
四、经典视觉SLAM流程与关键模块
以 ORB-SLAM 系列为代表的经典视觉SLAM，通常包含以下模块：传感器数据读取与预处理、前端跟踪、局部建图、回环检测、全局优化与地图输出。各模块之间通过共享的位姿与地图点数据库耦合，形成闭环。
五、案例场景：室内机器人建图
假设一台搭载 RGB-D 相机的室内服务机器人，在房间中缓慢移动。前端从深度图中提取 ORB 特征并匹配，估计相邻帧位姿；后端维护一个滑动窗口，对最近若干帧做局部 BA；当机器人绕行一圈回到起点附近时，回环检测通过词袋模型识别出相似场景，后端加入回环约束并做全局位姿图优化，轨迹漂移被显著修正。最终输出一张稀疏点云地图与优化后的轨迹，供后续导航与语义标注使用。
六、常见误区与复盘
- 误区一：把前端当全部。 只做视觉里程计而不做后端优化，误差会随时间无界累积，长距离运行必然漂移。
- 误区二：忽视回环检测。 没有回环约束，后端只能在局部保持一致，无法消除全局漂移。
- 误区三：地图越稠密越好。 地图表示应与任务匹配，导航任务用栅格或语义地图往往比原始稠密点云更实用。
- 复盘要点： 遇到轨迹漂移时，先检查前端特征匹配质量与标定参数，再检查后端优化窗口与回环阈值，最后检查传感器时间同步与运动模型是否合理。