g2o学习笔记-理解顶点（一）

一、基本了解

g2o主要用来做非线形优化，是一个基于图优化的c++框架，全称General Graphic Optimization 的简称，主要用来优化非线性误差函数。

g2o帮助我们实现了很多内部的算法，只是在进行构造的时候，需要遵循一些规则，使用过程可以多看看例子或者别人的代码，积累别人是怎么使用的。

目前SLAM的后端一般分为两种处理方法，一种是以扩展卡尔曼滤波（EKF）为代表的滤波方法，一种是以图优化为代表的非线性优化方法。

简单说下EKF，在计算资源受限、待估计量比较简单的情况下，EKF为代表的滤波方法比较有效，经常用在激光SLAM中。它的一个大缺点就是存储量和状态量是平方增长关系，因为存储的是协方差矩阵，因此不适合大型场景。而现在基于视觉的SLAM方案，路标点（特征点）数据很大，滤波方法根本吃不消，所以此时滤波的方法效率非常低。

因此图优化的理论开始出来。

在SLAM里，图优化一般分解为两个任务：

1、构建图。机器人位姿作为顶点，位姿间关系作为边。

2、优化图。调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。

比如一个机器人在房屋里移动，它在某个时刻 t 的位姿（pose）就是一个顶点，这个也是待优化的变量。而位姿之间的关系就构成了一个边，比如时刻 t 和时刻 t+1 之间的相对位姿变换矩阵就是边，边通常表示误差项。

顶点：待优化变量

边：连接变量，由于带误差，因此叫误差项

边起到了约束的作用，使得图优化向着满足边约束的方向优化，最后得到了一个优化后的地图。

1.1 g2o的基本框架结构

SparseOptimizer是整个图的核心，我们注意右上角的 is-a 实心箭头，这个SparseOptimizer它是一个Optimizable Graph，从而也是一个超图（HyperGraph）。

超图包含了许多顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）。而这些顶点顶点继承自 Base Vertex，也就是OptimizableGraph::Vertex，而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge。

整个图的核心SparseOptimizer 包含一个优化算法（OptimizationAlgorithm）的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell's dogleg 三者中间选择一个（我们常用的是GN和LM）。

那么如何求解呢？OptimizationWithHessian 内部包含一个求解器（Solver），这个Solver实际是由一个BlockSolver组成的。这个BlockSolver有两个部分，一个是SparseBlockMatrix ，用于计算稀疏的雅可比和Hessian矩阵；一个是线性方程的求解器（LinearSolver），它用于计算迭代过程中最关键的一步HΔx=−b，LinearSolver有几种方法可以选择：PCG, CSparse, Choldmod。

1.2 g2o编程流程

从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码 (qq.com) 参考至这里，感谢分享。

1. 创建一个线性求解器LinearSolver

2. 创建BlockSolver。并用上面定义的线性求解器初始化。

3. 创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化

4. 创建终极大boss 稀疏优化器（SparseOptimizer），并用已定义求解器作为求解方法。

5. 定义图的顶点和边。并添加到SparseOptimizer中。

6. 设置优化参数，开始执行优化。

二、理解顶点

g2o本身内部定义了一些常用的顶点类型，能够直接用，不需要自己定义

1、2D pose Vertex, (x,y,theta)
VertexSE2 : public BaseVertex<3, SE2>  2、6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexSE3 : public BaseVertex<6, Isometry3>  3、
VertexPointXY : public BaseVertex<2, Vector2>
4、
VertexPointXYZ : public BaseVertex<3, Vector3>
5、
VertexSBAPointXYZ : public BaseVertex<3, Vector3>6、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.7、qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation
VertexCam : public BaseVertex<6, SBACam>8、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>

有时候我们需要的顶点类型这里面没有，就需要自己定义顶点类型。重新定义顶点一般需要考虑重写如下函数：

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();

read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以

setToOriginImpl：顶点重置函数，设定被优化变量的原始值。

oplusImpl：顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。

如：

在如十四讲中曲线拟合中定义的顶点

 注意有些顶点不满足加法运算时候，不支持这么简单的更新操作。

比如

 _estimate = SE3Quat();

它内部使用了四元数表达旋转，然后加上位移来存储位姿，同时支持李代数上的运算，比如对数映射（log函数）、李代数上增量（update函数）等操作

之所以不使用变换矩阵，是因为变换矩阵不满足加法封闭。相机位姿顶点类VertexSE3Expmap使用了李代数表示相机位姿，而不是使用旋转矩阵和平移矩阵。还有一个重要原因就是，旋转矩阵是有约束的矩阵，它必须是正交矩阵且行列式为1。使用它作为优化变量就会引入额外的约束条件，从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示，就可以把位姿估计变成无约束的优化问题，求解难度降低。

三、向图添加顶点

再如十四讲中的例子：

四、总结

目前刚接触了下g2o，了解下g2o做了件什么事情，帮助自己在看orbslam2的代码时，更好地理解。这里记录下顶点的理解。后年逐步深化去学习。