G2O 库

欢迎访问我的博客首页。

---

1. G2O 安装与配置

---

1.1 G2O 介绍

---

  G2O 是基于图模型的优化，顶点代表估计值（优化对象），边代表约束关系。在 g2o/g2o 文件夹中有下面几个文件夹，我们介绍一下它们的用途。

文件夹	用途
EXTERNAL	工具函数
apps	可视化
core	定义顶点、边和求解器的基类，以及线性增量方程求解器接口、图模型、优化算法、鲁棒核函数等
examples	例子
solvers	线性增量方程求解器
stuff
types	定义顶点和边

  g2o/types 中的 ICP、SBA、sclam2d/slam2d、sim3、slam3d 分别表示迭代最近点、稀疏 BA、二维李群、相似变换群、三维李群。

1.2 版本与安装

---

  G2O 依赖 Eigen3、Ceres、CSparse，安装这些依赖后再从 github 上下载 G2O 编译安装。不同版本的 G2O 定义的顶点和边会有差异：

1. G2O 20201223 及以后的版本用 VertexPointXYZ 替代了 VertexSBAPointXYZ。
2. G2O 20200410 及以后的版本中，创建优化器需要借助智能指针。

1.3 配置

---

  CMake 查找依赖库的命令是 find_package(XXX REQUIRED)，该命令根据配置文件 XXXConfig.cmake 或 FindXXX.cmake 查找名字为 XXX 的依赖库。如果这两个配置文件不在系统路径，需要使用 set 命令指出它们所在路径。下面是具体用法：

# 假设 XXXConfig.cmake 和 FindXXX.cmake 都在路径 path/to/config 中。

# 1. 根据 XXXConfig.cmake 查找依赖库。
set(XXX_DIR path/to/config)
find_package(XXX REQUIRED)

# 2. 根据 FindXXX.cmake 查找依赖库。
set(CMAKE_MODULE_PATH path/to/config)
find_package(XXX REQUIRED)

  一般优先使用 XXXConfig.cmake 查找依赖库，因为使用这个文件要么能同时查到头文件和库文件，要么什么也查不到。有些库没有提供可用的 XXXConfig.cmake，比如 G2O、CSparse。这时我们可以使用 FindXXX.cmake，在 G2O 源码的 cmake_modules 文件夹中就有好几个 FindXXX.cmake。

  打开 FindG2O.cmake，可以看到它在 ${G2O_ROOT}/include 下找 g2o/core/base_vertex.h，所以我们需要先用 set(G2O_ROOT path/to/G2O) 为 G2O_ROOT 赋值。配置文件 CMakeLists.txt 怎么写，可以参考 G2O 求解 PnP 问题。

2. 曲线拟合

---

  本节讲解基类顶点和基类边，以及怎么自定义顶点和边。

  使用 G2O 拟合曲线 $y=x^2+2x+3$，参考《视觉 SLAM 十四讲》第 6.4.3 节 《使用 g2o 拟合曲线》，其源码在 Github。

2.1 基类顶点

---

  自定义顶点类很简单，先继承基类顶点 g2o::BaseVertex，再重写一些函数。下面是 g2o 定义基类顶点的部分代码。

#define G2O_VERTEX_DIM ((D == Eigen::Dynamic) ? _dimension : D)

template <int D, typename T>
class BaseVertex : public OptimizableGraph::Vertex {
public:
    typedef T EstimateType;
    // dimension of the estimate (minimal) in the manifold space.
    static const int Dimension = D;

public:
    // return the current estimate of the vertex.
    const EstimateType &estimate() const { return _estimate; }
    // set the estimate for the vertex also calls updateCache().
    void setEstimate(const EstimateType &et) {
        _estimate = et;
        updateCache();
    }

protected:
    EstimateType _estimate;
};

  第一个模板参数指明估计值（优化对象） _estimate 的维度；第二个模板参数指明估计值（优化对象）的类型。

  为了存取估计值（优化对象） _estimate，顶点定义了两个成员函数 setEstimate 和 estimate。前者在创建顶点后、向优化器添加顶点前被调用，用于设置顶点的估计值（优化对象）。后者在优化结束后调用，用于获取优化结果。这两个函数是基类顶点已经定义的，我们自定义顶点时无须再次定义。

2.2 自定义顶点

---

  现在我们继承基类顶点，定义用于拟合曲线的顶点。

// 定义顶点：用于更新优化对象。模板参数：优化对象的维度和数据类型。
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	// 设置初始值。
	virtual void setToOriginImpl() {
		_estimate << 0, 0, 0;
	}
	// 更新。
	virtual void oplusImpl(const double* update) {
		_estimate += Eigen::Vector3d(update);
	}
	// 存盘和读盘：留空。
	virtual bool read(istream& in) {}
	virtual bool write(ostream& out) const {}
};

  从上面的代码可以看出，估计值（优化对象）存储在顶点中，顶点负责估计值（优化对象）的初始化和更新。

  定义顶点需要重写 4 个函数：

1. 成员函数 setToOriginImpl 在该顶点被初始化时被自动调用，用于为成员变量 _estimate 赋初值。

2. 成员函数 oplusImpl 用于在自变量上产生一个增量，即实现 $x_{k+1}=x_k+\Delta x$。其参数 update 是个数组，第 1 个模板参数 D 用于指定该数组的长度。

3. 成员函数 read 和 write 是两个虚函数，所以即使我们不需要它们，也要实现它们。

2.3 基类边

---

  和自定义顶点类相同，自定义边类也是先继承基类边，再重写一些函数。

  不同的是，边有一元边、二元边、多元边，原因是边可以与一个顶点、二个顶点、多个顶点有关系。可以认为边就是它所连接的若干个顶点的约束关系。边用它所连接的若干个顶点计算误差，优化的目的就是调整各顶点的估计值 _estimate，使边的误差 _error 趋向于 0。

  我们可以使用一元边拟合曲线，因此继承 g2o::BaseUnaryEdge。下面是 g2o 定义基类一元边的部分代码。

template <int D>
struct BaseEdgeTraits {
    static constexpr int Dimension = D;
    typedef Eigen::Matrix<number_t, D, 1, Eigen::ColMajor> ErrorVector;
    typedef Eigen::Matrix<number_t, D, D, Eigen::ColMajor> InformationType;
};

template <int D, typename E>
class BaseEdge : public OptimizableGraph::Edge {
public:
    static constexpr int Dimension = internal::BaseEdgeTraits<D>::Dimension;
    typedef E Measurement;
    typedef typename internal::BaseEdgeTraits<D>::ErrorVector ErrorVector;
    typedef typename internal::BaseEdgeTraits<D>::InformationType InformationType;
    BaseEdge() : OptimizableGraph::Edge() { _dimension = D; }
    // accessor functions for the measurement represented by the edge.
    EIGEN_STRONG_INLINE const Measurement &measurement() const { return _measurement; }
    virtual void setMeasurement(const Measurement &m) { _measurement = m; }
protected:
    Measurement _measurement;     // the measurement of the edge.
    InformationType _information; // information matrix of the edge. Information = inv(covariance).
    ErrorVector _error;           // error vector, stores the result after computeError() is called.
};

  第一个模板参数指明测量值 _measurement 的维度；第二个模板参数指明测量值的类型；第三个模板参数指定该一元边连接的顶点类型。

  基类一元边 BaseUnaryEdge 和基类二元边 BaseBinaryEdge 都继承该基类边。实际应用的边通常又继承基类一元边或基类二元边。

2.4 自定义边

---

  现在我们继承基类一元边，定义用于拟合曲线的一元边。

// 定义边：用于计算误差。模板参数：观测值维度、类型、连接的顶点类型。
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
	CurveFittingEdge(double x) : _x(x) {}
	// 计算误差。
	void computeError() {
		const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
		const Eigen::Vector3d abc = v->estimate();
		_error(0, 0) = _measurement - (abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
	}
	virtual bool read(istream& in) {}
	virtual bool write(ostream& out) const {}
public:
	double _x;
};

  从上面的代码可以看出，边先通过调用顶点的 estimate() 函数获取顶点的估计值（优化对象） _estimate，再利用测量值 _measurement 和观测值 _x 计算误差 _error。

  下面是自定义一元边、二元边、N 元边的例子。

class EdgeUnary : public g2o::BaseUnaryEdge<D, T0, T1> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    virtual void computeError() override {
        // _error = f(_measurement, (T1)Vertex1);
    }
    virtual bool read(istream &in) {}
    virtual bool write(ostream &out) const {}
};

class EdgeBinary : public g2o::BaseBinaryEdge<D, T0, T1, T2> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    virtual void computeError() override {
        // _error = f(_measurement, (T1)Vertex1, (T2)Vertex2);
    }
    virtual bool read(istream &in) {}
    virtual bool write(ostream &out) const {}
};

class EdgeN : public g2o::BaseMultiEdge<D, T0> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeN() {
        this->resize(N);
    }
    virtual void computeError() override {
        // _error = f(_measurement, (T1)Vertex1, (T2)Vertex2, ..., (TN)VertexN);
    }
    virtual bool read(istream &in) {}
    virtual bool write(ostream &out) const {}
};

  定义边需要重写 3 个函数：

1. 成员函数 computeError 用于根据各顶点的估计值（优化对象）计算误差。

2. 成员函数 read 和 write。

  G2O 可以根据导数（雅可比矩阵）求导，也可以自动进行数值求导。我们可以在边的定义中重写函数 linearizeOplus，根据变量的当前值计算新的导数值，即根据 $x_{k+1}$ 计算 $f^{\prime }(x_{k+1})$。如果不重写该函数，G2O 将自动进行数值求导。

virtual void linearizeOplus() {
    _jacobianOplusXi = f();
}

2.5 拟合算法

---

  生成观测数据并使用 G2O 优化。

#include <iostream>
#include <random>
#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 <cmath>
#include <chrono>
using namespace std;

int main(int argc, char** argv) {
	// 设置随机数产生方法。
	std::random_device rd;
	std::mt19937 gen(rd());
	std::normal_distribution<double> normal(0.0, 1.0);
	// 1.产生 N 组观测数据，y = x^2 + 2x + c + 噪声。
	const int N = 100;
	std::vector<double> data_x;
	std::vector<double> data_y;
	data_x.reserve(N);
	data_y.reserve(N);
	for (int i = 0; i < N; i++) {
		data_x[i] = i;
		data_y[i] = 1 * i * i + 2 * i + 3 + normal(gen);
	}
	// 2.构建 g2o 优化图。
	// 2.1矩阵块求解器。优化变量维度为 3，误差值维度为 1。
	typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> Block;
	// 2.2使用线性方程求解器创建块求解器。
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
	Block* solver_ptr = new Block(linearSolver);
	// 2.3梯度下降算法。
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
	//g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
	//g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr);
	// 2.4设置求解器，打开调试输出。
	g2o::SparseOptimizer optimizer;
	optimizer.setAlgorithm(solver);
	optimizer.setVerbose(true);
	// 3.添加顶点和随意初始化的优化对象。
	CurveFittingVertex* v = new CurveFittingVertex();
	v->setEstimate(Eigen::Vector3d(0, 0, 0));
	v->setId(0);
	optimizer.addVertex(v);
	// 4.添加边。
	double w_sigma = 1.0; // 噪声 Sigma 值。
	for (int i = 0; i < N; i++) {
		CurveFittingEdge* edge = new CurveFittingEdge(data_x[i]);
		edge->setId(i);
		edge->setVertex(0, v);																		// 设置连接的顶点。
		edge->setMeasurement(data_y[i]);															// 观测数值。
		edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma));	// 信息矩阵：协方差矩阵之逆。
		optimizer.addEdge(edge);
	}
	// 5.优化。
	cout << "start optimization..." << endl;
	optimizer.initializeOptimization();
	optimizer.optimize(100);
	// 6.获取优化结果。
	Eigen::Vector3d res = v->estimate();
	cout << "estimated model: " << res.transpose() << endl;
	return 0;
}

2.6 配置文件

---

  CMakeLists.txt 中的内容。

cmake_minimum_required(VERSION 3.5.1)
project(demo)

set(G2O_ROOT D:/MinGW/libraries/g2o_20170730)
set(CMAKE_MODULE_PATH ${G2O_ROOT})
find_package(G2O REQUIRED)
find_package(Ceres REQUIRED)

include_directories( 
    ${G2O_INCLUDE_DIR}
    ${CERES_INCLUDE_DIRS}
)
link_directories(${G2O_ROOT}/lib)

add_executable(main
    main.cc
)
target_link_libraries(main
	g2o_core
    g2o_stuff
    g2o_types_sba
    g2o_types_slam3d
    g2o_types_icp
    ${CERES_LIBRARIES}
)

3. 优化器

---

  有了上面的例子，下面我们总结一下 G2O 的用法。

  使用 G2O 时需要先创建优化器，然后向优化器中添加顶点和边。下面是创建优化器的过程：

// 块求解器。
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;

// 使用线性方程求解器创建块求解器。
Block::LinearSolverType *linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
Block *solver_ptr= new Block(linearSolver);

// 梯度下降算法。
g2o::OptimizationAlgorithmGaussNewton *solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);

  G2O 20200410 及以后的版本中，创建优化器需要借助智能指针，详细的使用方法可参考其例程 g2o/examples。

#include<memory>

typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;
unique_ptr<Block::LinearSolverType> linearSolver(new g2o::LinearSolverEigen<Block::PoseMatrixType>());
unique_ptr<Block> solver_ptr(new Block(std::move(linearSolver)));
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);

3.1 块求解器类型

---

  块求解器的类型可以使用下面 3 种形式声明：

typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> Block;
typedef g2o::BlockSolver<g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>> Block;
using g2o::BlockSolverX Block;

  第 1 种形式在上面拟合曲线中已经使用过，其中 g2o::BlockSolverTraits 的两个模板参数分别指定估计值（优化对象）的维度和误差的维度。第 2 种形式的模板参数使用动态分配。第 3 种形式使用通用的块求解器。

  从 g2o/core/block_solver.h 可以看出，第 3 种形式是使用第 2 种形式定义的：

template <int p, int l>
using BlockSolverPL = BlockSolver<BlockSolverTraits<p, l>>;
// variable size solver
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

  在 g2o/core/block_solver.h 中，G2O 还为模板类 g2o::BlockSolver 定义了别名：

// 2Dof landmarks 3Dof poses
using BlockSolver_3_2 = BlockSolverPL<3, 2>;

// solver for BA/3D SLAM
using BlockSolver_6_3 = BlockSolverPL<6, 3>;

// solver fo BA with scale
using BlockSolver_7_3 = BlockSolverPL<7, 3>;

  这 3 个别名是为 SLAM 定义的：第 1 个模板参数是使用 $\mathfrak{so}(3)$、$\mathfrak{se}(3)$、$\mathfrak{sim}(3)$ 表示的待优化位姿的维度，第 2 个模板参数是二维像素坐标或三维空间坐标的维度。

3.2 线性增量方程求解器

---

  块求解器类型确定后，就可以利用它定义线性增量方程求解器。G2O 已经实现了 5 种线性增量方程求解器，定义在 g2o/solvers 中。

Block::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<Block::PoseMatrixType>();
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
Block::LinearSolverType* linearSolver = new g2o::LinearSolverPCG<Block::PoseMatrixType>();

3.3 优化算法

---

  有了块求解器对象，下一步选择优化算法。G2O 已经实现了 4 种优化算法，定义在 g2o/core 中。

g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr);
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
g2o::OptimizationAlgorithmWithHessian* solver = new g2o::OptimizationAlgorithmWithHessian(solver_ptr);

4. G2O 定义的顶点

---

  第 2 节介绍了基类顶点和自定义顶点。现在我们介绍 G2O 为 SLAM 定义的 4 个顶点及其用法。

// 1.代表空间坐标的顶点，它的模板参数是一个三维向量。
class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3D> {};
Eigen::Vector3d p(0, 0, 0);
v.setEstimate(p);

// 2.代表位姿的顶点。它的模板参数是李代数 se(3) 的指数映射。
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat> {};
Eigen::Matrix3d R;
R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
Eigen::Vector3d t(0, 0, 0);
v.setEstimate(g2o::SE3Quat(R, t));

// 3.代表位姿的顶点，它的模板参数是李群 SE(3)。
class G2O_TYPES_SLAM3D_API VertexSE3 : public BaseVertex<6, Isometry3D> {};
Eigen::Matrix3d R;
R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
Eigen::Vector3d t(0, 0, 0);
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(R);
T.pretranslate(t);
v.setEstimate(T);

// 4.代表位姿的顶点。它的模板参数是李代数 sim(3) 的指数映射。
class VertexSim3Expmap : public BaseVertex<7, Sim3> {};
Eigen::Matrix3d R;
R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
Eigen::Vector3d t(0, 0, 0);
double s = 1.0;
g2o::Sim3 Siw(R, t, s);
v.setEstimate(Siw);

5. G2O 定义的边

---

5.1 用于 PnP 问题的边

---

  G2O 定义的用于 PnP 问题的边。这 5 个边的区别与用法在《PnP 问题》。

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV
	: public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{...};

class EdgeSE3ProjectXYZ
	: public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap> {...};
class EdgeSE3ProjectXYZOnlyPose
	: public BaseUnaryEdge<2, Vector2D, VertexSE3Expmap> {...};
class EdgeStereoSE3ProjectXYZ
	: public BaseBinaryEdge<3, Vector3D, VertexSBAPointXYZ, VertexSE3Expmap> {...};
class EdgeStereoSE3ProjectXYZOnlyPose
	: public BaseUnaryEdge<3, Vector3D, VertexSE3Expmap> {...};

  边连接顶点的顺序一定要和模板参数中的顶点类型顺序一致。以边 EdgeProjectXYZ2UV 为例，0 号顶点必须是 VertexSBAPointXYZ 类型，1 号顶点必须是 VertexSE3Expmap 类型：

edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(edge_idx0)));
edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(edge_idx1)));

// 由于基类指针可以指向派生类对象，所以也可以使用基类顶点类型。
edge->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(edge_idx0)));
edge->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(edge_idx1)));

5.2 用于 ICP 问题的边

---

  G2O 定义的用于 ICP 问题的边。从定义可用看出，Edge_V_V_GICP 是二元边，它连接的两个顶点类型都是 VertexSE3。用法在《ICP 问题之 NLO》。

class G2O_TYPES_ICP_API Edge_V_V_GICP
	: public BaseBinaryEdge<3, EdgeGICP, VertexSE3, VertexSE3> {...};

5.3 用于 Sim3 问题的边

---

  G2O 定义的用于 Sim3 问题的 3 个边。从定义可用看出，三个边都是二元边。用法在《Sim3 问题》。

class EdgeSim3
	: public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap> {...};
class EdgeSim3ProjectXYZ
	: public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSim3Expmap> {...};
class EdgeInverseSim3ProjectXYZ
	: public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSim3Expmap> {...};

6. 信息矩阵

---

edge->setInformation

7. 参考文献

---

1. 半闲居士，博客园，高翔。
2. G2O 源码，github。
3. G2O 论文。
4. G2O PPT。