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

引入

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

SLAM中的滑动窗口法 1. 问题建模

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

变量定义：

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

优化目标：最小化窗口内所有观测的重投影误差： E ( x ) = ∑ i = 1 M ∑ j ∈ V i ∥ e i j ∥ 2 , e i j = p i j − π ( T i P j ) E(\mathbf{x}) = \sum_{i=1}^M \sum_{j \in \mathcal{V}_i} \left\| \mathbf{e}_{ij} \right\|^2, \quad \mathbf{e}_{ij} = \mathbf{p}_{ij} - \pi(T_i \mathbf{P}_j) E(x)=i=1∑M​j∈Vi​∑​∥eij​∥2,eij​=pij​−π(Ti​Pj​) 其中 V i \mathcal{V}_i Vi​表示第 i i i 个相机观测到的地图点集合。

---

2. 滑动窗口更新流程

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

(1) 添加新帧 将新相机位姿 ξ M + 1 \xi_{M+1} ξM+1​ 加入窗口，关联的地图点通过三角化或特征匹配确定。扩展状态向量 x \mathbf{x} x： x = [ ξ 1 , … , ξ M , P 1 , … , P N ] → [ ξ 1 , … , ξ M + 1 , P 1 , … , P N + K ] \mathbf{x} = [\xi_1, \dots, \xi_M, \mathbf{P}_1, \dots, \mathbf{P}_N] \rightarrow [\xi_1, \dots, \xi_{M+1}, \mathbf{P}_1, \dots, \mathbf{P}_{N+K}] x=[ξ1​,…,ξM​,P1​,…,PN​]→[ξ1​,…,ξM+1​,P1​,…,PN+K​] (2) 移除旧帧 若窗口大小超过设定值 M max M_{\text{max}} Mmax​ ，移除最旧的帧 ξ 1 \xi_1 ξ1​。关键操作：边缘化 ξ 1 \xi_1 ξ1​ 及其独有观测的地图点，保留其对剩余状态的约束。

---

3. 边缘化的数学推导

目标：将旧状态 x m = [ ξ 1 , P k ] \mathbf{x}_m = [\xi_1, \mathbf{P}_k] xm​=[ξ1​,Pk​]的影响转化为剩余状态 x r \mathbf{x}_r xr​ 的先验信息。

(1) 原始优化问题

误差函数可分解为： E ( x ) = E r ( x r , x m ) + E m ( x m ) E(\mathbf{x}) = E_r(\mathbf{x}_r, \mathbf{x}_m) + E_m(\mathbf{x}_m) E(x)=Er​(xr​,xm​)+Em​(xm​) 其中：

E r E_r Er​ 为与 x r \mathbf{x}_r xr​ 和 x m \mathbf{x}_m xm​ 相关的误差项。 E m E_m Em​为仅与 x m \mathbf{x}_m xm​ 相关的误差项。 (2) 边缘化操作

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

构建信息矩阵： H = [ H r r H r m H m r H m m ] , b = [ b r b m ] \mathbf{H} = \begin{bmatrix} \mathbf{H}_{rr} & \mathbf{H}_{rm} \\ \mathbf{H}_{mr} & \mathbf{H}_{mm} \end{bmatrix}, \quad \mathbf{b} = \begin{bmatrix} \mathbf{b}_r \\ \mathbf{b}_m \end{bmatrix} H=[Hrr​Hmr​​Hrm​Hmm​​],b=[br​bm​​]舒尔补计算： H ′ = H r r − H r m H m m − 1 H m r , b ′ = b r − H r m H m m − 1 b m \mathbf{H}' = \mathbf{H}_{rr} - \mathbf{H}_{rm} \mathbf{H}_{mm}^{-1} \mathbf{H}_{mr}, \quad \mathbf{b}' = \mathbf{b}_r - \mathbf{H}_{rm} \mathbf{H}_{mm}^{-1} \mathbf{b}_m H′=Hrr​−Hrm​Hmm−1​Hmr​,b′=br​−Hrm​Hmm−1​bm​ 新误差函数为： E ′ ( x r ) = x r T H ′ x r − 2 b ′ T x r + const. E'(\mathbf{x}_r) = \mathbf{x}_r^T \mathbf{H}' \mathbf{x}_r - 2 \mathbf{b}'^T \mathbf{x}_r + \text{const.} E′(xr​)=xrT​H′xr​−2b′Txr​+const. (3) 先验项的线性化点 边缘化时的线性化点需固定（如使用FEJ策略），确保后续优化的雅可比矩阵与先验一致。

---

4. 滑动窗口优化步骤 初始化：维护窗口内状态 x = [ x r , x m ] \mathbf{x} = [\mathbf{x}_r, \mathbf{x}_m] x=[xr​,xm​]。边缘化旧状态： 计算 H ′ \mathbf{H}' H′ 和 b ′ \mathbf{b}' b′，生成先验项 E prior = 1 2 Δ x r T H ′ Δ x r + b ′ T Δ x r E_{\text{prior}} = \frac{1}{2} \Delta \mathbf{x}_r^T \mathbf{H}' \Delta \mathbf{x}_r + \mathbf{b}'^T \Delta \mathbf{x}_r Eprior​=21​ΔxrT​H′Δxr​+b′TΔxr​。 添加新观测： 将新帧的观测误差 E new = ∑ ∥ e i j ∥ 2 E_{\text{new}} = \sum \| \mathbf{e}_{ij} \|^2 Enew​=∑∥eij​∥2 加入目标函数。 构建新优化问题： min ⁡ Δ x r ( E prior + E new ) \min_{\Delta \mathbf{x}_r} \left( E_{\text{prior}} + E_{\text{new}} \right) Δxr​min​(Eprior​+Enew​)求解增量方程： ( H ′ + H new ) Δ x r = − ( b ′ + b new ) (\mathbf{H}' + \mathbf{H}_{\text{new}}) \Delta \mathbf{x}_r = -(\mathbf{b}' + \mathbf{b}_{\text{new}}) (H′+Hnew​)Δxr​=−(b′+bnew​)更新状态： x r ← x r + Δ x r \mathbf{x}_r \leftarrow \mathbf{x}_r + \Delta \mathbf{x}_r xr​←xr​+Δxr​。

---

5. 关键技术与注意事项 稀疏性保持： 窗口内状态的海塞矩阵天然稀疏，需使用稀疏求解器（如Cholesky分解或共轭梯度法）。 FEJ（First Estimate Jacobians）： 所有观测的雅可比矩阵在第一次估计时固定，避免边缘化引入线性化点不一致。 鲁棒性处理： 对误匹配使用Huber核函数，降低外点影响： ρ ( e ) = { 1 2 e 2 ∣ e ∣ ≤ δ δ ∣ e ∣ − 1 2 δ 2 ∣ e ∣ > δ \rho(e) = \begin{cases} \frac{1}{2} e^2 & |e| \leq \delta \\ \delta |e| - \frac{1}{2} \delta^2 & |e| > \delta \end{cases} ρ(e)={21​e2δ∣e∣−21​δ2​∣e∣≤δ∣e∣>δ​ 窗口大小选择： 平衡计算量与精度，通常选择 M max ∈ [ 5 , 15 ] M_{\text{max}} \in [5, 15] Mmax​∈[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. 总结

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

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

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