3、g2o库入门：安装配置与第一个图优化问题

说到图优化，g2o 绝对是个绕不开的库。我最早接触它是在做后端优化的时候，那时候自己手写高斯牛顿，写到最后发现矩阵求逆都成瓶颈了。后来换了 g2o，嗯，真香。

这一节我们就来聊聊 g2o 的安装、配置，以及怎么用它构建第一个图优化问题。说白了，就是让你能跑起来一个最小二乘的 demo，感受一下图优化的流程。

3.1 g2o 是什么？

g2o 的全称是 General Graph Optimization，一个通用的图优化库。它把优化问题抽象成「顶点」和「边」——顶点表示待优化的变量，边表示观测约束。你想想看，SLAM 里的位姿、路标点，是不是天然就能用图来表示？

我个人习惯把 g2o 看作一个「搭积木」的工具：你定义好顶点类型、边类型，然后往图里塞数据，最后调用优化器跑一下，结果就出来了。底层的高斯牛顿、LM 算法，它都帮你封装好了。

3.2 安装与配置

g2o 的安装其实不复杂，但有几个坑。我曾经在 Ubuntu 18.04 上折腾了一下午，最后发现是依赖没装全。这里我把步骤整理一下，你照着来就行。

3.2.1 依赖安装

先装基础依赖，缺一不可：

sudo apt-get install cmake libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-dev

注意，libqglviewer-dev 这个包在 Ubuntu 20.04 以后改名叫 libqglviewer-dev-qt5 了。如果你用的是新系统，记得换成：

sudo apt-get install libqglviewer-dev-qt5

3.2.2 编译安装

从 GitHub 上 clone 下来，然后编译：

git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o
mkdir build && cd build
cmake ..
make -j4
sudo make install

编译时有个小技巧：如果你不需要 GUI 可视化，可以在 cmake 时加上 -DBUILD_WITH_QT4=OFF，这样能省掉 Qt 的依赖。我个人习惯是保留 GUI，因为调试时能看到图结构，挺方便的。

3.2.3 验证安装

装完后，写个简单的 CMakeLists.txt 来验证：

cmake_minimum_required(VERSION 3.0)
project(g2o_test)

find_package(g2o REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

add_executable(test_g2o test_g2o.cpp)
target_link_libraries(test_g2o ${G2O_LIBS})

如果能顺利找到 g2o 的库，说明安装成功了。如果报找不到，多半是安装路径没在默认搜索目录里，可以手动设置 CMAKE_PREFIX_PATH。

3.3 构建第一个图优化问题

好了，环境搭好了，我们来写一个最简单的图优化 demo。这个例子不涉及 SLAM，就是一个曲线拟合问题——说白了，就是给定一些带噪声的点，拟合出一条二次曲线。

3.3.1 问题定义

假设真实曲线是：

y = exp( a * x^2 + b * x + c )

我们观测到一些带噪声的 (x, y) 点，想反推出 a, b, c 三个参数。在图优化里，a, b, c 就是三个顶点，每个观测点就是一条边。

3.3.2 代码实现

先定义顶点和边。g2o 里已经提供了 BaseVertex 和 BaseUnaryEdge 基类，我们继承一下就行：

#include <iostream>
#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/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>

// 顶点：待优化的参数 (a, b, c)
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() override {
        _estimate << 0, 0, 0;
    }
    virtual void oplusImpl(const double* update) override {
        _estimate += Eigen::Vector3d(update);
    }
    virtual bool read(std::istream& in) override {}
    virtual bool write(std::ostream& out) const override {}
};

// 边：单个观测点的误差
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
    
    virtual void computeError() override {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        double y_pred = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
        _error(0, 0) = _measurement - y_pred;
    }
    
    virtual bool read(std::istream& in) override {}
    virtual bool write(std::ostream& out) const override {}
    
private:
    double _x;
};

接下来是主函数，构建图并求解：

int main() {
    // 生成模拟数据
    double a = 1.0, b = 2.0, c = 1.0;
    std::vector<double> x_data, y_data;
    for (int i = 0; i < 100; ++i) {
        double x = i / 100.0;
        double y = exp(a * x * x + b * x + c) + (rand() % 1000) / 1000.0 * 0.1;
        x_data.push_back(x);
        y_data.push_back(y);
    }

    // 构建图优化器
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3, 1> > BlockSolverType;
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
    
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
    );
    
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    // 添加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate(Eigen::Vector3d(0, 0, 0));
    v->setId(0);
    optimizer.addVertex(v);

    // 添加边
    for (int i = 0; i < x_data.size(); ++i) {
        CurveFittingEdge* edge = new CurveFittingEdge(x_data[i]);
        edge->setId(i);
        edge->setVertex(0, v);
        edge->setMeasurement(y_data[i]);
        edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
        optimizer.addEdge(edge);
    }

    // 执行优化
    optimizer.initializeOptimization();
    optimizer.optimize(100);

    // 输出结果
    Eigen::Vector3d abc_estimate = v->estimate();
    std::cout << "估计参数: " << abc_estimate.transpose() << std::endl;
    std::cout << "真实参数: " << a << " " << b << " " << c << std::endl;

    return 0;
}

3.3.3 运行结果

编译运行后，你会看到类似这样的输出：

iteration= 0     chi2= 1234.56    time= 0.001
iteration= 1     chi2= 567.89     time= 0.001
...
iteration= 99    chi2= 0.12       time= 0.001

估计参数: 0.998 2.001 0.997
真实参数: 1.0 2.0 1.0

你看，优化后的参数和真实值非常接近。chi2 从一千多降到零点几，说明误差收敛得很好。

3.4 核心流程总结

用 g2o 做图优化，其实就四步：

1. 定义顶点：继承 BaseVertex，实现 setToOriginImpl 和 oplusImpl
2. 定义边：继承 BaseUnaryEdge 或 BaseBinaryEdge，实现 computeError
3. 构建图：创建优化器，添加顶点和边
4. 求解：调用 initializeOptimization 和 optimize

说白了，你只要把 SLAM 问题里的位姿、路标点映射成顶点，把观测约束映射成边，剩下的交给 g2o 就行。我刚开始用的时候，总觉得这玩意儿是个黑盒，后来自己手写了一遍高斯牛顿，才明白它底层到底在干什么。

3.5 本章知识体系

下面这张图帮你梳理了 g2o 的核心结构和工作流程：

嗯，到这里 g2o 的入门就差不多了。你把这个 demo 跑通，基本就掌握了图优化的核心套路。后面在 SLAM 闭环检测里，我们会用 g2o 来优化整个位姿图，那时候你就知道今天打下的基础有多重要了。

---

公众号:蓝海资料掘金营，微信deep3321