g2o实践笔记

本文旨在学习通过《SLAM十四讲》中曲线拟合和pnp的对比，梳理g2o求解优化问题的实现步骤和具体操作，对具体原理不做探讨，感兴趣的可以移步：

g2o学习笔记 - 简书 (jianshu.com)
理解图优化，一步步带你看懂g2o代码 - 云+社区 - 腾讯云 (tencent.com)

深入理解图优化与g2o：g2o篇 - 半闲居士 - 博客园 (cnblogs.com)

图优化分为9步：

1. 重写顶点类
2. 重写边类
3. 创建BlockSolver
4. 创建LinearSolver
5. 创建总求解器Solver
6. 创建稀疏优化器
7. 在图中添加顶点
8. 在图中添加边
9. 设置优化参数，开始执行优化

图中包含顶点和边，简单地讲，顶点就是待优化的变量的，边就是链接顶点的约束条件

1.重写顶点类

1.1源代码对比

曲线拟合顶点定义：

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 {}
};

pnp顶点定义：

class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }

  /// left multiplication on SE3
  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }

  virtual bool read(istream &in) override {}

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

可以看到，对于定义顶点我们要做的四件事：

1.2定义顶点要重写四个函数

1. virtual void setToOriginImpl()
2. virtual void oplusImpl(const double *update)
3. virtual bool read(istream &in)
4. virtual bool write(ostream &out)
   一般情况下，write和read置空即可，我们重点讨论setToOriginImpl()和oplusImpl（）

1.2.1setToOriginImpl()

setToOriginImpl()函数的用途就是初始化估计值_estimate,初始化的具体形式随_estimate数据类型而定，_estimate类型可以通过指定BaseVertex模板参数指定

在曲线拟合中class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> 指定了_estimate是一个Eigen::Vector3d ，3是指Eigen::Vector3d可以由3个量来确定。

在pnp中，class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> 指定了_estimate是一个Sophus::SE3d ，6是指Sophus::SE3d>可以由6个量来确定。

不同的类型决定了不同的初始化方式，在曲线拟合中是初始化一个向量，在pnp中则是初始化一个位姿。

1.2.2oplusImpl()

oplusImpl()函数的作用是定义增量，以及每一次优化迭代的“加法”，这是很有必要的，我们知道，在优化问题中，我们需要计算，这个公式在三维空间是很好解决的，就是向量的每一维相加即可，但如果是在李群上（李群没有加法），这个“+”好就变成了左乘，因此我们必须对oplusImpl()重写。

在曲线拟合中_estimate += Eigen::Vector3d(update); ，就是普通的向量相加。

在pnp中，由于_estimate是一个李群，因而加法就变成了左乘_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;

2.重写边

边就是链接节点之间的约束条件，在曲线拟合问题中，定义为单边，在pnp中定义为双边

2.1源代码对比

曲线拟合问题：

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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
};

pnp问题：

class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}

  virtual void computeError() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
    pos_pixel /= pos_pixel[2];
    _error = _measurement - pos_pixel.head<2>();
  }

  virtual void linearizeOplus() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Eigen::Vector3d pos_cam = T * _pos3d;
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);
    double X = pos_cam[0];
    double Y = pos_cam[1];
    double Z = pos_cam[2];
    double Z2 = Z * Z;
    _jacobianOplusXi
      << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
      0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
  }

2.2如何确定父类边的模板参数

在重写边的过程中，要继承边，分为单边，双边，多边，这里结合具体代码介绍一下单边。

2.2.1单边BaseUnaryEdge<D,E,VertexXi>

首先讲什么情况下选择单边，仅优化一个变量地时候选择单边，在曲线拟合中仅优化向量（a,b,c）,在pnp中仅优化相机位姿pose

• D即为观测值维度
• E为观测值的类型
• VertexXi为边链接的节点类型
  具体结合曲线拟合来说：
  曲线拟合是要求解这样一个优化问题，
  
  我们的观测值是,这是一个一维的double类型，并且便连接的只有一种节点，所以模板参数定义为：BaseUnaryEdge<1, double, CurveFittingVertex> ,这也就是后面程序使用的_measurement。
  具体结合pnp来说：

pnp是要求解这样一个优化问题，

我们要求解T的估计，因为选用单边，我们的观测值是，即像素坐标，是一个2维的double向量，这也就是后面程序使用的_measurement，并且图中含有两种顶点，所以声明为BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>，请注意，我们根据仅优化一个变量****VertexPose ,而选择了单边 BaseUnaryEdge ，而非根据图中具有的顶点种类。

2.3重写边类中的五个函数

一般边内需要重写5个函数

1. 边的构造函数
2. virtual void computeError()
3. virtual void linearizeOplus()
4. virtual bool read(istream &in)
5. virtual bool write(ostream &out)

其中，read以及write函数，置空即可，重点探讨剩下的三个函数

2.3.1边的构造函数

构造函数关键在于需要传入什么参数构造边。

在曲线拟合中，_estimate代表(a,b,c)的估计,_measurement代表，那么为了估计这个问题

我们还需要知道,因此我们选择在构造函数中传入x：CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

在pnp问题中，_estimate代表T的估计，_measurment代表，那么为了估计这个问题

我们还需要知道，因此需要在构造函数中传入：EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K){} ,pos代表三维空间点，K代表相机参数

2.3.2virtual void computeError()

这个函数需要计算估计和测量值的差，也就是最小二乘问题中的。

在曲线拟合问题中，表现为，因此函数表示为

virtual void computeError() override {
    //1、定义要估计的顶点
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    //2、取得顶点的估计
    const Eigen::Vector3d abc = v->estimate();
    //3、计算误差e
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}

在pnp问题中，表现为,因此函数表示为

  virtual void computeError() override {
    //1、定义要估计的顶点
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    //2、取得顶点的估计
    Sophus::SE3d T = v->estimate();
    //3、计算误差e
    Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
    pos_pixel /= pos_pixel[2];
    _error = _measurement - pos_pixel.head<2>();
  }

2.3.3virtual void linearizeOplus()

这个函数我们计算关于估计量的雅可比，如果我们没有给出雅可比，g2o也会进行数值求导，但是会比显示给出雅可比慢。

在曲线拟合问题中，根据e关于估计量（a,b,c）的雅可比，我们可以写出这个virtual void linearizeOplus()

  // 计算雅可比矩阵
  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;
  }

同样的，在pnp问题中，virtual void linearizeOplus()的形式是

  virtual void linearizeOplus() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Eigen::Vector3d pos_cam = T * _pos3d;
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);
    double X = pos_cam[0];
    double Y = pos_cam[1];
    double Z = pos_cam[2];
    double Z2 = Z * Z;
    _jacobianOplusXi
      << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
      0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
  }

3.创建BlockSolver

3.1源码对比

在曲线拟合问题中：

typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;

在pnp问题中

typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;

g2o::BlockSolverTraits<p，l>，p是估计值的维度，l为误差维度，在曲线拟合中，估计值(a,b,c)维度为3，误差维度1，在pnp中，估计值即位姿6维，这里的3比较难理解，的维度是3，这里将处理为齐次坐标，即

g2o还提供了其他的块求解器：

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

特别地，当你不想了解到底是多少维，可以使用typedef g2o::BlockSolverX BlockSolverType; 他会自定推测数据的维度。

4.创建线性求解器LinearSolver

这一步中我们可以选择不同的求解方式来求解线性方程

，g2o中提供的求解方式主要有：

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

4.1源码对比

曲线拟合：

typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;

pnp：

typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;

5.创建总求解器solver

有三种求解器可以选择

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

5.1源码对比：

曲线拟合：

auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

pnp:

auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

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

创建稀疏优化器的过程有以下三步：

1. 建立图模型

g2o::SparseOptimizer  optimizer;

2. 设置求解方法：

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)；

3. 设置优化过程输出信息：

SparseOptimizer::setVerbose(bool verbose)；

6.1源码对比

曲线拟合：

  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

pnp:

  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

7.在图中添加顶点

添加顶点分为三步：

1. 创建点并初始化
2. 定义节点编号
3. 把节点添加到图中

7.1源码对比

曲线拟合：

  CurveFittingVertex *v = new CurveFittingVertex();//1.创建点
  v->setEstimate(Eigen::Vector3d(ae, be, ce));//1.设定初值
  v->setId(0);//2.定义节点标号
  optimizer.addVertex(v);//3.添加点到图中

pnp:

  VertexPose *vertex_pose = new VertexPose(); //1.创建点
  vertex_pose->setId(0);//2.定义节点标号
  vertex_pose->setEstimate(Sophus::SE3d());//1.设定初值
  optimizer.addVertex(vertex_pose);//3.添加点到图中

8.向图中添加边

添加边总共有五步：

1. 创建边
2. 设置边id
3. 设置连接的顶点
4. 设置观测值（_measurement）
5. 设置信息矩阵
6. 向图中添加边

8.1源码对比

曲线拟合：

  for (int i = 0; i < N; i++) {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);//1.创建边
    edge->setId(i);                        //2.设置边的ID
    edge->setVertex(0, v);                //3.设置连接的顶点
    edge->setMeasurement(y_data[i]);      //4.设置观测数值
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma));                //5.设置信息矩阵：协方差矩阵之逆
    optimizer.addEdge(edge);            //6.向图中添加边
  }

pnp:

  int index = 1;
  for (size_t i = 0; i < points_2d.size(); ++i) {
    auto p2d = points_2d[i];
    auto p3d = points_3d[i];
    EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);//1.创建边
    edge->setId(index);                      //2.设置边的ID
    edge->setVertex(0, vertex_pose);         //3.设置连接的顶点
    edge->setMeasurement(p2d);               //4.设置观测值
    edge->setInformation(Eigen::Matrix2d::Identity());//5.设置信息矩阵
    optimizer.addEdge(edge);                 //6.向图中添加边
    index++;
  }

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

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

1. 初始化：

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

2. 设置迭代次数：

SparseOptimizer::optimize(int iterations,bool online)；

9.1源码对比

曲线拟合：

  optimizer.initializeOptimization();    //1.初始化
  optimizer.optimize(10);                //2.设置迭代次数

pnp:

  optimizer.initializeOptimization();    //1.初始化
  optimizer.optimize(10);                //2.设置迭代次数

参考：

SLAM从0到1——图优化g2o：从看懂代码到动手编写（长文）-技术圈 (proginn.com)