Home / 机器人经典基础 / g2o：把不同估计问题放进同一个稀疏优化框架

063｜g2o：把不同估计问题放进同一个稀疏优化框架

论文：Rainer Kümmerle、Giorgio Grisetti、Hauke Strasdat、Kurt Konolige、Wolfram Burgard，g2o: A General Framework for Graph Optimization，ICRA 2011。作者论文入口 · 可读全文镜像 · 官方代码。

先接住前作的问题

GraphSLAM写出了稀疏目标，不同任务各写求解器又会重复工程。g2o提供通用参数块和残差框架；ORB-SLAM可将它作后端，与iSAM的增量路线并行。

五个前置概念

残差：预测与观测的差异，是优化直接压低的量。
信息矩阵：协方差的逆，用于按不确定性加权误差。
雅可比：描述变量微小变化怎样影响残差。
填充：稀疏消元时出现原矩阵中没有的非零项。
规范自由度：不改变观测预测的整体坐标变换，需要参考约束处理。

一、问题设定：不同估计任务怎样复用稀疏非线性求解基础设施

机器人估计位置、相机恢复三维点，都可以写成同一种问题：调整一组未知变量，让它们尽量解释测量。

2.1 顶点是未知量，边是测量约束

位姿图中，每个顶点是一时刻的机器人位姿；边可以表示里程计或回环给出的相对位姿。BA 中，一类顶点是相机，另一类是三维点；图像观测约束相机与点之间的投影关系。

这里的“图优化”不是在地图上找最短路线，而是优化图中各变量的数值。连接关系通常来自前端数据关联，优化器并不知道一条错误回环为何被加入。

没有 RL 的动作、奖励或终止回合。输入是测量与初值，输出是优化后的变量；停止条件是收敛判据或迭代上限。若定位系统持续收到新测量，可以重复调用优化器，但这不等于框架本身使用了专门的增量分解算法。

2.2 稀疏性是效率来源

一张图像只看见部分三维点，一个里程计测量通常只联系两个相邻位姿。因此一个残差只依赖全部变量中的小部分。

假设共有一百万个变量，而一个残差只连接六个。如果每次都构造对所有变量的稠密导数，就会存储大量零。图结构让框架提前知道哪些块可能非零，把内存和计算集中到实际关系上。

但稀疏输入不保证消元后的矩阵同样稀疏。变量消去会产生填充，排序和问题结构因此十分重要。

二、分离变量更新、残差与块矩阵求解

如果为每个应用重新写非线性优化器，许多工作会重复；如果使用完全忽略结构的通用求解器，大型问题又可能太慢。g2o 在两者之间提供了一种设计：应用只定义变量、误差和更新方式，框架负责稀疏线性化、矩阵组织与数值求解。

3.1 目标函数和统计解释

论文式（1）的基本形式为：

$F(x)=\sum_{(i,j)}e_{ij}(x_i,x_j)^\top \Omega_{ij}e_{ij}(x_i,x_j).$

$x_i$ 是参数块， $e_{ij}$ 是预测与测量不一致的残差， $\Omega_{ij}$ 是信息矩阵。高斯噪声假设下，它与最大似然估计相联系。

信息矩阵不是任意“置信度分数”。若某个方向噪声较小，那个方向的误差通常应受到更大惩罚。坐标单位和协方差约定写错，会让求解器高效地优化错误目标。

3.2 一次迭代做什么

在当前估计附近线性化残差：

$e(x\boxplus\Delta x)\approx e(x)+J\Delta x, \quad H=\sum J^\top\Omega J, \quad b=\sum J^\top\Omega e.$

Gauss–Newton 求解 $H\Delta x=-b$ 。LM 则增加阻尼，并根据新状态的实际成本决定是否接受更新。框架并没有发明这两种算法，它将其与高效构建线性系统的实现组合。

论文图 3 的方程示意把右侧画成 $b$ ，而式（14）、（16）及上面的 $b$ 定义对应 $-b$ 。复现应坚持一套一致符号约定；不能把示意图中的正号混入正文推导。

原论文图3：g2o由误差和雅可比到线性结构、求解器和状态更新的流程 — Rainer Kümmerle、Giorgio Grisetti、Hauke Strasdat、Kurt Konolige、Wolfram Burgard（2011），图 3，PDF 第 4 页；原文与图注。原版权保留。

读图：线性结构与求解器分别可选，系统效率不能仅归于一种线性求解方法。

3.3 为什么使用 $\boxplus$

三维旋转不是任意九个数，也不是任意四元数。直接把普通向量增量加到旋转矩阵，可能破坏正交性；任意改变四元数分量也可能离开单位球面。

框架允许状态采用稳定的冗余表示，而增量采用局部最小表示，再通过合法组合更新。这将“变量如何存储”与“优化在什么局部坐标中前进”分开。

3.4 Schur 补为什么有时有效

把相机变量记作 $p$ ，三维点记作 $l$ ，线性系统分为四块。先消去点变量，得到相机系统：

$(H_{pp}-H_{pl}H_{ll}^{-1}H_{lp})\Delta p =-b_p+H_{pl}H_{ll}^{-1}b_l.$

普通 BA 的点之间没有直接观测约束， $H_{ll}$ 是块对角，局部求解便宜。相机通常远少于三维点，消元能大幅缩小主系统。

这是代数消元，不是把三维点永久删除。求出相机增量后还要回代点增量。如果相机很多而点很少，消元可能只缩小一点规模，却让剩余相机之间变得密集，反而更慢。

固定规范自由度是实际建模的必要检查：若只有相对位姿约束，整体平移和旋转不改变残差，线性系统可能有零空间。阻尼使方程可解也不能替代对可观性和参考系的理解。

信息矩阵的一个权重，就是一种噪声承诺

两条独立标量约束给出 $x=0$ 、标准差1，以及 $x=2$ 、标准差0.5。相应信息权重为1和4，代价 $F=x^2+4(x-2)^2$ ，最优解 $x=1.6$ 。若把标准差本身当权重，答案会反向偏向较差测量。这个教学例说明g2o求解器只执行你提供的统计模型，不会替输入残差纠错。

对流形状态，更新应为 $x\leftarrow x\boxplus\delta$ ，其中 $\boxplus$ 将最小局部增量映到合法状态；雅可比必须对同一局部增量定义。四元数的四个存储分量并不等于四维独立旋转增量。信息矩阵也要与残差坐标和单位一致。

本篇把GraphSLAM类目标组织为通用优化框架，与iSAM是求解调度上的平行路线；ORB-SLAM将其作为后端模块。原文第III节明确参数块、约束均值及信息矩阵。计时比较还须保持相同目标、停止条件和稀疏求解器，否则不是单一算法速度的隔离检验。

图接口规定变量与残差，求解效率还由块结构和线性代数决定。下面用同一小图检查装配与更新，再按组件而非只按总时间解读基准。

三、装配并求解线性化系统，用小图检查信息矩阵

3.5 教学伪代码

输入：变量初值、测量图、各类残差及信息矩阵
为变量定义合法局部更新，为测量定义误差与雅可比
固定必要的规范自由度，选择线性求解器
重复直到收敛或达到预算：
    清空稀疏块矩阵 H 和向量 b
    遍历测量约束：
        计算残差及解析或数值雅可比
        仅向相连变量对应的矩阵块累加贡献
    按需要加入 LM 阻尼
    若结构适合：先消去地标，求相机增量，再回代
    否则：直接或迭代求解稀疏系统
    用各变量的 boxplus 构造候选状态
    按所选算法接受更新或调大阻尼重试
输出：估计状态、最终成本及停止信息

自构、未运行的三节点例子

固定 $x_0=0$ ，测得 $x_1-x_0=1$ 、 $x_2-x_1=1$ 、 $x_2-x_0=2.2$ ，各测量权重相同。三个等式无法同时成立，优化器会在残差间折中，而不是挑一个等式作为绝对事实。

若把最后一个错误回环的权重放大一万倍，结果会更接近 2.2，即使真实位移只有 2。这说明框架的工作是服从目标函数；判断哪条测量可信，需要模型或额外鲁棒机制。

四、对照计算预算、求解器和数据集条件

5.1 图 7：通用性不必意味着巨大开销

论文中 g2o 与专用 SPA、sSBA 的表现接近，sSBA 在 BA 上还略快。结论应是通用框架具有竞争力，而非所有场景第一。

Garage 的数值雅可比与解析雅可比对照，在 §V.A 给出每迭代 80 毫秒与 40 毫秒。这说明建模接口的选择有实际代价；不能把两个数字误写成不同地图精度。

5.2 图 9：相同问题，不同局部参数化

两种位姿更新方式在所测 BA 数据上趋向相同解，但迭代过程不同。这支持局部坐标会影响数值收敛，却不证明某种参数化对所有初始化都更好。横轴是迭代次数，也不直接等于总时间。

5.3 表 II：没有万能线性求解器

Venice 的线性求解时间，CHOLMOD 为 1.86 秒、PCG 为 0.287 秒；Victoria 则分别为 0.026 和 1.559 秒。两组反向结果是本篇最值得记住的证据：选择要考虑矩阵结构、条件与停止精度。

表 II 是线性求解阶段，不能与表 III 的组装、消元总时间随意交叉相除，宣称端到端加速。

5.4 表 III：消元也有适用边界

Venice 的直接求解为 33.87 秒，Schur 构建加求解合计 13.03 秒；Victoria 则从 0.026 秒变为 0.150 秒。地标数量多不多、消元后系统有多密，比“用了 Schur 补”这个名字更能解释性能。

这些结果来自旧硬件和特定版本，不能当作现代机器的性能预报。它们的长期价值是指出该测量哪些阶段、哪些结构会改变求解器排名。

Fig. 9：两种图上比较不同旋转参数化的目标下降，横轴为迭代数 — 原论文结果图／表摘引：Rainer Kümmerle、Giorgio Grisetti、Hauke Strasdat、Kurt Konolige、Wolfram Burgard（2011），Fig. 9，PDF 第 6 页；核对原文。原版权保留。

读图：两种图上比较不同旋转参数化的目标下降，横轴为迭代数。更少迭代不一定意味着相同比例的实际加速，还要看残差构建、线性求解与参数更新成本。

图 7 比的是每迭代时间，表 II 是线性求解阶段，表 III 还涉及消元构建；跨范围相除会把局部提速误写成端到端提速。

原论文图7：g2o与多个专用方法在不同数据集上的每迭代耗时柱状图 — Rainer Kümmerle、Giorgio Grisetti、Hauke Strasdat、Kurt Konolige、Wolfram Burgard（2011），图 7，PDF 第 5 页；原文与图注。原版权保留。

读图：纵轴为对数时间刻度；每轮更快与达到相同精度所需总时间要分开比较。

实验口径见 §V、表 I；不同实验测量的时间范围不同。

项目	设置与实际检验问题
任务类型	二维/三维位姿图、地标 SLAM、BA、尺度漂移图
规模示例	Venice：871 相机、530,304 点、2,838,740 约束
计算资源	i7-930 2.8 GHz 的一个核心
主要基线	√SAM、SPA、sSBA、RobotVision；各自覆盖不同任务子集
图 7 预算	完整问题运行 10 次迭代，比较每迭代平均时间
线性求解设置	主框架对照共享 CHOLMOD；另比较 CSparse 与 PCG
PCG 停止	§V.C 使用相对残差阈值 $10^{-8}$
在线对照	Manhattan3500，每节点或每 10 节点触发优化
重复与统计	迭代平均不能等同独立随机试验；公开种子和跨机器重复未报告

公平性的亮点是主框架对照采用相同底层求解器，有助于观察组装效率。局限是十次迭代不保证每个方法达到完全相同终止精度；要比较“达到指定误差所需时间”，还需成本收敛曲线。

五、区分框架效率、建模正确性与全局最优

作者贡献是将通用图表达、合法参数更新、稀疏块计算和可替换求解器统一为可扩展实现。许多数学组件沿用已有方法，新意在于共同架构及效率实现。

实验支持的是测试数据上的优化速度和扩展能力。图中地图更一致不等于每处几何都有独立真值验证；残差降低也不自动消除错误数据关联。

本文分析认为最重要的使用边界有三点。第一，非线性最小二乘依赖初值，不能默认全局最优。第二，测量权重、残差定义和坐标方向仍由使用者负责。第三，线性求解快不等于前端提特征、匹配、回环检测和建图整体都快。

历史版本、编译优化、底层稀疏库和线程设置会影响运行时间。官方仓库持续发展，复现论文应记录具体提交，而非直接把当前默认配置作为原论文设置。

六、追问：因子和测量都没变，为什么重新给一组初值仍要重建线性方程？

因子和测量都没变，为什么重新给一组初值仍要重建线性方程？

参考答案：因子保存的是残差函数与测量，当前估值决定在哪里计算残差和雅可比。借用Dellaert教程§2.2–2.3把因子图与Values分开的讲法，构造一个单位权重的标量例子： $r(x)=x^2-1$ ，故 $J=2x$ 、 $H=4x^2$ 、 $b=2x(x^2-1)$ 。在 $x=2$ 处， $H=16,b=12$ ，Gauss–Newton增量为 $-3/4$ ；在 $x=0$ 处， $H=b=0$ ，局部线性模型连改进方向都没有给出。测量仍是同一个1，变化来自线性化点。这个自构例子说明图结构可复用不等于数值矩阵可原样复用，也不代表原文用这个问题做过实验。

七、把通用求解器接入系统与增量估计

模块组合：ORB-SLAM：从持续跟踪走向能够重访和恢复的单目地图。作为几何后端求解稀疏估计，前端与数据关联仍由系统完成。
平行路线：iSAM2：把不断增长的SLAM问题变成局部树更新。比较增量结构更新与批量求解组织，保持问题和精度口径一致。

八、自测与参考答案

问题 1：g2o 能把一个错误回环优化正确吗？

答案：仅凭最小二乘框架不能。它会按设定权重解释该测量，错误回环可能扭曲整张图。正确数据关联、异常处理和优化求解是不同环节；低残差只说明更符合所给模型。

问题 2：为什么地标消元之后可能更慢？

答案：消元会让共同观察一个地标的相机相互耦合，增加剩余矩阵的连接密度。若消去的变量很少，规模收益不足以抵消构建与填充开销，表 III 的 Victoria 就展示了这种情况。

问题 3：论文里 PCG 快很多，为什么不能统一换成 PCG？

答案：它的迭代次数与矩阵条件、初始估计和容差有关。Venice 上的优势不能推广到 Victoria；比较时还必须分清线性求解时间、完整迭代时间与达到相同最终精度的总时间。