视觉SLAM的后端主要解决状态估计问题，它是优化相机轨迹和地图点的过程，从数学上看属于非线性优化问题。后端的目标是结合传感器数据，通过最优估计获取系统的状态（包括相机位姿和场景结构），在状态估计过程中通常引入概率模型，以最大化后验概率或最小化代价函数的方式求解。以下从概述、BA（Bundle Adjustment，捆绑调整）与图优化的角度，全面介绍视觉SLAM的后端实现。
概述
状态估计的概率解释
SLAM问题本质上是一个状态估计问题，常用贝叶斯滤波进行建模： p ( x t ∣ z 1 : t , u 1 : t ) ∝ p ( z t ∣ x t ) ∫ p ( x t ∣ x t − 1 , u t ) p ( x t − 1 ∣ z 1 : t − 1 , u 1 : t − 1 ) d x t − 1 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} p(xt∣z1:t,u1:t)∝p(zt∣xt)∫p(xt∣xt−1,ut)p(xt−1∣z1:t−1,u1:t−1)dxt−1
- x t \mathbf{x}_t xt ：系统状态（如相机位姿）。
- z t \mathbf{z}_t zt ：观测（如图像特征点）。
- u t \mathbf{u}_t ut ：控制量或运动模型。
该公式表明，状态估计是基于当前观测、运动模型和历史状态的一种递归估计。
线性系统和卡尔曼滤波（KF）
- 线性系统模型：卡尔曼滤波假设状态转移方程和观测方程是线性关系：
x t = A t x t − 1 + B t u t + w t \mathbf{x}_t=\mathbf{A}t\mathbf{x}{t-1}+\mathbf{B}_t\mathbf{u}_t+\mathbf{w}_t xt=Atxt−1+Btut+wt
z t = H t x t + v t \mathbf{z}_t=\mathbf{H}_t\mathbf{x}_t+\mathbf{v}_t zt=Htxt+vt
其中， w t \mathbf{w}_t wt和 v t \mathbf{v}_t vt是噪声，通常服从高斯分布。 - 卡尔曼滤波提供了最优状态估计：
- 预测：基于运动模型预测当前状态。
- 更新：结合观测数据修正状态。
卡尔曼滤波具有很高的计算效率，但只能处理线性系统。
非线性系统和扩展卡尔曼滤波（EKF）
- 在实际SLAM中，状态转移方程和观测方程通常是非线性的： x t = f ( x t − 1 , u t ) + w t \mathbf{x}t=f(\mathbf{x}{t-1},\mathbf{u}_t)+\mathbf{w}_t xt=f(xt−1,ut)+wt
z t = h ( x t ) + v t \mathbf{z}_t=h(\mathbf{x}_t)+\mathbf{v}_t zt=h(xt)+vt - EKF通过对非线性函数进行一阶线性化，将非线性问题近似为线性问题：
- 使用雅可比矩阵线性化 𝑓(⋅) 和 ℎ(⋅)
- 在每一步更新中，使用卡尔曼滤波进行递归估计。
EKF的讨论
- 优点：
- EKF可以处理非线性系统，适用于SLAM问题。
- 局限性：
- 一阶线性化导致近似误差，特别是在非线性程度较高时效果较差。
- 计算复杂度较高，不适合大规模SLAM系统。
因此，在实际的视觉SLAM中，后端更多使用非线性优化方法，如捆绑调整（BA）与图优化。
BA与图优化
投影模型与BA代价函数
- 投影模型
在视觉SLAM中，2D观测点 z i \mathbf{z}_i zi 与3D地图点 P j \mathbf{P}_j Pj 的关系由投影模型描述： z i = π ( K [ R ∣ t ] P j ) \mathbf{z}_i=\pi(\mathbf{K}[\mathbf{R}|\mathbf{t}]\mathbf{P}_j) zi=π(K[R∣t]Pj)- K：相机内参矩阵。
-
R ∣ t \] \[\\mathbf{R}\|\\mathbf{t}\] \[R∣t\]：相机位姿。
- BA代价函数
BA的目标是最小化重投影误差，代价函数为： E = ∑ i , j ρ ( ∥ z i − π ( K [ R i ∣ t i ] P j ) ∥ 2 ) E=\sum_{i,j}\rho\left(\|\mathbf{z}_i-\pi(\mathbf{K}[\mathbf{R}_i|\mathbf{t}_i]\mathbf{P}_j)\|^2\right) E=i,j∑ρ(∥zi−π(K[Ri∣ti]Pj)∥2)- ρ ( ⋅ ) \rho(\cdot) ρ(⋅)：鲁棒核函数，用于减小异常值的影响。
BA的求解
BA是一个非线性最小二乘优化问题，通常使用高斯-牛顿法或列文伯格-马夸尔特（LM）法求解：
- 高斯-牛顿法：直接使用二阶近似求解非线性问题，收敛快但对初值敏感。
- LM方法：在梯度下降和高斯-牛顿之间平衡，鲁棒性更好。
具体流程：
- 初始化相机位姿和地图点坐标。
- 计算雅可比矩阵，并构造稀疏的线性方程。
- 迭代更新位姿和地图点，直至误差收敛。
稀疏性与边缘化
- 稀疏性
BA问题的雅可比矩阵具有稀疏性，因为每个观测点仅依赖于特定的相机位姿和地图点。利用稀疏矩阵计算，可以显著提高优化效率。 - 边缘化
在SLAM中，为了减小计算量，会将旧的状态变量边缘化：- 将不再需要优化的变量（如历史关键帧）边缘化。
- 通过边缘化保持稀疏结构，并降低优化问题的维度。
鲁棒核函数
实际观测中常有异常值（outliers），如错误匹配的特征点。为减小异常值对优化的影响，BA中引入鲁棒核函数：
- 常见核函数有Huber核、Cauchy核等。
- 核函数通过降低异常值的权重，使得优化结果更加鲁棒。
总结
视觉SLAM后端通过概率模型和非线性优化方法实现状态估计和地图构建。其核心任务是通过最大化后验概率或最小化重投影误差，优化相机轨迹和地图点位置。
- 状态估计：从卡尔曼滤波（KF）到扩展卡尔曼滤波（EKF），提供递归的状态更新方案。
- BA与图优化：通过非线性最小二乘优化（如BA），实现全局优化。
- 鲁棒性和效率：利用稀疏矩阵计算、边缘化和鲁棒核函数，提升系统的计算效率和鲁棒性。
这种多层次的优化体系是视觉SLAM后端的核心，也是其能在复杂环境中实现鲁棒性能的关键。