g2o的简介、安装、使用说明和曲线拟合例程

最新推荐文章于 2025-02-28 11:12:43 发布

非晚非晚

最新推荐文章于 2025-02-28 11:12:43 发布

阅读量1.3w

点赞数 27

分类专栏：数学理论与工具文章标签： G2O 图优化非线性优化

本文链接：https://blog.youkuaiyun.com/QLeelq/article/details/115497273

版权

数学理论与工具专栏收录该内容

10 篇文章

订阅专栏

本文详细介绍了G2O库的安装步骤，核心概念，如顶点、边和稀疏优化器的使用，以及针对相机定位、曲线拟合等场景的示例。涵盖了定制顶点边、安装依赖、优化流程和常见问题解决等内容。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

1. 简介

g2o的论文：g2o: A General Framework for Graph Optimization
(在源码文件中…g2o/doc/g2o.pdf，有更详细版本)
g2o的github地址：https://github.com/RainerKuemmerle/g2o

g2o(General Graphic Optimization)是基于图优化的库。图优化是把优化问题表现成图的一种方式。一个图由若干个顶点(Vertex)，以及连接这这些顶点的边(Edge)组成。用顶点表示优化变量，用边表示误差项。

对任意非线性最小二乘问题都可以构建一个与之对应的图。也可以用概率图里的定义，称之为贝叶斯图或因子图。

以相机为例，相机不同时刻的位姿和路标点可以表示为顶点，相机的运动模型可以表示相机之间的边，相机的观测模型可以表示相机与路标之间的边。如下关系图：

在这里插入图片描述

2. g2o的安装

安装依赖项：

sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev libcxsparse3.1.2 libcholmod-dev

安装下列命令依次执行安装：

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

安装完成后在目录/usr/local/includ 下能找到g2o目录，在/usr/local/lib 下能找到libg2o_**.so的文件。

3. g2o使用的简约说明

g2o使用的大致步骤：

创建一个线性求解器LinearSolver。
创建BlockSolver，并用上面定义的线性求解器初始化。
创建总求解器solver，并从GN/LM/DogLeg 中选一个作为迭代策略，再用上述块求解器BlockSolver初始化。
创建图优化的核心：稀疏优化器（SparseOptimizer）。
定义图的顶点和边，并添加到SparseOptimizer中。
设置优化参数，开始执行优化。

如果g2o的标准顶点和边不能满足我们的需求，一般需要对它们进行重写。标准顶点和边为：

g2o::BaseVertex；
g2o::BaseUnaryEdge；//一元边，Unary Edge
g2o::BaseBinaryEdge；//二元边

顶点部分说明：

oplusImpl(计算下一次的估计值，相当于一次setEstimate)：函数处理的是 xk+1 = xk + ∆x 的过程；
setToOriginImpl(清零：_estimate)：估计值清零；
setEstimate(设置：_estimate)：设置估计值，一般会有一个初始值；(在曲线拟合中有了估计值，也就可以算出y的估计值。)。
setId（设置顶点的id：_id）
estimate（返回： _estimate）：优化的结果。
addVertex（插入到_vertices，vertices()函数可以返回_vertices的值。）：添加顶点
setFixed：要优化的变量设置为false。
setMarginalized ():设置该顶点是否被边缘化，_marginalized默认为false

边的说明

computeError(返回：_error)：边的误差项，观测值与估计值的差距；(曲线拟合中y的测量值与估计值的差)。
setVertex（设置：_vertices）：设置连接的顶点
setMeasurement(设置：_measurement)：观测数据的输入，在曲线拟合中也就是y真实值(x值会在构建边的时候输入)；
setId：边的id
addEdge：v->edges().insert(e);添加边的数据，edges函数可以返回_edges的值。
setInformation：设置信息矩阵(_information)

优化器说明

可以选择LM、GN或者DogLeg方法

//LM
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr )
;
//GN 
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(
solver_ptr );
//DogLeg
g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );

设置方式(_algorithm)：

optimizer.setAlgorithm(solver);//继承，基类的指针会指向子类。

设置停止优化标志

 optimizer.setForceStopFlag(pbStopFlag);

设置是否打开调试器

optimizer.setVerbose(false);

4. g2o使用的常用说明

4.1 顶点

（1）李代数顶点

class  VertexSE3Expmap : public BaseVertex<6, SE3Quat>

参数6 ：SE3Quat类型为六维，三维旋转，三维平移
参数SE3Quat ：该类型旋转在前，平移在后，注意：类型内部使用的其实是四元数，不是李代数

该顶点需要设置的参数：

g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
//【1】设置待优化位姿（这是粗略位姿）
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
//【2】设置Id号
vSE3->setId(pKFi->mnId);
//【3】设置是否固定，第一帧固定
vSE3->setFixed(pKFi->mnId==0);

（2）空间点位置

class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>

该顶点需要设置的参数：

g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
//【1】设置待优化空间点3D位置XYZ
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
//【2】设置Id号
vPoint->setId(id);
//【3】是否边缘化（以便稀疏化求解）
vPoint->setMarginalized(true);

4.2 边

（1）Point-Pose 二元边（PointXYZ-SE3边）

即要优化MapPoints的位置，又要优化相机的位姿

class  EdgeSE3ProjectXYZ: public  BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>

需要设置的模板参数：

参数2 ：观测值（这里是3D点在像素坐标系下的投影坐标）的维度
参数Vector ：观测值类型，piexl.x，piexl.y
参数VertexSBAPointXYZ：第一个顶点类型
参数VertexSE3Expmap ：第二个顶点类型

该边需要设置的参数：

g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();

//【1】设置第一个顶点，注意该顶点类型与模板参数第一个顶点类型对应
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
//【2】设置第二个顶点
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
//【3】设置观测值，类型与模板参数对应
e->setMeasurement(obs);
const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
//【4】设置信息矩阵，协方差
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

//【5】设置鲁棒核函数
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);

//【6】设置相机内参
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;

（2） Pose 一元边（SE3）

仅优化相机位姿，为了构造出投影方程，需要按下面的方式把MapPoints的位置作为常量加入

class  EdgeSE3ProjectXYZOnlyPose: public  BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>

该继承于BaseUnaryEdge这个一元边模板类，需要设置的模板参数如上。

该边需要设置的参数：

g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();

// 注意这里只设置一个顶点，其它一样
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
e->setMeasurement(obs);
const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(deltaMono); /** @attention 设置阈值，卡方自由度为2，内点概率95%对应的临界值*/

e->fx = pFrame->fx;
e->fy = pFrame->fy;
e->cx = pFrame->cx;
e->cy = pFrame->cy;

/** @attention 需要在这里设置<不做优化>的MapPoints的位置*/
cv::Mat Xw = pMP->GetWorldPos();
e->Xw[0] = Xw.at<float>(0);
e->Xw[1] = Xw.at<float>(1);
e->Xw[2] = Xw.at<float>(2);

（3） Pose-Pose 二元边（SE3-SE3边）

优化变量是相机相邻两个关键帧位姿，约束来自对这两个关键帧位姿变换的测量（里程计、IMU等）

class G2O_TYPES_SBA_API EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>

需要设置的参数如下：

Se2 measure_se2 = pMsrOdo->se2;
// 【1】里程计测量的协方差
g2o::Matrix3D covariance = toEigenMatrixXd(pMsrOdo->info).inverse(); 

// 【2】将里程计测量转换成g2o::SE3Quat类型
Eigen::AngleAxisd rotz(measure_se2.theta, Eigen::Vector3d::UnitZ());
g2o::SE3Quat relativePose_SE3Quat(rotz.toRotationMatrix(), Eigen::Vector3d(measure_se2.x, measure_se2.y, 0));

// 【3】将`里程计测量协方差`转换为`相机坐标系下协方差`
// 注意：g2o::SE3Quat是旋转在前，平移在后
g2o::Matrix6d covariance_6d = g2o::Matrix6d::Identity();
covariance_6d(0,0) = covariance(2,2);
covariance_6d(0,4) = covariance(2,0); covariance_6d(0,5) = covariance(2,1);
covariance_6d(4,0) = covariance(0,2); covariance_6d(5,0) = covariance(1,2);
covariance_6d(3,3) = covariance(0,0);
covariance_6d(4,4) = covariance(1,1);
covariance_6d(1,1) = 0.00001;
covariance_6d(2,2) = 0.01;
covariance_6d(5,5) = 0.0001;

g2o::Matrix6d Info = g2o::Matrix6d::Identity();
Info = covariance_6d.inverse();

// 【4】构造边
g2o::EdgeOnlineCalibration* e = new g2o::EdgeOnlineCalibration;
e->vertices()[0] = optimizer.vertex(id0);
e->vertices()[1] = optimizer.vertex(id1);
e->setMeasurement(relativePose_SE3Quat);
e->setInformation(Info);
optimizer.addEdge(e);

上面的比较麻烦的协方差的计算，准确的协方差计算可以参考论文《Odometry-Vision-Based Ground Vehicle Motion Estimation With SE(2)-Constrained SE(3) Poses》中的处理.

4.3 g2o优化

//【1】指定pose维度为6, landmark维度为3
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;
//【2】线性方程求解器，使用CSparse分解
//    还有g2o::LinearSolverDense，使用cholesky分解
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); 
//【3】矩阵块求解器
Block* solver_ptr = new Block ( linearSolver );     
//【4】梯度下降方法，从GN, LM, DogLeg 中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
// 图模型
g2o::SparseOptimizer optimizer;
//【5】设置求解器
optimizer.setAlgorithm ( solver );
......
......
......
//【6】打开调试输出
optimizer.setVerbose ( true );
optimizer.initializeOptimization();
//【7】指定迭代次数:100次
optimizer.optimize ( 100 );

g2o中经常使用的BlockSolver_6_3、BlockSolver_7_3 即是上面程序中【1】所作的事，如下：

  // solver for BA/3D SLAM
  typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;  
  // solver fo BA with scale
  typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3;

4.4 检测outliner

优化完成后，对每一条边都进行检查，剔除误差较大的边（认为是错误的边），并设置setLevel为0，即下次不再对该边进行优化。

第二次优化完成后，会对连接偏差比较大的边，在关键帧中剔除对该MapPoint的观测，在MapPoint中剔除对该关键帧的观测，具体实现参考orb-slam源码：Optimizer::LocalBundleAdjustment

optimizer.optimize ( 100 );
// 优化完成后，进行Edge的检查
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
    g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
    MapPoint* pMP = vpMapPointEdgeMono[i];

    if(pMP->isBad())
        continue;

    // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
    // 第二个判断条件，用于检查构成该边的MapPoint在该相机坐标系下的深度是否为正？
    if(e->chi2()>5.991 || !e->isDepthPositive())
    {
        e->setLevel(1);// 不优化
    }

    // 因为剔除了错误的边，所以下次优化不再使用核函数
    e->setRobustKernel(0);
}

4.5 加入对相机内参的优化

g2o::CameraParameters* camera = new g2o::CameraParameters (
    K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
);
// 设置Id号为0即可
camera->setId ( 0 );
optimizer.addParameter ( camera );

如果不想优化相机内参，则内参按照4.2中二元边中的程序demo中设置。

5. g2o的曲线拟合实例

曲线拟合中的边为一元边，带估计的为系数。
在这里插入图片描述

参考《视觉slam十四讲》的曲线拟合代码。

使用cmake进行编译，cmake的具体使用步骤可以参考链接。

使用步骤：

cd ch6
mkdir build
cmake ..
make 
./g2oCurveFitting #运行

#include <iostream>
#include <g2o/core/g2o_core_api.h>
#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/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>

using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
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(istream &in) {}

  virtual bool write(ostream &out) const {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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();
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
  }

  // 计算雅可比矩阵
  virtual void linearizeOplus() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }

  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}

public:
  double _x;  // x 值， y 值为 _measurement
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector<double> x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  // 构建图优化，先设定g2o
 // 每个误差项优化变量维度为3，误差值维度为1，误差值也就是y的差值
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; 
  // 线性方程求解器：稠密的增量方程
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; 

  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    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(ae, be, ce));
  v->setId(0);
  optimizer.addVertex(v);

  // 往图中增加边
  for (int i = 0; i < N; i++) {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);                // 设置连接的顶点，这里只有一个顶点，所以是第0个顶点。
    edge->setMeasurement(y_data[i]);      // 观测数值
    
    // 信息矩阵：协方差矩阵之逆
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); 
    optimizer.addEdge(edge);
  }

  // 执行优化
  cout << "start optimization" << endl;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  // 输出优化值
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;
}

代码说明：

在这个程序中，我们从 g2o 派生出了用于曲线拟合的图优化顶点和边：CurveFittingVertex 和 CurveFittingEdge，这实质上是扩展了 g2o 的使用方式。在这两个派生类中，我们重写了重要的虚函数：

顶点的更新函数：oplusImpl。我们知道优化过程最重要的是增量 ∆x 的计算，而该函数处理的是 xk+1 = xk + ∆x 的过程。每个优化的加法运算不一致，可能还包括向量，所以需要重写。
顶点的重置函数：setToOriginImpl。这是平凡的，我们把估计值置零即可。
边的误差计算函数：computeError。该函数需要取出边所连接的顶点的当前估计值，根据曲线模型，与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的。
存盘和读盘函数：read, write。由于我们并不想进行读写操作，就留空了。

6. g2o优化步骤说明

6.1 总体架构

续着本文第3节内容，记录g2o内部的结构。g2o的整体结构如下图所示。

实线箭头:is - a关系，表示从派生类指向基类；
虚线箭头：has - a关系，表示成员变量；
多虚线箭头：has - many关系，多个成员变量。Vertex和Edge都是class，在HyperGraph中分别定义了它们的集合set。

在这里插入图片描述

g2o结构说明如下：

整个g2o框架可以分为上下两部分，两部分中间的连接点：SparseOptimizer 就是整个g2o的核心部分。
往上看，SparseOptimizer是一个派生类，其实是一个Optimizable Graph，从而也是一个超图（HyperGraph）。
超图有很多顶点和边。顶点继承自 Base Vertex，也即OptimizableGraph::Vertex；而边可以继承自 BaseUnaryEdge（单边）,
BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge。
往下看，SparseOptimizer包含一个优化算法部分OptimizationAlgorithm，它是通过OptimizationWithHessian
来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）、Levernberg-Marquardt（简称LM法）和Powell’s dogleg中选择。
对优化算法部分进行求解的时求解器solver，它实际由BlockSolver组成。BlockSolver由两部分组成：一个是SparseBlockMatrix，它由于求解稀疏矩阵(雅克比和海塞)；另一个部分是LinearSolver，它用来求解线性方程 $H\triangle x = -b$ 得到待求增量，因此这一部分是非常重要的，它可以从PCG/CSparse/Choldmod选择求解方法。

6.2 使用的分步介绍

把g2o步骤再写一遍：

创建一个线性求解器LinearSolver。
创建BlockSolver，并用上面定义的线性求解器初始化。
创建总求解器solver，并从GN/LM/DogLeg 中选一个作为迭代策略，再用上述块求解器BlockSolver初始化。
创建图优化的核心：稀疏优化器（SparseOptimizer）。
定义图的顶点和边，并添加到SparseOptimizer中。
设置优化参数，开始执行优化。

依次说明具体步骤：

1. 创建一个线性求解器LinearSolver

这一步中我们可以选择不同的求解方式来求解线性方程 $H\triangle x = -b$ ，g2o中提供的求解方式主要有：

LinearSolverCholmod ：使用sparse cholesky分解法，继承自LinearSolverCCS。
LinearSolverCSparse：使用CSparse法，继承自LinearSolverCCS。
LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver。
LinearSolverDense ：使用dense cholesky分解法，继承自LinearSolver。
LinearSolverEigen：依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多，继承自LinearSolver。

_batchStatistics：全局统计信息，包括顶点和边的数量、时间、cholesky因子等信息。
_ivMap：初始化之后的顶点。

2. 创建BlockSolver，并用定义的线性求解器初始化

BlockSolver有两种定义方式：

// 固定变量的solver。 p代表pose的维度（是流形manifold下的最小表示），l表示landmark的维度
using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

// 可变尺寸的solver。Pose和Landmark在程序开始时并不能确定，所有参数都在中间过程中被确定。
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

此外g2o还预定义了以下几种常用类型：

BlockSolver_6_3 ：表示pose 是6维，观测点是3维，用于3D SLAM中的BA。
BlockSolver_7_3：在BlockSolver_6_3 的基础上多了一个scale。
BlockSolver_3_2：表示pose 是3维，观测点是2维。

3. 创建总求解器solver

在这一部分，我们有三迭代策略可以选择：

g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg
g2o::OptimizationAlgorithmDogleg

4. 创建图优化的核心：稀疏优化器

创建稀疏优化器：

g2o::SparseOptimizer  optimizer;

设置求解方法：

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

设置优化过程输出信息：

SparseOptimizer::setVerbose(bool verbose)

5. 定义图的顶点和边，并添加到SparseOptimizer中

6. 设置优化参数，开始执行优化

设置SparseOptimizer的初始化、迭代次数、保存结果等。

初始化：

SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)

设置迭代次数：

SparseOptimizer::optimize(int iterations,bool online)

参考：
《视觉SLAM十四讲》

g2o使用总结：https://blog.youkuaiyun.com/hzwwpgmwy/article/details/79884070

g2o结构：https://zhuanlan.zhihu.com/p/121628349