一、总体介绍
g2o（General Graphic Optimization）是一个通用图优化算法库，目前主流的SLAM研究基本都是基于优化的，可以在此处查阅其论文原文。
图优化中的点是相机位姿，边是位姿之间的变换关系，通常表示误差项。
g2o的类关系图
从整个结构框图分析，稀疏优化器SparseOptimizer是整个g2o的核心。
二、搭建步骤
要使用g2o求解优化问题，框架的搭建步骤如下：
（1）创建一个线性求解器 LinearSolver。
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
g2o提供的求解方式主要有5种，
继承自 LinearSolverCCS 的有2种：① LinearSolverCholmod，使用sparse cholesky分解法，继承自LinearSolverCCS；② LinearSolverCSparse，使用CSparse法，继承自LinearSolverCCS；
继承自 LinearSolver 的有3种：① LinearSolverPCG，使用preconditioned conjugate gradient 法，继承自LinearSolver。② LinearSolverDense，使用dense cholesky分解法，继承自LinearSolver。③ LinearSolverEigen，依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多，继承自LinearSolver。
（2）创建块求解器 BlockSolver，并使用上面定义的线性求解器初始化
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1
Block* solver_ptr = new Block( linearSolver );
solver的定义有固定变量和可变尺寸两种，其定义方式分别为：
// 固定维度，p为pose的维度，l表示landmark的维度
using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;
// 可边尺寸
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;
另外，g2o还预定义了以下3种常用类型：① BlockSolver_6_3，6维pose + 3维landmark，常用于Bundle Adjustment。 ② BlockSolver_7_3，在BlockSolver_6_3基础上多出一个scale维度。 ③ BlockSolver_3_2，3维pose + 2维lankmark，常用于平面世界。
（3）创建总求解器 solver，并从GN，LM，Dogleg中选一个，用上述块求解器BlockSolver初始化。
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
这里有三种迭代策略可供选择：g2o::OptimizationAlgorithmGaussNewton，g2o::OptimizationAlgorithmLevenberg，g2o::OptimizationAlgorithmDogleg。
（4）创建图优化的核心：稀疏优化器（SparseOptimizer）。
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm( solver ); // 设置求解方法
optimizer.setVerbose( true ); // 设置优化过程输出信息
（5）定义图的顶点和边，并添加到SparseOptimizer中
CurveFittingVertex* v = new CurveFittingVertex();
v->setId(0);
optimizer.addVertex( v );
for ( int i=0; i<N; i++ ) // 往图中增加边
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()* 1 / (w_sigma * w_sigma) ); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge( edge );
}
（6）设置优化参数，开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100); //设置迭代次数
三、图的顶点与边
3.1 顶点
g2o中定义Vertex有一个通用的类模板：BaseVertex，它有D/T两个参数，D是 int 类型，表示vertex的最小维度，T是待估计的 vertex 的数据类型，是状态在其流形空间中的最小表示。（存在疑问）
一些常用的顶点类型：
ertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3> //Isometry3使欧式变换矩阵T，实质是4*4矩阵//6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexPointXY : public BaseVertex<2, Vector2>VertexPointXYZ : public BaseVertex<3, Vector3>VertexSBAPointXYZ : public BaseVertex<3, Vector3>
// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential mapVertexSE3Expmap : public BaseVertex<6, SE3Quat>
// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.// qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotationVertexCam : public BaseVertex<6, SBACam>
// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.VertexSim3Expmap : public BaseVertex<7, Sim3>
当自己来定义顶点时，需要重写下面几个函数：
class myVertex: public g2o::BaseVertex<Dim, Type>
{
public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW
myVertex(){
}
// 读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
virtual void read(std::istream& is) {
}
virtual void write(std::ostream& os) const {
}
// 顶点重置函数，设定被优化变量的原始值。
virtual void setOriginImpl(){
_estimate = Type(); }
// 顶点更新函数，主要用于优化过程中增量△x 的计算，根据增量方程计算出增量后，通过这个函数对估计值进行调整
virtual void oplusImpl(const double* update) override{
_estimate += update; }
}
将顶点添加到图中：
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) )；// 设定初始值v->setId(0); // 定义节点编号
optimizer.addVertex( v ); // 把节点添加到图中
3.2 边
g2o中定义的边有一元边、二元边、多元边，以二元边为例，它的参数有：D, E, VertexXi, VertexXj。D是 int 型，表示测量值维度；E表示测量值数据类型；VertexXi，VertexXj 分别表示不同顶点的类型。由此可以得到一个二元边的定义如下：
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
当自己来定义边时，需要重写下面几个函数：
class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
{
public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW
myEdge(){
}
virtual bool read(istream& in) {
}
virtual bool write(ostream& out) const {
}
// 是使用当前顶点值计算的测量值与真实测量值之间的误差
virtual void computeError() override {
// ...
_error = _measurement - Something;
}
// 是在当前顶点的值下，该误差对优化变量的偏导数，也就是Jacobian矩阵
virtual void linearizeOplus() override // 求误差对优化变量的偏导数，雅克比矩阵
{
_jacobianOplusXi(pos, pos) = something; // ... /* _
jocobianOplusXj(pos, pos) = something; ... */
}
private:
data
}
参考博客：