【视觉SLAM:八、后端Ⅰ】

视觉SLAM的后端主要解决状态估计问题，它是优化相机轨迹和地图点的过程，从数学上看属于非线性优化问题。后端的目标是结合传感器数据，通过最优估计获取系统的状态（包括相机位姿和场景结构），在状态估计过程中通常引入概率模型，以最大化后验概率或最小化代价函数的方式求解。以下从概述、BA（Bundle Adjustment，捆绑调整）与图优化的角度，全面介绍视觉SLAM的后端实现。

概述

状态估计的概率解释

SLAM问题本质上是一个状态估计问题，常用贝叶斯滤波进行建模：$$p({\mathbf{x}}_t|{\mathbf{z}}_{1:t},{\mathbf{u}}_{1:t})\propto p({\mathbf{z}}_t|{\mathbf{x}}_t)\int p({\mathbf{x}}_t|{\mathbf{x}}_{t-1},{\mathbf{u}}_t)p({\mathbf{x}}_{t-1}|{\mathbf{z}}_{1:t-1},{\mathbf{u}}_{1:t-1})d{\mathbf{x}}_{t-1}$$

• ${\mathbf{x}}_t$ ：系统状态（如相机位姿）。
• ${\mathbf{z}}_t$ ：观测（如图像特征点）。
• ${\mathbf{u}}_t$ ：控制量或运动模型。

该公式表明，状态估计是基于当前观测、运动模型和历史状态的一种递归估计。

线性系统和卡尔曼滤波（KF）

• 线性系统模型：卡尔曼滤波假设状态转移方程和观测方程是线性关系：
  $${\mathbf{x}}_t={\mathbf{A}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\mathbf{w}}_t$$
  $${\mathbf{z}}_t={\mathbf{H}}_t{\mathbf{x}}_t+{\mathbf{v}}_t$$
  其中，${\mathbf{w}}_t$和${\mathbf{v}}_t$是噪声，通常服从高斯分布。
• 卡尔曼滤波提供了最优状态估计：
  • 预测：基于运动模型预测当前状态。
  • 更新：结合观测数据修正状态。

卡尔曼滤波具有很高的计算效率，但只能处理线性系统。

非线性系统和扩展卡尔曼滤波（EKF）

• 在实际SLAM中，状态转移方程和观测方程通常是非线性的：$${\mathbf{x}}_t=f({\mathbf{x}}_{t-1},{\mathbf{u}}_t)+{\mathbf{w}}_t$$
  $${\mathbf{z}}_t=h({\mathbf{x}}_t)+{\mathbf{v}}_t$$
• EKF通过对非线性函数进行一阶线性化，将非线性问题近似为线性问题：
  • 使用雅可比矩阵线性化 𝑓(⋅) 和 ℎ(⋅)
  • 在每一步更新中，使用卡尔曼滤波进行递归估计。

EKF的讨论

• 优点：
  • EKF可以处理非线性系统，适用于SLAM问题。
• 局限性：
  • 一阶线性化导致近似误差，特别是在非线性程度较高时效果较差。
  • 计算复杂度较高，不适合大规模SLAM系统。

因此，在实际的视觉SLAM中，后端更多使用非线性优化方法，如捆绑调整（BA）与图优化。

BA与图优化

投影模型与BA代价函数

• 投影模型
  在视觉SLAM中，2D观测点${\mathbf{z}}_i$ 与3D地图点 ${\mathbf{P}}_j$ 的关系由投影模型描述：$${\mathbf{z}}_i=\pi (\mathbf{K}[\mathbf{R}|\mathbf{t}]{\mathbf{P}}_j)$$
  • K：相机内参矩阵。
  • $[\mathbf{R}|\mathbf{t}]$：相机位姿。
  • π(⋅)：将3D点投影到图像平面。
• BA代价函数
  BA的目标是最小化重投影误差，代价函数为：$$E=\sum _{i,j}\rho \left(\Vert {\mathbf{z}}_i-\pi (\mathbf{K}[{\mathbf{R}}_i|{\mathbf{t}}_i]{\mathbf{P}}_j){\Vert }^2\right)$$
  • $\rho (\cdot )$：鲁棒核函数，用于减小异常值的影响。

BA的求解

BA是一个非线性最小二乘优化问题，通常使用高斯-牛顿法或列文伯格-马夸尔特（LM）法求解：

• 高斯-牛顿法：直接使用二阶近似求解非线性问题，收敛快但对初值敏感。
• LM方法：在梯度下降和高斯-牛顿之间平衡，鲁棒性更好。

具体流程：

• 初始化相机位姿和地图点坐标。
• 计算雅可比矩阵，并构造稀疏的线性方程。
• 迭代更新位姿和地图点，直至误差收敛。

稀疏性与边缘化

• 稀疏性
  BA问题的雅可比矩阵具有稀疏性，因为每个观测点仅依赖于特定的相机位姿和地图点。利用稀疏矩阵计算，可以显著提高优化效率。
• 边缘化
  在SLAM中，为了减小计算量，会将旧的状态变量边缘化：
  • 将不再需要优化的变量（如历史关键帧）边缘化。
  • 通过边缘化保持稀疏结构，并降低优化问题的维度。

鲁棒核函数

实际观测中常有异常值（outliers），如错误匹配的特征点。为减小异常值对优化的影响，BA中引入鲁棒核函数：

• 常见核函数有Huber核、Cauchy核等。
• 核函数通过降低异常值的权重，使得优化结果更加鲁棒。

总结

视觉SLAM后端通过概率模型和非线性优化方法实现状态估计和地图构建。其核心任务是通过最大化后验概率或最小化重投影误差，优化相机轨迹和地图点位置。

• 状态估计：从卡尔曼滤波（KF）到扩展卡尔曼滤波（EKF），提供递归的状态更新方案。
• BA与图优化：通过非线性最小二乘优化（如BA），实现全局优化。
• 鲁棒性和效率：利用稀疏矩阵计算、边缘化和鲁棒核函数，提升系统的计算效率和鲁棒性。

这种多层次的优化体系是视觉SLAM后端的核心，也是其能在复杂环境中实现鲁棒性能的关键。