【视觉SLAM:八、后端Ⅱ】

视觉SLAM后端的核心任务是估计相机的轨迹和场景的三维结构，这需要解决非线性优化问题。为了保证效率和精度，后端主要依赖以下两种方法：滑动窗口法（基于局部优化的策略）和位姿图优化（基于全局优化的策略）。以下从滑动窗口滤波和优化、位姿图的构建与优化等角度进行详细介绍。

滑动窗口滤波和优化

视觉SLAM后端中，捆绑调整（BA, Bundle Adjustment）是最常见的优化方法。它通过最小化所有观测点的重投影误差来优化相机位姿和地图点的三维位置。然而，实际环境下直接对整个轨迹和地图点进行全局BA有以下问题：

因此，后端通常采用局部优化结构，即在一个有限的窗口内进行优化，滑动窗口法是一种高效的实现。

滑动窗口的核心思想

滑动窗口法是一种局部优化策略，它只在最近的若干帧内进行优化，而对更早的帧进行边缘化处理。这种方法有效降低了优化的维度，同时保留了对当前状态的高精度估计。
滑动窗口的基本流程
- 选取窗口内的关键帧：窗口包含当前帧和最近的若干关键帧，通常大小为固定数量（如10-20帧）。
- 构建优化问题：对窗口内的相机位姿、地图点进行联合优化，最小化重投影误差。
- 边缘化早期关键帧：将窗口外的关键帧边缘化，生成约束条件，减少优化变量的数量。
优化目标

滑动窗口法的优化目标是最小化以下代价函数： E = ∑ i , j ρ ( ∥ z i j − π ( K [ R i ∣ t i ] P j ) ∥ 2 ) E=\sum_{i,j}\rho\left(\|\mathbf{z}{i}^{j}-\pi(\mathbf{K}[\mathbf{R}{i}|\mathbf{t}{i}]\mathbf{P}{j})\|^{2}\right) E=i,j∑ρ(∥zij−π(K[Ri∣ti]Pj)∥2)
- z i j z_i^j zij：第 𝑖 个关键帧中观测到第 𝑗 个地图点的2D坐标。
- P j \mathbf{P}_j Pj ：地图点的三维坐标。
- $R i ∣ t i \] \[\\mathbf{R}_i\|\\mathbf{t}_i\] \[Ri∣ti\]：第 𝑖 个相机的位姿。$
滑动窗口法的优点
- 计算效率高：只优化窗口内的关键帧，优化维度远小于全局BA。
- 实时性强：适合嵌入式或资源有限的系统，实现实时位姿估计。
- 局部优化结果稳定：在局部窗口内保证较高的精度，适合动态场景。

位姿图是SLAM后端优化的另一种重要方法，适用于更大规模的优化问题。它将SLAM中的相机位姿表示为图中的顶点，观测关系表示为图中的边，通过优化图结构来获取全局一致的轨迹和地图。

什么是位姿图
- 顶点：每个顶点表示一个相机位姿，通常用六维的 (𝑅,𝑡) 或李代数形式 𝜉∈𝑠𝑒(3) 表示。
- 边：每条边表示两个相机位姿之间的相对运动约束，约束可以来自于视觉里程计（如相邻帧的匹配）、回环检测、IMU等。
位姿图在SLAM中的作用
- 在局部优化（如滑动窗口）基础上，位姿图优化通过全局约束（如回环检测）消除累积误差，获取全局一致的相机轨迹。
- 位姿图能够同时优化多种约束，融合多传感器数据，提升系统鲁棒性。

优化目标
位姿图的优化目标是最小化所有边的误差： E = ∑ i , j ρ ( ∥ e i j ∥ 2 ) E=\sum_{i,j}\rho\left(\|\mathbf{e}_{ij}\|^2\right) E=i,j∑ρ(∥eij∥2)
- e i j \mathrm{eij} eij ：第 𝑖i和 𝑗 个相机之间的误差，定义为： e i j = T i j − ( T i − 1 ⋅ T j ) \mathbf{e}{ij}=\mathbf{T}{ij}-(\mathbf{T}_i^{-1}\cdot\mathbf{T}_j) eij=Tij−(Ti−1⋅Tj)
  其中， T i , T j \mathrm{T}_i,\mathrm{T}j Ti,Tj 是相机的位姿， T i j \mathbf{T}{ij} Tij 是两帧间的相对位姿。
优化方法
位姿图优化属于非线性最小二乘问题，通常使用高斯-牛顿法或列文伯格-马夸尔特法（LM法）。在实际应用中，常使用稀疏线性代数库（如g2o、Ceres Solver）进行高效求解。
误差约束的来源
- 视觉里程计：相邻关键帧之间的相对位姿。
- 回环检测：闭环帧之间的相对位姿约束。
- IMU约束：来自惯性传感器的位姿约束。
- GPS约束：如果有GPS数据，可以将绝对位置信息作为约束。

视觉SLAM后端的两种核心优化方法：

在实际SLAM系统中，这两种方法往往结合使用：前端实时使用滑动窗口优化相机轨迹，后端通过位姿图优化消除全局误差，提升系统的全局精度和一致性。