g2o源码解析

g2o的源码比较复杂，主要分为以下几个部分：

1. core：核心部分，包括图模型（Graph）、顶点（Vertex）、边（Edge）、优化算法（OptimizationAlgorithm）等。

2. solver：求解器部分，包括线性求解器（LinearSolver）、稀疏矩阵（SparseBlockMatrix）、稠密矩阵（DenseBlockMatrix）等。

3. types：类型部分，包括各种图优化问题相关的数据类型，如向量（Vector）、旋转矩阵（SE3）、四元数（Quaternion）等。

4. stuff：工具部分，包括文件读写、时间计算、随机数生成、命令行参数解析等。

下面以求解一个简单的图优化问题为例，简单介绍g2o的源码结构和使用方法。

首先，我们需要定义一个图模型，并向模型中添加顶点和边。比如，我们可以定义如下的图模型：

#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>

typedef g2o::BlockSolver<g2o::BlockSolverTraits<2,1>> BlockSolverType;
typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
typedef g2o::OptimizationAlgorithmLevenberg OptimizationAlgorithmType;

int main(int argc, char** argv)
{
  // 构建图优化模型
  g2o::SparseOptimizer optimizer;
  optimizer.setVerbose(true);

  // 设置线性求解器和块求解器
  LinearSolverType *linearSolver = new LinearSolverType();
  BlockSolverType *blockSolver = new BlockSolverType(linearSolver);
  OptimizationAlgorithmType *optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);

  // 设置求解算法和优化器
  optimizer.setAlgorithm(optimizationAlgorithm);

  // 添加顶点
  g2o::VertexSE2 *v1 = new g2o::VertexSE2();
  v1->setId(0);
  v1->setEstimate(g2o::SE2(0,0,0));
  optimizer.addVertex(v1);

  g2o::VertexSE2 *v2 = new g2o::VertexSE2();
  v2->setId(1);
  v2->setEstimate(g2o::SE2(1,0,0));
  optimizer.addVertex(v2);

  // 添加边
  g2o::EdgeSE2 *e = new g2o::EdgeSE2();
  e->setVertex(0, v1);
  e->setVertex(1, v2);
  e->setMeasurement(g2o::SE2(1,0,0));
  e->setInformation(Eigen::Matrix3d::Identity());
  optimizer.addEdge(e);

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

  // 输出结果
  std::cout << v1->estimate().toVector().transpose() << std::endl;
  std::cout << v2->estimate().toVector().transpose() << std::endl;

  return 0;
}

在上面的代码中，我们首先定义了一个g2o::SparseOptimizer对象，然后设置了线性求解器和块求解器，以及Levenberg-Marquardt优化算法。接着，我们向模型中添加了两个顶点和一条边，其中边的测量值为(1,0,0)，信息矩阵为单位矩阵。最后，我们执行了10次优化，并输出了优化后的顶点估计值。

g2o的源码结构比较复杂，但是其设计思想清晰，代码风格规范，易于扩展和修改。使用g2o库可以快速高效地解决各种图优化问题，是图优化领域不可或缺的工具。