前言：g2o对于slam等问题来说十分的重要，将一个slam问题构建成图结构表达的优化问题是目前大多数的slam算法的通用做法。之前在学习了高博对g2o的讲解后至今感觉意犹未尽，表现在对于简单问题能够理解其含义而在slam问题中构建的各种复杂的优化问题则感到无力，因此决定按照g2o源码给出的实例系统的学习g2o库的使用。
预备知识：《视觉slam14讲》ch6部分
博文风格：由于这是第一篇博客，先介绍撰写博客时的思路。首先介绍每篇博客需要求解的问题定义，然后介绍使用到的g2o接口最后便是源码剖析。
曲线拟合
1. 问题定义
给定一系列点，拟合非线性曲线
已知：一系列点的坐标,拟合曲线
待求：参数
2. g2o相关API介绍
2.1 节点
BaseVertex 类，模板类
头文件：#include "g2o/core/base_vertex.h"自定义的节点继承自该基类，由于需要估计的参数为三个因此模板类具化为g2o::BaseVertex<3,Eigen::Vector3d>
由于在该类定义时部分函数为虚函数，需要在继承的子类中实现具体函数及含义如下：
- 构造函数（继承时并不继承构造函数）
- 读取函数，暂时没有用
- 写入函数，暂时没用
- 设置优化的起点，暂时没用
- 更新优化变量
virtual bool read(std::istream& /*is*/)
virtual bool write(std::ostream& /*os*/) const
virtual void setToOriginImpl()
virtual void oplusImpl(const double* update)
在本例中仅仅用到了更新优化变量函数，而对于欧式空间中的优化问题只需要在状态中加上更新量即可，因此实现如下：
virtual void oplusImpl(const double* update)
{Eigen::Vector3d::ConstMapType v(update);// 每次的更新量_estimate += v;// 直接加上跟新量即可
}而整体继承类实现如下
class VertexParams : public g2o::BaseVertex<3,Eigen::Vector3d>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;VertexParams(){}virtual bool read(std::istream& /*is*/){cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}virtual bool write(std::ostream& /*os*/) const{cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}virtual void setToOriginImpl(){std::cout<<"not implemented!"<<std::endl;}virtual void oplusImpl(const double* update){Eigen::Vector3d::ConstMapType v(update);_estimate += v;}
};2.2 边
BaseUnaryEdge类，模板类
头文件：#include "g2o/core/base_unary_edge.h"自定义边继承自该基类，由于约束项为2d点因此模板类具化为 g2o::BaseUnaryEdge<1,Eigen::Vector2d,VertexParams>
该类需要在子类中实现的虚函数如下：
- 构造函数（继承时并不继承构造函数）
- 读取函数，暂时没有用
- 写入函数，暂时没用
- 误差计算函数
virtual bool read(std::istream& /*is*/)
virtual bool write(std::ostream& /*os*/) const
void computeError()
在本例中使用到的主要函数为误差计算函数，即计算当前拟合曲线和约束点之间的误差，因此实现如下
void computeError()
{// 需要优化的节点const VertexParams* params = static_cast<const VertexParams*>(vertex(0));// 节点参数const double& a = params->estimate()(0);const double& b = params->estimate()(1);const double& lambda = params->estimate()(2);// 计算误差double fval = a * exp(-lambda * measurement()(0)) + b;// 记录误差_error(0) = fval - measurement()(1);
}整体实现代码
class EdgePointOnCurve : public g2o::BaseUnaryEdge<1,Eigen::Vector2d,VertexParams>
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgePointOnCurve(){}virtual bool read(std::istream& /*is*/){cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}virtual bool write(std::ostream& /*os*/) const{cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;return false;}void computeError(){const VertexParams* params = static_cast<const VertexParams*>(vertex(0));const double& a = params->estimate()(0);const double& b = params->estimate()(1);const double& lambda = params->estimate()(2);double fval = a * exp(-lambda * measurement()(0)) + b;_error(0) = fval - measurement()(1);}};2.3 优化流程
2.3.1 重命名
由于模板类名称太长，一般会采用typedef重命名，同时定义求解器的类型.其中第一行的3代表一个节点的维度为3，而2代表一条边的维度为2.
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3,2> > MyBlockSolver;typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;2.3.2 设置求解器
// 设置求解器g2o::SparseOptimizer optimizer;optimizer.setVerbose(false);g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<MyBlockSolver>(g2o::make_unique<MyLinearSolver>()));optimizer.setAlgorithm(solver);2.3.3 构造图结构
// 1.添加待优化的节点VertexParams* params = new VertexParams();params->setId(0);params->setEstimate(Eigen::Vector3d(1,1,1));optimizer.addVertex(params);// 2.添加约束for(int i=0;i<numPoints;i++){EdgePointOnCurve* e = new EdgePointOnCurve();e->setInformation(Eigen::Matrix<double,1,1>::Identity());e->setVertex(0,params);e->setMeasurement(points[i]);optimizer.addEdge(e);}2.3.4 求解
// 执行优化optimizer.initializeOptimization();optimizer.setVerbose(verbose);optimizer.optimize(maxIterations);3. 代码实现
由于实现代码较为冗长因此给出源码github链接
4. 小结
通过对曲线拟合的简单例子了解到了g2o库的调用流程
- 自定义边、节点类继承自模板类，重新实现部分虚函数
- 初始化求解器，选择优化方法
- 构造图结构，往图中添加节点和边
- 调用函数进行求解
大部分的g2o教程到此便结束了，但是学习g2o的目的并非为了拟合曲线而是求解slam中各种复杂的问题，因此后面将进行进一步深入的讨论。