g2o图优化库

前段时间因学习slam，其中涉及到了非线性优化的问题,采用了g2o图优化库解决此问题。在此记录下g2o实现原理及步骤。
1、g2o 简介
g2o是一个图优化库,图优化里的图就是数据结构里的图，一个图由若干个顶点（vertex），以及连接这些顶点的边（edge）组成。
在SLAM里，图优化一般分解为两个任务：
1）构建图：机器人位姿作为顶点，位姿间关系作为边。
2）优化图：调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。
2、g2o基本框架图

SparseOptimizer是整个图的核心，注意右上角的 is-a 实心箭头，这个SparseOptimizer它是一个Optimizable Graph，从而也是一个超图（HyperGraph）。
如上图,G2O的框架是从顶层到底层，但是编程实现时需要反过来，从底层开始搭建框架一直到顶层。
3、g2o的整个框架就是按照下图这个顺序来写的。

typedef g2o::BlockSolver<g2o::BlockSolverTraits<3,1> > Block;

第1步:创建线性求解器

Block::LinearSolverType* lineasolver = newg2o::LinearSolverDense<Block::PoseMatrixType>();

2步:创建BlockSolver,并用上面的线性求解器初始化

Block* solver_ptr = new Block (lineasolver);

第3步:创建总求解器solver并从GN\LM\DOGLEG中选一个,再用上述块求解器BlockSolver初始化Block

g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);

第4步:创建稀疏优化器SparseOptimizer

g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);

4、顶点
g2o::BaseVertex<Dim, Type>
Dim:优化变量维度
Type:优化变量类型
重新定义顶点一般需要考虑重写如下函数：
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以。
virtual void setToOriginImpl(); 顶点重置函数，设定被优化变量的原始值。
virtual void oplusImpl(const number_t* update):顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的。
5、边
BaseBinaryEdge<int D, typename E, typename VertexXi, typename VertexXj>
D 是 int 型，表示测量值的维度 （dimension）
E 表示测量值的数据类型
VertexXi，VertexXj 分别表示不同顶点的类型
BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。
边主要有以下几个重要的成员函数
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下。
virtual void computeError(); 使用当前顶点的值计算的测量值与真实的测量值之间的误差。
virtual void linearizeOplus();当前顶点的值下，该误差对优化变量的偏导数，也就是Jacobian。

.文中内容参考
https://www.cnblogs.com/CV-life/archive/2019/03/13/10525579.html