g2o学习心得

本文主要是介绍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_NEWCurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}virtual void computeError() override { // 重写computeError函数，用于计算误差// 通过从API文档中翻阅，可以查到 _vertices 是一个 vecotr<Vertex*>，所以取[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);				// 边也需要设定唯一的IDedge->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提供的更多例子。

这篇关于g2o学习心得的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---