【视觉惯性SLAM：六、图优化库(1):g2o的使用指南】

在SLAM领域，常用的图优化库有两个，一个是g2o，另一个是 Ceres Solver，它们都是基于C++的非线性优化库。其中g2o（General Graph Optimization）是一个通用的图优化框架，广泛用于 SLAM（Simultaneous Localization and Mapping）和机器人定位等领域。它提供了一种灵活、高效的方式来解决非线性优化问题，尤其是以图的形式建模的问题。
下面将以g2O优化库为例，从 g2o 编程框架、构建顶点、构建边 三个方面，介绍如何使用 g2o 进行图优化。

g2o 编程框架

g2o 的核心思想

g2o 的核心思想是将优化问题建模为一个图结构，图由**顶点（Vertices）和边（Edges）**组成：

• 顶点：表示优化变量（如相机位姿、三维点等）。
• 边：表示约束（如传感器的观测值、相对位姿等）。

优化过程的目标是通过调整顶点的值，使得所有边的误差最小化。

g2o 的主要组成部分

• 顶点（Vertex）：优化变量。
• 边（Edge）：定义误差函数，约束顶点之间的关系。
• 优化器（SparseOptimizer）：负责求解图优化问题。
• 线性求解器：用于高效地求解稀疏线性方程组。
• 损失函数（Loss Function）：用于鲁棒化处理，减少异常值对优化的影响。

构建 g2o 顶点

顶点从哪里来？

• 顶点表示优化变量，例如：
  • SLAM 中的相机位姿（例如，SE3 表示位姿）。
  • 地图中的三维点（例如，点云）。
  • 其他场景中的特定变量（例如，传感器状态）。
• 顶点的数量和种类通常取决于问题的需求。例如，SLAM 中的图优化问题会有两类顶点：
  • 位姿顶点：表示相机的世界坐标系中的位置和姿态。
  • 三维点顶点：表示地图点的三维坐标。

如何自己定义顶点？

g2o 提供了一些基本顶点类，但也支持用户自定义顶点。自定义顶点需要继承 g2o::BaseVertex 并实现相应的虚函数。
示例：自定义一个三维点顶点

#include <g2o/core/base_vertex.h>
#include <Eigen/Core>

// 定义一个3维点的顶点
class VertexPointXYZ : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // 初始化函数，设置顶点初始值
    void setToOriginImpl() override {
        _estimate.setZero();  // 初始化为 (0, 0, 0)
    }

    // 更新顶点值
    void oplusImpl(const double* update) override {
        Eigen::Map<const Eigen::Vector3d> v(update);
        _estimate += v;  // 更新顶点估计值
    }

    // 可选：保存和加载顶点信息（序列化）
    bool read(std::istream& /*is*/) override { return false; }
    bool write(std::ostream& /*os*/) const override { return false; }
};

如何向图中添加顶点？

添加顶点到图中，需要通过 g2o::SparseOptimizer 的 addVertex() 函数。
示例：添加顶点到优化器

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/optimization_algorithm_levenberg.h>

// 初始化优化器
g2o::SparseOptimizer optimizer;
auto linearSolver = g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>();
auto blockSolver = g2o::make_unique<g2o::BlockSolverX>(std::move(linearSolver));
auto solver = new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver));
optimizer.setAlgorithm(solver);

// 创建顶点并添加到优化器
auto vertex = new VertexPointXYZ();
vertex->setId(0);  // 设置顶点ID
vertex->setEstimate(Eigen::Vector3d(1.0, 2.0, 3.0));  // 设置初始值
optimizer.addVertex(vertex);  // 添加顶点到优化器

构建 g2o 边

初步认识图的边

• 边（Edge）表示顶点之间的关系，也可以单独与一个顶点关联。
• 边定义了误差函数（Error Function），即优化目标。
• 边的核心：
  • 误差计算（computeError()）。
  • 信息矩阵（Information Matrix）：衡量误差的置信度。

如何自定义边？

自定义边需要继承 g2o::BaseUnaryEdge（一元边）、g2o::BaseBinaryEdge（二元边）或 g2o::BaseMultiEdge（多元边）。
示例：自定义二元边

#include <g2o/core/base_binary_edge.h>
#include <Eigen/Core>

// 定义一个简单的二元边
class EdgeSE3Point : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSE3, VertexPointXYZ> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // 误差计算函数
    void computeError() override {
        const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
        const VertexPointXYZ* v2 = static_cast<const VertexPointXYZ*>(_vertices[1]);
        _error = _measurement - (v1->estimate() * v2->estimate());  // 假设误差为测量值减去估计值
    }

    // 可选：保存和加载边的信息
    bool read(std::istream& /*is*/) override { return false; }
    bool write(std::ostream& /*os*/) const override { return false; }
};

如何向图中添加边？

边的构造需要关联顶点，并设置误差信息和信息矩阵。
示例：添加边到优化器

// 创建边并关联顶点
auto edge = new EdgeSE3Point();
edge->setId(0);  // 设置边ID
edge->setVertex(0, vertexSE3);  // 关联顶点1
edge->setVertex(1, vertexPointXYZ);  // 关联顶点2
edge->setMeasurement(Eigen::Vector3d(1.0, 0.0, 0.0));  // 设置测量值
edge->setInformation(Eigen::Matrix3d::Identity());  // 设置信息矩阵
optimizer.addEdge(edge);  // 添加边到优化器

完整示例：构建并优化一个简单图

示例：包含 2 个顶点和 1 条边的简单图

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <iostream>

int main() {
    // 初始化优化器
    g2o::SparseOptimizer optimizer;
    auto linearSolver = g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>();
    auto blockSolver = g2o::make_unique<g2o::BlockSolverX>(std::move(linearSolver));
    auto solver = new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver));
    optimizer.setAlgorithm(solver);

    // 添加顶点
    auto vertex1 = new g2o::VertexSE3();
    vertex1->setId(0);
    vertex1->setEstimate(g2o::SE3Quat());
    optimizer.addVertex(vertex1);

    auto vertex2 = new VertexPointXYZ();
    vertex2->setId(1);
    vertex2->setEstimate(Eigen::Vector3d(1.0, 2.0, 3.0));
    optimizer.addVertex(vertex2);

    // 添加边
    auto edge = new EdgeSE3Point();
    edge->setId(0);
    edge->setVertex(0, vertex1);
    edge->setVertex(1, vertex2);
    edge->setMeasurement(Eigen::Vector3d(1.0, 2.0, 3.0));
    edge->setInformation(Eigen::Matrix3d::Identity());
    optimizer.addEdge(edge);

    // 优化
    optimizer.initializeOptimization();
    optimizer.optimize(10);

    // 输出优化结果
    std::cout << "Vertex 1: " << vertex1->estimate().translation().transpose() << std::endl;
    std::cout << "Vertex 2: " << vertex2->estimate().transpose() << std::endl;

    return 0;
}

注意事项

• 顶点 ID 和边 ID 必须唯一，否则会报错。
• 信息矩阵必须为正定矩阵，否则优化可能失败。
• 在定义边的误差计算时，需要确保误差函数的正确性，否则可能导致优化结果不收敛。
• 优化器的初始化和配置（如线性求解器的选择）会显著影响性能和结果。

通过以上步骤和示例，g2o 可高效解决图优化问题，并为 SLAM 等复杂任务提供支持。