ORBSLAM2中的优化（一） --orbslam2中用到的优化库g2o，orbslam2图优化代码讲解

ORBSLAM2中的优化（一）

本讲将介绍ORBSLAM2中用到的优化库g2o

1.g2o

1.1 g2o的框架

介绍了g2o的框架

以 SparseOptimizer 为核心，分为两个部分：

1. 图部分，这部分主要是有
   1. 顶点 Vertex
   2. 边 Edge
2. 优化算法部分，这部分主要是有求解器，包括：
   1. 对雅可比矩阵和H矩阵的求解：SpareBlockMatrix
   2. 对线性方程的求解：LinearSolver

1.2 g2o的顶点

介绍了顶点的常用模板类，以及如何添加顶点到图中

1. g2o中顶点最常用的是模板类：BaseVertex<D,T>

   • 这个模板类继承自OptimizableGraph::Vertex

   • 而OptimizableGraph::Vertex继承自HyperGraph::Vertex

2. BaseVertex<D,T>的模板参数的具体含义：

   • D是int类型，代表该点的最小维度，如三维旋转则 D == 3

   • T是数据类型，如果用四元数表示三维旋转，则 T == Quaternion

3. g2o根据该模板预设了很多类，可以直接被使用：

顶点类型	实际的类型	说明
VertexSE2	BaseVertex<3, SE2>	代表二维姿态，即x，y，theta
VertexSE3	BaseVertex<6, Isometry3>	6维向量，x，y，z，qx，qy，qz（就是省略了qw）
VertexPointXY	BaseVertex<2, Vector2>
VertexPointXYZ	BaseVertex<3, Vector3>
VertexSBAPointXYZ	BaseVertex<3, Vector3>	三维点（地图点）
VertexSE3Expmap	BaseVertex<6, SE3Quat>	李群相机位姿
VertexCam	BaseVertex<6, SBACam>
VertexSim3Expmap	BaseVertex<7, Sim3>	sim3相似变换群，7个自由度：尺度因子s，R和t，即[sR|t]

4. 自定义的顶点则需要对以下虚函数进行重写：其中最重要的是顶点更新函数，主要用于对增量 $△x$ 的计算，根据增量方程计算出增量后，就用这个函数进行更新，也就是完成了 $x=x+△x$ 这一步。

  virtual bool read(std::istream& is);             //读盘函数
  virtual bool write(std::ostream& os) const;      //写盘函数
  virtual void oplusImpl(const number_t* update);  //顶点重置函数，设定被优化变量的原始值
  virtual void setToOriginImpl();                  //顶点更新函数

5. 添加顶点到图中，slam十四讲示例代码如下：其中
   • setId 方法是设置点的 id ，方便后续操作
   • setMarginalized 是设置是否边缘化这个点

    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); 
        optimizer.addVertex ( point );
    }

1.3 g2o中的边

介绍了g2o中常用的二元边模板类以及其重要成员函数

1. 在slam优化中，最多使用的边的模板类是：BaseBinaryEdge<D, E, VertexXi, VertexXj>
   1. D代表测量值的维度
   2. E代表测量值的数据类型
   3. VertexXi，VertexXj分别代表一个顶点的类型

	//以以下代码为例
	BaseBinaryEdge< 2,                   //测量值的维度是2
					Vector2D,            //测量值的数据类型是 Vector2D，也就是2维向量
					VertexSBAPointXYZ,   //一个顶点是 三维点
					VertexSE3Expmap>     //一个顶点是 李群位姿
   //由此变量我们可以推测，这是图有 位姿 和 路标，而误差 则是用三维点投影到二维平面上的重投影误差

2. 自定义边，和自定义顶点一样，需要复写一些函数

virtual bool read(std::istream& is);          //读盘
virtual bool write(std::ostream& os) const;   //写盘
virtual void computeError();                  //计算误差：即使用当前顶点计算得出的测量值 和 真实值 之间的误差
virtual void linearizeOplus();                //计算偏导：即计算雅可比矩阵，即在当前顶点的值的情况下，
                                              //        该误差对优化变量的偏导数

3. 此外，有一些重要的成员函数和变量

	//成员变量
	_measuremen 			//存储观测值
	_error					//存储computeError() 函数计算的误差
	_vertices[]				//存储顶点信息，
        					//	比如二元边的话，_vertices[] 的大小为2，
        					//	存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1）
	
    //成员函数
	setId(int)				//来定义边的编号（决定了在H矩阵中的位置）
	setMeasurement(type) 	//函数来定义观测值
	setVertex(int, vertex) 	//来定义顶点
	setInformation() 		//来定义协方差矩阵的逆

1.4 g2o优化流程

介绍了g2o的优化流程

一个完整的优化流程分为以下的步骤：

1. 定义 顶点 和 边
   1. 顶点：数据类型 + 顶点更新 函数
   2. 边：边数据类型 + 计算 误差 和 偏导 的函数
2. 实例化优化器，配置 求解器
3. 添加 顶点 和 边
4. 执行优化迭代

更加具体的细节，在后续文章中，结合 ORBSLAM 代码进行讲解