g2o学习心得

最近在学习g2o库的使用，折腾了几天，虽然整体还不是很清楚，但有不少收获，先做总结。

1. 学习方法

学习资料比较杂乱，我个人感觉最佳的学习方法是：

1. 了解图优化基本思想
2. 大致阅读g2o的教程文档：g2o: A general Framework for (Hyper) Graph Optimization
3. 清楚写一个g2o需要做什么
4. 对照API文档，逐个学习真实代码中的细节是在干什么。
5. 不断实践学习

2. 具体内容

2.1 理论学习

SLAM图优化相关博客：https://blog.csdn.net/heyijia0327/article/details/47686523
g2o文档：g2o: A general Framework for (Hyper) Graph Optimization
文档来源：1. g2o源码中：g2o/doc/g2o.pdf
2. 我上传了百度网盘：https://pan.baidu.com/s/1zk4CXWOjpnmYLORhxQxa0Q 提取码: v7tx

2.2 g2o的下载与安装

g2o github: https://github.com/RainerKuemmerle/g2o
下载下来以后，执行常规操作即可。

生成API文档：
g2o的源码自带了API文档，但需要doxygen生成，同时保证安装了dot这个东西。

1. doxygen下载与安装：https://blog.csdn.net/tfb760/article/details/103593100
2. 安装完成doxygen，需要安装graphviz/dot，方法如下：

sudo apt-get install graphviz
# 之后遇到报错: 依赖libcdt5...等balabala
sudo apt-get -f install 

3. 生成API: 进入g2o/doc/doxygen文件，按照readme.txt执行

doxygen doxy.config

之后生成了html文件，可以在这里下载(提取码efma)我自己生成的

2.3 使用g2o优化需要定义什么？

代码中使用g2o时，需要定义以下三项内容：
1. 定义边（edge）和顶点（vertex）：包括边和顶点的数据类型，顶点的数据更新方式，边的误差计算方法；
2. 确定优化求解器，包括：线性方程求解器、BlockSolver和迭代策略
3. 将定义的边和顶点连接成图，并开始优化

3. 具体例子

下面使用《视觉SLAM十四讲》中第6讲非线性优化的代码，注释说明具体的使用方法。
完整代码下载：https://pan.baidu.com/s/1YAIMiWPmPV2CDL8MTNV4tA 提取码: evzt

STEP 1. 定义顶点

class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {	
// 自定义一个Vertex类，继承基本的BaseVertex，3维向量，数据格式为Vector3d
public:
// 这里时Eigen的一个宏定义，声明后后面的new方法使用eigen自己定义的方法，避免系统new的一些问题
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // 定义“设置为零的实现(implement)”，覆盖默认的虚函数
  virtual void setToOriginImpl() override {	
    _estimate << 0, 0, 0;
  }
  // 定义Oplus(自定义加法)的实现，即顶点的更新方法
  virtual void oplusImpl(const double *update) override {	
    _estimate += Eigen::Vector3d(update);
  }
  virtual bool read(istream &in) {}		// 读写函数不覆盖
  virtual bool write(ostream &out) const {}
};

可以看出，需要重写四个函数，其中最重要的oplusImpl，表示更新方法。由于顶点数据可能是自己定义的，所对应的加法（写作"$\oplus$"，所以函数名是"o-plus"，而Impl指implement，这个是自己推测的……）需要自己定义而不是简单的相加。但这里进行拟合只是1维数字，所以直接加就行。

STEP 2. 定义边

// 自定义一个Edge类，继承自BaseUnaryEdge，有1个变量，类型是double，连接的vertex（由于是unary所以只有一个）类型时CurveFittingVertex
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {	
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}
  virtual void computeError() override { // 重写computeError函数，用于计算误差
  // 通过从API文档中翻阅，可以查到 _vertices 是一个 vecotr，所以取[0]为第一个Vertex，强制转化成我们自定义的 CurveFittingVertex类。
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);		
    const Eigen::Vector3d abc = v->estimate();	// 读取当前边的估计值
    // 计算误差，误差是由measurement减去的当前估计值算出来的结果，_error是由Eigen定义的一个向量，向量的维度和类型默认被父类定义好了
    _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是 JacobianXiOplusType，本质是一个Eigen::Matrix
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }
  virtual bool read(istream &in) {}
  virtual bool write(ostream &out) const {}
public:
  double _x;
};

STEP 3. 在main中定义优化器

typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 简写BlockSolver，其由一个具体的BlockSolverTraits定义维度3x1.
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 定义先行求解器的简写，来自于BlockSolver的PoseMatrixType。
// 定义一个求解器，使用GN算法（只能是GN，LM，DogLeg的一种），具体定义形式参看API文档。自己还不是很清楚
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;		// 定义图模型
optimizer.setAlgorithm(solver);		// 选择求解算法，使用刚刚定义的求解器

STEP 4. 连接成图

// 首先增加节点
CurveFittingVertex *v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(ae, be, ce));	// 设定初始值为估计值
v->setId(0);			// 每个顶点需要确定唯一的id
optimizer.addVertex(v);	// 将定义好的节点加到优化器中

// 再增加边
for (int i = 0; i < N; i++) {	// 把所有观测数据加上，每个都是一条边
	CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
	edge->setId(i);				// 边也需要设定唯一的ID
	edge->setVertex(0, v);		// 设置连接的顶点，第一个0表示这个边的一个节点（如果是Binary的edge，则需要设定0和1两个节点，分别连接那个节点）
	edge->setMeasurement(y_data[i]);	// 将观测值设置为测量值
	// 信息矩阵，用于加权，取协方差矩阵的倒数。具体的取值根据经验吧。
	edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); 			
	optimizer.addEdge(edge);		// 把边加进去
}

STEP 5. 开始优化求解

optimizer.initializeOptimization();		// 初始化：
optimizer.optimize(10);					// 设定迭代次数并开始迭代优化

4. 其他

可以参考g2o中example提供的更多例子。