2. 视觉SLAM基础
视觉SLAM的本质，是在一个未知环境中，让机器人一边估计自身运动轨迹，一边构建环境地图。这个看似循环依赖的问题，必须通过严格的数学建模才能求解。本节从概率图模型出发，串联状态估计、运动模型与观测模型，建立后续工程实现的统一理论框架。
一、SLAM问题的概率建模
SLAM可以形式化为一个后验概率估计问题。设机器人在离散时刻 t = 1, 2, ..., T 的状态为 x_t（通常包含位姿、速度等），观测为 z_t，控制输入为 u_t。我们要求的是联合后验概率：
P(x_{1:T}, m | z_{1:T}, u_{1:T})其中 m 表示地图。这个后验概率同时包含了轨迹估计与地图构建。直接求解该联合分布维度极高，因此工程上通常利用条件独立性和马尔可夫性质进行分解。
二、状态估计：从贝叶斯滤波到因子图
状态估计的目标是在噪声观测下递推地估计系统状态。贝叶斯滤波给出了通用递推公式：
预测：P(x_t | z_{1:t-1}, u_{1:t}) = ∫ P(x_t | x_{t-1}, u_t) P(x_{t-1} | z_{1:t-1}, u_{1:t-1}) dx_{t-1}
更新：P(x_t | z_{1:t}, u_{1:t}) ∝ P(z_t | x_t) P(x_t | z_{1:t-1}, u_{1:t})在视觉SLAM中，由于状态维度高且观测模型非线性强，常用实现方式包括扩展卡尔曼滤波（EKF）、粒子滤波以及基于优化的方法。现代视觉SLAM系统（如ORB-SLAM、VINS）更倾向于使用因子图优化，将SLAM建模为最大后验估计问题：
x* = argmax P(x | z) = argmin Σ ||r_i(x)||²_Σi其中 r_i 是残差项，Σi 是信息矩阵。因子图将每个观测或运动约束表示为一个因子，节点为待估计变量，从而把概率推断转化为稀疏非线性最小二乘问题。
三、运动模型与观测模型
运动模型描述状态如何随控制输入演化，通常写作：
x_t = f(x_{t-1}, u_t) + w_t, w_t ~ N(0, Q_t)在视觉SLAM中，u_t 可能来自轮式里程计、IMU预积分或恒速运动假设。运动模型决定了预测的不确定性传播，是前端跟踪和局部优化的基础。
观测模型描述状态如何产生观测：
z_t = h(x_t, m) + v_t, v_t ~ N(0, R_t)对于单目相机，观测通常是图像特征点。若地图点为 P_j，相机位姿为 T_t，则投影方程为：
z_tj = π(T_t · P_j) + v_tj其中 π 是针孔投影函数。观测模型直接决定了重投影误差的形式，也是BA（Bundle Adjustment）优化的核心。
四、概率图模型：从贝叶斯网络到因子图
概率图模型用图结构表达随机变量之间的条件依赖关系。在SLAM中，最常用的两种表示是：
- 贝叶斯网络（有向图）：节点表示状态或观测，有向边表示因果依赖。例如
x_{t-1} → x_t表示运动模型，x_t → z_t表示观测模型。 - 因子图（无向二分图）：变量节点与因子节点交替连接，因子表示局部概率约束。因子图更适合稀疏优化，是现代SLAM后端的主流表示。
因子图将联合概率分解为：
P(X) ∝ ∏_i φ_i(X_i)其中 φ_i 是因子函数，X_i 是该因子涉及的变量子集。在视觉SLAM中，因子通常包括：
| 因子类型 | 连接变量 | 物理含义 | 典型来源 |
|---|---|---|---|
| 运动因子 | x_{t-1}, x_t | 相邻时刻位姿约束 | IMU预积分、轮式里程计 |
| 重投影因子 | x_t, P_j | 地图点投影到图像平面的误差 | 特征点观测 |
| 回环因子 | x_i, x_j | 非相邻时刻的位姿约束 | 回环检测 |
| 先验因子 | x_0 | 初始状态约束 | 初始化或外部参考 |
五、案例：单目相机一帧的建模过程
假设机器人携带单目相机，在时刻 t 观测到三个地图点。其SLAM建模流程如下：
- 读取IMU或里程计数据，构建运动因子
f(x_{t-1}, x_t, u_t)，得到预测位姿。 - 提取图像特征点，与地图点匹配，构建重投影因子
h(x_t, P_j, z_{tj})。 - 将运动因子与重投影因子加入因子图，形成当前窗口的优化问题。
- 使用高斯-牛顿或列文伯格-马夸尔特方法求解，得到位姿与地图点的最大后验估计。
- 根据残差与协方差判断是否触发回环或地图更新。
六、常见误区与复盘
- 误区一：把SLAM当成纯几何问题。忽略概率建模会导致对噪声、外点、不确定性传播缺乏处理能力，系统鲁棒性差。
- 误区二：运动模型与观测模型权重随意设置。Q和R的比值决定了优化更信任预测还是观测，设置不当会导致轨迹漂移或抖动。
- 误区三：认为因子图只能离线优化。现代系统通过滑动窗口、边缘化和增量式求解，已能实现实时因子图优化。
- 误区四：忽略观测模型中的外点。单目特征匹配容易产生误匹配，需引入鲁棒核函数（如Huber）或RANSAC剔除。
综上，视觉SLAM的数学骨架由状态估计、运动模型、观测模型和概率图模型共同构成。运动模型回答“我可能在哪里”，观测模型回答“我看到的是什么”，因子图则回答“如何把两者最优地融合在一起”。掌握这一建模思路，才能在工程实现中正确设计残差、配置噪声参数并诊断系统行为。