《视觉惯性SLAM理论与源码解析》阅读笔记-g2o图优化库学习使用

问题学生

已于 2024-07-28 16:58:16 修改

阅读量202

点赞数 3

文章标签：笔记学习

于 2024-07-28 11:43:25 首次发布

本文链接：https://blog.csdn.net/m0_56651652/article/details/140748568

版权

使用g2o库进行图优化的具体编程步骤。

参考文章：【SLAM】G2O优化库超详细解析-CSDN博客

整体框架如下图

具体的，使用该书中求解曲线参数的例子具体说明

// 每个误差项优化变量维度为3，误差维度为1
typedef g2o::BlockSolver< g2o::BlockSloverTraits<3,1> > Block;

// 第1步：创建一个线性求解器（LinearSolver）
Block::LinearSolverType* LinearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();

// 第2步：构建块求解器（BlockSolver），并用上面定义的线性求解器初始化
Block* solver_ptr = new Block(linearSolver);

// 第3步：创建总求解器（Solver），并从GN、LM、DogLeg中选择一个，再用上述块求解器初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));

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

// 第5步：定义图的顶点（Vertex），并添加到优化器中
CurveFittingVertex* v = new CurveFittingVertex();

v->setEstimate ( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex(v);

// 第6步：定义图的边（edge），并添加到优化器中
for (int i=0; i<N; i++)    // 向图中增加边
{
    CurveFittingEdge* edge = new CurveFittingEdge( x_data(i) );
    edge->setId(i);
    edge->setVertex( 0, v);            // 设置连接的顶点
    edge->setMesurement( y_data(i) );  // 观测数值    
    // 信息矩阵：协方差矩阵之逆
    edge->setInformation (Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) );
    optimizer.addEdge(edge);
}

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

第1步：创建一个线性求解器（LinearSolver）

由于在迭代的过程中，基本都需要求解近似于 $H\Delta x = -b$ 的增量方程。通常情况下想到的方法就是直接求逆，也就是 $\Delta x =-H^{-1}b$ 。看起来好像很简单，但这有个前提，就是H的维度较小，此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。

在g2o中主要有以下几种线性求解方法

LinearSolverCholmod : public LinearSolverCCS<MatrixType>;    // 使用sparse cholesky分解法。继承自 LinearSolverCCS

LinearSolverCSparse : public LinearSolverCCS<MatrixType>;    // 使用CSparse法。继承自 LinearSolverCCS

LinearSolverPCG : public LinearSolver<MatrixType>;           // 使用preconditioned conjugate gradient 法。继承自 LinearSolver

LinearSolverDense : public LinearSolver<MatrixType>;         // 使用dense cholesky分解法。继承自 LinearSolver

LinearSolverEigen : public LinearSolverCCS<MatrixType>;      // 依赖项只有eigen，使用eigen中sparse Cholesky求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多

第2步：构建块求解器（BlockSolver），并用上面定义的线性求解器初始化。

在该类中，会对稀疏（Schur舒尔补）的雅可比和Hessian矩阵进行计算，这部分由SparseBlockMatrix负责。但由于SparseBlockMatrix部分比较固定，一般不做什么修改或自定义，因此，初始化的时候仅有LinearSolver。

块求解器由两种定义方式：

// 固定变量的BlockSolver
// p：pose的维度（注意一定是流形manifold下的最小表示）
// l：landmark的维度（通常也是残差的维度）
template <int p, int l>
using BlockSolverPL = BlockSolver<BlockSolverTraits<p, l> >;

// 可变尺寸的BlockSolver
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

// 有些场景，位姿和路标点在程序开始时并不能被确定，此时块求解器就没办法固定变量，应该使用可变尺寸的求解器。

BlockSolver_6_3 = BlockSolverPL<6, 3>;    // 表示pose是6维，观测点是3维，常用于3D SLAM中的BA操作

BlockSolver_7_3 = BlockSolverPL<7, 3>;    // 在BlockSolver_6_3的基础上多了一个scale

BlockSolver_3_2 = BlockSolverPL<3, 2>;    // 表示pose是3维，观测点是2维

第3步：创建总求解器（Solver），并从GN、LM、DogLeg中选择一个，再用上述块求解器初始化。

在该阶段可以选择以下3中方法，其中用得比较多的方法是LM。

g2o::OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian;    // 高斯牛顿（GaussNewton）法

g2o::OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian;     // LM（Levenberg–Marquardt）法

g2o::OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian;        // Dogleg法

第4步：创建稀疏优化器（SparseOptimizer）。

第5步：定义图的顶点（Vertex），并添加到优化器中。

顶点Vertex的主要继承关系为：HyperGraph::Vertex（最基类）、OptimizableGraph::Vertex（较基类）、BaseVertex、BaseDynamicVertex（派生类）。其中，后两者是比较通用的适合大部分情况的模板，另外两个类都比较底层，通常不会直接使用。

G2O的顶点分为两种类型的顶点，固定长度的顶点（BaseVertex）、可变长度的顶点（BaseDynamicVertex）。可根据不同的场景实现特定的Vertex。这里的不同场景包括：不同的应用场景（二维空间、三维空间），不同的待优化变量（位姿、空间点），不同的优化类型（李代数位姿、李群位姿）等。

g2o内部定义了一些常用的顶点类型，如下：

// 2D二维点Vertex
VertexPointXY : public BaseVertex<2, Vector2>

// 3D空间点Vertex
VertexPointXYZ : public BaseVertex<3, Vector3>

// 2D位姿Vertex，(x、y、theta)
VertexSE2 : public BaseVertex<3, SE2>

// 3D位姿Vertex，(x、y、z、qx、qy、qz)，四元数表示旋转，qw通过模长为1约束计算
VertexSE3 : public BaseVertex<6, Isometry3>

// 3D位姿Vertex，(x、y、z、se(3))，李代数表示旋转，内部用变换矩阵参数化，外部用指数映射参数化
VertexSE3Expmap : public BaseVertex<6, SE3Quat>

// Sim3 Vertex(相似变换群)
VertexSim3Expmap : public BaseVertex<7, Sim3>

当然我们可以直接用这些，但是有时候我们需要的顶点类型这里面没有，就得自己定义了。重新定义顶点一般需要考虑重写如下函数（有时也需要重写其他函数）：

// 读盘操作，一般情况下不需要进行读操作的话，仅仅声明一下就可以
virtual bool read(std::istream& is);
// 存盘操作，一般情况下不需要进行写操作的话，仅仅声明一下就可以
virtual bool write(std::ostream& os) const;

// 顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视
virtual void oplusImpl(const number_t* update);
// 顶点重置函数，设定被优化变量的原始值
virtual void setToOriginImpl();

除了上面几个成员函数，还有几个重要的成员变量和函数也一并解释一下：

// 顶点的估计值，其中，该EstimateType类型就是BaseVertex的第二个模板参数类型
EstimateType _estimate;

// 设置顶点的初始值
void setEstimate(const EstimateType& et);
// 设置顶点的id
virtual void setId(int id);
// 该顶点在优化期间应被视为固定
void setFixed(bool fixed);
// 该顶点应在优化过程中被边缘化
void setMarginalized(bool marginalized);

就以官方源码中预设的VertexPointXYZ顶点类型为例：

class VertexPointXYZ : public BaseVertex<3, Vector3> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  VertexPointXYZ() {}

  // 读盘/存盘操作
  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;

  // 重置操作
  virtual void setToOriginImpl() { _estimate.fill(0.); }

  // 增量更新操作
  virtual void oplusImpl(const double* update_) {
    Eigen::Map<const Vector3> update(update_);
    _estimate += update;
  }

  ...
};

第6步：定义图的边（edge），并添加到优化器中。

边表示残差项。每条边对应着若干个顶点（ ≥ 1）和一个测量值，依靠对应的顶点可以计算出一个和测量值意义一致的估计值，而这个估计值和测量值之间的误差则表示对应的残差。

边Edge的主要继承关系为：HyperGraph::Edge（最基类）、OptimizableGraph::Edge、BaseEdge（较基类）、BaseUnaryEdge（一元边）、BaseBinaryEdge（二元边）、BaseMultiEdge（派生类，多元边）。其中，后两者是比较通用的适合大部分情况的模板，另外三个类都比较底层，通常不会直接使用。

比如我们用边表示三维点投影到图像平面上的重投影误差，就可以设置如下输入参数：

//该类型的边为二元边，2表示测量值是2维，测量值为图像坐标，类型为Vector2D，边连接的两个顶点分别为三维点VertexSBAPointXYZ和李群位姿VertexSE3Expmap
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>;

也就是说，此时该边连接的是两个顶点，空间三维点VertexPointXYZ、3D位姿VertexSE3，根据这两个点再结合已知的相机内参可以计算出该空间三维点在图像平面的坐标。而此时该边还有一个测量值Vector2D，它的维度为2，也是一个图像平面的坐标。此时重投影误差就可以通过两个坐标计算出来了。

和顶点一样，优化库也预设了一些边的定义，但是有时候我们需要的边类型这里面没有，就得自己定义了。重新定义边一般需要考虑重写如下函数（有时也需要重写其他函数）：

// 读盘操作，一般情况下不需要进行读操作的话，仅仅声明一下就可以
virtual bool read(std::istream& is);
// 存盘操作，一般情况下不需要进行写操作的话，仅仅声明一下就可以
virtual bool write(std::ostream& os) const;
// 边残差计算函数，使用当前边对应的顶点的值计算的测量值与真实的测量值之间的残差
virtual void computeError();
// 边雅可比函数，计算在当前顶点的值下，该残差对优化变量的偏导数
virtual void linearizeOplus();

需要注意的是，一旦指定了linearizeOplus()，就会使用该雅可比矩阵进行优化；如果不对该函数进行重写，就会使用自动求导的方式进行优化。

除了上面几个成员函数，还有几个重要的成员变量和函数也一并解释一下：

// 边的测量值，其中，该Measurement类型就是Edge的第二个模板参数类型
Measurement _measurement;
// 残差量，computeError()函数计算后的值
// 其中，该Measurement类型是Matrix<double, D, 1>，D为Edge的第一个模板参数类型
ErrorVector _error;
// 信息矩阵，其中，该Measurement类型是Matrix<double, D, D>，D为Edge的第一个模板参数类型
InformationType _information;
// 顶点信息，存储顺序与调用setVertex()的入参有关
VertexContainer _vertices;
// 雅可比矩阵（双元边）
JacobianXiOplusType _jacobianOplusXi; // （单元边）
JacobianXjOplusType _jacobianOplusXj;
// 雅可比矩阵（多元边）
std::vector<JacobianType> _jacobianOplus;

// 设置边的id
void setId(int id);
// 设置边上编号为i的顶点
void setVertex(size_t i, Vertex* v);
// 设置边的测量值
virtual void setMeasurement(const Measurement& m);
// 设置边的信息矩阵（协方差矩阵的逆）
void setInformation(const InformationType& information);
// level等级设置成1，表示不优化（默认情况下，g2o只处理level=0的边）
void setLevel(int l);

需要注意的是，_vertices内存放了该边对应的顶点信息，它可以通过方括号[]运算符进行取顶点操作，它的顺序完全由setVertex()的第一个size_t类型的参数决定的。

就以官方源码中预设的EdgeSE3边类型为例（边对应的两个顶点都是3D位姿，对应的观测值是两个顶点之间的相对3D位姿测量）：

class EdgeSE3
    : public BaseBinaryEdge<6, Isometry3, VertexSE3, VertexSE3> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeSE3();

  // 读盘/存盘操作
  virtual bool read(std::istream& is);
  virtual bool write(std::ostream& os) const;

  // 计算残差操作
  void EdgeSE3::computeError() {
    VertexSE3* from = static_cast<VertexSE3*>(_vertices[0]);
    VertexSE3* to = static_cast<VertexSE3*>(_vertices[1]);
    Isometry3 delta =
        _inverseMeasurement * from->estimate().inverse() * to->estimate();    // 相对位姿测量值和相对位姿估计值的差
    _error = internal::toVectorMQT(delta);
  }

  // 雅可比求解操作
  void EdgeSE3::linearizeOplus() {
    VertexSE3* from = static_cast<VertexSE3*>(_vertices[0]);
    VertexSE3* to = static_cast<VertexSE3*>(_vertices[1]);
    Isometry3 E;
    const Isometry3& Xi = from->estimate();
    const Isometry3& Xj = to->estimate();
    const Isometry3& Z = _measurement;
    internal::computeEdgeSE3Gradient(E, _jacobianOplusXi, _jacobianOplusXj, Z, Xi,
                                    Xj);
  }

  virtual void setMeasurement(const Isometry3& m) {
    _measurement = m;
    _inverseMeasurement = m.inverse();
  }

  ...

 protected:
  Isometry3 _inverseMeasurement;      // 相对位姿测量值的逆
};

第7步：设置优化参数，开始执行优化。