滑动窗口法——实践中的BA

引入

带有相机位姿和空间点的图优化称为BA，它能够有效地求解大规模的定位与建图问题。但是在SLAM过程中，往往需要控制BA的规模，以保持计算的实时性。
最简单的控制BA规模的思路，是仅保留离当前时刻最近的N个关键帧。于是，我们的BA将被固定在一个时间窗口内，离开这个窗口的则被丢弃。这种方法称为滑动窗口法。

SLAM中的滑动窗口法

1. 问题建模

目标：在计算资源受限的情况下，通过维护一个固定大小的状态窗口，持续优化最新的相机位姿和地图点，同时边缘化旧状态以保持计算效率。

变量定义：

• 滑动窗口内包含 $M$ 个相机位姿 { ξ 1 , ξ 2 , … , ξ M } \{\xi_1, \xi_2, \dots, \xi_M\} {ξ1​,ξ2​,…,ξM​}（李代数表示）。
• 窗口内关联的$N$ 个地图点 { P 1 , P 2 , … , P N } \{\mathbf{P}_1, \mathbf{P}_2, \dots, \mathbf{P}_N\} {P1​,P2​,…,PN​}。

优化目标：最小化窗口内所有观测的重投影误差：
$$E(\mathbf{x})=\sum _{i=1}^M\sum _{j\in {\mathcal{V}}_i}{\Vert {\mathbf{e}}_{ij}\Vert }^2,{\mathbf{e}}_{ij}={\mathbf{p}}_{ij}-\pi (T_i{\mathbf{P}}_j)$$
其中${\mathcal{V}}_i$表示第$i$ 个相机观测到的地图点集合。

---

2. 滑动窗口更新流程

当新帧到来时，按以下步骤更新窗口：

(1) 添加新帧

• 将新相机位姿${\xi }_{M+1}$ 加入窗口，关联的地图点通过三角化或特征匹配确定。
• 扩展状态向量$\mathbf{x}$：
  $$\mathbf{x}=[{\xi }_1,\dots ,{\xi }_M,{\mathbf{P}}_1,\dots ,{\mathbf{P}}_N]\to [{\xi }_1,\dots ,{\xi }_{M+1},{\mathbf{P}}_1,\dots ,{\mathbf{P}}_{N+K}]$$

(2) 移除旧帧

• 若窗口大小超过设定值 $M_{\text{max}}$ ，移除最旧的帧${\xi }_1$。
• 关键操作：边缘化${\xi }_1$ 及其独有观测的地图点，保留其对剩余状态的约束。

---

3. 边缘化的数学推导

目标：将旧状态 ${\mathbf{x}}_m=[{\xi }_1,{\mathbf{P}}_k]$的影响转化为剩余状态 ${\mathbf{x}}_r$ 的先验信息。

(1) 原始优化问题

误差函数可分解为：
$$E(\mathbf{x})=E_r({\mathbf{x}}_r,{\mathbf{x}}_m)+E_m({\mathbf{x}}_m)$$
其中：

• $E_r$ 为与${\mathbf{x}}_r$ 和 ${\mathbf{x}}_m$ 相关的误差项。
• $E_m$为仅与${\mathbf{x}}_m$ 相关的误差项。

(2) 边缘化操作

通过舒尔补（Schur Complement）将${\mathbf{x}}_m$ 积分掉：

1. 构建信息矩阵：
   $$\mathbf{H}=\left[\begin{array}{cc}{\mathbf{H}}_{rr}& {\mathbf{H}}_{rm}\\ {\mathbf{H}}_{mr}& {\mathbf{H}}_{mm}\end{array}\right],\mathbf{b}=\left[\begin{array}{c}{\mathbf{b}}_r\\ {\mathbf{b}}_m\end{array}\right]$$
2. 舒尔补计算：
   $${\mathbf{H}}^{\prime }={\mathbf{H}}_{rr}-{\mathbf{H}}_{rm}{\mathbf{H}}_{mm}^{-1}{\mathbf{H}}_{mr},{\mathbf{b}}^{\prime }={\mathbf{b}}_r-{\mathbf{H}}_{rm}{\mathbf{H}}_{mm}^{-1}{\mathbf{b}}_m$$
   新误差函数为：
   $$E^{\prime }({\mathbf{x}}_r)={\mathbf{x}}_r^T{\mathbf{H}}^{\prime }{\mathbf{x}}_r-2{\mathbf{b}}^{\prime T}{\mathbf{x}}_r+\text{const.}$$

(3) 先验项的线性化点

• 边缘化时的线性化点需固定（如使用FEJ策略），确保后续优化的雅可比矩阵与先验一致。

---

4. 滑动窗口优化步骤

1. 初始化：维护窗口内状态$\mathbf{x}=[{\mathbf{x}}_r,{\mathbf{x}}_m]$。
2. 边缘化旧状态：
   • 计算${\mathbf{H}}^{\prime }$ 和${\mathbf{b}}^{\prime }$，生成先验项 $E_{\text{prior}}=\frac{1}{2}\Delta {\mathbf{x}}_r^T{\mathbf{H}}^{\prime }\Delta {\mathbf{x}}_r+{\mathbf{b}}^{\prime T}\Delta {\mathbf{x}}_r$。
3. 添加新观测：
   • 将新帧的观测误差$E_{\text{new}}=\sum \Vert {\mathbf{e}}_{ij}{\Vert }^2$ 加入目标函数。
4. 构建新优化问题：
   $$\underset{\Delta {\mathbf{x}}_r}{\min }\left(E_{\text{prior}}+E_{\text{new}}\right)$$
5. 求解增量方程：
   $$({\mathbf{H}}^{\prime }+{\mathbf{H}}_{\text{new}})\Delta {\mathbf{x}}_r=-({\mathbf{b}}^{\prime }+{\mathbf{b}}_{\text{new}})$$
6. 更新状态：${\mathbf{x}}_r\leftarrow {\mathbf{x}}_r+\Delta {\mathbf{x}}_r$。

---

5. 关键技术与注意事项

1. 稀疏性保持：
   • 窗口内状态的海塞矩阵天然稀疏，需使用稀疏求解器（如Cholesky分解或共轭梯度法）。
2. FEJ（First Estimate Jacobians）：
   • 所有观测的雅可比矩阵在第一次估计时固定，避免边缘化引入线性化点不一致。
3. 鲁棒性处理：
   • 对误匹配使用Huber核函数，降低外点影响：
     $$\rho (e)=\{\begin{array}{ll}\frac{1}{2}{e}^2& |e|\le \delta \\ \delta |e|-\frac{1}{2}{\delta }^2& |e|>\delta \end{array}$$
4. 窗口大小选择：
   • 平衡计算量与精度，通常选择 $M_{\text{max}}\in [5,15]$。

---

6. 算法伪代码

Algorithm: Sliding Window SLAM
Input: 新帧图像及观测数据
Output: 窗口内优化后的相机位姿和地图点

1. while 新帧到达 do
2.   添加新帧到窗口末尾
3.   if 窗口大小 > M_max then
4.     标记待边缘化的旧帧及独有地图点
5.     构建原问题的H和b，分割为x_r和x_m
6.     计算舒尔补得到H'和b'
7.     从窗口中移除旧帧及独有地图点
8.   end if
9.   构建新观测的误差项，更新H_total = H' + H_new
10.  求解 H_total Δx = -b_total
11.  更新窗口内状态 x_r ← x_r + Δx
12. end while

---

7. 与全局BA的对比

特性	滑动窗口法	全局BA
优化变量	窗口内局部状态	全部历史状态
计算复杂度	( O(M^3 + N) )	( O((M+N)^3) )
内存占用	低（仅维护窗口）	高（需存储全部数据）
精度	局部一致，可能累积漂移	全局一致
适用场景	实时SLAM（如VIO、无人机）	离线重建、闭环优化后调整

---

8. 总结

滑动窗口法通过动态维护一个局部状态窗口，结合边缘化技术保留旧状态的约束，在有限计算资源下实现高效优化。其核心在于：

1. 窗口更新：动态添加新帧、移除旧帧。
2. 边缘化操作：通过舒尔补将移除状态的影响转化为先验信息。
3. 稀疏优化：利用海塞矩阵的稀疏性加速求解。

该方法在实时SLAM系统（如OKVIS、VINS-Mono）中被广泛应用，平衡了计算效率与精度。