g2o 学习笔记 一

g2o

g2o 是ORB_SLAM 用到的后端优化库，这里我尝试将自己的学习过程记录下来。

以便自己日后复习，查漏补缺。如果有不正确的地方，请各位指正。

图优化

这个还好，我研究生阶段做的有点相关，例如社交网络的两个人，他们存在好友关系，那么两个人就是顶点，好友关系这个约束就是边。

我以前做的隐特征分析模型，用的是张量和矩阵对图网络进行建模，然后用的全梯度下降和其他东西做分解的。

而g2o也是要建模为矩阵，然后进行解算。

学习路上，多谢各位大佬的分享，以下是我看的一些资料：

g2o开源库 : https://github.com/RainerKuemmerle/g2o

g2o论文 : http://ais.informatik.uni-freiburg.de/publications/papers/kuemmerle11icra.pdf

深入理解图优化与g2o：g2o篇 : https://www.cnblogs.com/gaoxiang12/p/5304272.html

非线性优化库g2o使用教程 : https://blog.csdn.net/u011341856/article/details/112134763

g2o库简单入门 : https://blog.csdn.net/qq_34761779/article/details/127183682

非线性优化库之g2o : https://zhuanlan.zhihu.com/p/384569456

视觉惯性SLAM：理论与源码解析 程小六

g2o 库的文件树

• EXTERNAL      ~~~~      第三方库，最新的版本里面只有一个freeglut和CMakeLists.txt文件
• apps                ~~~~~~~~~~~~~~                这个是使用g2o的主要应用，包裹了命令行界面和GUI应用
• autodiff             ~~~~~~~~~~~             看名字应该是自动求导
• core                 ~~~~~~~~~~~~~~~                 g2o的核心库，顶点、边和图结构的定义，算法的定义，求解器的定义都在这里
• examples          ~~~~~~~~          一些应用的例子，会构建出一些例子的可执行文件。
• solvers             ~~~~~~~~~~~             线性求解器
• stuff                  ~~~~~~~~~~~~~~~~                  一些工具，我们只是用g2o的话，好像用不上
• types                ~~~~~~~~~~~~~~                定义好的类型，一般用就行了，没有想我们需要的类型，需要自己定义
• what_is_in_these_directories.txt 该目录下的文件夹的解释，最好直接看这个，我的理解不一定对

g2o 框架

g2o最基本的类结构如下图：

emmmmmmmmm，好复杂，慢慢看吧。

图上所有箭头都是从SparseOptimizer上射出，所以这个就是我们的最终目的就是维护这个稀疏优化器。

SparseOptimizer是一个OptimizableGraph，OptimizableGraph是一个HyperGraph。然后HyperGraph有各种vertex和edge。

SparseOptimizer有一个OptimizationAlgorithm(优化算法)。而优化算法是OptimizationWithHessian(二阶算法)的一种，然后二阶算法包含了高斯牛顿法、LM法和狗腿法，同时二阶算法有一个Solver，Solver是一个BlockSolver，其包含了SparseBlockMatrix和LinearSolver。

也就是说，使用g2o其实可以按上图分为上下两个部分，按照例子examples/data_fitting/curve_fit.cpp，我们梳理一下，g2o的使用顺序可以写成下面这样。（自己看链接里的源码更好）

// 定义
typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > MyBlockSolver;
typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;

// step 1 创建线性求解器
MyLinearSolver* linearSolver = new MyLinearSolver();

// step 2 创建块求解器，并用上面定义的线性求解器初始化
MyBlockSolver* solver_ptr = new MyBlockSolver(linearSolver);

// step 3 创建总求解器(Solver)，GN、LM、DogLeg 中选择一个，再用上述块求解器初始化
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );

// step 4 创建稀疏优化器(SparseOptimizer)
g2o::SparseOptimizer optimizer;     // 创建优化器
optimizer.setAlgorithm( solver );   // 用前面定义好的求解器作为求解方法
optimizer.setVerbose( true );       // 在优化过程中输出调试信息

// step 5 定义图的顶点，并添加到优化器中
CurveFittingVertex *v = new CurveFittingVertex();   // 向图中增加顶点
v->setId(0);                                        // 设置顶点的ID
v->setEstimate ( Eigen::Vector3d(1, 1 , 1) );       // 设置顶点的观测值
optimizer.addVertex(v);                             // 往优化器里添加顶点

// step 6 定义图的边，并添加到优化器中
for (int i i=0; i<N; i++)
{
    CurveFittingEdge *edge = New CureveFittingEdge( x_data[i] );
    edge->setId(i);                                                                             // 设置边ID
    edge->setVertex(0, v);                                                                      // 设置边连接的顶点
    edge->setMeasurement(y_data[i]);                                                            // 设置边的观测值
    edge->setInformation( Eigen::Matrix<double, 1, 1>::Identity() * 1) / (w_sigma * w_sigma);   // 设置信息矩阵
    optimizer.addEdge(edge);                                                                    // 往优化器中添加边
}

// step 7 设置优化参数，开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100); // 最多迭代100次

线性求解器

g2o中主要有这几种线性求解器

g2o::LinearSolverCholmod();     // 使用 sparse cholesky 分解法
g2o::LinearSolverCSparse();     // 使用 CSparse 法
g2o::LinearSolverPCG();         // 使用 preconditioned conjugate gradient 法
g2o::LinearSolverDense();       // 使用 dense cholesky 分解法
g2o::LinearSolverEigen();       // 使用 Eigen 中的 sparse Cholesky 分解法，依赖项只有 eigen 

块求解器

创建块求解器需要用到定义好的线性求解器进行初始化。

块求解器有两种定义方式，一种是固定变量的求解器，定义如下:

using BlockSolverPL = BlockSolver

其中，p和q表示边链接的两个顶点的维度。

此外，g2o还能用Eigen的动态矩阵来实现动态变量求解器，定义如下：

using BlockSolverX = BlockSolverPL

block_solver.h里有一些预定义的变量

BlockSolver_6_3;  // 用于3d SLAM, 位姿是6维，地图点是3维

总求解器

总求解器一共有三个优化方法: Gauss Newton 、Levenberg-Marquardt 和 Dog-Leg。这三个都继承了OptimizationWithHessian，其又继承了OptimizationAlgotithm。

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

稀疏优化器

用定义好的总求解器，创建一个稀疏优化器，然后设置一些稀疏优化器的参数。

顶点

文件在：core/base_vertex.h

g2o提供了一个比较通用的，适合大部分情况的模板BaseVertex，其继承了OptimizableGraph::Vertex。其中 int D 是表示顶点的最小维度，例如三维地图点的话，int D = 3。

  template <int D, typename T>
  class BaseVertex : public OptimizableGraph::Vertex {
    public:
    typedef T EstimateType;
    typedef std::stack<EstimateType, 
                       std::vector<EstimateType,  Eigen::aligned_allocator<EstimateType> > >
    BackupStackType;

    static const int Dimension = D;           ///< dimension of the estimate (minimal) in the manifold space
    ...some code
  }

g2o里有许多已经定义好的顶点：

VertexSE2 : public BaseVertex<3, SE2>       // SE表示的2D顶点（x, y, yaw）
VertexSE3 : public BaseVertex<6, Isometry3> // 等距变换的3D位姿（x, y, z, qx, qy, qz） 没有qw

VertexPointXY : public BaseVertex<2, Vector2> // 2D 点
VertexPointXYZ : public BaseVertex<3, Vector3> // 3D 地图点
VertexSBAPointXYZ : public BaseVertex<3, Vector3> // 3D 地图点

VertexSE3Expmap : public BaseVertex<6, SE3Quat> // SE3 顶点，内部用变换矩阵参数化，外部用指数映射参数化

VertexCam : public BaseVertex<6, SBACam> // 位姿 SBACam顶点

VertexSim3Expmap : public BaseVertex<7, Sim3> // Sim3 顶点

自定义顶点

如果说g2o的预定义顶点不满足我们的使用要求，还可以自定义顶点。根据3D/2D的应用场景，待优化变量: 位姿/三维地图点，不同的优化类型: 李群/李代数，可以定义自己的顶点。

模板如下：

class myVertex : public g2o::BaseVertex<Dim, Type>
{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        myVertex();

        virtual void read(std::istream& is) {}
        virtual void write(std::osteam& os) const {}
        // 设置顶点的初始值
        virtual void setOriginImpl()
        {
            _estimate = Type();
        }
        // 顶点更新函数
        // 其实就是 x + δx
        virtual void oplusImpl(const double* update) override
        {
            _estimate += some kind of (update);
        }
}

除了加法更新外，还存在乘法更新，例如 types_six_dof_expmap.h 中的SE(3) 位姿。

/**
 * \brief SE3 Vertex parameterized internally with a transformation matrix
 and externally with its exponential map
 */
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat> // 6 是存储优化变量的维度，六维李代数； SE3Quat 是优化变量的类型，内部使用了四元数表达旋转，然后加上位移
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  VertexSE3Expmap();

  bool read(std::istream& is);

  bool write(std::ostream& os) const;

  // 设置顶点的初始值
  virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }

  // 顶点更新函数
  // 乘法更新
  virtual void oplusImpl(const double* update_)  {
    Eigen::Map<const Vector6d> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }
};

向图中添加顶点

无论是哪种顶点，向图中添加的方式都是一模一样的：创建顶点->设置ID->设置观测值(初始值)->添加到优化器

CurveFittingVertex *v = new CurveFittingVertex();   // 新建顶点
v->setId(0);                                        // 设置顶点的ID
v->setEstimate ( Eigen::Vector3d(1, 1 , 1) );       // 设置顶点的观测值
optimizer.addVertex(v);                             // 往优化器里添加顶点

还有是否需要边缘化的选项，我们用 VectorSBAPointXYZ 类型的三维地图点为例

int index = 0;
for (const Point3f p : points)
{
  
  g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();     // 新建顶点
  
  point->setId(index++);                                            // 设定顶点编号
  
  point->setEstimate(Eigen::Vector3d (p.x, p.y, p.z) );             // 设定观测值(初始值)
  
  point->setMarginalized( true );                                   // 设定是否需要边缘化
  
  optimizer.addVertex(point);                                       // 将顶点添加到优化器中
}

边

g2o 中有三种类型的边，BaseUnaryEdge(一元边)、BaseBinaryEdge(二元边)、BaseMultiEdge(多元边)。

一条边可以连接多少个顶点，就叫几元边，我们常用的是二元边。

我们以三维点及其重投影误差作为例子，其两个顶点是三维地图点和位姿，边是重投影误差。

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

二元边的定义有4个参数，其中

- 2                    --> 测量值的维度，这里是2维，也就是说图像的二维像素坐标
- Vertex2D             --> 测量值的类型，Vertex2D
- VertexSBAPointXYZ    --> 第一个顶点，地图三维点的类型
- VertexSE3Expmap      --> 第二个顶点，李群位姿

我们可以看一下BaseBinaryEdge的定义

class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    myEdge();

    virtual bool read(istream& in) {} // 读
    virtual bool write(ostream& out) const {} // 写

    // 误差 = 测量值 - 估计值
    virtual void computeError() override
    {
      _error = _measurement - /*估计值*/
    }

    virtual void linearizeOplus() override 
    {
      /*计算偏导数(雅克比矩阵)*/
    }

}

看起来很简单，我们来看一个最小化重投影误差的问题，源码在：
edge_project_xyz2uv.h

// edge_project_xyz2uv.h

#ifndef G2O_SBA_EDGEPROJECTXYZ2UV_H
#define G2O_SBA_EDGEPROJECTXYZ2UV_H

#include "g2o/core/base_binary_edge.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"
#include "g2o_types_sba_api.h"
#include "parameter_cameraparameters.h"
#include "vertex_se3_expmap.h"

namespace g2o {

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjectXYZ2UV();
  bool read(std::istream& is);           // 读
  bool write(std::ostream& os) const;    // 写
  void computeError();                   // 计算误差
  virtual void linearizeOplus();         // 计算增量

 public:
  CameraParameters* _cam;                // 相机参数
};

}  // namespace g2o
#endif

edge_project_xyz2uv.cpp

// edge_project_xyz2uv.cpp

#include "edge_project_xyz2uv.h"

namespace g2o {

EdgeProjectXYZ2UV::EdgeProjectXYZ2UV() : BaseBinaryEdge<2, Vector2, VertexPointXYZ, VertexSE3Expmap>() 
{
  _cam = 0;
  resizeParameters(1);
  installParameter(_cam, 0);
}

bool EdgeProjectXYZ2UV::read(std::istream& is) 
{
  readParamIds(is);
  internal::readVector(is, _measurement);
  return readInformationMatrix(is);
}

bool EdgeProjectXYZ2UV::write(std::ostream& os) const 
{
  writeParamIds(os);
  internal::writeVector(os, measurement());
  return writeInformationMatrix(os);
}

// 计算误差
void EdgeProjectXYZ2UV::computeError() 
{
  const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);      // 顶点1，李群位姿
  const VertexPointXYZ* v2 = static_cast<const VertexPointXYZ*>(_vertices[0]);        // 顶点2，三维地图点
  const CameraParameters* cam = static_cast<const CameraParameters*>(parameter(0));   // 相机参数
  _error = measurement() - cam->cam_map(v1->estimate().map(v2->estimate()));          // 误差 = 测量值 - 估计值
  // 先用map，将顶点2的三维地图点转换到相机坐标系下
  // 后用cam_map，将三维地图点转换为像素坐标
}

// 计算增量
void EdgeProjectXYZ2UV::linearizeOplus() 
{
  VertexSE3Expmap* vj = static_cast<VertexSE3Expmap*>(_vertices[1]);
  SE3Quat T(vj->estimate());
  VertexPointXYZ* vi = static_cast<VertexPointXYZ*>(_vertices[0]);
  Vector3 xyz = vi->estimate();
  Vector3 xyz_trans = T.map(xyz);

  number_t x = xyz_trans[0];
  number_t y = xyz_trans[1];
  number_t z = xyz_trans[2];
  number_t z_2 = z * z;

  const CameraParameters* cam = static_cast<const CameraParameters*>(parameter(0));

  // 重投影误差 对 三维点求偏导数的 雅克比矩阵
  // 2维像素点 3维地图点 --> 2x6矩阵
  Eigen::Matrix<number_t, 2, 3, Eigen::ColMajor> tmp;
  tmp(0, 0) = cam->focal_length;
  tmp(0, 1) = 0;
  tmp(0, 2) = -x / z * cam->focal_length;

  tmp(1, 0) = 0;
  tmp(1, 1) = cam->focal_length;
  tmp(1, 2) = -y / z * cam->focal_length;

  _jacobianOplusXi = -1. / z * tmp * T.rotation().toRotationMatrix();

  // 重头误差 对 相机位姿求偏导数的 雅克比矩阵
  // 2维像素点 6维位姿 --> 2x6矩阵
  _jacobianOplusXj(0, 0) = x * y / z_2 * cam->focal_length;
  _jacobianOplusXj(0, 1) = -(1 + (x * x / z_2)) * cam->focal_length;
  _jacobianOplusXj(0, 2) = y / z * cam->focal_length;
  _jacobianOplusXj(0, 3) = -1. / z * cam->focal_length;
  _jacobianOplusXj(0, 4) = 0;
  _jacobianOplusXj(0, 5) = x / z_2 * cam->focal_length;

  _jacobianOplusXj(1, 0) = (1 + y * y / z_2) * cam->focal_length;
  _jacobianOplusXj(1, 1) = -x * y / z_2 * cam->focal_length;
  _jacobianOplusXj(1, 2) = -x / z * cam->focal_length;
  _jacobianOplusXj(1, 3) = 0;
  _jacobianOplusXj(1, 4) = -1. / z * cam->focal_length;
  _jacobianOplusXj(1, 5) = y / z_2 * cam->focal_length;
}

}  // namespace g2o

定义完了，就是如何往图里添加边了。这个其实和添加点一样，是程式化的。

下面看一下添加重投影误差的二元边的例子

index = 1;

for (const Point2f p : points)
{
  g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();                          // 新建边
  edge->setId(index);                                                                   // 设置边的ID
  edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(index)));   // 设置第一个顶点，三维地图点
  edge->setVertex(1, pose);                                                             // 设置第二个顶点，位姿
  edge->setMeasurement(Eigen::Vector2d(p.x, p.y));                                      // 设置观测值（初始值），二维像素坐标
  edge->setInformation(Eigen::Matrix2d::Identity());                                    // 设置信息矩阵
  optimizer.addEdge(edge);                                                              // 将边添加到优化器中
  index++;
}

实际应用

TODO …
后面写第二篇