第26章：g2o 图优化——读懂 ORB-SLAM3 的钥匙¶

核心定位：g2o（General Graph Optimization）是特征法视觉 SLAM 的事实标准优化库。如果你的目标是阅读、修改或扩展 ORB-SLAM2/3 代码，掌握 g2o 是绕不过去的必修课。本章从架构设计到源码精读，带你彻底拆解 g2o 的每一层抽象。

前置自测¶

📋 前置自测（答不出 ≥ 2 题 → 先回对应章节复习）

[通用库·李群manif] 给定李群 \(SE(3)\) 上的元素 \(T\)，其切空间（李代数）的维度是多少？oplusImpl 中的 update 向量应该有几个分量？
[通用库·李群manif] 什么是 \(SE(3)\) 上的 Plus 操作（\(\boxplus\)）？左乘更新 \(T \leftarrow \exp(\hat{\xi}) \cdot T\) 和右乘更新 \(T \leftarrow T \cdot \exp(\hat{\xi})\) 在什么条件下等价？
[通用库·Ceres] 在 Ceres Solver 中，CostFunction 的 Evaluate 方法负责计算什么？残差向量和 Jacobian 矩阵的含义分别是什么？
[通用库·Ceres] Ceres 的 LocalParameterization（或新版的 Manifold）解决什么问题？为什么四元数需要它？
[SLAM库·GTSAM] GTSAM 中 BetweenFactor<Pose3> 的误差函数是怎么定义的？信息矩阵（噪声模型的逆协方差）在优化中起什么作用？

本章目标¶

学完本章后，你将能够：

理解 g2o 的三层架构（SparseOptimizer → OptimizationAlgorithm → LinearSolver），并独立完成初始化配置
自定义 g2o 顶点和边，实现 oplusImpl、computeError、linearizeOplus
阅读 ORB-SLAM3 Optimizer.cc 中的 BA / PoseOptimization / LocalBA / EssentialGraph 四大优化函数
对比 g2o、GTSAM、Ceres 三大优化库的设计哲学与适用场景
实战完成 3D 位姿图优化，并与通用库·Ceres/SLAM库·GTSAM 的结果对比验证

26.1 g2o 设计哲学与历史 ⭐⭐¶

这一节解决什么问题¶

在进入 g2o 的代码之前，我们需要先搞清楚三个问题：g2o 是什么？它为什么被设计成现在这个样子？在 2026 年的今天，它还值得学吗？

26.1.1 历史背景：Kümmerle et al. 2011¶

2011 年，Rainer Kümmerle、Giorgio Grisetti、Hauke Strasdat、Kurt Konolige 和 Wolfram Burgard 在 ICRA 上发表了论文 "g2o: A General Framework for Graph Optimization"。这篇论文提出了一个核心观察：

SLAM 问题的本质是一个稀疏图上的非线性最小二乘问题。

这个观察并不新鲜——Lu & Milios (1997) 就提出了基于图的 SLAM 后端。但 g2o 的贡献在于：它提供了一个**通用的、高效的、可扩展的 C++ 框架**，让研究者可以快速定义自己的图优化问题，而不需要从头写求解器。

在 g2o 之前，SLAM 研究者面临的困境是：

方案	问题
自己写高斯牛顿求解器	重复造轮子，且难以利用稀疏性
使用通用优化库（如 MATLAB、早期 Ceres）	不了解 SLAM 问题的特殊结构（block sparsity）
使用 iSAM/TORO 等专用工具	功能单一，难以扩展到新的问题类型

g2o 的设计目标是：既保持 SLAM 问题的结构化信息（图、顶点、边、block 稀疏性），又提供足够的灵活性让用户定义新的顶点和边类型。

26.1.2 图优化的核心思想¶

SLAM 的后端优化可以自然地表达为一个图：

顶点（Vertex）：待优化的变量，如机器人位姿 \(T_i \in SE(3)\)、路标点 \(p_j \in \mathbb{R}^3\)
边（Edge）：变量之间的约束/观测，如相对位姿约束 \(T_{ij}\)、路标点的重投影观测 \(z_{ij}\)

优化目标是最小化所有边上的加权残差平方和：

\[ \min_{\mathbf{x}} \sum_{(i,j) \in \mathcal{E}} \mathbf{e}_{ij}(\mathbf{x}_i, \mathbf{x}_j)^T \boldsymbol{\Omega}_{ij} \, \mathbf{e}_{ij}(\mathbf{x}_i, \mathbf{x}_j) \]

其中 \(\mathbf{e}_{ij}\) 是残差函数，\(\boldsymbol{\Omega}_{ij}\) 是信息矩阵（协方差矩阵的逆）。

这个公式看起来和通用库·Ceres 中 Ceres 的最小二乘问题完全一样——确实如此，底层数学是相同的。区别在于**如何组织和求解这个问题**。

26.1.3 g2o vs Ceres：设计哲学的根本差异¶

这是初学者最容易混淆的点，值得用一个详细的对比来理解：

维度	g2o	Ceres Solver
设计出发点	SLAM 问题	通用非线性最小二乘
核心抽象	图（顶点 + 边）	残差块（CostFunction）
稀疏结构	通过 BlockSolver 显式利用 SLAM 的 block 结构	通过自动检测稀疏模式
流形处理	在 Vertex 的 `oplusImpl` 中内置	通过 Manifold/LocalParameterization
预定义类型	丰富的 SLAM 类型（VertexSE3Expmap, EdgeSE3ProjectXYZ...）	几乎没有预定义残差块
用户心智模型	"我在构建一个图"	"我在添加残差项"
生态绑定	ORB-SLAM 系列	Google 生态、自动驾驶、通用优化

关键理解：g2o 不是"SLAM 专用版的 Ceres"。它们的设计哲学从根本上不同：

Ceres 说："给我任何非线性最小二乘问题，我帮你求解。你只需要提供残差和 Jacobian。"
g2o 说："给我一个图结构，告诉我哪些是顶点、哪些是边、它们之间怎么连接。我知道 SLAM 问题的稀疏模式，我能更高效地求解。"

26.1.4 什么时候用 g2o¶

在 2026 年，g2o 的主要使用场景是：

阅读和修改 ORB-SLAM2/3 代码：这是最主要的理由。ORB-SLAM3 的 Optimizer.cc 有 3000+ 行代码，全部基于 g2o。不懂 g2o，就读不懂 ORB-SLAM3
轻量级位姿图优化：如果你只需要简单的位姿图优化（比如回环检测后的图优化），g2o 的代码量比 Ceres 少得多
快速原型验证：g2o 丰富的预定义类型让你可以很快搭建一个 BA 或位姿图优化

⚠️ 概念误区：认为 g2o 已经过时

新手想法："g2o 的 GitHub 更新不频繁，GTSAM 和 Ceres 更活跃，所以 g2o 过时了吧？"

实际情况：g2o（GitHub ⭐3.4k+）仍然是特征法 VSLAM 的主流选择。ORB-SLAM3（2021年发布，SLAM领域引用最多的开源系统之一）仍然使用 g2o 作为后端。在可预见的未来，只要 ORB-SLAM 系列还在被广泛使用和引用，g2o 就不会"过时"。

正确理解：g2o 更新不频繁恰恰说明它已经足够成熟和稳定。对于新项目，你可以考虑用 Ceres 或 GTSAM；但对于阅读现有 SLAM 代码，g2o 是必学的。

自检方法：搜索最近3年的 VSLAM 论文（如 ORB-SLAM3, SuperPoint-SLAM, DynaSLAM），查看它们使用的优化后端。

练习¶

[概念] 为什么 SLAM 问题的 Hessian 矩阵是 block-sparse 的？画出一个包含 3 个位姿和 2 个路标的因子图，写出对应的 Hessian 矩阵的非零 block 模式。
[对比] 在通用库·Ceres 中我们用 Ceres 实现了曲线拟合。如果用 g2o 来实现同一个问题，你需要定义哪些类？与 Ceres 相比，哪种方式更自然？
[思考] 如果一个新的 SLAM 系统要同时集成 IMU 预积分因子和视觉重投影因子，你会选 g2o 还是 GTSAM？为什么？

26.2 g2o 三层架构 ⭐⭐¶

这一节解决什么问题¶

g2o 的初始化代码对新手来说像一堵墙——十几行模板嵌套，不知道每一行在做什么。这一节将彻底拆解 g2o 的三层架构，让你理解每一行初始化代码的含义。

26.2.1 架构总览¶

g2o 的核心架构可以用一个简洁的三层模型来理解：

┌──────────────────────────────────────┐
│         SparseOptimizer              │  ← 用户接口：添加顶点/边，调用 optimize()
│  (继承自 OptimizableGraph)           │
├──────────────────────────────────────┤
│     OptimizationAlgorithm            │  ← 优化策略：GN / LM / Dogleg
│  (GaussNewton / Levenberg / Dogleg)  │
├──────────────────────────────────────┤
│         BlockSolver                  │  ← 利用 block 稀疏结构求解线性系统
│  (BlockSolver_6_3 / _7_3 / X)       │
├──────────────────────────────────────┤
│         LinearSolver                 │  ← 底层线性代数：Cholmod / CSparse / Eigen / PCG
│  (Cholmod / CSparse / EigenSparse)   │
└──────────────────────────────────────┘

让我们自底向上地理解每一层。

26.2.2 第三层：LinearSolver（线性求解器）¶

解决什么问题？ 在每次迭代中，非线性优化都需要求解一个线性方程组 \(H \Delta \mathbf{x} = -\mathbf{b}\)，其中 \(H\) 是 Hessian 矩阵（或其近似），\(\mathbf{b}\) 是梯度向量。LinearSolver 负责高效地求解这个线性系统。

为什么有这么多选择？ 因为不同的线性求解方法在不同规模的问题上有不同的性能表现：

求解器	实现来源	适用场景	特点
`LinearSolverDense`	Eigen Dense	非常小的问题（<100变量）	直接用 Dense 分解，无稀疏优化
`LinearSolverCholmod`	SuiteSparse	大规模稀疏问题	最快的稀疏 Cholesky，但需要额外依赖
`LinearSolverCSparse`	CSparse	中等规模稀疏问题	纯 C 实现，依赖轻量
`LinearSolverEigen`	Eigen Sparse	通用场景	纯 Eigen，无外部依赖，性能适中
`LinearSolverPCG`	g2o 内部	超大规模问题	预条件共轭梯度，迭代法

工程选择指南： - 如果你的系统已经有 SuiteSparse → 用 Cholmod（性能最优） - 如果你想最小化依赖 → 用 Eigen Sparse - ORB-SLAM3 默认用 Cholmod（因为它本身依赖 SuiteSparse）

26.2.3 第二层：BlockSolver（块求解器）¶

解决什么问题？ SLAM 问题有一个特殊的稀疏模式：Hessian 矩阵天然具有 block 结构。比如在 BA 问题中，每个位姿贡献一个 \(6 \times 6\) 的 block，每个 3D 点贡献一个 \(3 \times 3\) 的 block。BlockSolver 利用这个结构，避免逐元素操作，直接以 block 为单位进行计算。

为什么这很重要？ 考虑一个 BA 问题：100 个位姿（\(6\)-DOF）+ 10000 个路标点（\(3\)-DOF）： - Hessian 矩阵总维度：\(100 \times 6 + 10000 \times 3 = 30600\) - 如果逐元素存储和操作：\(30600^2 \approx 9.4 \times 10^8\) 个元素 - 如果以 block 为单位：只需要存储非零 block，利用 Schur complement 可以将路标点边缘化，降维到 \(600 \times 600\)

这就是 BlockSolver 的威力。

预定义的 BlockSolver 类型：

// BA 问题：6-DOF 位姿 + 3-DOF 路标点
using BlockSolver_6_3 = BlockSolverPL<6, 3>;

// 带尺度的 BA（Sim3）：7-DOF 位姿 + 3-DOF 路标点
using BlockSolver_7_3 = BlockSolverPL<7, 3>;

// 2D SLAM：3-DOF 位姿 + 2-DOF 路标点
using BlockSolver_3_2 = BlockSolverPL<3, 2>;

// 动态大小：编译时不确定 block 维度
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

模板参数的含义：BlockSolverPL<p, l> 中的 p 和 l 分别是： - p（Pose）：位姿变量在切空间中的维度（不是表示维度！SE(3) 用四元数+平移有 7 个数，但切空间维度是 6） - l（Landmark）：路标变量的维度

如何选择 BlockSolver？

问题类型	BlockSolver	原因
单目/双目 BA	`BlockSolver_6_3`	位姿 SE(3) 切空间 6 维，路标 3D
含尺度的 BA（Sim3）	`BlockSolver_7_3`	Sim3 切空间 7 维，路标 3D
2D 位姿图	`BlockSolver_3_2`	SE(2) 切空间 3 维，路标 2D
纯位姿图（无路标）	`BlockSolverX`	没有路标变量，block 大小可变
不确定维度	`BlockSolverX`	通用但稍慢

⚠️ 编程陷阱：BlockSolver 维度选错

错误做法：对 BA 问题使用 BlockSolverX 代替 BlockSolver_6_3

现象：代码能运行，但优化速度显著变慢（可能慢 2-5 倍）

根本原因：BlockSolverX 在运行时动态推断 block 大小，无法利用编译期固定维度带来的模板优化（如 Eigen 固定大小矩阵的栈分配和 SIMD 向量化）。这和通用库·Eigen 中 Matrix3d vs MatrixXd 的性能差异是同一个道理。

正确做法：只要编译时知道 block 维度，就用固定维度的 BlockSolver。BlockSolverX 只用于混合维度的特殊场景。

26.2.4 第二层（上）：OptimizationAlgorithm（优化算法）¶

解决什么问题？ 给定当前迭代的 Hessian 和梯度，如何确定下一步的更新量 \(\Delta \mathbf{x}\)？不同的算法有不同的策略：

算法	g2o 类名	核心思想	适用场景
Gauss-Newton	`OptimizationAlgorithmGaussNewton`	假设 Hessian 正定，直接求解 \(H\Delta x = -b\)	初始值接近最优时
Levenberg-Marquardt	`OptimizationAlgorithmLevenberg`	在 \(H\) 对角线加阻尼 \((H + \lambda I)\Delta x = -b\)	最常用，自动调节步长
Dogleg	`OptimizationAlgorithmDogleg`	Gauss-Newton 步和梯度下降步的插值	介于 GN 和 LM 之间

为什么 Levenberg-Marquardt 最常用？

回忆通用库·Ceres 中的讨论：Gauss-Newton 在远离最优解时可能发散（Hessian 近似不准确导致步长过大），而 LM 通过阻尼因子 \(\lambda\) 自动调节： - 当 \(\lambda\) 大时，\((H + \lambda I) \approx \lambda I\)，步长退化为梯度下降（保守但稳定） - 当 \(\lambda\) 小时，\((H + \lambda I) \approx H\)，步长接近 Gauss-Newton（激进但快速） - \(\lambda\) 根据每次迭代的实际下降量自动调整

在 SLAM 中，初始值通常来自前端的粗略估计（如 PnP、ICP），可能离最优解有一定距离。LM 的自适应特性使其成为最安全的选择。

26.2.5 第一层：SparseOptimizer（稀疏优化器）¶

SparseOptimizer 是用户直接打交道的最外层接口。它继承自 OptimizableGraph（它本身继承自 HyperGraph），是一个真正的"图"数据结构，你可以向其中添加顶点和边。

核心方法：

// 添加顶点和边
optimizer.addVertex(vertex);
optimizer.addEdge(edge);

// 初始化并优化
optimizer.initializeOptimization();  // 分析图结构，准备数据结构
optimizer.optimize(num_iterations);  // 执行迭代优化

// 加载/保存 .g2o 文件
optimizer.load("input.g2o");
optimizer.save("output.g2o");

26.2.6 完整初始化代码详解¶

现在让我们看一个完整的初始化流程，并**逐行解释**：

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>

// ⚡ 以下代码适用于 g2o 上游主分支（unique_ptr API）
// ORB-SLAM3 内置的 g2o 分支使用旧式 raw pointer API，见 §26.6

// === Step 1: 选择线性求解器 ===
// 模板参数 <6, 3> 表示 Hessian 的 block 结构：位姿 block 6x6，路标 block 3x3
// LinearSolverCholmod 使用 SuiteSparse 的 Cholmod 做稀疏 Cholesky 分解
using LinearSolverType = g2o::LinearSolverCholmod<
    g2o::BlockSolver_6_3::PoseMatrixType  // Hessian 的 block 类型
>;
auto linearSolver = std::make_unique<LinearSolverType>();

// === Step 2: 用线性求解器构造 BlockSolver ===
// BlockSolver_6_3 知道 SLAM 的 block 结构，会做 Schur complement
auto blockSolver = std::make_unique<g2o::BlockSolver_6_3>(
    std::move(linearSolver)  // 所有权转移给 BlockSolver
);

// === Step 3: 用 BlockSolver 构造优化算法 ===
// LevenbergMarquardt 包装了 BlockSolver，提供阻尼和步长控制
auto algorithm = new g2o::OptimizationAlgorithmLevenberg(
    std::move(blockSolver)   // 所有权转移给 Algorithm
);

// === Step 4: 创建 SparseOptimizer 并设置算法 ===
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(algorithm);  // SparseOptimizer 接管 algorithm 的所有权
optimizer.setVerbose(true);         // 打印每次迭代的信息（调试时开启）

为什么这么复杂？ 因为 g2o 使用了经典的**策略模式（Strategy Pattern）**：每一层都可以独立替换。你可以只换线性求解器（从 Cholmod 换成 Eigen），而不影响其他层。这种灵活性是 g2o 作为"通用框架"的核心设计。

让我们看一个使用 Eigen Sparse 的替代初始化（无需 SuiteSparse 依赖）：

// ✅ 替代方案：使用 Eigen Sparse（无外部依赖）
#include <g2o/solvers/eigen/linear_solver_eigen.h>

using LinearSolverType = g2o::LinearSolverEigen<
    g2o::BlockSolver_6_3::PoseMatrixType
>;
auto linearSolver = std::make_unique<LinearSolverType>();
auto blockSolver = std::make_unique<g2o::BlockSolver_6_3>(
    std::move(linearSolver)
);
auto algorithm = new g2o::OptimizationAlgorithmLevenberg(
    std::move(blockSolver)
);

以及一个使用 BlockSolverX 的通用初始化（当 block 维度不确定时）：

// ✅ 通用版本：BlockSolverX（维度动态推断）
using LinearSolverType = g2o::LinearSolverEigen<
    g2o::BlockSolverX::PoseMatrixType
>;
auto linearSolver = std::make_unique<LinearSolverType>();
auto blockSolver = std::make_unique<g2o::BlockSolverX>(
    std::move(linearSolver)
);
auto algorithm = new g2o::OptimizationAlgorithmGaussNewton(  // 也可以换 GN
    std::move(blockSolver)
);

⚠️ 编程陷阱：忘记 setAlgorithm

错误做法：创建 SparseOptimizer 后直接添加顶点和边，不调用 setAlgorithm()

现象：调用 optimizer.optimize(10) 时，程序崩溃或打印 "solver not set" 错误

根本原因：SparseOptimizer 默认没有优化算法，必须显式设置。这不像 Ceres 的 Problem 对象可以直接使用（Ceres 在调用 Solve 时才指定选项）。

正确做法：在添加任何顶点/边之前，先完成三层初始化并调用 setAlgorithm()。

💡 概念误区：g2o 的所有权管理

新手想法："我 new 出来的 algorithm、vertex、edge 需要我自己 delete 吗？"

实际情况：g2o 的所有权规则是——一旦你把对象交给 SparseOptimizer（通过 addVertex、addEdge、setAlgorithm），SparseOptimizer 就接管了所有权，在析构时自动 delete。你**不应该**在 optimizer 析构之前手动 delete 这些对象。但注意：ORB-SLAM3 在某些地方手动管理这些指针（这是历史代码风格，不是最佳实践）。

练习¶

[编程] 写出一个使用 CSparse 线性求解器 + GaussNewton 算法 + BlockSolverX 的初始化代码。然后替换为 Cholmod + LevenbergMarquardt + BlockSolver_6_3。对比两者需要修改哪些行。
[概念] 解释 Schur complement 在 BA 中的作用。为什么 BlockSolver_6_3 能够利用 Schur complement，而 BlockSolverX 不一定能？（提示：Schur complement 需要区分哪些是位姿变量、哪些是路标变量）
[工程] 如果你的 SLAM 系统运行在嵌入式平台（无法安装 SuiteSparse），你会如何配置 g2o 的线性求解器？分析这对性能的影响。

26.3 顶点 (Vertex) ⭐⭐¶

这一节解决什么问题¶

g2o 的顶点（Vertex）代表优化图中的变量——位姿、路标点等。要读懂 ORB-SLAM3 或自定义新的顶点类型，你必须理解 BaseVertex 的模板参数和四个必须实现的方法。

26.3.1 BaseVertex 模板解析¶

g2o 中所有顶点类型都继承自 BaseVertex<D, T>，模板参数含义如下：

template <int D, typename T>
class BaseVertex : public OptimizableGraph::Vertex {
    // D = 优化维度（切空间维度，NOT 表示维度！）
    // T = 估计值的类型（estimate type）
protected:
    T _estimate;  // 当前估计值
};

D 和 T 的关系是理解 g2o 的关键。让我们通过几个例子来说明：

顶点类型	D（切空间维度）	T（估计值类型）	解释
`VertexSE3Expmap`	6	`SE3Quat`	SE(3) 有 7 个参数（4 四元数 + 3 平移），但切空间只有 6 维
`VertexPointXYZ`	3	`Vector3`	3D 点，维度一致
`VertexSim3Expmap`	7	`Sim3`	Sim(3) 有 7-DOF：3 旋转 + 3 平移 + 1 尺度
`VertexSE2`	3	`SE2`	2D 位姿：2 平移 + 1 旋转角

为什么 D 不等于 T 的参数个数？ 这就是通用库·李群manif 中李群/流形的核心思想：

\(SE(3)\) 是一个 6 维流形，嵌入在 12 维空间中（旋转矩阵 9 + 平移 3），用四元数表示时有 7 个参数
优化在**切空间**进行，切空间的维度是 6（3 旋转 + 3 平移）
更新量 \(\Delta \mathbf{x}\) 的维度是 D=6，然后通过指数映射映射回流形

这直接对应通用库·李群manif 中的 Plus 操作：\(T' = T \boxplus \delta = T \cdot \exp(\hat{\delta})\)。

26.3.2 必须实现的四个方法¶

要定义一个自定义顶点，你需要实现以下方法：

方法1：setToOriginImpl() —— 设置初始估计值

// 将顶点设置为"原点"（单位元或零）
// 对 SE(3)：单位变换 T = I
// 对 Vector3：零向量 p = (0,0,0)
void setToOriginImpl() override {
    _estimate = SE3Quat();  // SE(3) 的单位元
}

方法2：oplusImpl(const double* update) —— 应用切空间增量

这是最重要的方法——它定义了如何将优化器计算的更新量（在切空间中）应用到当前估计值上。

// update 是一个长度为 D 的 double 数组
// 对于 VertexSE3Expmap（D=6），update 包含 6 个分量
void oplusImpl(const double* update) override {
    // 将 update 映射为 SE3Quat 的切向量
    Eigen::Map<const Vector6> v(update);
    // 左乘更新：T_new = exp(δξ) * T_old
    _estimate = SE3Quat::exp(v) * _estimate;
}

这里有一个深刻的联系： oplusImpl 的实现直接对应通用库·李群manif 中的 \(\boxplus\) 操作。VertexSE3Expmap 使用**左乘**更新：

\[ T_{\text{new}} = \exp(\hat{\delta\xi}) \cdot T_{\text{old}} \]

这个选择不是随意的——它决定了 Jacobian 的计算方式（左乘 Jacobian vs 右乘 Jacobian）。

方法3 和 4：read() 和 write() —— 序列化

// 从 .g2o 文件读取顶点数据
bool read(std::istream& is) override {
    // 读取 SE3Quat 的 7 个参数（平移 x,y,z + 四元数 qx,qy,qz,qw）
    // ...
    return true;
}

// 将顶点数据写入 .g2o 文件
bool write(std::ostream& os) const override {
    // 写出 SE3Quat 的 7 个参数
    // ...
    return true;
}

26.3.3 内置顶点类型详解¶

g2o 提供了丰富的预定义顶点类型，覆盖了 SLAM 中最常见的变量：

VertexSE3Expmap ⭐⭐（ORB-SLAM3 中表示相机位姿）

// 源码定义（简化版）
class VertexSE3Expmap : public BaseVertex<6, SE3Quat> {
public:
    void setToOriginImpl() override {
        _estimate = SE3Quat();  // 单位变换
    }

    // 左乘更新
    void oplusImpl(const double* update) override {
        Eigen::Map<const Vector6> v(update);
        _estimate = SE3Quat::exp(v) * _estimate;
    }
};

使用示例（ORB-SLAM3 中的典型用法）：

g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
vSE3->setId(0);                    // 顶点 ID，必须唯一
vSE3->setEstimate(g2o::SE3Quat(   // 设置初始估计
    R_cw,   // Eigen::Matrix3d 旋转矩阵（world-to-camera）
    t_cw    // Eigen::Vector3d 平移向量
));
vSE3->setFixed(false);             // 不固定，参与优化
optimizer.addVertex(vSE3);

VertexSBAPointXYZ ⭐⭐（ORB-SLAM3 中表示 3D 路标点）

⚠️ 注意区分：g2o 中有 VertexPointXYZ（在 types/slam3d/）和 VertexSBAPointXYZ（在 types/sba/）。ORB-SLAM3 的 BA 使用后者（VertexSBAPointXYZ），与 EdgeSE3ProjectXYZ 配套。两者实现几乎相同（都是 BaseVertex<3, Vector3>），但所在命名空间和头文件不同。

class VertexPointXYZ : public BaseVertex<3, Vector3> {
public:
    void setToOriginImpl() override {
        _estimate.fill(0);  // 零向量
    }

    void oplusImpl(const double* update) override {
        Eigen::Map<const Vector3> v(update);
        _estimate += v;  // 欧几里得空间，直接加法更新
    }
};

注意对比：VertexPointXYZ 的 oplusImpl 是简单的向量加法，因为 \(\mathbb{R}^3\) 本身就是一个向量空间（切空间和原空间重合）。而 VertexSE3Expmap 需要指数映射，因为 \(SE(3)\) 是一个非线性流形。

VertexSim3Expmap ⭐⭐⭐（ORB-SLAM3 回环检测中使用）

class VertexSim3Expmap : public BaseVertex<7, Sim3> {
    // D=7: 3 旋转 + 3 平移 + 1 尺度（log scale）
    // Sim(3) = {sR, t} 其中 s > 0 是尺度因子

    void oplusImpl(const double* update) override {
        Eigen::Map<const Vector7> v(update);
        _estimate = Sim3::exp(v) * _estimate;
    }
};

为什么需要 Sim3？ 在单目 SLAM 中，尺度是不可观的。当检测到回环时，回环两端的地图可能有不同的尺度。Sim(3) 变换（相似变换）可以同时处理旋转、平移和尺度对齐。这在 ORB-SLAM3 的 OptimizeEssentialGraph 中被广泛使用。

VertexSE2 ⭐（2D SLAM 位姿）

class VertexSE2 : public BaseVertex<3, SE2> {
    // D=3: 2 平移 + 1 旋转角
    // 适用于 2D 激光 SLAM（如 Cartographer 的某些变体）
};

26.3.4 固定顶点与规范自由度¶

在优化问题中，并非所有顶点都参与优化。固定顶点（fixed vertex）的值在优化过程中保持不变。

vSE3->setFixed(true);  // 固定此顶点

为什么需要固定顶点？

考虑一个位姿图优化问题：所有约束都是**相对的**（\(T_{ij}\)），没有**绝对的**参考。这意味着所有位姿同时平移或旋转一个常量，残差不变——问题有**规范自由度（gauge freedom）**。

数学上，这意味着 Hessian 矩阵是**奇异的**（有零特征值），线性系统无唯一解。

解决方案：固定一个或多个顶点，消除规范自由度。

问题类型	自由度	至少需要固定
3D 位姿图	6-DOF（SE3）	固定 1 个位姿
3D BA	7-DOF（SE3 + 尺度）	固定 2 个位姿（距离确定尺度）
2D 位姿图	3-DOF（SE2）	固定 1 个位姿

在 ORB-SLAM3 的 BA 中，通常第一个关键帧被固定：

// ORB-SLAM3 Optimizer::BundleAdjustment 中的典型模式
if (pKF == pMap->GetOriginKF()) {
    vSE3->setFixed(true);  // 固定原点关键帧
}

⚠️ 编程陷阱：oplusImpl 中左乘与右乘搞混

错误做法：在 oplusImpl 中使用右乘更新 _estimate = _estimate * SE3Quat::exp(v)，但在计算 Jacobian 时假设左乘更新

现象：优化可以运行但收敛极慢，或者收敛到错误的解

根本原因：左乘更新 \(T' = \exp(\hat{\delta}) \cdot T\) 对应左扰动 Jacobian，右乘更新 \(T' = T \cdot \exp(\hat{\delta})\) 对应右扰动 Jacobian。如果 oplusImpl 用左乘而 linearizeOplus 算的是右扰动 Jacobian（或反过来），Jacobian 和实际更新不一致，优化器的线性化近似就是错的。

正确做法：确保 oplusImpl 和 linearizeOplus 使用一致的更新约定。g2o 内置类型全部使用**左乘**更新。如果你自定义顶点，务必保持一致。

自检方法：对一个小规模问题（如 5 个位姿），对比手动计算的 Jacobian 和 g2o 数值微分计算的 Jacobian。如果不一致，说明 oplusImpl 和 linearizeOplus 不匹配。

🧠 思维陷阱：认为 D 就是估计值的参数个数

新手想法："VertexSE3Expmap 的 SE3Quat 有 7 个参数（4 四元数 + 3 平移），所以 D 应该是 7。"

实际情况：D 是**切空间**维度，不是**参数空间**维度。SE(3) 是一个 6 维流形，即使用 7 个参数表示，优化更新量 \(\Delta x\) 只有 6 个分量。这就是为什么 VertexSE3Expmap 是 BaseVertex<6, SE3Quat> 而不是 BaseVertex<7, SE3Quat>。

类比：地球表面是一个 2 维流形，嵌入在 3 维空间中。你的位置用 3 个坐标 \((x, y, z)\) 表示，但你只能沿 2 个方向移动（东西和南北）。D=2，T 的参数个数=3。

练习¶

[手推] 写出 VertexSim3Expmap 的 oplusImpl 的数学表达式。\(\text{Sim}(3)\) 的指数映射公式是什么？（提示：参考通用库·李群manif 中 \(\text{Sim}(3)\) 的定义）
[编程] 自定义一个 VertexAngleAxis 顶点，使用角轴表示旋转（\(\mathbf{v} \in \mathbb{R}^3\)，\(\|\mathbf{v}\|\) 表示旋转角度，\(\mathbf{v}/\|\mathbf{v}\|\) 表示旋转轴）。D 应该是多少？T 应该是什么类型？实现 oplusImpl。
[思考] 为什么 g2o 没有提供 VertexQuaternion 类型？如果你直接优化四元数的 4 个分量（不用李群），会出现什么问题？

26.4 边 (Edge) ⭐⭐¶

这一节解决什么问题¶

边（Edge）是 g2o 图中的约束——它连接顶点，定义观测和残差。ORB-SLAM3 中 90% 的 g2o 使用都是在构造边。理解边的模板参数和接口方法，是读懂 ORB-SLAM3 Optimizer.cc 的关键。

26.4.1 三种边的基类¶

g2o 提供三种边的基类，区别在于连接的顶点数量：

// 一元边：连接 1 个顶点
template <int D, typename E, typename VertexXi>
class BaseUnaryEdge;

// 二元边：连接 2 个顶点（最常用）
template <int D, typename E, typename VertexXi, typename VertexXj>
class BaseBinaryEdge;

// 多元边：连接任意个顶点
template <int D, typename E>
class BaseMultiEdge;

模板参数含义：

参数	含义	示例
D	误差（残差）向量的维度	重投影误差 → D=2（像素坐标差）
E	观测值（measurement）的数据类型	重投影 → `Vector2`（像素坐标）
VertexXi	第一个顶点类型	`VertexSBAPointXYZ`（3D 点）
VertexXj	第二个顶点类型	`VertexSE3Expmap`（相机位姿）

26.4.2 边的核心方法¶

方法1：computeError() —— 计算残差向量

这是最核心的方法，计算观测值和预测值之间的残差：

void computeError() override {
    // 获取连接的顶点
    const VertexSBAPointXYZ* v0
        = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
    const VertexSE3Expmap* v1
        = static_cast<const VertexSE3Expmap*>(_vertices[1]);

    // 将 3D 点投影到相机坐标系，再投影到像素坐标
    Vector2 predicted = cam_project(v1->estimate().map(v0->estimate()));

    // 残差 = 观测值 - 预测值
    _error = _measurement - predicted;
}

理解残差的含义：_error 是一个 D 维向量，它衡量"如果当前估计值是对的，我们期望的观测值"与"实际观测值"之间的差异。优化的目标就是调整顶点的估计值，使得所有边上的加权残差平方和最小。

方法2：linearizeOplus() —— 计算 Jacobian 矩阵

void linearizeOplus() override {
    // _jacobianOplusXi: 残差对第一个顶点的 Jacobian (D x dim_vertex_0)
    // _jacobianOplusXj: 残差对第二个顶点的 Jacobian (D x dim_vertex_1)

    // 示例：重投影误差对 3D 点的 Jacobian (2 x 3)
    _jacobianOplusXi = ...;

    // 示例：重投影误差对相机位姿的 Jacobian (2 x 6)
    _jacobianOplusXj = ...;
}

为什么 linearizeOplus 是可选的？

如果你不实现 linearizeOplus，g2o 会自动使用**数值微分**来近似 Jacobian：

\[ \frac{\partial \mathbf{e}}{\partial x_k} \approx \frac{\mathbf{e}(x_k + \delta) - \mathbf{e}(x_k - \delta)}{2\delta} \]

这在原型开发时很方便，但有两个问题： 1. 速度慢：每个 Jacobian 元素都需要两次 computeError 调用。对于 BA（上万条边），这会成为瓶颈 2. 精度差：数值微分受步长 \(\delta\) 选择影响，可能有截断误差

最佳实践：先用数值微分验证正确性，然后实现解析 Jacobian 提升性能。

26.4.3 内置边类型详解¶

EdgeSE3ProjectXYZ ⭐⭐（ORB-SLAM3 BA 核心边）

这是 ORB-SLAM3 中使用最频繁的边类型——重投影误差边。

// 继承关系
class EdgeSE3ProjectXYZ : public BaseBinaryEdge<2, Vector2,
    VertexSBAPointXYZ,   // 顶点 0: 3D 路标点
    VertexSE3Expmap      // 顶点 1: 相机位姿
> {
public:
    void computeError() override {
        // 获取 3D 点和相机位姿
        const VertexSBAPointXYZ* point
            = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
        const VertexSE3Expmap* pose
            = static_cast<const VertexSE3Expmap*>(_vertices[1]);

        // 3D 点投影到相机坐标系
        Vector3 pc = pose->estimate().map(point->estimate());
        // pc = R * pw + t

        // 针孔投影模型
        double u = fx * pc[0] / pc[2] + cx;
        double v = fy * pc[1] / pc[2] + cy;

        // 残差 = 观测像素 - 投影像素
        _error[0] = _measurement[0] - u;
        _error[1] = _measurement[1] - v;
    }

    void linearizeOplus() override {
        // 解析 Jacobian（这是 ORB-SLAM3 性能的关键！）
        // 详见 26.4.4 节的推导
    }
};

ORB-SLAM3 中的典型用法：

// 创建重投影误差边
g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
    optimizer.vertex(mapPointId)));   // 3D 点顶点
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
    optimizer.vertex(keyFrameId)));   // 相机位姿顶点
e->setMeasurement(obs);              // Vector2: 观测到的像素坐标
e->setInformation(                   // 信息矩阵（协方差的逆）
    Eigen::Matrix2d::Identity() * invSigma2);  // 根据金字塔层级加权
e->setRobustKernel(new g2o::RobustKernelHuber());  // Huber 鲁棒核
e->setLevel(0);                      // 优化层级（用于 outlier 管理）

// 设置相机内参（EdgeSE3ProjectXYZ 特有）
e->fx = pKF->fx;
e->fy = pKF->fy;
e->cx = pKF->cx;
e->cy = pKF->cy;

optimizer.addEdge(e);

EdgeSE3 ⭐⭐（相对位姿约束边）

class EdgeSE3 : public BaseBinaryEdge<6, Isometry3,
    VertexSE3,   // 顶点 0: 位姿 i
    VertexSE3    // 顶点 1: 位姿 j
> {
    // 残差定义：
    // e = log(Z_ij^{-1} * T_i^{-1} * T_j)
    // 其中 Z_ij 是观测到的相对位姿，T_i, T_j 是当前估计的位姿
};

这个残差的含义是：如果估计的位姿 \(T_i, T_j\) 是准确的，那么 \(T_i^{-1} T_j\) 应该等于观测值 \(Z_{ij}\)。两者的"差"通过对数映射变成切空间中的 6 维向量。

EdgeSim3 ⭐⭐⭐（ORB-SLAM3 回环闭合中使用）

class EdgeSim3 : public BaseBinaryEdge<7, Sim3,
    VertexSim3Expmap,  // 顶点 0: Sim3 位姿 i
    VertexSim3Expmap   // 顶点 1: Sim3 位姿 j
> {
    // 残差类似 EdgeSE3，但在 Sim(3) 群上
    // 维度为 7：3 旋转 + 3 平移 + 1 尺度
};

26.4.4 重投影误差 Jacobian 推导¶

让我们详细推导 EdgeSE3ProjectXYZ 的 Jacobian——这个推导在通用库·Ceres（Ceres BA）中已经见过，但这里从 g2o 的左乘更新角度重新推导。

设置：相机位姿 \(T \in SE(3)\)，3D 点 \(P_w \in \mathbb{R}^3\)，相机内参 \((f_x, f_y, c_x, c_y)\)。

Step 1：3D 点变换到相机坐标系

\[ P_c = T \cdot P_w = R P_w + t = \begin{pmatrix} X_c \\ Y_c \\ Z_c \end{pmatrix} \]

Step 2：针孔投影

\[ u = f_x \frac{X_c}{Z_c} + c_x, \quad v = f_y \frac{Y_c}{Z_c} + c_y \]

Step 3：投影对相机坐标的 Jacobian

\[ \frac{\partial (u, v)}{\partial P_c} = \begin{pmatrix} \frac{f_x}{Z_c} & 0 & -\frac{f_x X_c}{Z_c^2} \\ 0 & \frac{f_y}{Z_c} & -\frac{f_y Y_c}{Z_c^2} \end{pmatrix} \]

Step 4：相机坐标对路标点的 Jacobian

\[ \frac{\partial P_c}{\partial P_w} = R \]

因此，重投影误差对 3D 点的 Jacobian 为：

\[ \frac{\partial \mathbf{e}}{\partial P_w} = -\frac{\partial (u,v)}{\partial P_c} \cdot R \quad (\text{2} \times \text{3 矩阵}) \]

Step 5：相机坐标对位姿（左扰动）的 Jacobian

对位姿施加左扰动 \(\delta \boldsymbol{\xi} \in \mathfrak{se}(3)\)：

\[ P_c' = \exp(\hat{\delta\boldsymbol{\xi}}) \cdot T \cdot P_w \approx (I + \hat{\delta\boldsymbol{\xi}}) \cdot P_c \]

利用 \(\hat{\delta\boldsymbol{\xi}} \cdot P_c = \begin{pmatrix} \delta\boldsymbol{\phi} \times P_c + \delta\boldsymbol{\rho} \end{pmatrix}\)（其中 \(\delta\boldsymbol{\xi} = (\delta\boldsymbol{\rho}, \delta\boldsymbol{\phi})^T\)，本推导采用平移在前、旋转在后的约定）：

\[ \frac{\partial P_c}{\partial \delta\boldsymbol{\xi}} = \begin{pmatrix} I & -[P_c]_\times \end{pmatrix} \quad (\text{3} \times \text{6 矩阵}) \]

⚠️ 约定警告：本推导使用 \((\boldsymbol{\rho}, \boldsymbol{\phi})^T\)（平移在前、旋转在后）的约定。g2o 代码中使用相反的排列 \((\boldsymbol{\phi}, \boldsymbol{\rho})^T\)（旋转在前、平移在后）——参见 SE3Quat::exp() 中 update[0..2] 对应旋转。因此 g2o 源码中的 Jacobian 列顺序与本推导相反：_jacobianOplusXj 的前 3 列对应旋转，后 3 列对应平移。阅读 g2o 源码时注意这一差异。

其中 \([P_c]_\times\) 是 \(P_c\) 的反对称矩阵。因此：

\[ \frac{\partial \mathbf{e}}{\partial \delta\boldsymbol{\xi}} = -\frac{\partial (u,v)}{\partial P_c} \cdot \begin{pmatrix} I & -[P_c]_\times \end{pmatrix} \quad (\text{2} \times \text{6 矩阵}) \]

26.4.5 信息矩阵与鲁棒核函数¶

信息矩阵（Information Matrix）

e->setInformation(Omega);  // Omega 是 D x D 的对称正定矩阵

信息矩阵 \(\boldsymbol{\Omega}_{ij}\) 是观测协方差矩阵的逆：\(\boldsymbol{\Omega}_{ij} = \boldsymbol{\Sigma}_{ij}^{-1}\)。

它的物理含义是**观测的可信度权重**： - \(\boldsymbol{\Omega}\) 大 → 观测精度高 → 对应的残差权重大 → 优化器更倾向于满足这个约束 - \(\boldsymbol{\Omega}\) 小 → 观测精度低 → 对应的残差权重小 → 优化器可以"容忍"这个约束有较大残差

在 ORB-SLAM3 中，重投影误差边的信息矩阵与特征点的金字塔层级有关：

// ORB-SLAM3 中的信息矩阵设置
const float invSigma2 = pKF->GetInvLevelSigma2(kpUn.octave);
e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
// 高金字塔层级 → invSigma2 小 → 这个观测权重低（因为金字塔上层分辨率低）

鲁棒核函数（Robust Kernel）

在实际 SLAM 中，不可避免地存在**外点（outlier）**——错误的特征匹配、动态物体等。标准最小二乘对外点极其敏感（\(e^2\) 在 \(e\) 大时增长很快）。鲁棒核函数通过"削弱"大残差的影响来抵抗外点。

// g2o 的鲁棒核设置
auto rk = new g2o::RobustKernelHuber();
rk->setDelta(sqrt(5.991));  // 阈值，与 chi2 检验相关
e->setRobustKernel(rk);

g2o vs Ceres 的鲁棒核对比：

特性	g2o `RobustKernel`	Ceres `LossFunction`
设置方式	`edge->setRobustKernel(kernel)`	`problem.AddResidualBlock(cost, loss, ...)`
Huber	`RobustKernelHuber`	`HuberLoss`
Cauchy	`RobustKernelCauchy`	`CauchyLoss`
内置种类	较少	更多（Tukey、Arctan 等）

⚠️ 编程陷阱：信息矩阵维度不匹配

错误做法：对 D=2 的边设置 3x3 的信息矩阵
// ❌ 错误：边的残差维度是 2，信息矩阵应该是 2x2
e->setInformation(Eigen::Matrix3d::Identity());
现象：编译时不会报错（因为 setInformation 接受 MatrixXd），但运行时可能越界访问导致段错误，或者更隐蔽地，优化结果完全错误。

正确做法：信息矩阵的维度必须与 D（残差维度）一致。
// ✅ 正确：D=2 对应 2x2 信息矩阵
e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
⚠️ 编程陷阱：忘记设置 measurement

错误做法：创建边，设置了顶点和信息矩阵，但忘记调用 setMeasurement()

现象：_measurement 保持默认值（通常是零或垃圾值），computeError() 计算出的残差完全错误，但程序不会崩溃——优化"正常"运行，结果却完全不对

根本原因：g2o 不会检查你是否设置了 measurement。这是一个典型的"静默错误"（silent bug）

自检方法：在优化前打印所有边的残差和 chi2 值。如果初始残差异常大或异常小，检查是否所有边都正确设置了 measurement

练习¶

[手推] 推导 EdgeSE3（相对位姿约束边）的残差 \(\mathbf{e} = \text{Log}(Z_{ij}^{-1} \cdot T_i^{-1} \cdot T_j)\) 对 \(T_i\) 和 \(T_j\) 的 Jacobian。提示：使用通用库·李群manif 中的伴随表示。
[编程] 实现一个自定义的 EdgePointDistance（一元边），测量 3D 点到原点的距离。D=1，E=double，连接 VertexPointXYZ。实现 computeError 和 linearizeOplus。
[对比] 将同一个 BA 问题分别用 g2o 数值微分和解析 Jacobian 求解。比较：(a) 优化结果是否一致；(b) 运行时间差多少。

26.5 .g2o 文件格式与序列化 ⭐¶

这一节解决什么问题¶

g2o 定义了一种标准的文本文件格式 .g2o，用于存储和交换图优化数据。Ceres Solver、GTSAM、各种评估工具都能读写 .g2o 文件。理解这个格式对于调试、可视化和跨库对比至关重要。

26.5.1 .g2o 文件格式详解¶

.g2o 文件是纯文本格式，每一行描述一个顶点或边：

3D SLAM 格式（最常用）：

# 顶点格式：VERTEX_SE3:QUAT id x y z qx qy qz qw
VERTEX_SE3:QUAT 0 0.0 0.0 0.0 0.0 0.0 0.0 1.0
VERTEX_SE3:QUAT 1 1.0 0.0 0.0 0.0 0.0 0.0 1.0
VERTEX_SE3:QUAT 2 2.0 0.0 0.0 0.0 0.0 0.0 1.0

# 边格式：EDGE_SE3:QUAT id_a id_b dx dy dz dqx dqy dqz dqw info(上三角21个元素)
EDGE_SE3:QUAT 0 1 1.0 0.0 0.0 0.0 0.0 0.0 1.0  500 0 0 0 0 0  500 0 0 0 0  500 0 0 0  500 0 0  500 0  500

字段说明：

字段	含义	注意事项
`VERTEX_SE3:QUAT`	3D 位姿顶点（四元数表示）	标识符，大小写敏感
`id`	顶点唯一 ID	整数，从 0 开始
`x y z`	平移分量	顺序固定
`qx qy qz qw`	四元数分量	注意顺序：(x, y, z, w)，不是 (w, x, y, z)！
`info_ij`	信息矩阵上三角元素	\(6 \times 6\) 矩阵的 21 个独立元素

2D SLAM 格式：

VERTEX_SE2 0 0.0 0.0 0.0
VERTEX_SE2 1 1.0 0.0 0.5
EDGE_SE2 0 1 1.0 0.0 0.5  500 0 0  500 0  500

⚠️ 编程陷阱：四元数顺序搞混

错误做法：按照 Eigen 的 Quaterniond(w, x, y, z) 构造函数顺序写 .g2o 文件

现象：读取 .g2o 文件后，所有位姿的旋转都是错的。可能出现机器人"翻转"或"旋转90度"

根本原因：.g2o 文件中四元数的存储顺序是 qx qy qz qw（虚部在前，实部在后），而 Eigen 的 Quaterniond 构造函数是 (w, x, y, z)（实部在前）。这两种约定在机器人领域都很常见，混淆它们是最常见的 bug 之一。

正确做法：
// 读取 .g2o 时
double qx, qy, qz, qw;
is >> qx >> qy >> qz >> qw;
Eigen::Quaterniond q(qw, qx, qy, qz);  // Eigen 构造函数：w 在前！

// 写入 .g2o 时
os << q.x() << " " << q.y() << " " << q.z() << " " << q.w();  // 文件中 x,y,z,w
自检方法：对单位四元数 \((0, 0, 0, 1)\)，检查读入后 q.w() 是否为 1。

26.5.2 类型注册机制：G2O_REGISTER_TYPE¶

g2o 使用**工厂模式 + 宏注册**实现类型的动态注册。这使得 .g2o 文件中的类型字符串（如 VERTEX_SE3:QUAT）能够自动映射到对应的 C++ 类。

// g2o 内部的类型注册
G2O_REGISTER_TYPE(VERTEX_SE3:QUAT, VertexSE3);
G2O_REGISTER_TYPE(EDGE_SE3:QUAT, EdgeSE3);
G2O_REGISTER_TYPE(VERTEX_SE2, VertexSE2);
G2O_REGISTER_TYPE(EDGE_SE2, EdgeSE2);

工作原理： 1. G2O_REGISTER_TYPE 宏在程序启动时（通过静态初始化）向全局工厂注册一个 "字符串 → 类创建函数" 的映射 2. 当 optimizer.load("file.g2o") 读到 VERTEX_SE3:QUAT 时，工厂查找对应的创建函数并实例化 VertexSE3 3. 调用实例的 read() 方法解析后续数据

如果你定义了自定义顶点/边类型，也需要注册：

G2O_REGISTER_TYPE(MY_CUSTOM_VERTEX, MyCustomVertex);

否则 .g2o 文件中使用该类型时，加载会失败。

26.5.3 加载、优化、保存的完整流程¶

// 完整的 .g2o 文件处理流程
g2o::SparseOptimizer optimizer;
// ... 初始化三层架构（见 26.2 节）...

// 加载 .g2o 文件
bool ok = optimizer.load("sphere.g2o");
if (!ok) {
    std::cerr << "Error loading graph!" << std::endl;
    return -1;
}

// 固定第一个顶点（消除规范自由度）
auto firstVertex = optimizer.vertex(0);
firstVertex->setFixed(true);

// 初始化并优化
optimizer.initializeOptimization();
optimizer.optimize(30);  // 30 次迭代

// 保存优化结果
optimizer.save("sphere_optimized.g2o");

26.5.4 g2o_viewer 可视化工具¶

g2o 自带一个 Qt 可视化工具 g2o_viewer，可以直接加载 .g2o 文件并可视化图结构：

# 安装（Ubuntu）
sudo apt install libg2o-dev  # 可能包含 g2o_viewer

# 使用
g2o_viewer sphere.g2o
# 按 'o' 开始优化，可以实时观察图的收敛过程

练习¶

[实操] 手写一个包含 3 个 SE2 顶点和 3 条 SE2 边的 .g2o 文件（形成一个三角形回环）。用 g2o 加载并优化。
[编程] 写一个 Python 脚本解析 .g2o 文件，提取所有顶点位置并用 matplotlib 绘制 3D 轨迹。注意正确处理四元数顺序。
[对比] Ceres 的 examples/slam/common/read_g2o.h 也能读取 .g2o 文件。比较 g2o 和 Ceres 读取同一个 .g2o 文件的代码量差异。

26.6 ORB-SLAM3 Optimizer.cc 精读 ⭐⭐⭐¶

这一节解决什么问题¶

这是本章最重要的一节。学习 g2o 的最终目的是读懂 ORB-SLAM3 的代码。ORB-SLAM3 的 src/Optimizer.cc 有 3000+ 行代码，包含了图优化在实际 SLAM 系统中的所有典型用法。我们将精读其中最核心的四个函数。

26.6.1 总览：四大优化函数¶

ORB-SLAM3 的 Optimizer.cc 中定义了以下关键优化函数：

函数	调用位置	顶点	边	用途
`BundleAdjustment`	全局 BA	SE3 位姿 + 3D 点	重投影误差	全局优化所有位姿和路标
`PoseOptimization`	跟踪线程	当前帧位姿	重投影误差	单帧位姿优化（只优化位姿，固定地图点）
`LocalBundleAdjustment`	局部建图线程	共视关键帧 + 局部地图点	重投影误差	局部 BA
`OptimizeEssentialGraph`	回环闭合线程	Sim3 位姿	Sim3 约束	回环后的位姿图优化

26.6.2 BundleAdjustment：全局 BA¶

函数签名（简化）：

void Optimizer::BundleAdjustment(
    const vector<KeyFrame*>& vpKFs,    // 所有关键帧
    const vector<MapPoint*>& vpMP,     // 所有地图点
    int nIterations,                    // 迭代次数
    bool* pbStopFlag,                   // 中断标志
    const unsigned long nLoopKF,        // 回环关键帧 ID
    const bool bRobust)                 // 是否使用鲁棒核

代码结构精读：

第一步：初始化优化器

g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType* linearSolver;
linearSolver = new g2o::LinearSolverCholmod<
    g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3* solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver =
    new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);

注意：这里使用的是**裸指针 + new**，没有用 std::unique_ptr。这是 ORB-SLAM3 的历史代码风格（源自 ORB-SLAM 2015 年的代码），不是现代 C++ 最佳实践。

第二步：添加关键帧顶点

for (size_t i = 0; i < vpKFs.size(); i++) {
    KeyFrame* pKF = vpKFs[i];
    if (pKF->isBad()) continue;

    g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
    vSE3->setId(pKF->mnId);

    // 固定第一个关键帧（消除规范自由度）
    vSE3->setFixed(pKF->mnId == 0);

    optimizer.addVertex(vSE3);
}

关键理解：Converter::toSE3Quat 是 ORB-SLAM3 自定义的转换函数，将内部的 \(4 \times 4\) 变换矩阵（cv::Mat 或 Sophus::SE3f）转换为 g2o 的 SE3Quat。

第三步：添加地图点顶点

for (size_t i = 0; i < vpMP.size(); i++) {
    MapPoint* pMP = vpMP[i];
    if (pMP->isBad()) continue;

    g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
    vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
    vPoint->setId(pMP->mnId + maxKFid + 1);  // ID 偏移，避免与关键帧冲突
    vPoint->setMarginalized(true);  // 标记为边缘化变量（Schur complement）

    optimizer.addVertex(vPoint);
}

关键理解：setMarginalized(true) 告诉 BlockSolver 这个顶点是"路标变量"，应该在 Schur complement 中被边缘化。这是 BA 中加速求解的关键——先消去大量路标点，将 Hessian 从 \((6N + 3M) \times (6N + 3M)\) 降维到 \(6N \times 6N\)。

第四步：添加重投影误差边

for (size_t i = 0; i < vpEdges.size(); i++) {
    // ... 获取观测信息 ...

    g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
        optimizer.vertex(id_point)));
    e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
        optimizer.vertex(id_kf)));
    e->setMeasurement(obs);  // 像素坐标

    const float& invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
    e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);

    if (bRobust) {
        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
        e->setRobustKernel(rk);
        rk->setDelta(thHuberMono);  // thHuberMono = sqrt(5.991)
    }

    e->fx = pKF->fx;
    e->fy = pKF->fy;
    e->cx = pKF->cx;
    e->cy = pKF->cy;

    optimizer.addEdge(e);
}

第五步：优化

optimizer.initializeOptimization();
optimizer.optimize(nIterations);  // 通常 nIterations = 10 或 20

第六步：回写结果

// 更新关键帧位姿
for (size_t i = 0; i < vpKFs.size(); i++) {
    KeyFrame* pKF = vpKFs[i];
    g2o::VertexSE3Expmap* vSE3 =
        static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
    g2o::SE3Quat SE3quat = vSE3->estimate();
    pKF->SetPose(Converter::toCvMat(SE3quat));
}

// 更新地图点位置
for (size_t i = 0; i < vpMP.size(); i++) {
    MapPoint* pMP = vpMP[i];
    g2o::VertexSBAPointXYZ* vPoint =
        static_cast<g2o::VertexSBAPointXYZ*>(
            optimizer.vertex(pMP->mnId + maxKFid + 1));
    pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
}

26.6.3 PoseOptimization：单帧位姿优化¶

这是 ORB-SLAM3 跟踪线程中每一帧都会调用的函数，因此对性能要求极高。

核心思路：只优化当前帧的位姿，所有地图点固定不动。这使得问题大幅简化——只有 1 个顶点需要优化。

int Optimizer::PoseOptimization(Frame* pFrame) {
    g2o::SparseOptimizer optimizer;
    // 使用 BlockSolver_6_3：6-DOF 位姿
    // 但实际上只有 1 个位姿变量和 N 条一元边

    // 添加唯一的位姿顶点
    g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
    vSE3->setId(0);
    vSE3->setFixed(false);
    optimizer.addVertex(vSE3);

    // 对每个地图点观测添加一元边（EdgeSE3ProjectXYZOnlyPose）
    // 注意：这里用的是 OnlyPose 变体——不连接地图点顶点
    for (int i = 0; i < N; i++) {
        g2o::EdgeSE3ProjectXYZOnlyPose* e =
            new g2o::EdgeSE3ProjectXYZOnlyPose();
        e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(
            optimizer.vertex(0)));
        e->setMeasurement(obs);

        // 地图点的 3D 坐标作为边的参数（不是顶点）
        e->Xw = Converter::toVector3d(pMP->GetWorldPos());

        // ... 设置信息矩阵和鲁棒核 ...
    }

关键模式：四轮优化 + 外点剔除

PoseOptimization 不是简单地优化一次就结束，而是进行 4 轮优化，每轮之后进行外点剔除：

// 四轮优化
for (int it = 0; it < 4; it++) {
    vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
    optimizer.initializeOptimization(0);
    optimizer.optimize(10);  // 每轮 10 次迭代

    for (int i = 0; i < N; i++) {
        EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];

        if (pFrame->mvbOutlier[i]) {
            e->computeError();  // 重新计算外点的误差
        }

        const float chi2 = e->chi2();

        if (chi2 > 5.991) {
            // chi2 > 阈值 → 标记为外点，不参与下一轮优化
            pFrame->mvbOutlier[i] = true;
            e->setLevel(1);  // level=1 不参与 level=0 的优化
        } else {
            // chi2 <= 阈值 → 标记为内点，可能之前被误判
            pFrame->mvbOutlier[i] = false;
            e->setLevel(0);  // level=0 参与优化
        }

        if (it == 2) {
            e->setRobustKernel(nullptr);  // 第 3、4 轮取消鲁棒核
        }
    }
}

理解 chi2 阈值：

维度 (D)	chi2 阈值	含义
D=2（单目重投影）	5.991	自由度为 2 的卡方分布，95% 置信度
D=3（双目重投影）	7.815	自由度为 3 的卡方分布，95% 置信度

为什么是 5.991？ 如果观测噪声服从 \(\mathcal{N}(0, \sigma^2 I)\)，且估计值是正确的，那么加权残差的平方和 \(\chi^2 = \mathbf{e}^T \boldsymbol{\Omega} \mathbf{e}\) 服从自由度为 D 的卡方分布。\(\chi^2_{2, 0.05} = 5.991\) 意味着：在 95% 的情况下，内点的 chi2 值应小于 5.991。超过这个阈值的观测大概率是外点。

26.6.4 LocalBundleAdjustment：局部 BA¶

局部 BA 是 ORB-SLAM3 建图线程的核心优化步骤。它不优化整个地图，而是只优化与当前关键帧有**共视关系**的局部区域。

┌─────────────────────────────────────────────────┐
│ 共视图中的关键帧 → 设为 NOT fixed → 参与优化      │
│ 能观测到局部地图点但不在共视图中的关键帧 → fixed   │
│ 被共视关键帧观测到的地图点 → 参与优化              │
│ 其他 → 不在优化中                                 │
└─────────────────────────────────────────────────┘

关键设计决策：

为什么不全局 BA？ 全局 BA 的计算量是 \(O(N^3)\)（通过 Schur complement 可以降到更低，但仍然很大）。在实时 SLAM 中，每插入一个关键帧就做全局 BA 是不现实的。
为什么不只优化最近几帧？ 因为时间上相近的帧可能观测到不同的场景。ORB-SLAM3 使用**共视图（Covisibility Graph）**来选择优化范围：如果两个关键帧观测到很多相同的地图点，它们就有强共视关系。
固定的关键帧起什么作用？ 它们提供了"锚点"——局部区域的地图点也被这些固定帧观测到，如果不包含它们的约束，优化结果会不一致。

26.6.5 OptimizeEssentialGraph：本质图优化¶

这是回环闭合（Loop Closing）的核心优化步骤。当检测到回环后，需要在整个地图上传播回环约束，消除累积漂移。

与前三个函数的关键区别：这里使用的是 Sim3 位姿图，而不是 SE3 BA。

void Optimizer::OptimizeEssentialGraph(
    Map* pMap,
    KeyFrame* pLoopKF,           // 回环关键帧
    KeyFrame* pCurKF,            // 当前关键帧
    const LoopClosing::KeyFrameAndPose& NonCorrectedSim3,  // 未修正的 Sim3
    const LoopClosing::KeyFrameAndPose& CorrectedSim3,     // 修正后的 Sim3
    ...)
{
    g2o::SparseOptimizer optimizer;
    // 使用 BlockSolver_7_3（Sim3 的 7-DOF）
    // 但实际上是纯位姿图，没有路标

    // 顶点：每个关键帧一个 VertexSim3Expmap
    for (/* each keyframe pKF */) {
        g2o::VertexSim3Expmap* vSim3 = new g2o::VertexSim3Expmap();
        // 初始值：如果有修正的 Sim3 用修正值，否则用原始值
        vSim3->setEstimate(/* Sim3 pose */);
        vSim3->setFixed(pKF == pLoopKF);  // 固定回环帧
        optimizer.addVertex(vSim3);
    }

    // 边：回环边 + 共视边 + 生成树边
    // 1. 回环边（最关键的约束）
    // 2. 共视图中的强连接
    // 3. 生成树（Spanning Tree）中的父子关系

    optimizer.initializeOptimization();
    optimizer.optimize(20);  // 20 次迭代

    // 回写修正后的位姿，并更新所有地图点的位置
}

为什么用 Sim3 而不是 SE3？

在单目 SLAM 中，不同时间段估计的地图可能有不同的尺度。回环约束 \(S_{ij} \in \text{Sim}(3)\) 包含了尺度信息，可以同时修正旋转、平移和尺度。对于双目/RGB-D SLAM（尺度可观），Sim3 退化为 SE3（尺度因子 \(s = 1\)）。

26.6.6 ORB-SLAM3 中的 g2o 使用模式总结¶

模式	具体做法	注意事项
裸指针管理	`new` 出顶点/边，交给 optimizer	不要手动 delete
ID 管理	位姿 ID = 关键帧 ID，点 ID = 地图点 ID + 偏移	避免 ID 冲突
外点剔除	chi2 检验 + setLevel	多轮优化逐步剔除
鲁棒核	前两轮使用 Huber，后两轮取消	逐步收紧约束
Schur complement	setMarginalized(true) 给地图点	只在有路标的 BA 中使用

⚠️ 概念误区：ORB-SLAM3 的 g2o 用法是现代 C++ 最佳实践

新手想法："ORB-SLAM3 是顶级 SLAM 系统，它的代码风格一定是最好的。"

实际情况：ORB-SLAM3 的代码风格继承自 2015 年的 ORB-SLAM，使用了大量裸指针 new、全局锁、庞大的 Optimizer.cc 单文件等。这些是当时的惯例，但在 2026 年的 C++ 开发中不推荐： - 应使用 std::unique_ptr/std::make_unique 管理内存 - 应将 3000 行的 Optimizer.cc 拆分为多个模块 - 应使用 RAII 管理锁的生命周期

正确态度：学习 ORB-SLAM3 的**算法思想和设计模式**，但在自己的项目中使用**现代 C++ 风格**。

🧠 思维陷阱：认为"读懂 Optimizer.cc = 理解了 ORB-SLAM3"

新手想法："只要看懂 Optimizer.cc，就算掌握 ORB-SLAM3 了。"

实际情况：Optimizer.cc 只是 ORB-SLAM3 的后端优化部分。一个完整的 SLAM 系统还包括：特征提取与匹配（ORBextractor）、跟踪（Tracking）、关键帧管理（KeyFrameDatabase）、回环检测（LoopClosing）、地图管理（Atlas）等。优化器是被这些模块**调用**的——理解"谁在什么时候调用哪个优化函数"同样重要。

正确方法：先理解 ORB-SLAM3 的整体架构（三线程模型），再深入各个模块。

练习¶

[源码阅读] 在 ORB-SLAM3 的 Optimizer::PoseOptimization 中，为什么第 3、4 轮优化要取消鲁棒核（setRobustKernel(nullptr)）？如果一直保留鲁棒核会怎样？
[设计分析] 在 LocalBundleAdjustment 中，"固定但参与约束"的关键帧扮演什么角色？如果去掉这些固定帧（只保留优化帧），优化结果会怎么变？
[扩展] ORB-SLAM3 的 OptimizeEssentialGraph 中，回环边、共视边、生成树边分别起什么约束作用？如果只保留回环边，优化结果会是什么样？

26.7 g2o vs GTSAM vs Ceres 全面对比 ⭐⭐¶

这一节解决什么问题¶

在 SLAM 和机器人学中，g2o、GTSAM、Ceres 是三大主流优化库。选错库会导致开发效率低下或性能瓶颈。这一节提供一个全面的对比框架，帮助你在不同场景下做出正确选择。

26.7.1 API 风格对比¶

用同一个问题（两个位姿之间的相对约束）来对比三个库的 API 风格：

g2o 风格：

// g2o: 图的思维
g2o::VertexSE3* v0 = new g2o::VertexSE3();
v0->setId(0);
v0->setEstimate(Eigen::Isometry3d::Identity());
v0->setFixed(true);
optimizer.addVertex(v0);

g2o::VertexSE3* v1 = new g2o::VertexSE3();
v1->setId(1);
v1->setEstimate(initial_pose);
optimizer.addVertex(v1);

g2o::EdgeSE3* e = new g2o::EdgeSE3();
e->setVertex(0, v0);
e->setVertex(1, v1);
e->setMeasurement(relative_pose);
e->setInformation(information_matrix);
optimizer.addEdge(e);

GTSAM 风格：

// GTSAM: 因子图的思维
gtsam::NonlinearFactorGraph graph;
gtsam::Values initial;

initial.insert(0, gtsam::Pose3::Identity());
initial.insert(1, initial_pose);

graph.addPrior(0, gtsam::Pose3::Identity(), prior_noise);
graph.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(
    0, 1, relative_pose, noise_model);

Ceres 风格：

// Ceres: 残差块的思维
ceres::Problem problem;

problem.AddResidualBlock(
    PoseGraphResidual::Create(relative_pose, information_matrix),
    nullptr,  // loss function
    pose0_data, pose1_data);

problem.SetParameterBlockConstant(pose0_data);
problem.SetManifold(pose0_data, new ceres::QuaternionManifold());

26.7.2 全面对比表¶

维度	g2o	GTSAM	Ceres
设计哲学	SLAM 专用图优化	概率推理+因子图	通用最小二乘
核心抽象	顶点+边	变量+因子	参数块+残差块
增量优化	不支持（每次全量优化）	iSAM2（增量 Bayes tree）	不支持
自动微分	不支持（手写或数值）	Expression 因子	强大的自动微分模板
流形处理	Vertex::oplusImpl	traits 系统	Manifold 接口
鲁棒核	RobustKernel（较少种类）	通过噪声模型	LossFunction（丰富）
预定义类型	丰富的 SLAM 类型	丰富的 SLAM 因子	几乎没有
多线程	不支持	部分支持	支持（Jacobian 求值）
文档质量	较差（靠读源码）	优秀（教程+论文+API文档）	优秀（官方教程详尽）
GitHub Stars	~3.4k	~3.1k	~3.4k
活跃度	低（成熟稳定）	高（持续更新）	高（Google 维护）
主要生态	ORB-SLAM2/3	LIO-SAM, GTSAM 生态	Google 自动驾驶

26.7.3 性能对比¶

根据发表的基准测试和社区经验：

小规模位姿图（< 500 节点）：三者性能差异不大，瓶颈在于问题构建而非求解。

大规模位姿图（> 5000 节点）： - g2o 和 Ceres 性能接近（都使用 Cholmod/CSparse） - GTSAM 的 iSAM2 在增量场景中有巨大优势（只重新计算受影响的变量）

BA 问题： - g2o 通过 BlockSolver 的 Schur complement 优化，性能出色 - Ceres 的 Schur complement 实现同样高效，且支持多线程 Jacobian 求值 - GTSAM 的批量 BA 性能与 g2o 接近

关键结论：对于批量优化，三个库性能相当；对于在线增量优化，GTSAM 的 iSAM2 具有不可替代的优势。

26.7.4 选型指南¶

场景	推荐	理由
阅读/修改 ORB-SLAM3	g2o	ORB-SLAM3 直接使用 g2o
在线 SLAM（LIO-SAM 风格）	GTSAM	iSAM2 增量优化不可替代
自动驾驶/自定义优化	Ceres	强大的自动微分，Google 生态
新的 VSLAM 研究项目	Ceres 或 GTSAM	更好的文档和社区支持
教学/快速原型	g2o 或 GTSAM	丰富的预定义类型
需要增量+边缘化	GTSAM	iSAM2 + Bayes tree
需要极致自定义控制	Ceres	通用性最强

26.7.5 迁移路径：g2o → Ceres¶

如果你在新项目中需要从 g2o 迁移到 Ceres，核心对应关系如下：

g2o 概念	Ceres 对应
`BaseVertex<D,T>`	参数块 + `Manifold`
`BaseBinaryEdge<D,E,V1,V2>`	`CostFunction`
`computeError()`	`CostFunction::Evaluate()`
`linearizeOplus()`	解析导数 / 自动微分
`setInformation()`	残差乘以 \(\Omega^{1/2}\)
`setRobustKernel()`	`LossFunction`
`SparseOptimizer`	`ceres::Problem`
`optimizer.optimize(n)`	`ceres::Solve(options, &problem, &summary)`

⚠️ 编程陷阱：在一个项目中混用多个优化库

错误做法：在同一个 CMake 项目中同时链接 g2o、GTSAM 和 Ceres

现象：链接错误、运行时崩溃、或者更隐蔽的——数值结果不一致

根本原因：三个库可能依赖不同版本的 Eigen、SuiteSparse 或其他线性代数库。不同版本的 Eigen 有不同的 ABI（Application Binary Interface），混用会导致内存布局不一致。

正确做法： 1. 尽量只使用一个优化库 2. 如果必须混用，确保所有库编译时使用**完全相同版本**的 Eigen 3. 使用 CMake 的 find_package(Eigen3 3.4 EXACT) 强制版本一致

自检方法：ldd your_binary | grep eigen 检查是否链接了多个 Eigen 版本

练习¶

[对比实战] 用 g2o、GTSAM、Ceres 分别实现同一个 2D 位姿图优化问题（使用相同的 .g2o 数据文件）。比较：(a) 代码行数；(b) 优化结果；(c) 运行时间。
[设计分析] 为什么 g2o 不支持自动微分，而 Ceres 的自动微分是其核心卖点？这与两个库的设计目标有什么关系？
[思考] 如果你要从零开始设计一个新的激光-惯性里程计（类似 LIO-SAM），你会选择哪个优化库？为什么？考虑在线增量优化、IMU 预积分因子、回环检测等需求。

26.8 实战：3D 位姿图优化 ⭐⭐¶

这一节解决什么问题¶

前面所有的理论知识，最终要落实到可运行的代码上。这一节提供一个完整的 3D 位姿图优化示例，并与通用库·Ceres（Ceres）和 SLAM库·GTSAM（GTSAM）的实现进行对比。

26.8.1 问题描述¶

我们使用 g2o 仓库自带的 sphere.g2o 数据集——一个 2500 个位姿的 3D 球面轨迹，包含噪声和回环约束。

数据规模：
- 2500 个 VERTEX_SE3:QUAT 顶点
- ~10000 条 EDGE_SE3:QUAT 边（里程计 + 回环）
- 初始值有显著漂移

26.8.2 完整实现¶

#include <iostream>
#include <fstream>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/slam3d/types_slam3d.h>

int main(int argc, char** argv) {
    if (argc != 2) {
        std::cerr << "Usage: pose_graph_3d input.g2o" << std::endl;
        return 1;
    }

    // ============================================
    // Step 1: 初始化 g2o 三层架构
    // ============================================

    // 位姿图优化：没有路标变量，使用 BlockSolverX（动态 block 大小）
    // 也可以用 BlockSolverPL<6, 6> 但 BlockSolverX 更通用
    using BlockSolverType = g2o::BlockSolverX;
    using LinearSolverType = g2o::LinearSolverEigen<
        BlockSolverType::PoseMatrixType>;

    auto linearSolver = std::make_unique<LinearSolverType>();
    auto blockSolver = std::make_unique<BlockSolverType>(
        std::move(linearSolver));
    auto algorithm = new g2o::OptimizationAlgorithmLevenberg(
        std::move(blockSolver));

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(algorithm);
    optimizer.setVerbose(true);

    // ============================================
    // Step 2: 加载 .g2o 文件
    // ============================================
    std::ifstream fin(argv[1]);
    if (!fin) {
        std::cerr << "File " << argv[1] << " does not exist!" << std::endl;
        return 1;
    }

    if (!optimizer.load(argv[1])) {
        std::cerr << "Error loading " << argv[1] << std::endl;
        return 1;
    }

    std::cout << "Loaded graph with "
              << optimizer.vertices().size() << " vertices and "
              << optimizer.edges().size() << " edges." << std::endl;

    // ============================================
    // Step 3: 固定第一个顶点（消除规范自由度）
    // ============================================
    auto firstVertex = optimizer.vertex(0);
    if (firstVertex) {
        firstVertex->setFixed(true);
        std::cout << "Fixed vertex 0 as anchor." << std::endl;
    }

    // ============================================
    // Step 4: 优化
    // ============================================
    optimizer.initializeOptimization();

    std::cout << "Initial chi2 = " << optimizer.chi2() << std::endl;
    optimizer.optimize(30);
    std::cout << "Final chi2 = " << optimizer.chi2() << std::endl;

    // ============================================
    // Step 5: 保存结果
    // ============================================
    optimizer.save("sphere_optimized.g2o");
    std::cout << "Saved to sphere_optimized.g2o" << std::endl;

    return 0;
}

CMakeLists.txt：

cmake_minimum_required(VERSION 3.10)
project(pose_graph_3d)

set(CMAKE_CXX_STANDARD 17)

find_package(g2o REQUIRED)
find_package(Eigen3 REQUIRED)

add_executable(pose_graph_3d pose_graph_3d.cpp)
target_link_libraries(pose_graph_3d
    g2o::core
    g2o::types_slam3d
    g2o::solver_eigen
    Eigen3::Eigen
)

26.8.3 与通用库·Ceres/SLAM库·GTSAM 的对比¶

维度	g2o（本章）	Ceres（通用库·Ceres）	GTSAM（SLAM库·GTSAM）
初始化	三层模板嵌套，~10行	Options 结构体，~5行	直接构造 Graph + Values
顶点/变量	`addVertex(new VertexSE3())`	裸 double 数组 + Manifold	`values.insert(key, Pose3)`
边/约束	`addEdge(new EdgeSE3())`	`AddResidualBlock(cost, loss, ...)`	`graph.add(BetweenFactor)`
数据加载	`optimizer.load("file.g2o")`	自己写解析器	自己写解析器
固定变量	`setFixed(true)`	`SetParameterBlockConstant`	`addPrior(key, value, noise)`
代码总行数	~50行	~100行	~40行
优化结果	chi2 收敛到相同值	chi2 收敛到相同值	chi2 收敛到相同值

关键观察：对于位姿图优化这个特定问题，g2o 的代码最短（因为 .g2o 文件加载是内置功能），GTSAM 次之（API 更简洁），Ceres 最长（需要自己定义残差函数和解析器）。但对于自定义问题，Ceres 的灵活性和自动微分优势会逐渐显现。

26.8.4 常见错误排查¶

在运行实战代码时，你可能遇到以下问题：

现象	原因	解决方案
链接错误：undefined reference to g2o::	g2o 库路径未正确配置	检查 `find_package(g2o)` 和 `target_link_libraries`
运行时崩溃：segfault	忘记 `setAlgorithm()`	确保在添加顶点前完成初始化
chi2 不收敛	未固定任何顶点	至少固定一个顶点消除规范自由度
结果全是 NaN	信息矩阵不正定或有 NaN	检查输入数据的信息矩阵
优化后与初始值一样	`initializeOptimization()` 未调用	在 `optimize()` 前调用 `initializeOptimization()`

练习¶

[实操] 编译并运行上述代码。对比优化前后的轨迹（可以用 Python + matplotlib 可视化 .g2o 文件）。
[扩展] 修改代码，使用 GaussNewton 代替 LevenbergMarquardt。比较收敛速度和最终结果。在什么情况下 GN 会发散？
[跨库对比] 用 Ceres 和 GTSAM 分别实现相同的 3D 位姿图优化（使用相同的 sphere.g2o 数据）。验证三个库的优化结果一致。

26.9 g2o 工程边界与验证清单 ⭐⭐¶

这一节解决什么问题：g2o 的接口更接近底层 C++：顶点、边、求解器和鲁棒核大量使用裸指针和模板类型。它给了开发者很高自由度，也要求开发者自己守住类型、所有权、规范自由度和信息矩阵边界。

动机：g2o 的错误通常发生在图结构层¶

回顾通用库·Ceres：Ceres 由残差块组织问题；回顾 SLAM库·GTSAM：GTSAM 用 Key 和因子图表达概率结构。g2o 则直接暴露顶点和边。你必须明确每个顶点的 id、维度、估计类型、更新方式，以及每条边连接的顶点类型。

如果不做图结构验证会怎样？程序可能在 initializeOptimization() 后段错误，也可能正常运行但没有任何变量被优化。更隐蔽的是信息矩阵不正定：g2o 仍会组装线性系统，但 Hessian 可能病态，最终 chi2 不下降或产生 NaN。

本质洞察：g2o 的核心边界是"模板类型和运行时图结构必须一致"。模板告诉编译器一条边期望什么顶点类型，运行时 setVertex() 必须真的传入这些类型；否则错误会在优化时才暴露。

图结构构建检查表¶

检查项	必须满足	失败现象
Optimizer 初始化	`setAlgorithm()` 在优化前完成	段错误或 `optimize()` 无效
顶点 id	全局唯一、连续性非必需但建议可读	添加失败或覆盖
固定规范自由度	位姿图至少固定一个位姿	chi2 下降但整体漂移，Hessian 秩亏
边顶点类型	`BaseBinaryEdge<D,E,V1,V2>` 与 `setVertex` 一致	`dynamic_cast` 失败或运行时崩溃
信息矩阵	对称正定，维度为残差维度	NaN、线性求解失败
鲁棒核	只用于可能有外点的边	过度降权导致图约束不足

g2o 的设计更像手动搭电路：每个顶点是节点，每条边是元件，信息矩阵是元件强度。只要某个接口接错，整张图的电路仍然可能连通，但电流方向已经不对。

信息矩阵验证代码¶

信息矩阵是图优化中最容易被忽略的数值边界。下面的函数在添加边前检查对称性和正定性。对于 6D 位姿约束，它能提前发现很多 .g2o 文件和手写边中的问题。

#include <Eigen/Dense>
#include <stdexcept>

template <int Dim>
void validateInformationMatrix(const Eigen::Matrix<double, Dim, Dim>& info,
                               const std::string& edge_name) {
    if (!info.allFinite()) {
        throw std::runtime_error(edge_name + ": information matrix has NaN or Inf");
    }

    const double symmetry_error = (info - info.transpose()).norm();
    if (symmetry_error > 1e-10) {
        throw std::runtime_error(edge_name + ": information matrix is not symmetric");
    }

    Eigen::LLT<Eigen::Matrix<double, Dim, Dim>> llt(info);
    if (llt.info() != Eigen::Success) {
        throw std::runtime_error(edge_name + ": information matrix is not positive definite");
    }
}

对象生命周期边界也很重要：顶点和边通常用 new 创建并交给 SparseOptimizer。添加成功后不要在外部手动释放，也不要把同一个裸指针加入两个 optimizer。工程项目中建议用工厂函数集中创建顶点/边，并在添加失败时立即释放或抛出异常，避免泄漏。

ORB-SLAM3 风格的外点处理边界¶

ORB-SLAM3 的 PoseOptimization 不是"加 Huber 核跑一次"这么简单，而是多轮优化：

加入所有投影边，设置 Huber 核。
优化若干轮。
根据 \(\chi^2\) 阈值标记外点。
关闭或弱化鲁棒核，重新优化内点。
最终仅使用内点更新位姿。

这个流程背后的逻辑是：鲁棒核适合在初期避免外点拖偏解；当内外点已经分离后，继续保留强鲁棒核会降低内点的信息量。鲁棒核不是永久屏障，而是外点判别的过渡工具。

与 Ceres/GTSAM 的互验证¶

对同一个 .g2o 文件，建议至少做一次跨库互验证：

验证	目的
g2o 优化前后 chi2	确认图结构和信息矩阵有效
Ceres 读取同一文件优化	验证残差定义和四元数顺序
GTSAM 构造 BetweenFactor	验证噪声模型与相对位姿方向
evo 评测轨迹	验证输出格式和坐标系

跨库结果不需要逐位相同，因为更新约定和终止条件不同；但最终 cost、轨迹形状和回环闭合效果应当一致。若差异很大，优先检查相对位姿方向 \(T_{ij}\) 是 \(T_i^{-1}T_j\) 还是 \(T_j^{-1}T_i\)。

练习¶

信息矩阵题：构造一个非对称信息矩阵和一个半正定信息矩阵，验证 validateInformationMatrix() 能在添加边前报错。
外点流程题：仿照 ORB-SLAM3，对 2D 位姿图加入 5 条错误边，比较单轮 Huber 和多轮 chi2 剔除的结果。
跨章综合题：将通用库·文件IO 的 .g2o 解析器输出分别送入 g2o、Ceres、GTSAM，比较三者优化后的第一条回环残差。

本章小结¶

知识点	核心内容	难度	对应 ORB-SLAM3
g2o 三层架构	SparseOptimizer → Algorithm → LinearSolver	⭐⭐	所有 Optimizer 函数的初始化部分
BlockSolver	利用 SLAM block 稀疏结构加速	⭐⭐	BlockSolver_6_3 用于 BA
BaseVertex	D=切空间维度，T=估计值类型	⭐⭐	VertexSE3Expmap, VertexSBAPointXYZ
oplusImpl	切空间更新 → 流形更新	⭐⭐	左乘 SE3 更新
BaseBinaryEdge	D=残差维度，E=观测类型	⭐⭐	EdgeSE3ProjectXYZ
computeError	计算残差向量	⭐⭐	重投影误差
linearizeOplus	解析 Jacobian（可选）	⭐⭐⭐	BA 的 Jacobian
信息矩阵	观测精度的权重	⭐⭐	金字塔层级加权
鲁棒核	抵抗外点	⭐⭐	Huber 核
chi2 阈值	外点检测	⭐⭐	5.991（2-DOF），7.815（3-DOF）
.g2o 文件格式	标准图优化数据交换格式	⭐	调试和可视化
四轮优化模式	多轮优化+逐步外点剔除	⭐⭐⭐	PoseOptimization
Sim3 位姿图	回环闭合中消除尺度漂移	⭐⭐⭐	OptimizeEssentialGraph
g2o vs GTSAM vs Ceres	选型决策框架	⭐⭐	—

累积项目：Mini-LIO¶

本章新增模块：位姿图优化后端

在 Mini-LIO 项目中，本章为后端优化模块提供了基础：

Mini-LIO/
├── src/
│   ├── preprocess/        # 通用库·PCL: 点云预处理
│   ├── frontend/          # 通用库·OpenCV: ICP 配准
│   ├── backend/
│   │   ├── pose_graph.h   # ← 本章新增：g2o 位姿图优化
│   │   ├── pose_graph.cpp # ← 本章新增
│   │   └── loop_closure/  # 回环检测（后续章节）
│   └── ...

任务：

实现 PoseGraph 类，封装 g2o 的三层初始化
提供 addOdometryEdge()、addLoopEdge()、optimize() 接口
对接前端 ICP 配准的输出（相对位姿约束）
保存优化轨迹为 .g2o 文件，用 g2o_viewer 可视化

延伸阅读¶

论文¶

论文	内容	难度
Kümmerle et al., "g2o: A General Framework for Graph Optimization", ICRA 2011	g2o 的原始论文，必读	⭐⭐
Mur-Artal et al., "ORB-SLAM: A Versatile and Accurate Monocular SLAM System", IEEE T-RO 2015	ORB-SLAM 系列开篇，理解 g2o 的使用上下文	⭐⭐⭐
Campos et al., "ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM", IEEE T-RO 2021	ORB-SLAM3，Sim3 位姿图优化的典型应用	⭐⭐⭐
Strasdat et al., "Double Window Optimisation for Constant Time Visual SLAM", ICCV 2011	g2o 作者之一的工作，BA 的工程优化	⭐⭐⭐⭐

源码¶

资源	内容	推荐阅读顺序
g2o GitHub	g2o 源码	`core/` → `types/slam3d/` → `examples/`
ORB-SLAM3 GitHub	`src/Optimizer.cc`	PoseOptimization → BA → LocalBA → EssentialGraph
g2o Wiki: File Format	.g2o 文件格式规范	快速参考
Ceres read_g2o.h	Ceres 解析 .g2o 文件的实现	了解跨库互操作

教程¶

资源	内容	难度
高翔《视觉SLAM十四讲》第10章	g2o 入门教程（中文）	⭐⭐
g2o Tutorial by IanFlow	英文 g2o 使用教程	⭐⭐
SLAM from 0 to 1: g2o	从零到一的 g2o 图优化教程	⭐⭐

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
`optimize()` 前后结果完全不变	未调用 `initializeOptimization()`、未设置算法、所有顶点都被固定	1. 检查 `optimizer.algorithm()`；2. 确认至少有非 fixed 顶点；3. 打印 active vertices/edges 数量	SLAM库·g2o
程序在优化时段错误	边连接的顶点类型不匹配，或顶点指针为空	1. 检查 `setVertex(0/1, ...)` 顺序；2. 用 `dynamic_cast` 验证类型；3. 添加边前确认 vertex id 已存在	SLAM库·g2o
chi2 不下降或出现 NaN	信息矩阵非正定、初值太差、残差方向写反	1. 对信息矩阵做 LLT 检查；2. 用小图测试 `computeError()`；3. 对照 Ceres/GTSAM 实现	通用库·Ceres, SLAM库·GTSAM, SLAM库·g2o
g2o_viewer 无法识别自定义类型	类型未注册或链接时未包含对应 type 库	1. 检查 `G2O_REGISTER_TYPE`；2. 确认链接 `types_slam3d`/自定义库；3. 用内置类型文件做对照	通用库·文件IO, SLAM库·g2o
BA 求解速度远慢于预期	BlockSolver 维度选错，未利用 6-3 block 结构	1. 位姿+路标 BA 使用 `BlockSolver_6_3`；2. 位姿图使用 `BlockSolver_6_6` 或动态块；3. 查看线性求解器耗时	SLAM库·g2o

第26章：g2o 图优化——读懂 ORB-SLAM3 的钥匙¶

前置自测¶

本章目标¶

26.1 g2o 设计哲学与历史 ⭐⭐¶

这一节解决什么问题¶

26.1.1 历史背景：Kümmerle et al. 2011¶

26.1.2 图优化的核心思想¶

26.1.3 g2o vs Ceres：设计哲学的根本差异¶

26.1.4 什么时候用 g2o¶

练习¶

26.2 g2o 三层架构 ⭐⭐¶

这一节解决什么问题¶

26.2.1 架构总览¶

26.2.2 第三层：LinearSolver（线性求解器）¶

26.2.3 第二层：BlockSolver（块求解器）¶

26.2.4 第二层（上）：OptimizationAlgorithm（优化算法）¶

26.2.5 第一层：SparseOptimizer（稀疏优化器）¶

26.2.6 完整初始化代码详解¶

练习¶

26.3 顶点 (Vertex) ⭐⭐¶

这一节解决什么问题¶

26.3.1 BaseVertex 模板解析¶

26.3.2 必须实现的四个方法¶

26.3.3 内置顶点类型详解¶

26.3.4 固定顶点与规范自由度¶

练习¶

26.4 边 (Edge) ⭐⭐¶

这一节解决什么问题¶

26.4.1 三种边的基类¶

26.4.2 边的核心方法¶

26.4.3 内置边类型详解¶

26.4.4 重投影误差 Jacobian 推导¶

26.4.5 信息矩阵与鲁棒核函数¶

练习¶

26.5 .g2o 文件格式与序列化 ⭐¶

这一节解决什么问题¶

26.5.1 .g2o 文件格式详解¶

26.5.2 类型注册机制：G2O_REGISTER_TYPE¶

26.5.3 加载、优化、保存的完整流程¶

26.5.4 g2o_viewer 可视化工具¶

练习¶

26.6 ORB-SLAM3 Optimizer.cc 精读 ⭐⭐⭐¶

这一节解决什么问题¶

26.6.1 总览：四大优化函数¶

26.6.2 BundleAdjustment：全局 BA¶

26.6.3 PoseOptimization：单帧位姿优化¶

26.6.4 LocalBundleAdjustment：局部 BA¶

26.6.5 OptimizeEssentialGraph：本质图优化¶

26.6.6 ORB-SLAM3 中的 g2o 使用模式总结¶

练习¶

26.7 g2o vs GTSAM vs Ceres 全面对比 ⭐⭐¶

这一节解决什么问题¶

26.7.1 API 风格对比¶

26.7.2 全面对比表¶

26.7.3 性能对比¶

26.7.4 选型指南¶

26.7.5 迁移路径：g2o → Ceres¶

练习¶

26.8 实战：3D 位姿图优化 ⭐⭐¶

这一节解决什么问题¶

26.8.1 问题描述¶

26.8.2 完整实现¶

26.8.3 与 通用库·Ceres/SLAM库·GTSAM 的对比¶

26.8.4 常见错误排查¶

练习¶

26.9 g2o 工程边界与验证清单 ⭐⭐¶

动机：g2o 的错误通常发生在图结构层¶

图结构构建检查表¶

信息矩阵验证代码¶

ORB-SLAM3 风格的外点处理边界¶

与 Ceres/GTSAM 的互验证¶

练习¶

本章小结¶

累积项目：Mini-LIO¶

延伸阅读¶

论文¶

源码¶

教程¶

🔧 故障排查手册¶

26.8.3 与通用库·Ceres/SLAM库·GTSAM 的对比¶