做 SLAM 后端调试久了你会发现几乎所有开源框架里都藏着一个绕不开的库G2O全称 General Graph Optimization通用图优化。我第一次被它折磨是在跑视觉 SLAM 看后端代码时满屏的addVertex、addEdge、setInformation每个类名都似懂非懂后来换到激光 SLAM位姿图优化还是它。最崩溃的一次是曲线拟合例子都能跑通换成位姿图却怎么都不收敛报错信息还看不懂。这篇文章就是我“被 G2O 折腾到半夜”之后整理出来的入门笔记不讲太多理论推导重点说清它到底解决什么问题、代码为什么这样分层、如何照着写出第一个可运行例子以及 SLAM 里最常用的位姿图是怎么用它的。适合刚接触视觉或激光 SLAM 后端、想把 G2O 用起来而不是钻进源码出不来的人。1. 图优化要解决的是什么事从状态估计说起1.1 所有SLAM问题本质上都是带噪声的最小二乘要理解 G2O先得搞清楚它在整个系统里扮演什么角色。SLAM 的核心难题可以概括成我们不知道机器人或相机在哪儿也不知道路标在哪儿手里只有一堆带噪声的观测数据。相机图像上的特征点、激光雷达扫出来的几何特征、轮式里程计给的运动增量这些统统都是测量值而测量就免不了有噪声。那怎么办最朴素的想法是把所有待估计的参数记成一个向量把每一次观测都变成一个约束条件然后找一组参数使得所有约束的误差平方和尽量小。这就是非线性最小二乘问题的定义。举个例子假设你拟合一条曲线y exp(a*x^2 b*x c)手里有一堆带噪声的采样点你希望找到最优的a, b, c让曲线尽量贴合采样点。这个目标写成数学形式就是最小化残差平方和min sum_i ( y_i - exp(a*x_i^2 b*x_i c) )^2这个形式很简单但直接求解析解是不可能的因为模型函数带指数、三角函数、投影变换全都非线性。常规做法是迭代先给一组初始值把非线性函数在当前点做一阶泰勒展开把问题变成局部线性最小二乘解出增量更新变量再重复。Gauss-Newton 和 Levenberg-Marquardt 算法干的就是这件事。这里要澄清一个常见误区最小二乘并不稀奇任何一本数值优化教材都会讲。SLAM 的特殊之处在于参数之间天然存在“稀疏连接”关系。一个关键帧只和它观测到的地图点、相邻关键帧有约束和十万八千里之外的另一帧没有任何关系。把所有参数全部展开成一维向量后Hessian 矩阵里会存在大量零块。如果你用普通稠密优化库去解内存和计算量会直接爆炸。图优化的核心思想就是把这种稀疏结构显式地建模成一张图利用稀疏性来加速求解。1.2 G2O 为什么叫“通用”图优化G2O 全称 General Graph Optimization关键字在“General”。它的设计目标是不管你的优化问题来自视觉 SLAM、激光 SLAM、还是别的最小二乘场景只要你把问题抽象成“图”它就能帮你迭代求解。在 G2O 的世界里图由两类元素构成顶点表示待优化变量比如相机位姿、路标点坐标、曲线拟合参数。边表示顶点之间的约束关系边里定义了误差函数。你只需要告诉 G2O有哪些顶点、每个顶点初始值是多少、有哪些边、每条边的误差怎么算、这条约束有多可信。剩下的矩阵组装、稀疏分解、迭代更新G2O 全部替你搞定。这也是它“通用”二字的含义它不关心你的顶点是不是位姿也不关心你的误差是重投影误差还是 ICP 误差它只负责求解“带稀疏结构的最小二乘问题”。用大白话讲G2O 就像一个万能螺丝刀。你不需要知道螺丝内部金属怎么受力你只需要选对刀头顶点和边的类型拧就完事了。当然要想拧得又快又好你还得知道刀头怎么选、用多大力气信息矩阵权重、往哪个方向拧迭代策略这些就是本文后面要展开的内容。2. G2O 的五个主角顶点、边、求解器与算法2.1 顶点待估计变量的抽象G2O 中所有顶点类都继承自g2o::BaseVertexD, T两个模板参数含义很明确D是优化变量的维度。例如曲线拟合的a, b, c三个参数维度就是 32D 位姿(x, y, theta)维度是 33D 位姿SE3的自由度是 6。T是优化变量的数据类型。例如Eigen::Vector3d、g2o::SE3Quat、Eigen::Isometry3d等。使用内置顶点时比如g2o::VertexSE3、g2o::VertexSE2直接用就行。但真正常遇到的场景是要自定义顶点这时必须重写四个方法virtual void setToOriginImpl() override { _estimate.setZero(); // 把估计值设为零 } virtual void oplusImpl(const double* update) override { _estimate Eigen::Vector3d(update); // 普通向量直接加法 }setToOriginImpl决定顶点初始状态oplusImpl决定“迭代时怎么用增量更新估计值”。对于欧式空间向量直接加更新量没问题但对于位姿这种流形上的量简单加法会破坏旋转矩阵的约束必须用指数映射把增量映射到流形上。这一点在第四节细说它是自定义 SE3 顶点最容易出坑的地方。2.2 边误差函数的载体边代表观测约束。G2O 中边分为一元边、二元边、三元边分别对应误差只和一个、两个、三个顶点有关。继承g2o::BaseUnaryEdgeD, E, VertexType或g2o::BaseBinaryEdgeD, E, VertexXi, VertexXj时模板参数同样好理解D是误差向量的维度。E是误差的数据类型通常是double或Eigen::VectorXd。后面跟的是关联顶点的类型。每条边必须实现computeError()也就是告诉 G2O“这条边的误差怎么算”。如果需要更高效稳定的优化最好再实现linearizeOplus()也就是手动计算雅可比矩阵。这里必须强调一个容易踩的坑G2O 不像 Ceres 那样自带数值自动求导如果你不实现linearizeOplus()G2O 内部会用默认的空实现雅可比矩阵全是零优化根本走不动表现就是误差不下降、迭代停滞、甚至直接崩溃。一元边里雅可比存储在_jacobianOplusXi二元边里分别有_jacobianOplusXi和_jacobianOplusXj。这个命名要记住后面代码里直接用到。2.3 求解器与迭代算法优化器的三件套G2O 对外的主类是g2o::SparseOptimizer它就是那个“图”本身。但在正式优化之前你必须给它装配三件套线性求解器负责解每次迭代中的线性方程组H dx b。小规模稠密问题用LinearSolverDense大规模稀疏问题用LinearSolverCholmod或LinearSolverEigen。块求解器包装线性求解器负责按顶点和边的分块结构处理矩阵。类型是g2o::BlockSolverBlockSolverTraitsD, E其中D是顶点维度E是误差维度。迭代算法决定每次迭代采用什么策略。常用g2o::OptimizationAlgorithmLevenbergLM、g2o::OptimizationAlgorithmGaussNewtonGN、g2o::OptimizationAlgorithmDogleg。三件套的装配逻辑可以用做饭类比线性求解器就像是炉子负责把食材炖烂块求解器是锅决定食材怎么分堆下锅迭代算法是厨师长决定这一轮加多少调料、火开多大。三者配合不对菜就做砸了。实际上装配代码非常机械照着模板写就行但参数不能写错比如维度声明和顶点实际维度不匹配编译能过但运行会崩。3. 手写第一个可运行例子曲线拟合3.1 自定义顶点与边的完整实现曲线拟合是理解 G2O 的最小可运行例子麻雀虽小五脏俱全。假设真实曲线是y exp(1.0*x^2 2.0*x 1.0)我们采样生成带高斯噪声的数据点再让 G2O 从零初始值出发反推出a, b, c。自定义顶点代码如下#include g2o/core/base_vertex.h #include g2o/core/base_unary_edge.h #include g2o/core/block_solver.h #include g2o/core/optimization_algorithm_levenberg.h #include g2o/core/optimization_algorithm_gauss_newton.h #include g2o/core/solver.h #include g2o/core/sparse_optimizer.h #include g2o/solvers/dense/linear_solver_dense.h #include Eigen/Core #include cmath #include iostream #include random class CurveFittingVertex : public g2o::BaseVertex3, Eigen::Vector3d { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW virtual void setToOriginImpl() override { _estimate.setZero(); } virtual void oplusImpl(const double* update) override { _estimate Eigen::Vector3d(update); } virtual bool read(std::istream in) override { return true; } virtual bool write(std::ostream out) const override { return true; } };注意EIGEN_MAKE_ALIGNED_OPERATOR_NEW这个宏凡是顶点或边内部用到Eigen::Vector3d等固定大小向量时都应该加上。不加的话在某些平台和编译器组合下会出现对齐崩溃报错信息极其抽象排查半天才发现是这里漏了。自定义边需要计算残差。对于每个采样点(x_i, y_i)error_i y_i - exp(a*x_i^2 b*x_i c)雅可比对三个参数的偏导分别是de/da -exp(a*x^2 b*x c) * x^2 de/db -exp(a*x^2 b*x c) * x de/dc -exp(a*x^2 b*x c)对应代码如下class CurveFittingEdge : public g2o::BaseUnaryEdge1, double, CurveFittingVertex { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CurveFittingEdge(double x, double y) : _x(x), _y(y) {} virtual void computeError() override { const CurveFittingVertex* v static_castconst CurveFittingVertex*(_vertices[0]); const Eigen::Vector3d abc v-estimate(); _error(0, 0) _y - std::exp(abc(0, 0) * _x * _x abc(1, 0) * _x abc(2, 0)); } virtual void linearizeOplus() override { const CurveFittingVertex* v static_castconst CurveFittingVertex*(_vertices[0]); const Eigen::Vector3d abc v-estimate(); double exp_val std::exp(abc(0, 0) * _x * _x abc(1, 0) * _x abc(2, 0)); _jacobianOplusXi(0, 0) -exp_val * _x * _x; _jacobianOplusXi(0, 1) -exp_val * _x; _jacobianOplusXi(0, 2) -exp_val; } virtual bool read(std::istream in) override { return true; } virtual bool write(std::ostream out) const override { return true; } public: double _x; double _y; };强调一个细节_error是Eigen::Matrixdouble, 1, 1类型所以赋值时写_error(0, 0) ...不能直接_error ...后者编译会报类型不匹配。_jacobianOplusXi同理它是固定大小矩阵行数等于误差维度列数等于顶点优化维度此处是1 x 3。3.2 组装优化器并跑起来主函数流程是这样的设定顶点 id 和初值然后循环生成边、设置信息矩阵最后调用优化器。完整代码int main() { // 生成带噪声的观测数据 std::random_device rd; std::mt19937 gen(rd()); std::normal_distributiondouble noise(0.0, 0.05); double a_true 1.0, b_true 2.0, c_true 1.0; int N 100; std::vectordouble xs(N), ys(N); for (int i 0; i N; i) { xs[i] static_castdouble(i) / N; ys[i] std::exp(a_true * xs[i] * xs[i] b_true * xs[i] c_true) noise(gen); } // 建图 g2o::SparseOptimizer optimizer; // 三件套线性求解器 - 块求解器 - LM算法 using BlockSolverType g2o::BlockSolverg2o::BlockSolverTraits3, 1; using LinearSolverType g2o::LinearSolverDenseBlockSolverType::PoseMatrixType; auto* linearSolver new LinearSolverType(); auto* blockSolver new BlockSolverType(linearSolver); auto* algorithm new g2o::OptimizationAlgorithmLevenberg(blockSolver); optimizer.setAlgorithm(algorithm); optimizer.setVerbose(true); // 添加顶点 auto* v new CurveFittingVertex(); v-setId(0); v-setEstimate(Eigen::Vector3d(0.0, 0.0, 0.0)); optimizer.addVertex(v); // 添加边 for (int i 0; i N; i) { auto* edge new CurveFittingEdge(xs[i], ys[i]); edge-setId(i); edge-setVertex(0, v); edge-setInformation(Eigen::Matrixdouble, 1, 1::Identity()); optimizer.addEdge(edge); } // 启动优化 optimizer.initializeOptimization(); optimizer.optimize(100); // 输出结果 Eigen::Vector3d abc v-estimate(); std::cout estimated a, b, c abc.transpose() std::endl; return 0; }编译时需要链接 G2O 相关库。CMakeLists 大致可以这样写cmake_minimum_required(VERSION 3.10) project(g2o_curve_fitting) set(CMAKE_CXX_STANDARD 14) find_package(g2o REQUIRED) include_directories(${G2O_INCLUDE_DIRS}) add_executable(curve_fitting main.cpp) target_link_libraries(curve_fitting ${G2O_LIBS})如果系统里能找到find_package(g2o)模块直接用即可。找不到时可以手动指定 include 路径和链接路径一般常见安装路径是/usr/local/include/g2o、/usr/local/lib。3.3 结果怎么看verbose输出与参数收敛设置optimizer.setVerbose(true)后终端会打印每次迭代的误差。正常情况下应该看到类似这样的趋势iteration 0 chi2 1324.53 time 0.000251 cumTime 0.000251 edges 100 schur 0 iteration 1 chi2 318.672 time 0.000144 cumTime 0.000395 edges 100 schur 0 iteration 2 chi2 37.8186 time 0.000122 cumTime 0.000517 edges 100 schur 0 ... iteration 10 chi2 0.508796 time 0.000126 cumTime 0.001245 edges 100 schur 0这里chi2就是加权残差平方和是判断优化是否收敛的核心指标。如果看到chi2不断下降最后平稳说明没跑偏。如果chi2停留在很大的值不动或者震荡发散说明有地方写错了。我跑这个例子通常 20 次以内就能收敛到a≈1.0, b≈2.0, c≈1.0误差在噪声水平内。把噪声标准差调大到0.2再试结果会偏离真实值多一些这也符合最小二乘的统计性质说明 G2O 正常工作。4. 从曲线拟合走向位姿图SLAM中的常见用法4.1 位姿图的建模方式曲线拟合的例子虽然能跑通但 G2O 真正的大本营是位姿图优化。在视觉或激光 SLAM 中当路标点被边缘化之后问题会退化为只优化关键帧位姿。这时图的建模非常清晰每个关键帧位姿是一个顶点2D SLAM 用g2o::VertexSE23D SLAM 用g2o::VertexSE3。相邻关键帧之间的相对运动约束是一条边类型是g2o::EdgeSE2或g2o::EdgeSE3。回环检测给出的历史相对位姿也是一条边只是源和目标通常相隔很远。边的误差怎么定义拿 2D 情况举例。假设顶点 i 的位姿是T_i顶点 j 的位姿是T_j二者之间有一个通过 scan matching 或特征匹配得到的相对位姿观测Z_ij。那么预测出的相对变换是T_i^{-1} * T_j误差就是预测和观测之间的差值通常取成log(Z_ij^{-1} * T_i^{-1} * T_j)也就是把变换差映射到李代数上的向量。G2O 内置的位姿边已经实现好了这套误差计算和雅可比你不必重新推导。如果要用代码建一个简单的 2D 位姿图核心片段是这样的for (size_t i 0; i num_poses; i) { auto* v new g2o::VertexSE2(); v-setId(static_castint(i)); v-setEstimate(poses[i]); optimizer.addVertex(v); } for (size_t i 0; i 1 num_poses; i) { auto* e new g2o::EdgeSE2(); e-setVertex(0, optimizer.vertex(i)); e-setVertex(1, optimizer.vertex(i 1)); e-setMeasurement(relative_measurements[i]); e-setInformation(measurement_infos[i]); optimizer.addEdge(e); }这段代码的关键在于每条边的setMeasurement必须传入相对位姿而不是绝对位姿。我第一次写的时候误把绝对位姿塞进去结果优化结果完全乱套。4.2 信息矩阵为什么不能全部设成单位阵位姿图优化里最容易忽视但影响最大的是信息矩阵设置。信息矩阵等于观测协方差矩阵的逆矩阵它告诉 G2O“这条边有多值得信任”。不同约束的可靠性差别很大轮式里程计相邻帧之间的运动约束协方差会随距离累积而增大信息矩阵应该相应调小。回环约束是重新识别到曾经到过的地方位姿修正信息非常强协方差通常较小信息矩阵应该调大。如果把所有信息矩阵都设成单位阵等于告诉 G2O“所有约束同样可信”结果就是高噪声约束和低噪声约束被一视同仁优化结果被噪声较大的测量拖偏。所以每次加边之前先把约束的置信度想清楚再填信息矩阵。实际调参时常用的经验是先看每条边的初始残差chi2贡献如果某条边贡献异常大优先怀疑信息矩阵给大了或者测量数据本身是外点。回环边如果给太大可能把整条轨迹强行拧到一个错误位置比不给回环还糟糕。4.3 位姿顶点的更新机制与普通向量不同第四节开头说的“流形更新”问题在这里必须展开。g2o::VertexSE3内部存储的是g2o::SE3Quat也就是一个旋转四元数加一个平移向量本质上是一个 7 维对象但自由度为 6。你不能在oplusImpl里直接做estimate() update因为旋转分量加一个三维增量后得到的四元数大概率不再单位化旋转矩阵失去约束。G2O 内置的VertexSE3在oplusImpl中会把传入的 6 维更新向量先映射成李代数se(3)再通过指数映射转换成变换矩阵最后与原位姿复合。这样每次更新都能停留在合理的流形上。如果你要自定义位姿类型的顶点千万别照抄曲线拟合里的_estimate Eigen::Vector3d(update)一定要使用指数映射或至少用四元数球面插值。这个知识点也解释了为什么同样一个优化问题用内置顶点能收敛自己瞎写顶点就崩不是数学推导错了而是更新方式破坏了位姿约束。5. 常见问题与排查技巧实录5.1 维度不匹配编译通过但运行崩溃G2O 模板参数多维度写错的概率极高。最典型的是BlockSolverTraitsD, E中的D和顶点优化维度不一致。比如优化 3D 位姿顶点维度是 6但你写成了BlockSolverTraits3, 3编译阶段可能不会报错运行时SparseOptimizer内部取顶点时发现维度对不上直接断言失败或者访问越界。排查方法把顶点的BaseVertex第一参数、边的BaseBinaryEdge第一参数误差维度和BlockSolverTraits两个参数逐一对一遍。一个自己的习惯是先把顶点类型单独写出来用static_assert在编译期检查维度再进入优化环节能省很多调试时间。5.2 误差不下降优先检查雅可比和信息矩阵如果chi2卡住不动最常见原因是linearizeOplus没写或者写错了。G2O 没有自动求导这是和 Ceres 最大的区别。数值优化的流程是调用computeError计算当前残差。调用linearizeOplus计算雅可比矩阵。组装H J^T * W * J和b -J^T * W * e。解H dx b更新变量。第二步如果返回零矩阵整个迭代系统就瘫痪了。验证雅可比是否正确有一个土办法写一个数值差分函数给顶点加一个很小的扰动对比(e(xdelta) - e(x)) / delta和解析雅可比。如果数值对不上优化基本不会收敛。信息矩阵设成零矩阵也会导致问题G2O 内部会把这条边的贡献忽略即使其他配置都对误差也降不下去。用edge-setInformation(Eigen::Matrixdouble, dim, dim::Identity())是最基础保险的设置方式。5.3 初值距离真值太远迭代发散非线性最小二乘是局部优化方法初值离谱时会发散。这个不是 G2O 的 bug是算法本身的特性。表现特征是chi2先暴涨再崩溃或者直接变成 nan。解决策略有三种给顶点一个更合理的初值哪怕粗略估计也比全零强。换成 LM 算法而不是 Gauss-NewtonLM 的阻尼项对远离最优解的情况更鲁棒。减少每步更新步长也就是在oplusImpl中对增量乘以一个 0.1 这样的缩放因子变相实现“步长限制”等接近最优解再拿掉。SLAM 里最常见的做法是先用 ICP 或里程计积分出一个大致轨迹再交给 G2O 精化。直接拿零位姿去优化大概率会得到一个乱七八糟的结果。5.4 顶点 id 冲突与边指针悬空addVertex时顶点的 id 必须是唯一的。如果两个顶点共用一个 idG2O 内部的顶点索引表会覆盖后加的顶点把先加的顶点的_vertices数据顶掉优化时指向错误的顶点。边在setVertex(0, v)时要确保顶点指针有效不要用临时变量创建顶点后立刻销毁。一个比较隐蔽的问题是有些版本 G2O 在删除顶点时不会自动删除关联的边如果你在循环中动态增删边记得同步维护。我自己遇到过一次迭代中optimizer.removeEdge之后内部指针悬空排查了很久才发现是删除顺序问题。稳妥做法是先把要删除的边指针收集到容器里统一删除再清理顶点。5.5 调试技巧把问题规模缩到最小遇到搞不定的优化问题我的调试套路是先不要上百个顶点几十条边先构造一个只有 2 个顶点、1 条边的最简问题。手工算一遍误差和增量看 G2O 的输出是否和手算一致。如果这个最小问题都能通过再逐步增加顶点数定位是哪一个边引入的问题。打开setVerbose(true)后注意观察每轮chi2和time的数值。time可以帮你判断是不是数值求导导致的计算量暴涨正常曲线拟合问题每次迭代应该在毫秒级如果单次迭代要几秒检查是不是某个循环里写了 O(N^2) 的嵌套计算。另外G2O 官方仓库里自带一堆示例g2o/examples目录下有 curve fitting、tutorial_slam2d、tutorial_slam3d 等工程。遇到问题先跑一遍官方 demo确认自己环境里的 G2O 版本和依赖没问题再排查自己的代码。不同版本的 G2O API 有细微差异网上很多博客代码年代久远直接复制容易编译不过这也是新手非常容易卡住的地方。玩 G2O 这几年我最深的体会是这个库真正难的不是 API 调用而是“把问题正确建模成图”。误差公式、雅可比推导、信息矩阵权重这些东西写清楚代码只是机械翻译。建议你接手一个优化任务时先在纸上把误差方程和信息矩阵写出来再开始写类。省下来的调试时间绝对远远超过这点准备时间。如果还想继续深入下一步可以试试自定义 SE3 顶点、加鲁棒核函数处理外点、或者研究一下边缘化是怎么提升求解速度的。那些是 G2O 更进阶的玩法但第一步永远是先让一个最简例子跑通。希望这篇笔记能让你少踩一些我踩过的坑。