【视觉惯性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;
}