G20理论到ORB-SLAM优化实践

最新推荐文章于 2023-03-25 20:34:07 发布

行知SLAM

最新推荐文章于 2023-03-25 20:34:07 发布

阅读量413

点赞数

分类专栏：行知SLAM实践文章标签：计算机视觉 c++

本文链接：https://blog.csdn.net/qq_15258067/article/details/127332122

版权

行知SLAM实践专栏收录该内容

5 篇文章 4 订阅

订阅专栏

图优化是什么

SLAM的后端一般分为两种处理方法，一种是以扩展卡尔曼滤波（EKF）为代表的滤波方法，一种是以图优化为代表的非线性优化方法。不过，目前SLAM研究的主流热点几乎都是基于图优化的。

滤波方法的优缺点：

优点：在当时计算资源受限、待估计量比较简单的情况下，EKF为代表的滤波方法比较有效，经常用在激光SLAM中。

缺点：它的一个大缺点就是存储量和状态量是平方增长关系，因为存储的是协方差矩阵，因此不适合大型场景。而现在基于视觉的SLAM方案，路标点（特征点）数据很大，滤波方法根本吃不消，所以此时滤波的方法效率非常低。

图是由节点和边构成，SLAM问题怎么构成图呢？在graph-based SLAM中，机器人的位姿是一个节点（node）或顶点(vertex)，位姿之间的关系构成边(edge)。具体而言比如t+1时刻和t时刻之间的odometry关系构成边，或者由视觉计算出来的位姿转换矩阵也可以构成边。一旦图构建完成了，就要调整机器人的位姿去尽量满足这些边构成的约束。

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

所以图优化SLAM问题能够分解成两个任务：

\1. 构建图，机器人位姿当做顶点，位姿间关系当做边，这一步常常被成为前端（front-end），往往是传感器信息的堆积。

\2. 优化图，调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小，这一步称为后端（back-end）。

g2o理论

is-a:是一个，子类指向父类

has-a:有一个，父类包含子类，包含关系，比如SparseOptimizer 包含一个优化算法（OptimizationAlgorithm）的对象。；

has-many:有一些，比如超图有顶点和边。

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。

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1

// 第1步：创建一个线性求解器LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); 

// 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
Block* solver_ptr = new Block( linearSolver );      

// 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );

// 第4步：创建终极大boss 稀疏优化器（SparseOptimizer）
g2o::SparseOptimizer optimizer;     // 图模型
optimizer.setAlgorithm( solver );   // 设置求解器
optimizer.setVerbose( true );       // 打开调试输出

// 第5步：定义图的顶点和边。并添加到SparseOptimizer中
CurveFittingVertex* v = new CurveFittingVertex(); //往图中增加顶点
v->setEstimate( Eigen::Vector3d(0,0,0) );
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);

具体步骤：

1、创建一个线性求解器LinearSolver—用于计算迭代过程中最关键的一步HΔx=−b

我们要求的增量方程的形式是：H△X=-b，通常情况下想到的方法就是直接求逆，也就是△X=-H.inv*b。看起来好像很简单，但这有个前提，就是H的维度较小，此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。

g20里对矩阵进行求逆的方法：

LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver
LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen： 依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver

2、创建BlockSolver—用于计算稀疏的雅可比和Hessian矩阵

块求解器 BlockSolver 构造线性方程求解器所需要的矩阵块（H 和 b），需要用到边的雅克比

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-55EPCRB4-1665801011465)(/home/cidi/2_学习记录文档cidi/007-orbslam3/25.02 orb-slam中的优化.assets/image-20221013113628979.png)]

BlockSolver 内部包含 LinearSolver，用上面我们定义的线性求解器LinearSolver来初始化。它的定义在如下文件夹内：

g2o/g2o/core/block_solver.h

你点进去会发现 BlockSolver有两种定义方式

一种是指定的固定变量的solver，我们来看一下定义

 using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

其中p代表pose的维度（注意一定是流形manifold下的最小表示），l表示landmark的维度

需要指定优化变量的维度，常见的有以下几种，其中 6 表示待优化变量的维度， 3 表示误差项的维度，可以设置为动态的 BlockSolverX

// variable size solver
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

// solver for BA/3D SLAM
using BlockSolver_6_3 = BlockSolverPL<6, 3>;

// solver fo BA with scale
using BlockSolver_7_3 = BlockSolverPL<7, 3>;

// 2Dof landmarks 3Dof poses
using BlockSolver_3_2 = BlockSolverPL<3, 2>;

另外你看block_solver.h的最后，预定义了比较常用的几种类型，如下所示：

BlockSolver_6_3 ：表示pose 是6维，观测点是3维。用于3D SLAM中的BA
BlockSolver_7_3：在BlockSolver_6_3 的基础上多了一个scale
BlockSolver_3_2：表示pose 是3维，观测点是2维

另一种是可变尺寸的solver，定义如下

using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

在某些应用场景，我们的Pose和Landmark在程序开始时并不能确定，那么此时这个块状求解器就没办法固定变量，此时使用这个可变尺寸的solver，所有的参数都在中间过程中被确定

3、创建总求解器solver—选择下降迭代策略，从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化

我们来看g2o/g2o/core/ 目录下，发现Solver的优化方法有三种：分别是高斯牛顿（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法，如下图所示，也和前面的图相匹配

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

创建稀疏优化器

g2o::SparseOptimizer    optimizer;

用前面定义好的求解器作为求解方法：

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

其中setVerbose是设置优化过程输出信息用的

SparseOptimizer::setVerbose(bool verbose)

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

如何自己定义顶点？

我们知道了顶点的基本类型是 BaseVertex，那么下一步关心的就是如何使用了，因为在不同的应用场景（二维空间，三维空间），有不同的待优化变量（位姿，空间点），还涉及不同的优化类型（李代数位姿、李群位姿）。

g2o本身内部定义了一些常用的顶点类型，大概这些：

VertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3>  //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 map
VertexSE3Expmap : 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 rotation
VertexCam : 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>

但是有时候我们需要的顶点类型这里面没有，就得自己定义了。

重新定义顶点一般需要考虑重写如下函数：

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 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。

自己定义顶点一般是下面的格式：

  class myVertex: public g2::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();
      }
      virtual void oplusImpl(const double* update) override
      {
          _estimate += /*update*/;
      }
  }

顶点举例一： 以 SLAM 十四讲中曲线拟合的曲线模型顶点为例

定义顶点为 CurveFittingVertex，顶点维度为 3，类型为 Eigen::Vector3d；
初始值设为为 0, 0, 0；
更新函数中由于是向量直接加上更新量 _estimate += Eigen::Vector3d(update)；
读写函数留空。

// 曲线模型的顶点，模板参数：优化变量维度和数据类型

class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;
    }

    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

我们可以看到上面代码中顶点初值设置为0，更新时也是直接把更新量 update 加上去的（更新是 x + △x ），对于这个例子是可以直接加，因为顶点类型是Eigen::Vector3d，属于向量，是可以通过加法来更新的。但是但是有些例子就不行，比如下面这个复杂点例子：李代数表示位姿VertexSE3Expmap

顶点举例二：以李代数表示的位姿作为顶点，位于 `g2o/types/sba/types_six_dof_expmap.h` 中

定义顶点类为 VertexSE3Expmap，优化变量是 6 自由度的李代数；
更新函数采用李代数的增量扰动更新

/**
 * \brief SE3 Vertex parameterized internally with a transformation matrix
 and externally with its exponential map
 */
class  VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // 构造函数.
  VertexSE3Expmap();

  // 1. 读盘.
  bool read(std::istream& is);
  // 2. 写盘.
  bool write(std::ostream& os) const;

  // 3. 顶点重置函数，设定被优化变量的原始值
  virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }

  // 4. 顶点更新函数，增量更新
  virtual void oplusImpl(const double* update_)  {
    Eigen::Map<const Vector6d> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }
};

BaseVertex<6, SE3Quat>

第一个参数6 表示内部存储的优化变量维度，这是个6维的李代数

第二个参数是优化变量的类型，这里使用了g2o定义的相机位姿类型：SE3Quat。

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

这个例子更新时没有像上面那样直接加上去，原因是变换矩阵不满足加法封闭。那我再问你，为什么相机位姿顶点类VertexSE3Expmap使用了李代数表示相机位姿，而不是使用旋转矩阵和平移矩阵？

这是因为旋转矩阵是有约束的矩阵，它必须是正交矩阵且行列式为1。使用它作为优化变量就会引入额外的约束条件，从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示，就可以把位姿估计变成无约束的优化问题，求解难度降低。

顶点举例3三–空间点

三维向量表示的三维点作为顶点，位于 g2o/types/types_sba.h 中

定义顶点类为 VertexSBAPointXYZ，优化变量是 3 维度的 Vector3d 向量；
重置与更新函数类似于举例一中的形式，直接相加。

我们继续看例子，刚才是位姿的例子，下面是三维点的例子，空间点位置 VertexPointXYZ，维度为3，类型是Eigen的Vector3，比较简单，就不解释了

 class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    
    VertexSBAPointXYZ();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;
    virtual void setToOriginImpl() {
      _estimate.fill(0);
    }

    virtual void oplusImpl(const number_t* update)
    {
      Eigen::Map<const Vector3> v(update);
      _estimate += v;
    }
};

如何向图中添加顶点？

步骤：
- ① 创建顶点
- ② 设置初始值
- ③ 设置节点编号
- ④ 添加到优化器中

举例一：曲线拟合

往图中增加顶点比较简单，我们还是先看看第一个曲线拟合的例子，setEstimate(type) 函数来设定初始值；setId(int) 定义节点编号

    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );

举例二：三维坐标点

这个是添加 VertexSBAPointXYZ 的例子，都很容易看懂

/ch7/pose_estimation_3d2d.cpp

    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 );
    }

初步认识g2o的边

BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。

一元边你可以理解为一条边只连接一个顶点，两元边理解为一条边连接两个顶点，也就是我们常见的边啦，多元边理解为一条边可以连接多个（3个以上）顶点

主要参数有：D, E, VertexXi, VertexXj
- D 是 int 型，表示测量值的维度；
- E 表示测量值的数据类型；
- VertexXi，VertexXj 分别表示不同顶点的类型。
- 举例：用边表示三维点投影到图像平面的重投影误差
- ```
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
```
  - 首先顶点有两个是个，这是一个二元边；
  - 误差（测量值）的维度为 2，也就是像素坐标 u,v 的差值；
  - 误差（测量值）的数据类型为二维向量 Vector2D；
  - 两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ 和李群表示的相机位姿VertexSE3Expmap。

除了输入参数外，定义边我们通常需要复写一些重要的成员函数。

边和顶点的成员函数还是差别比较大的，边主要有以下几个重要的成员函数

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();

下面简单解释一下
read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
computeError函数：非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差
linearizeOplus函数：非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian

除了上面几个成员函数，还有几个重要的成员变量和函数也一并解释一下：

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

如何自定义g2o的边？

 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;
      }      
      virtual void linearizeOplus() override
      {
          _jacobianOplusXi(pos, pos) = something;
          // ...         
          /*
          _jocobianOplusXj(pos, pos) = something;
          ...
          */
      }      
      private:
      // data
  }

举例一：一元边。来源十四讲中的曲线拟合

定义误差边为 CurveFittingEdge，维度为 1，类型为 double，连接的顶点为 CurveFittingVertex；

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

举例二：二元边。3D-2D点的PnP 问题，也就是最小化重投影误差问题，来源于 g2o/types/sba/types_six_dof_expmap.h

构造边为 EdgeProjectXYZ2UV，维度为 2，类型为 Vector2D，连接的两个顶点分别为三维点 VertexSBAPointXYZ 和李群表示的相机位姿 VertexSE3Expmap；

也就是最小化重投影误差问题，这个问题非常常见，使用最常见的二元边，弄懂了这个基本跟边相关的代码也差不多都一通百通了

//继承了BaseBinaryEdge类，观测值是2维，类型Vector2D,顶点分别是三维点、李群位姿
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    //1. 默认初始化
    EdgeProjectXYZ2UV();
    //2. 计算误差
    void computeError()  {
      //李群相机位姿v1
      const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
      // 顶点v2
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      //相机参数
      const CameraParameters * cam
        = static_cast<const CameraParameters *>(parameter(0));
     //误差计算，测量值减去估计值，也就是重投影误差obs-cam
     //估计值计算方法是T*p,得到相机坐标系下坐标，然后在利用camera2pixel()函数得到像素坐标。
      Vector2D obs(_measurement);
      _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }
    //3. 线性增量函数，也就是雅克比矩阵J的计算方法
    virtual void linearizeOplus();
    //4. 相机参数
    CameraParameters * _cam; 
    bool read(std::istream& is);
    bool write(std::ostream& os) const;
};

关于误差计算函数：误差 = 观测 - 投影

_error = obs - cam->cam_map(v1->estimate().map(v2->estimate()));

其中 obs 是观测值，当前帧中观察到的特征点的像素坐标；
v1 是相机位姿，v2 是地图点坐标，cam 是相机参数；
v1->estimate().map(v2->estimate()) 是利用相机位姿（外参）将地图点坐标从世界坐标系转换到相机坐标系；

看 .map函数，它的功能是把世界坐标系下三维点变换到相机坐标系，函数在g2o/types/sim3/sim3.h具体定义是

 Vector3 map (const Vector3& xyz) const {
    return s*(r*xyz) + t;
 }

因此下面这个代码

v1->estimate().map(v2->estimate())

就是用V1估计的pose把V2代表的三维点，变换到相机坐标系下。

cam->cam_map(v1->estimate().map(v2->estimate())) 是利用相机内参将地图点从相机坐标系转换到图像坐标系。（cam_map 操作定义在 g2o/types/sba/types_six_dof_expmap.cpp 中）
cam_map 函数功能是把相机坐标系下三维点（输入）用内参转换为图像坐标（输出），具体代码如下所示

Vector2  CameraParameters::cam_map(const Vector3 & trans_xyz) const {
  Vector2 proj = project2d(trans_xyz);
  Vector2 res;
  res[0] = proj[0]*focal_length + principle_point[0];
  res[1] = proj[1]*focal_length + principle_point[1];
  return res;
}

关于雅克比矩阵的计算函数 `linearizeOplus()`：误差（观测相机方程）关于相机位姿和三维点的偏导数

上式分别对应下面代码中的 _jacobianOplusXj 和 _jacobianOplusXi（代码位于 g2o/g2o/types/sba/types_six_dof_expmap.cpp 中）

void EdgeProjectXYZ2UV::linearizeOplus() {
  VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
  SE3Quat T(vj->estimate());
  VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
  Vector3D xyz = vi->estimate();
  Vector3D xyz_trans = T.map(xyz);

  double x = xyz_trans[0];
  double y = xyz_trans[1];
  double z = xyz_trans[2];
  double z_2 = z*z;

  const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0));

  Matrix<double,2,3,Eigen::ColMajor> tmp;
  tmp(0,0) = cam->focal_length;
  tmp(0,1) = 0;
  tmp(0,2) = -x/z*cam->focal_length;

  tmp(1,0) = 0;
  tmp(1,1) = cam->focal_length;
  tmp(1,2) = -y/z*cam->focal_length;

  _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

  _jacobianOplusXj(0,0) =  x*y/z_2 *cam->focal_length;
  _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *cam->focal_length;
  _jacobianOplusXj(0,2) = y/z *cam->focal_length;
  _jacobianOplusXj(0,3) = -1./z *cam->focal_length;
  _jacobianOplusXj(0,4) = 0;
  _jacobianOplusXj(0,5) = x/z_2 *cam->focal_length;

  _jacobianOplusXj(1,0) = (1+y*y/z_2) *cam->focal_length;
  _jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length;
  _jacobianOplusXj(1,2) = -x/z *cam->focal_length;
  _jacobianOplusXj(1,3) = 0;
  _jacobianOplusXj(1,4) = -1./z *cam->focal_length;
  _jacobianOplusXj(1,5) = y/z_2 *cam->focal_length;
}

如何向图中添加边？

一元边的添加方法-- 以曲线拟合为例（slambook/ch6/g2o_curve_fitting/main.cpp）

for ( int i=0; i<N; i++ )
{
    // 创建边
    CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
      
    edge->setId(i);                         // 设置边的 id
    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 );
}

setMeasurement 函数的输入的观测值就是实际观测到的数据点。对于视觉SLAM来说，通常就是我们我们观测到的特征点坐标，下面就是一个例子。这个例子比刚才的复杂一点，因为它是二元边，需要用边连接两个顶点。
代码来自GitHub上slambook/ch7/pose_estimation_3d2d.cpp

index = 1;
for ( const Point2f p:points_2d )
{
    // 创建边.
    g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
    // 设置边的 id
    edge->setId ( index );
    // 指定连接的顶点
    edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
    edge->setVertex ( 1, pose );
      
    // 设置观测值
    edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
      
    edge->setParameterId ( 0,0 );
      
    edge->setInformation ( Eigen::Matrix2d::Identity() );
      
    optimizer.addEdge ( edge );
    index++;
}

注意在 setVertex() 中设置顶点的 id 和类型时要与边的类中定义的相对应，比如定义边时 v1 表示相机位姿，id 为 1，v2 表示三维点，id 为0

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV 
.....
 //李群相机位姿v1
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
// 顶点v2
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);

你看 _vertices[0] 对应的是 VertexSBAPointXYZ 类型的顶点，也就是三维点，_vertices[1] 对应的是VertexSE3Expmap 类型的顶点，也就是位姿pose。因此前面 1 对应的就应该是 pose，0对应的应该就是三维点。g2o不会帮我区分顶点的类型.

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

设置SparseOptimizer的初始化、迭代次数、保存结果等。

初始化

SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)

设置迭代次数，然后就开始执行图优化了。

SparseOptimizer::optimize(int iterations, bool online)

图优化曲线拟合实战完整例子

下面针对一个具体的问题来实际练习g2o的使用，拟合函数

𝑦=𝑒𝑥𝑝(3.5𝑥3+1.6𝑥2+0.3𝑥+7.8)

整个图优化的流程如下：

(1)定义顶点和边的类型
(2)构建图
(3)旋转优化算法
(4)调用g2o优化，返回结果

首先新建一个项目，然后在CMakeLists文件里引用g2o和相关库。

cmake_minimum_required(VERSION 2.6)
project(g2o_test)

set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 寻找G2O
include_directories( 
    ${G2O_INCLUDE_DIRS}
    "/usr/include/eigen3"
    "/usr/include/suitesparse"
)

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable(g2o_test main.cpp)

# 与G2O和OpenCV链接
target_link_libraries( g2o_test 
    ${OpenCV_LIBS}
    g2o_core g2o_stuff
)

install(TARGETS g2o_test RUNTIME DESTINATION bin)

然后编写代码文件:

#include <iostream>
#include <g2o/core/base_vertex.h>//顶点类型
#include <g2o/core/base_unary_edge.h>//一元边类型
#include <g2o/core/block_solver.h>//求解器的实现,主要来自choldmod, csparse
#include <g2o/core/optimization_algorithm_levenberg.h>//列文伯格－马夸尔特
#include <g2o/core/optimization_algorithm_gauss_newton.h>//高斯牛顿法
#include <g2o/core/optimization_algorithm_dogleg.h>//Dogleg（狗腿方法）
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>//矩阵库
#include <opencv2/core/core.hpp>//opencv2
#include <cmath>//数学库
#include <chrono>//时间库

using namespace std;
      
//定义曲线模型顶点，这也是我们的待优化变量
//这个顶点类继承于基础顶点类，基础顶点类是个模板类，模板参数表示优化变量的维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<4, Eigen::Vector4d>
{
public:
  //这在前面说过了，因为类中含有Eigen对象，为了防止内存不对齐，加上这句宏定义
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  
  //在基类中这是个=0的虚函数，所以必须重新定义
  //它用于重置顶点，例如全部重置为0
  virtual void setToOriginImpl()
  {
    //这里的_estimate是基类中的变量，直接用了
    _estimate << 0,0,0,0;
  }
  
  //这也是一个必须要重定义的虚函数
  //它的用途是对顶点进行更新，对应优化中X(k+1)=Xk+Dx
  //需要注意的是虽然这里的更新是个普通的加法，但并不是所有情况都是这样
  //例如相机的位姿，其本身没有加法运算
  //这时就需要我们自己定义"增量如何加到现有估计上"这样的算法了
  //这也就是为什么g2o没有为我们写好的原因
  virtual void oplusImpl( const double* update )
  {
    //注意这里的Vector4d，d是double的意思，f是float的意思
    _estimate += Eigen::Vector4d(update);
  }
  
  //虚函数  存盘和读盘：留空
  virtual bool read( istream& in ) {}
  virtual bool write( ostream& out ) const {}
};
      
      
//误差模型—— 曲线模型的边 
//模板参数：观测值维度(输入的参数维度)，类型，连接顶点类型(创建的顶点)   
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  double _x;
  
  //这种写法应该有一种似曾相识的感觉
  //在Ceres里定义代价函数结构体的时候，那里的构造函数也用了这种写法
  CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}

  //计算曲线模型误差 
  void computeError()
  {
    //顶点，用到了编译时类型检查
    //_vertices是基类中的成员变量
    const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]);
    //获取顶点的优化变量
    const Eigen::Vector4d abcd = v->estimate();
    //_error、_measurement都是基类中的成员变量
    _error(0,0) = _measurement - std::exp(abcd(0,0)*_x*_x*_x + abcd(1,0)*_x*_x + abcd(2,0)*_x + abcd(3,0));
  }
  
  //存盘和读盘：留空
  virtual bool read( istream& in ) {}
  virtual bool write( ostream& out ) const {}
};
      
int main( int argc, char** argv )
{
  //待估计函数为y=exp(3.5x^3+1.6x^2+0.3x+7.8)
  double a=3.5, b=1.6, c=0.3, d=7.8;
  int N=100;
  double w_sigma=1.0;
  cv::RNG rng;
  
  vector<double> x_data, y_data;
  
  cout<<"generating data: "<<endl;
  for (int i=0; i<N; i++)
  {
    double x = i/100.0;
    x_data.push_back (x);
    y_data.push_back (exp(a*x*x*x + b*x*x + c*x + d) + rng.gaussian (w_sigma));
    cout<<x_data[i]<<"\t"<<y_data[i]<<endl;
  }
  
  //构建图优化，先设定g2o
  //矩阵块：每个误差项优化变量维度为4，误差值维度为1
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<4,1>> Block;

  // Step1 选择一个线性方程求解器
  std::unique_ptr<Block::LinearSolverType> linearSolver (new g2o::LinearSolverDense<Block::PoseMatrixType>());
  // Step2 选择一个稀疏矩阵块求解器
  std::unique_ptr<Block> solver_ptr (new Block(std::move(linearSolver)));
  // Step3 选择一个梯度下降方法，从GN、LM、DogLeg中选
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));
  //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::move(solver_ptr));
  //g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(std::move(solver_ptr));

  //稀疏优化模型
  g2o::SparseOptimizer optimizer;
  //设置求解器
  optimizer.setAlgorithm(solver);
  //打开调试输出
  optimizer.setVerbose(true);
          
  //往图中增加顶点
  CurveFittingVertex* v = new CurveFittingVertex();
  //这里的(0,0,0,0)可以理解为顶点的初值了，不同的初值会导致迭代次数不同，可以试试
  v->setEstimate(Eigen::Vector4d(0,0,0,0));
  v->setId(0);
  optimizer.addVertex(v);

  //往图中增加边
  for (int i=0; i<N; i++)
  {
    //新建边带入观测数据
    CurveFittingEdge* edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    //设置连接的顶点，注意使用方式
    //这里第一个参数表示边连接的第几个节点(从0开始)，第二个参数是该节点的指针
    edge->setVertex(0, v);
    //观测数值
    edge->setMeasurement(y_data[i]);
    //信息矩阵：协方差矩阵之逆，这里各边权重相同。这里Eigen的Matrix其实也是模板类
    edge->setInformation(Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma));
    optimizer.addEdge(edge);
  }
  
  //执行优化
  cout<<"start optimization"<<endl;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//计时
  
  //初始化优化器
  optimizer.initializeOptimization();
  //优化次数
  optimizer.optimize(100);
  
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();//结束计时
  
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
  cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
          
  //输出优化值
  Eigen::Vector4d abc_estimate = v->estimate();
  cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
  
  return 0;
}

执行结果如下：

1. ORB-SLAM2 中优化的变量和误差

ORB-SLAM2 采用非线性优化的方式进行 BA 优化，由于 BA 的稀疏性（具体表现为雅克比矩阵和 H 矩阵的稀疏性），可以由图优化（将优化表示为图的形式，待优化的变量由顶点表示，误差项由边来表示）显式地形象化表示。在 ORB-SLAM2 中采用 g2o 库来进行图优化求解，需要我们做的就是建立优化问题、确定顶点和边。

1.1 优化的目标函数

三维点到二维特征的映射关系（通过投影矩阵）；
位姿和位姿之间的变换关系（通过三维刚体变换矩阵）；
二维特征到二维特征的匹配关系（通过 F 矩阵）；
其它关系（比如单目中有相似变换关系）。

1.2 需要优化的变量（g2o 的顶点）

ORB-SLAM2 中顶点有三个，SE(3) 相机位姿、地图点坐标、闭环检测时的 sim(3) 相机位姿；

在每帧的位姿优化 Optimizer::PoseOptimization() 函数中只将相机位姿作为顶点；
在 BA 优化 中将相机位姿和地图点两者作为顶点；
在闭环时将 sim(3) 相机位姿作为顶点。

① SE(3) 相机位姿 VertexSE3Expmap()

与前面自定义顶点的举例二中一样，即 g2o/types/types_six_dof_expmap.h 中 6 维李代数表示的 VertexSE3Expmap。

② 地图点坐标 VertexSBAPointXYZ()

与前面自定义顶点的举例三中一样，即 g2o/types/types_sba.h 中三维向量表示的 VertexSBAPointXYZ。

③ 闭环检测时的 sim(3) 相机位姿（归一化尺度） VertexSim3Expmap()

与 6 维李代数表示的相机为了多了一个尺度因子

/ BRIEF 7 维 Sim3 表示的相机位姿.（用于闭环），增加了一个尺度 s.
  class VertexSim3Expmap : public BaseVertex<7, Sim3>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    VertexSim3Expmap();

    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    // 重置函数，尺度因子 s 设置为 1.
    virtual void setToOriginImpl() {
      _estimate = Sim3();
    }

    // 顶点更新函数.
    virtual void oplusImpl(const double* update_)
    {
      Eigen::Map<Vector7d> update(const_cast<double*>(update_));

      if (_fix_scale)
        update[6] = 0;

      Sim3 s(update);
      setEstimate(s*estimate());
    }

    Vector2d _principle_point1, _principle_point2;
    Vector2d _focal_length1, _focal_length2;

    // 地图点投影到闭环帧的坐标？
    Vector2d cam_map1(const Vector2d & v) const
    {
      Vector2d res;
      res[0] = v[0]*_focal_length1[0] + _principle_point1[0];
      res[1] = v[1]*_focal_length1[1] + _principle_point1[1];
      return res;
    }

    // 地图点投影到当前帧的坐标？
    Vector2d cam_map2(const Vector2d & v) const
    {
      Vector2d res;
      res[0] = v[0]*_focal_length2[0] + _principle_point2[0];
      res[1] = v[1]*_focal_length2[1] + _principle_point2[1];
      return res;
    }

    bool _fix_scale;


  protected:
  };

1.3 误差来源（g2o 中的边）

在整个ORB-SLAM2系统中，没有自定义的顶点或边类型。在ORB-SLAM3里由于引入了IMU，所以有一些自定义边和节点，都放在了G2OTypes中，之后有空再介绍，这里就不涉及了。

① EdgeSE3ProjectXYZ()：BA 中的重投影误差（3D-2D（u,v）误差），将 3D 地图点投影到相机坐标系下的相机平面与观测的像素点产生的重投影误差 ；
② EdgeSE3ProjectXYZOnlyPose()：PoseEstimation 中的重投影误差，将地图点投影到相机坐标系下的相机平面。优化变量只有 pose，地图点位置固定，是一边元（双目中使用的是EdgeStereoSE3ProjectXYZOnlyPoze()）；
③ EdgeSim3()：Sim3 之间的相对误差，优化变量只有 Sim3 表示的 pose，用于 OptimizeEssentialGraph；
④ EdgeSim3ProjectXYZ()：重投影误差。优化变量 Sim3 位姿与地图点，用于闭环检测中的 OptimizeSim3。

1.3.1 位姿优化时的重投影误差

在 PoseOptimization() 仅优化相机位姿 时，重投影误差是一个一元边，顶点（优化变量）为相机的位姿。对应 g2o/types/types_six_dof_expmap.h 中定义的边 EdgeSE3ProjectXYZOnlyPose。

// BRIEF 一元边：重投影误差（仅用于优化相机位姿时）
class  EdgeSE3ProjectXYZOnlyPose: public  BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EdgeSE3ProjectXYZOnlyPose(){}

  bool read(std::istream& is);

  bool write(std::ostream& os) const;

  // 误差计算函数.
  void computeError()  
  {
    // 李代数表示的相机位姿 v1（顶点）.
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);

    // 观测值：特征点当前帧中的坐标.
    Vector2d obs(_measurement);

    // 重投影误差，.map() 是将地图点的世界坐标转换到相机坐标系，cam_project() 操作将其投影到当前帧.
    _error = obs-cam_project(v1->estimate().map(Xw));
  }

  // 检查地图点在相机坐标系中的深度是否为正.
  bool isDepthPositive() 
  {
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
    return (v1->estimate().map(Xw))(2)>0.0;
  }

  // 雅克比矩阵：重投影误差相对于相机位姿的一阶导.
  virtual void linearizeOplus();

  // 投影到当前帧.
  Vector2d cam_project(const Vector3d & trans_xyz) const;

  Vector3d Xw;  // 地图点的世界坐标.

  double fx, fy, cx, cy;  // 相机参数.
}; // 边：重投影误差（仅用于优化相机位姿时）.

其中 linearizeOplus() 函数构造雅克比矩阵，对应 g2o/types/types_six_dof_expmap.cpp 中的 void EdgeSE3ProjectXYZOnlyPose::linearizeOplus()

// BRIEF 优化相机位姿是重投影误差的雅克比矩阵（对相机位姿的偏导）
void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() 
{
  // 顶点：相机位姿.
  VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
  // 地图点的相机坐标.
  Vector3d xyz_trans = vi->estimate().map(Xw);

  double x = xyz_trans[0];
  double y = xyz_trans[1];
  double invz = 1.0/xyz_trans[2];
  double invz_2 = invz*invz;

  _jacobianOplusXi(0,0) =  x*y*invz_2 *fx;
  _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
  _jacobianOplusXi(0,2) = y*invz *fx;
  _jacobianOplusXi(0,3) = -invz *fx;
  _jacobianOplusXi(0,4) = 0;
  _jacobianOplusXi(0,5) = x*invz_2 *fx;

  _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
  _jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
  _jacobianOplusXi(1,2) = -x*invz *fy;
  _jacobianOplusXi(1,3) = 0;
  _jacobianOplusXi(1,4) = -invz *fy;
  _jacobianOplusXi(1,5) = y*invz_2 *fy;
} // 优化相机位姿是重投影误差的雅克比矩阵.

2 BA 优化时的重投影误差

在 BA 优化时优化的量包括相机位姿和地图点坐标，重投影误差是一个二元边，连接的顶点为李代数表示的相机位姿和三维向量表示的地图点坐标。对应 g2o/types/types_six_dof_expmap.h 中定义的边 EdgeSE3ProjectXYZ，其定义格式与前面自定义边中举例二的二元边一样。

// BRIEF 边：重投影误差（BA优化时）
class  EdgeSE3ProjectXYZ: public  BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EdgeSE3ProjectXYZ();

  bool read(std::istream& is);

  bool write(std::ostream& os) const;

  // 误差计算函数
  void computeError()  
  {
    // 顶点 v1：相机位姿，顶点的 id 为 1.
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
    // 顶点 v2：地图点，顶点的 id 为 0.
    const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
    // 观测值：特征点在当前帧中的像素坐标.
    Vector2d obs(_measurement);
    // 计算误差：观测-重投影.
    _error = obs-cam_project(v1->estimate().map(v2->estimate()));
  }

  // 检查地图点在相机坐标系中深度是否为正.
  bool isDepthPositive() 
  {
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
    const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
    return (v1->estimate().map(v2->estimate()))(2)>0.0;
  }

  // 构造雅克比矩阵：重投影误差相对于相机坐标和地图点的一阶导.
  virtual void linearizeOplus();

  // 投影到当前帧.
  Vector2d cam_project(const Vector3d & trans_xyz) const;

  double fx, fy, cx, cy;
}; // 边：重投影误差（BA优化时）.

其中 linearizeOplus() 函数用于构造雅克比矩阵，需要重投影误差对相机位姿和地图点求一阶偏导，对应 g2o/types/types_six_dof_expmap.cpp 中的 void EdgeSE3ProjectXYZ::linearizeOplus() 。

// BRIEF 重投影误差的雅克比矩阵（BA 优化时相对于相机位姿和地图点的一阶导）
void EdgeSE3ProjectXYZ::linearizeOplus() 
{
  VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
  SE3Quat T(vj->estimate());
  VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
  Vector3d xyz = vi->estimate();
  Vector3d xyz_trans = T.map(xyz);

  double x = xyz_trans[0];
  double y = xyz_trans[1];
  double z = xyz_trans[2];
  double z_2 = z*z;

  Matrix<double,2,3> tmp;
  tmp(0,0) = fx;
  tmp(0,1) = 0;
  tmp(0,2) = -x/z*fx;

  tmp(1,0) = 0;
  tmp(1,1) = fy;
  tmp(1,2) = -y/z*fy;

  _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

  _jacobianOplusXj(0,0) =  x*y/z_2 *fx;
  _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx;
  _jacobianOplusXj(0,2) = y/z *fx;
  _jacobianOplusXj(0,3) = -1./z *fx;
  _jacobianOplusXj(0,4) = 0;
  _jacobianOplusXj(0,5) = x/z_2 *fx;

  _jacobianOplusXj(1,0) = (1+y*y/z_2) *fy;
  _jacobianOplusXj(1,1) = -x*y/z_2 *fy;
  _jacobianOplusXj(1,2) = -x/z *fy;
  _jacobianOplusXj(1,3) = 0;
  _jacobianOplusXj(1,4) = -1./z *fy;
  _jacobianOplusXj(1,5) = y/z_2 *fy;
} // 重投影误差的雅克比矩阵（BA 优化时相对于相机位姿和地图点的一阶导）

.3 闭环检测时的重投影误差

闭环检测时执行 sim3 优化，边是重投影误差，顶点是 sim3 表示的相机位姿和地图点坐标，对应 g2o/types/types_seven_dof_expmap.h 中定义的边投影到当前帧 EdgeSim3ProjectXYZ 和投影到闭环帧 EdgeInverseSim3ProjectXYZ。

// BRIEF 边：重投影误差（闭环检测时）顶点：地图点坐标，sim3 表示的相机位姿.
class EdgeSim3ProjectXYZ : public  BaseBinaryEdge<2, Vector2d,  VertexSBAPointXYZ, VertexSim3Expmap>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeSim3ProjectXYZ();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    // 重投影误差计算.
    void computeError()
    {
      const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[1]);
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);

      Vector2d obs(_measurement);
      _error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate())));
    }

    // TODO 这里继承的 BaseBinaryEdge 雅克比矩阵构造函数？.
    // virtual void linearizeOplus();
};

4 Sim3 之间的相对误差

OptimizeEssentialGraph 会在成功进行闭环检测后、全局 BA 优化前进行，边为 Sim3 之间的相对误差，顶点为 Sim3 表示的相机位姿。对应 g2o/types/types_seven_dof_expmap.h 中定义的边 EdgeSim3。

/ BRIEF 边：Sim3 之间的相对误差，顶点：Sim3 表示的 pose。（OptimizeEssentialGraph 优化中）
  class EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeSim3();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    // 误差计算.
    void computeError()
    {
      const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[0]);
      const VertexSim3Expmap* v2 = static_cast<const VertexSim3Expmap*>(_vertices[1]);

      Sim3 C(_measurement);
      Sim3 error_=C*v1->estimate()*v2->estimate().inverse();
      _error = error_.log();
    }

    virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;}

    virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
    {
      VertexSim3Expmap* v1 = static_cast<VertexSim3Expmap*>(_vertices[0]);
      VertexSim3Expmap* v2 = static_cast<VertexSim3Expmap*>(_vertices[1]);
      if (from.count(v1) > 0)
        v2->setEstimate(measurement()*v1->estimate());
      else
        v1->setEstimate(measurement().inverse()*v2->estimate());
    }
  }; // 边：Sim3 之间的相对误差，顶点：Sim3 表示的 pose。（OptimizeEssentialGraph 优化中）

ORB-SLAM2 中的优化函数

SLAM系统中，一般的做法是通过P3P或者EPnP等方法进行投影估算出相机位姿，进而构建最小二乘对估计的相机位姿进行优化，从而得到精度比较高的位姿。优化完后可以剔除一些outlier点，这样的话后边再进行投影和优化的时候精度会有所保证。

1.位姿优化函数PoseOptimization

**一些说明：**PoseOptimization优化的是单帧图像的位姿。相机在运动的时候每个图像帧可以对应三维世界中多个地图点，那么此时我们可以知道三维世界中的多个3D地图点，也可以知道图像帧中对应的2D像素点坐标，就可以通过3D-2D的投影关系估计出相机的位姿。PoseOptimization就是对单个图像帧中的多个地图点建立多个一元连接边，构成图进行优化，优化单个图像帧的SE3位姿。

**调用时机：**当一个图像帧中的特征点和3D空间中多个地图点匹配，并且有一个“初始位姿”的时候，就可以通过位姿优化函数来对当前帧的位姿进行优化。ORB-SLAM系统当中在Tracking线程中多次调用了位姿优化函数，可以试想一下，毕竟Tracking线程的主要业务就是进行跟踪，一帧一帧图像的位姿描述了相机的轨迹。系统的前两帧图像的位姿都初始化为单位矩阵，在通过2D点匹配建立关联关系后，通过三角化创建了地图点，此时才可以对图像帧的位姿进行优化更新。只要3D-2D匹配关系发生变化，就需要对位姿进行优化。

ORB-SLAM系统中下面几个函数里边都调用了位姿优化函数：

1）Tracking::TrackReferenceKeyFrame()

2）Tracking::TrackWithMotionModel()

3）Tracking::TrackLocalMap()

4）Tracking::Relocalization()

函数描述：
- 在 Tracking 线程中进行位姿优化的时候，每进行过一次 PnP 投影操作将地图点投影到当前平面上之后，都会进行一次PoseOptimization 位姿优化最小化重投影误差；
- 3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw)；
- 只优化当前帧 pose，地图点固定；
- 用于 LocalTracking 中运动模型跟踪，参考帧跟踪，地图跟踪 TrackLocalMap，重定位。
顶点和边：

在本函数中，我们的目标是根据地图优化当前帧的位姿。因此我们把当前帧位姿变成一个节点、地图点的观测作为边(约束)。这里优化图中只有一个节点，因此，边就是一个一元边(Unary Edge)，也就是只连接到一个节点(或者理解为从自己指向自己)。

// 该优化函数主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位
/**
 * 3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw) 
 * 只优化Frame的Tcw，不优化MapPoints的坐标
 * 
1. Vertex: g2o::VertexSE3Expmap()，即当前帧的 Tcw
2. Edge: 重投影误差（一元边）
 - 单目情况：g2o::EdgeSE3ProjectXYZOnlyPose()，BaseUnaryEdge
     + Vertex：待优化当前帧的Tcw
     + measurement：MapPoint在当前帧中的二维位置(u,v)
     + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 - 双目情况：g2o::EdgeStereoSE3ProjectXYZOnlyPose()，BaseUnaryEdge
     + Vertex：待优化当前帧的Tcw
     + measurement：MapPoint在当前帧中的二维位置(ul,v,ur)
     + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 *
 * @param   pFrame Frame
 * @return  inliers数量
 */
int Optimizer::PoseOptimization(Frame *pFrame)

函数执行流程：

步骤一： 构造 g2o 优化器

如下图所示，函数中新建了一个稀疏优化器，并且将优化算法设置为Levenberg。这样对于优化器的基本配置就完成了。对这一块感兴趣的话可以参考这篇博客。

步骤二： 向优化器中添加顶点-当前帧的位姿

在这里，我们只需要构建一个节点，用于表示当前帧位姿，如下图所示。

可以看到，这里我们获取到了当前帧的位姿mTcw，将其赋给vSE3。我们设置当前节点的ID为0，可以看到，这里我们不固定节点(这样才能调整优化)。节点新建好以后，就将该节点添加到图中。

步骤三： 向优化器中添加边（以单目重投影误差为例）

前面说了，我们将对地图点的观测作为边(约束)，由于有多个地图点，所以我们构建vector来存放这些节点，如下所示。
- 这里可以看到，我们主要有两种类型的边，一种是单目边，另一种是双目边。由于PoseOptimization()函数会在单目、双目中调用，所以代码会根据不同的状态进行判断。什么算单目边或者双目边？如果是单目模式，自然只会有一种单目边。如果是双目模式，如果某个特征点双目匹配成功、有对应的地图点，那么这个地图点就会被构建双目边；如果左目影像中的特征点在右目影像中没有对应，那么就建立单目边。所以我们依次遍历地图点，构建边。
单目边构建

如下图所示，是构建单目边的代码。

可以看到，在代码中我们通过setVertex()函数将边与刚刚我们建立的节点关联起来。通过setMeasurement()函数设置地图点观测(误差计算其实就是和观测有关的)。然后，我们就可以将该地图边添加到优化图中。并且将这个边对象添加到vpEdgesMono中，并且将当前边对应的索引添加到vnIndexEdgeMono中。对于单目而言，我们构建的是g2o::EdgeSE3ProjectXYZOnlyPose类型的边，表达将地图点投影到相机坐标系下的相机平面。以下是具体步骤：

步骤 1： 实例化一个 EdgeSE3ProjectXYZOnlyPose 一元边
```
g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();
```
- 步骤 2： 设置边连接的顶点
```
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
```
- 步骤 3： 添加边的观测值（2D）
```
e->setMeasurement(obs);     // 观测：地图点在当前帧中的像素坐标.
```
- 步骤 4： 设置边的信息矩阵
```
const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
// note 设置权重（信息矩阵），与特征点金字塔有关，参考：https://www.zhihu.com/question/58762862
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);   // 信息矩阵（权重）
```
- 步骤 5： 设置核函数
- 步骤 6： 获取地图点的世界坐标（3D）
```
cv::Mat Xw = pMP->GetWorldPos();
e->Xw[0] = Xw.at<float>(0);
e->Xw[1] = Xw.at<float>(1);
e->Xw[2] = Xw.at<float>(2);
```
双目边构建

双目边其实是类似的，如下所示。

相比于单目的观测，双目的观测是一个三维向量。其它都是类似的。最后将这个边对象添加到优化图，并且将对象添加到vpEdgesStereo中、索引添加到vnIndexEdgeStereo中。构建的是g2o::EdgeStereoSE3ProjectXYZOnlyPose类型的边。
- 步骤 7： 将边添加到优化器中。
**步骤四：**开始优化，共优化 4 次，每次优化后将观测分为 outlier 和 inlier，outlier 不参与下次优化。

构造好优化图以后，就开始迭代优化，如下图所示。在ORB-SLAM系统中，可以看到，从大的层面来说，会执行4次优化，每次优化包含10次迭代。在每次优化后，都会分别针对单目边和双目边的结果对地图点的外点flag设为true或false。这个过程其实是很重要的，因为这直接决定了经过优化以后会保留多少内点。而内点的个数又会作为评判优化是否成功的唯一指标，返回给Tracking线程。所以如果没有异常的情况下，优化失败了，那么基本就是因为优化以后保留的内点个数太少。
- 但由于每次优化后是对所有的观测进行outlier和inlier判别，因此之前被判别为outlier有可能变成inlier，反之亦然。
- 基于卡方检验计算异常值的阈值（假设测量有一个像素的偏差）。
步骤五： 优化结束，用优化之后的位姿更新当前帧的位姿。

最后，返回优化以后的位姿结果给输入帧，结束优化，如下图所示。

步骤六：返回输出

执行完了优化以后，这个函数会输出什么呢？如果从“字面”上来看，会返回过滤后内点的个数，如下。但显然，以C++的习惯和这个函数的目的，并不会只有这一个返回值，一些返回值会直接赋给Frame对象。我们可以在函数中寻找，如下。

(1) pFrame->mvbOutlier[i] = false/true: 根据地图点边的误差大小判断该点是否属于外点，如果是的话就设为true，否则设为false
(2) pFrame->SetPose(pose): 将优化更新后的当前帧位姿重新赋给当前帧

所以可以看到，PoseOptimization()函数会范围三个东西：第一个是筛选以后地图内点的个数；第二个是哪些地图点是内点哪些是外点；第三个是更新以后的位姿。

2.优化两帧之间的位姿OptimizeSim3

**一些说明：**对单目相机来说具有尺度不确定性，当相机运动一段时间后不可避免会产生尺度漂移累积误差增大，当进行闭环检测的时候如果尺度不一致就不能进行很好的对应。相比于SE3来说，相似变换可以提供尺度的表达。那么这时候就可以借助相似变换群sim3来求解当前关键帧和闭环候选帧之间的sim3位姿，同时sim3位姿也可以和SE3进行转换。

要优化的两帧都能观测到多个相同的地图点，并且已经知道有哪些地图点是一致的。此时可以构造一个超定方程组，并对其求解最小化误差优化。优化帧间变化的SIM3位姿与地图的VertexSBAPointXYZ位姿。

**调用时机：**单目闭环检测中检测出闭环后，对当前关键帧和闭环帧之间计算sim3位姿，当计算出sim3位姿后，就需要调用sim3位姿优化函数。

ORB-SLAM当中在LoopClosing::ComputetSim3()中调用了OptimizeSim3函数。

/**
 * @brief 形成闭环时进行Sim3优化
 *
 * 1. Vertex:
 *     - g2o::VertexSim3Expmap()，两个关键帧的位姿
 *     - g2o::VertexSBAPointXYZ()，两个关键帧共有的MapPoints
 * 2. Edge:
 *     - g2o::EdgeSim3ProjectXYZ()，BaseBinaryEdge
 *         + Vertex：关键帧的Sim3，MapPoint的Pw
 *         + measurement：MapPoint在关键帧中的二维位置(u,v)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 *     - g2o::EdgeInverseSim3ProjectXYZ()，BaseBinaryEdge
 *         + Vertex：关键帧的Sim3，MapPoint的Pw
 *         + measurement：MapPoint在关键帧中的二维位置(u,v)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 *         
 * @param pKF1        KeyFrame
 * @param pKF2        KeyFrame
 * @param vpMatches1  两个关键帧的匹配关系
 * @param g2oS12      两个关键帧间的Sim3变换
 * @param th2         核函数阈值
 * @param bFixScale   是否优化尺度，弹目进行尺度优化，双目不进行尺度优化
 */
int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)

3.局部BA优化LocalBundleAdjustment（）

**一些说明：**局部BA优化，优化的是局部关键帧的位姿和这些局部关键帧可以观测到的地图点的3D坐标。相机在运动过程中，当前时刻的图像帧何其前后的若干个帧是能观测到一些相同的路标点的，也就是说这些帧之间有共视关系。这些共视帧和他们的所有地图点放在一块就体现了“局部”的意思。将局部的所有地图点与可以观测到他们的关键帧放在一起进行优化，将关键帧的SE3位姿和地图点的3D坐标作为待优化量添加到顶点当中。

**调用时机：**LocalMapping线程当中调用。LocalMapping线程中处理的就是当前关键帧和其共视关键帧、以及这些关键帧能够观测到的地图点之间的连接关系，当然少不了对局部关键帧位姿和地图点3D坐标的更新。

函数描述
- 执行条件：在已经处理完队列中的最后一个关键帧之后，并且闭环检测线程没有请求停止局部建图线程，则开始对当前帧进行局部 BA 优化，或者当新的关键帧加入到 convisibility graph 时，在关键帧附近进行一次局部优化，如下图所示；
  - Pos3 是新加入的关键帧，其初始估计位姿已经得到，此时，Pos2 是和 Pos3 相连的关键帧，X2 是 Pos3 看到的三维点，X1 是 Pos2 看到的三维点，这些都属于局部信息，共同参与Bundle Adjustment（圈内红色部分）；
  - 同时，Pos1 也可以看到 X1，但它和 Pos3 没有直接的联系，属于 Pos3 关联的局部信息，参与 Bundle Adjustment，但取值保持不变（圈内灰色部分）；
  - Pos0 和 X0 不参与 Bundle Adjustment（圈外灰色部分）。
  - 因此，参与优化的是上图中红色椭圆圈出的部分，其中红色代表取值会被优化，灰色代表取值保持不变。(u,v) 是 X 在 Pos 下的二维投影点，即 X 在 Pos 下的测量（measurement），优化的目标是让投影误差最小。

顶点与边

/**
 * @brief Local Bundle Adjustment
 *
 * 1. Vertex:
 *     - g2o::VertexSE3Expmap()，LocalKeyFrames，即当前关键帧的位姿、与当前关键帧相连的关键帧的位姿
 *     - g2o::VertexSE3Expmap()，FixedCameras，即能观测到LocalMapPoints的关键帧（并且不属于LocalKeyFrames）的位姿，在优化中这些关键帧的位姿不变
 *     - g2o::VertexSBAPointXYZ()，LocalMapPoints，即LocalKeyFrames能观测到的所有MapPoints的位置
 * 2. Edge:
 *     - g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
 *         + Vertex：关键帧的Tcw，MapPoint的Pw
 *         + measurement：MapPoint在关键帧中的二维位置(u,v)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 *     - g2o::EdgeStereoSE3ProjectXYZ()，BaseBinaryEdge
 *         + Vertex：关键帧的Tcw，MapPoint的Pw
 *         + measurement：MapPoint在关键帧中的二维位置(ul,v,ur)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 *         
 * @param pKF        KeyFrame
 * @param pbStopFlag 是否停止优化的标志
 * @param pMap       在优化后，更新状态时需要用到Map的互斥量mMutexMapUpdate
 */
void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)

函数执行流程

步骤一：构造局部关键帧列表
- 步骤 1：将当前帧加入到列表中
```
list<KeyFrame*> lLocalKeyFrames;
lLocalKeyFrames.push_back(pKF);
```
- 步骤 2：将与当前关键帧一级相连的关键帧加入到列表中
```
const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
```
步骤二：构造局部地图点列表。遍历前面的局部关键帧列表 lLocalKeyFrames，得到每个关键帧锁观测到的地图点，加入到局部地图点列表 lLocalMapPoints 中。
步骤三：构造能被局部地图点观测到的但不属于局部关键帧列表的关键帧 lFixedCameras 。这些关键帧参与局部 BA 但不改变其值。
步骤四：构造 g2o 优化器
步骤五：向优化器中添加顶点 VertexSE3Expmap - 局部关键帧。遍历局部关键帧序列 lLocalKeyFrames，将其创建为顶点，添加到优化器中

// 实例化一个 VertexSE3Expmap 顶点的对象.
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
      
// 设置顶点的关键帧位姿属性（SE3 形式）、ID 和固定的顶点.
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
vSE3->setFixed(pKFi->mnId==0); //第一帧位置固定
      
// 将顶点添加到的优化器中.
optimizer.addVertex(vSE3);

步骤六：向优化器中添加顶点 VertexSE3Expmap - 不修改值的关键帧。遍历前面构造的 lFixedCameras，分别对齐创建为顶点，注意 setFixed(true) 不改变其值。

g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
vSE3->setFixed(true);
optimizer.addVertex(vSE3);

步骤七：向优化器中添加顶点 VertexSBAPointXYZ - 地图点。遍历前面的局部地图点列表，依次创建顶点

// 实例化一个 VertexSBAPointXYZ 顶点.
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
// 设置顶点的位置、ID，边缘化.
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
int id = pMP->mnId+maxKFid+1;
vPoint->setId(id);
vPoint->setMarginalized(true);
// 将顶点添加到优化器中.
optimizer.addVertex(vPoint);

步骤八：为每一对关联的地图点和关键帧创建边（与上一步构建地图点顶点一起创建，以单目为例）。

步骤 1：实例化一条二元边 EdgeSE3ProjectXYZ

g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();

步骤 2：为边添加关联的顶点

e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));// 地图点 ID.
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));// 关键帧 ID.

步骤 3：添加测量值

e->setMeasurement(obs);     // 测量：地图点在当前帧中的二维位置.

步骤 4：构造信息矩阵

const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; // 与特征点所在的尺度有关.
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);   // 设置信息矩阵.

步骤 5：设置核函数
步骤 6：将边添加到优化器中

步骤九：开始执行优化，迭代 5 次

optimizer.initializeOptimization();
optimizer.optimize(5);      //

步骤十：检测 outlier，并设置下次不优化，同前面位姿优化时一样采用卡方检验计算阈值。

步骤十一：排除误差较大的 outlier 后再次优化，迭代 10 次

optimizer.initializeOptimization(0);
optimizer.optimize(10);

步骤十二：在优化后重新计算误差，剔除连接误差比较大的关键帧和地图点。

步骤十三：优化后更新关键帧位姿以及地图点坐标、平均观测方向等属性。

4.本质图优化OptimizeEssentialGraph

**一些说明：**函数名称是OptimizeEssentialGraph，那么只有形成了EssentialGraph之后才需要进行优化。EssentialGraph中不但有一般的共视关键帧之间的边，还有闭环边和共视图边。所以其中加入了帧间闭环的考虑，也加入了纠正后的帧的SIM3位姿。所有共视的关键帧、地图点、一般的边和闭环边放在一起进行优化，优化的目标是关键帧的sim3位姿和地图点的坐标。将优化后的sim3位姿转换为SE3，就得到了相机的位姿，并对地图点的坐标进行投影得到更新后的坐标。

**调用时机：**LoopClosing线程当中通过sim3调整完关键帧和其闭环帧之间位姿后调用。

/**
 * @brief 闭环检测后，EssentialGraph优化
 *
 * 1. Vertex:
 *     - g2o::VertexSim3Expmap，Essential graph中关键帧的位姿
 * 2. Edge:
 *     - g2o::EdgeSim3()，BaseBinaryEdge
 *         + Vertex：关键帧的Tcw，MapPoint的Pw
 *         + measurement：经过CorrectLoop函数步骤2，Sim3传播校正后的位姿
 *         + InfoMatrix: 单位矩阵     
 *
 * @param pMap               全局地图
 * @param pLoopKF            闭环匹配上的关键帧
 * @param pCurKF             当前关键帧
 * @param NonCorrectedSim3   未经过Sim3传播调整过的关键帧位姿
 * @param CorrectedSim3      经过Sim3传播调整过的关键帧位姿
 * @param LoopConnections    因闭环时MapPoints调整而新生成的边
 */
void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
                                       const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
                                       const LoopClosing::KeyFrameAndPose &CorrectedSim3,
                                       const map<KeyFrame *, set<KeyFrame *> > &LoopConnections, const bool &bFixScale)

5.全局BA优化GlobalBundleAdjustment

**一些说明：**这个是ORB-SLAM系统当中最大的优化，将全局地图当中所有的关键帧和地图点都放进来一起进行优化。对关键帧的位姿和地图点3D坐标进行优化。

**调用时机：**LoopClosing线程当中所有工作完成后创建线程进行整体优化。函数调用关系为：RunGlobalBundleAdjustment–>Optimizer::GlobalBundleAdjustemnt–>BundleAdjustment

// pMap中所有的MapPoints和关键帧做bundle adjustment优化
// 这个全局BA优化在本程序中有两个地方使用：
// a.单目初始化：CreateInitialMapMonocular函数
// b.闭环优化：RunGlobalBundleAdjustment函数
/**
 * @brief bundle adjustment Optimization
 * 
 * 3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw) \n
 * 
 * 1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw
 *            g2o::VertexSBAPointXYZ()，MapPoint的mWorldPos
 * 2. Edge:
 *     - g2o::EdgeSE3ProjectXYZ()，BaseBinaryEdge
 *         + Vertex：待优化当前帧的Tcw
 *         + Vertex：待优化MapPoint的mWorldPos
 *         + measurement：MapPoint在当前帧中的二维位置(u,v)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 *         
 * @param   vpKFs    关键帧 
 *          vpMP     MapPoints
 *          nIterations 迭代次数（20次）
 *          pbStopFlag  是否强制暂停
 *          nLoopKF  关键帧的个数
 *          bRobust  是否使用核函数
 */
void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,
                                 int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)

参考链接：

https://wym.netlify.app/2019-07-05-orb-slam2-optimization2/
https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486858&idx=1&sn=ce458d5eb6b1ad11b065d71899e31a04&chksm=97d7e81da0a0610b1e3e12415b6de1501329920c3074ab5b48e759edbb33d264a73f1a9f9faf&scene=21#wechat_redirect
https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247487082&idx=1&sn=d4a27e4c9a76760fffb571f57f4f7719&chksm=97d7ebfda0a062eba412877e9ecf5933f2051f0210c0d56f03267985512d97f2db434ab7356c&scene=178&cur_album_id=1361700104461467649#rd
https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247486992&idx=1&sn=ecb7c3ef9bd968e51914c2f5b767428d&chksm=97d7eb87a0a062912a9db9fb16a08129f373791fd3918952342d5db46c0bc4880326a7933671&scene=178&cur_album_id=1361700104461467649#rd
http://zhaoxuhui.top/blog/2022/01/08/orb-slam3-note-11-optimization-in-front-end.html
http://zhaoxuhui.top/blog/2018/04/10/g2o&bundle_adjustment.html