从VINS到g2o：聊聊SLAM/VIO里‘固定参考系’那点事儿，到底选哪种gauge handle？-平芜编程栈

视觉惯性里程计中不可观测自由度的工程实践指南

在视觉惯性里程计（VIO）和同步定位与地图构建（SLAM）系统中，如何处理不可观测自由度（gauge freedom）是每个工程师都会遇到的棘手问题。想象一下，当你精心设计的优化算法在测试数据上表现完美，却在真实场景中因为参考系漂移而崩溃时，那种挫败感足以让人彻夜难眠。本文将带你深入理解四种主流处理方法的内在机理，从工程实现的角度分析它们的适用场景和潜在陷阱。

1. 不可观测自由度的本质与影响

不可观测自由度源于系统缺乏绝对参考信息。在仅使用相机和IMU的VIO系统中，我们常遇到四种不可观测自由度：三维平移和绕重力方向的旋转（yaw）。这意味着整个系统可以在空间中自由移动和旋转而不影响观测残差。

数学上，这表现为Hessian矩阵H的秩亏缺。以一个简单的线性系统为例：

H = J^TJ = \begin{bmatrix} 2 & -1 & 0 & -1 \\ -1 & 3 & -1 & -1 \\ 0 & -1 & 2 & -1 \\ -1 & -1 & -1 & 3 \end{bmatrix}

这个矩阵的每一行和为零，rank(H)=3，表明系统存在一个自由度不可观。直接求解这样的系统会导致数值不稳定，甚至发散。

工程实践中常见的症状包括：

轨迹整体漂移
协方差矩阵计算异常
闭环检测失效
不同运行结果间无法对齐

2. Free Gauge方法：VINS-Mono的实践智慧

VINS-Mono采用了一种看似简单却极为精妙的解决方案——在优化过程中不显式处理不可观自由度，而在优化后手动对齐参考系。这种方法的核心优势在于保持了优化问题的原始形式，避免了人为引入的约束可能带来的数值问题。

具体实现分为三个步骤：

自由优化阶段：使用标准的Levenberg-Marquardt算法求解，依赖λI项保证数值稳定性
参考系对齐：将滑窗中第0帧的位姿固定为优化前的值
全局一致性维护：通过位姿图优化或闭环校正处理累积误差

MATLAB实现的关键代码片段：

% 自由优化 for i = 1:5 r = compute_residuals(x); b = -J' * r; delta_x = (H + miu*eye(4)) \ b; x = x + delta_x; end % 手动对齐 x = x + 0 - x(1); % 固定第0帧位置

适用场景：

实时性要求高的系统
需要频繁进行闭环校正的场景
多传感器融合系统中存在其他绝对观测

3. Fix Gauge方法：数学技巧的工程实现

Fix Gauge通过直接修改雅可比矩阵来消除不可观自由度。具体做法是将与参考帧（通常为第0帧）相关的所有雅可比项置零，相当于在数学上移除了这些状态变量对残差的影响。

这种方法会产生如下形式的Hessian矩阵：

H_{fix} = \begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 3 & -1 & -1 \\ 0 & -1 & 2 & -1 \\ 0 & -1 & -1 & 3 \end{bmatrix}

在L-M算法中，添加的μI项会使得ΔP0的方程变为μΔP0=0，从而固定参考帧。这种方法的迭代速度通常较慢，需要通过调节μ值来平衡收敛速度和稳定性。

工程注意事项：

固定过多的自由度可能导致系统过度约束
需要仔细选择参考帧（通常选择观测最丰富的帧）
在边缘化操作时要特别注意固定帧的处理

4. Prior Gauge方法：物理意义明确的解决方案

Prior Gauge是最符合概率学原理的方法，它通过添加先验信息来解决不可观问题。这种方法有明确的物理解释——相当于为系统提供了虚拟的绝对观测。

实现时需要特别注意先验权重的选择：

w = 30; % 先验权重 J_prior = [-w, 0, 0, 0; % 先验残差的雅可比 1, 0, 0, -1; 1, -1, 0, 0; 0, 1, -1, 0; 0, 1, 0, -1; 0, 0, 1, -1];

权重选择经验法则：

权重应远大于其他残差项（通常大1-2个数量级）
但不宜过大，否则会导致H矩阵病态
可以通过实验观察收敛速度来调整

优势：

保持概率框架的完整性
便于扩展（如融合GPS等绝对观测）
能正确计算协方差矩阵

5. G2O内部方法：库作者的工程妥协

g2o等优化库通常采用一种特殊的处理方式——在H矩阵的对角线上直接添加单位矩阵。这种方法本质上是通过修改问题本身来保证数值可解性。

数学上，这相当于在原始问题中添加了额外的约束：

\begin{bmatrix} h_{11}+1 & h_{12} & h_{13} & h_{14} \\ h_{21} & h_{22} & h_{23} & h_{24} \\ h_{31} & h_{32} & h_{33} & h_{34} \\ h_{41} & h_{42} & h_{43} & h_{44} \end{bmatrix} \begin{bmatrix} \Delta P_0 \\ \Delta P_1 \\ \Delta P_2 \\ \Delta L \end{bmatrix} = \begin{bmatrix} b_1 \\ b_2 \\ b_3 \\ b_4 \end{bmatrix}

实现技巧：

通常只需修改H矩阵，不改变b向量
可以灵活选择要固定的自由度
适合嵌入到现有的优化框架中

6. 工程选型指南：何时用何种方法

根据我们在多个实际项目中的经验，给出以下建议：

方法	适用场景	优点	缺点
Free Gauge	实时VIO系统，有闭环校正	实现简单，数值稳定	需要后处理对齐
Fix Gauge	离线处理，固定轨迹起点	概念清晰	可能过度约束
Prior Gauge	需要协方差估计，多传感器融合	概率严谨	权重选择敏感
G2O方式	使用g2o库的系统	库原生支持	黑箱操作

特殊场景建议：

无人机应用：Prior Gauge更适合，因为需要可靠的协方差估计
AR/VR设备：Free Gauge+VINS式后处理组合效果最佳
大规模建图：考虑分层处理，局部用Free Gauge，全局用Prior Gauge

7. 协方差计算的隐藏陷阱

不可观测自由度的处理方式直接影响协方差矩阵的计算。我们发现：

Free Gauge方法计算的协方差过于乐观
Fix Gauge会低估不确定度
Prior Gauge能得到最合理的结果
G2O方式的协方差解释性较差

一个实用的工程解决方案是：

# Python伪代码 def compute_covariance(H, method='prior'): if method == 'prior': H += prior_weight * np.eye(H.shape[0]) elif method == 'free': H += 1e-6 * np.eye(H.shape[0]) # 微小正则化 return np.linalg.pinv(H)

在VINS-Mono的实际实现中，开发者选择在边缘化时保留先验信息，这实际上是一种混合策略，既保持了数值稳定性，又获得了合理的协方差估计。