G2O优化

r囧r小猫 2023-03-04 09:21 67阅读 0赞

### G2O优化 ###

*   *  0.引言
     *   *  0.1.参考
         *  0.1.G2O的解决问题
         *  0.2.G2O整体框架
     *  1. 基本使用
     *   *  1.1.构造 g2o 模型
         *  1.2.g2o 类图
     *  2.g2o 的顶点（Vertex）
     *   *  2.1.顶点的格式
         *  2.2.自定义顶点
         *  2.3.将顶点添加到图中
     *  3.g2o 中的边
     *   *  3.1.边的格式
         *  3.2.自定义边
         *  3.3.将边添加到图中
     *  4.ORB-SLAM2 中的顶点
     *   *  4.1.相机位姿顶点
         *  4.2.地图点坐标顶点
         *  4.3.闭环时的 sim3 相机位姿
     *  5. ORB-SLAM2 中的边
     *   *  5.1.位姿优化时的重投影误差
         *  5.2.BA 优化时的重投影误差
         *  5.3.闭环检测时的重投影误差
         *  5.4.Sim3 之间的相对误差
     *  6.ORB-SLAM2 中的优化函数
     *   *  6.1.位姿优化函数 PoseOptimization()
         *  6.2.局部优化 LocalBundleAdjustment()
         *  6.3.全局优化 GlobalBundleAdjustemnt()
         *  6.4.闭环处的 Sim3 优化 OptimizeSim3()
         *  6.5.闭环后的 Sim3 位姿优化 OptimizeEssentialGraph()

## 0.引言 ##

### 0.1.参考 ###

*  [ORB-SLAM2 代码解读（三）：优化 2（详解 + g2o 使用）][ORB-SLAM2 _ 2_ _ g2o].
 *  [深入理解图优化与g2o：g2o篇][g2o_g2o]
 *  [G2O图优化基础和SLAM的Bundle Adjustment(光束法平差)][G2O_SLAM_Bundle Adjustment].
 *  [图优化和g2o][g2o].
 *  [从零开始一起学习SLAM | 掌握g2o边的代码套路][SLAM _ _g2o].

本篇基于高博的系列文章实践练习.绝大部分搬运自参考博客一。参考博客一写得太好了，以作备忘！博客一博主为东北大学硕士，[github主页][github],学习态度值得学习！参考博客三中对G2O源码进行了一些分析，对我的帮助也很大。在练习过程中自己想将优化结果打印出来查看，捣鼓了很久在博客三中找到了答案：

> (5)static\_cast关键字`static_cast <type-id> (expression)`  
> 　　static\_cast叫做编译时类型检查。该运算符把expression转换为type-id类型，这样做的目的是在程序还没有运行时进行类型检查，来保证转换的安全性。  
> 　　而与之对应的，dynamic\_cast关键字（运行时类型检查）主要用于类层次结构中父类和子类之间指针和引用的转换，由于具有运行时类型检查，因此可以保证下行转换的安全性。即转换成功就返回转换后的正确类型指针，如果转换失败，则返回NULL。  
> 　　所谓上行转换是指由子类指针到父类指针的转换，下行转换是指由父类指针到子类指针的转换。

//打印优化后的相机姿态
    for(int i = 0;i<globalOptimizer.vertices().size() -1;i++){
        
       g2o::VertexSE3* vertex = dynamic_cast<g2o::VertexSE3*>(globalOptimizer.vertex(i));
       Eigen::Isometry3d pose = vertex->estimate(); //该帧优化后的位姿
       cout << "current is "<<i<<" T=\n" << pose.matrix() << endl;
    }

### 0.1.G2O的解决问题 ###

G2O本质是求解的非线性问题，可以求解BA问题。有如下最小二乘系统，对应图模型如图.

ξ = argmin ⁡ ξ 1 2 ∑ i ∥ r i ∥ Σ i 2 \\boldsymbol\{\\xi\}=\\underset\{\\boldsymbol\{\\xi\}\}\{\\operatorname\{argmin\}\} \\frac\{1\}\{2\} \\sum\_\{i\}\\left\\|\\mathbf\{r\}\_\{i\}\\right\\|\_\{\\boldsymbol\{\\Sigma\}\_\{i\}\}^\{2\} ξ=ξargmin21i∑∥ri∥Σi2  
![在这里插入图片描述][20200803104304250.png_pic_center]  
对应的高斯牛顿求解，normal equation：  
 J ⊤ Σ − 1 J ⏟ H or Λ δ ξ = − J ⊤ Σ − 1 r ⏟ b \\underbrace\{\\mathbf\{J\}^\{\\top\} \\boldsymbol\{\\Sigma\}^\{-1\} \\mathbf\{J\}\}\_\{\\mathbf\{H\} \\text \{ or \} \\mathbf\{\\Lambda\}\} \\delta \\boldsymbol\{\\xi\}=\\underbrace\{-\\mathbf\{J\}^\{\\top\} \\boldsymbol\{\\Sigma\}^\{-1\} \\mathbf\{r\}\}\_\{\\mathbf\{b\}\} H or ΛJ⊤Σ−1Jδξ=b−J⊤Σ−1r

连加形式：  
 ∑ i = 1 n J i ⊤ Σ i − 1 J i δ ξ = − ∑ i = 1 n J ⊤ Σ i − 1 r \\sum\_\{i=1\}^\{n\} \\mathbf\{J\}\_\{i\}^\{\\top\} \\boldsymbol\{\\Sigma\}\_\{i\}^\{-1\} \\mathbf\{J\}\_\{i\} \\delta \\boldsymbol\{\\xi\}=-\\sum\_\{i=1\}^\{n\} \\mathbf\{J\}^\{\\top\} \\boldsymbol\{\\Sigma\}\_\{i\}^\{-1\} \\mathbf\{r\} i=1∑nJi⊤Σi−1Jiδξ=−i=1∑nJ⊤Σi−1r

### 0.2.G2O整体框架 ###

使用G2O求解：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ZiXzk0MTIxOQ_size_16_color_FFFFFF_t_70_pic_center]  
![在这里插入图片描述][20200803104627532.png_pic_center]  
在SLAM中防止乱飘，会固定第一帧，当然也有其他方式防止漂移。在高博RGBD系列博客中，固定第一帧不做优化来防止漂移：

// 向globalOptimizer增加第一个顶点
    // The parameterization for the increments constructed is a 6d vector (x,y,z,qx,qy,qz)
    g2o::VertexSE3* v = new g2o::VertexSE3();//顶点：相机位姿+ID
    v->setId( 0 );
    v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵
    v->setFixed( true ); //第一个顶点固定，不用优化
    globalOptimizer.addVertex( v );

## 1. 基本使用 ##

### 1.1.构造 g2o 模型 ###

首先构造 g2o 模型，包括选择线性方程求解器、矩阵求解器和下降算法；

// 分别对应图中的前四个步骤
    // step.4.1.设置图模型创建优化器.
    g2o::SparseOptimizer optimizer;
    // step.1.使用Cholmod中的线性方程求解器得到 linearSolver
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
    // step.2.再用稀疏矩阵块求解器 solver_ptr
    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
    // step.3.选择梯度下降方法：L-M 算法 求解上面的 solver_ptr 得到 solver.
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    // step.4.2.设置求解器.
    optimizer.setAlgorithm(solver);
    // 打开调试输出
    optimizer.setVerbose( true );

### 1.2.g2o 类图 ###

先给出前面初始化的代码再看类图吧，直接看图一脸懵，参考[高博博客 ][g2o_g2o].

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ZiXzk0MTIxOQ_size_16_color_FFFFFF_t_70_pic_center]  
上图中左侧的 `SparseOptimizer` 是第一步初始化的图模型，也是最终要维护的；

先看下半部分，对应前面的初始化代码，用于指定求解器和迭代算法，一个 `SparseOptimizer` 拥有一个`Optimization Algorithm` ，其优化算法继承自 `Gauss-Newton, Levernberg-Marquardt, Powell's dogleg` 三者之一，不同的方法表现为最终的 H 矩阵构造不同；同时这个 `Optimization Algorithm` 拥有一个 `Solver` ，其包含两个部分：

*  一个是 `SparseBlockMatrix` ，用于计算稀疏的雅克比和海森矩阵；
 *  另一个是构造线性方程求解器，可以从`PCG, CSparse, Choldmod` 三者选则，用于计算迭代过程中最关键的一步  
     H Δ x = − b H \\Delta x=-b HΔx=−b

然后看上半部分 `SparseOptimizer` 是一个 `Optimizable Graph` ，从而也是一个 `Hyper Graph`（超图）， 一个`SparseOptimizer`含有很多个顶点和很多个边；

*  顶点继承自 `Base Vertex`，也就是 `OptimizableGraph::Vertex`；
 *  边继承自 `OptimizableGraph::Edge`，又分为 `BaseUnaryEdge`（单边）, `BaseBinaryEdge`（双边）或 `BaseMultiEdge`（多边）；
 *  这些 `Base Vertex`和 `Base Edge` 都是抽象的基类，而实际用的顶点和边，都是它们的派生类；
 *  用 `SparseOptimizer.addVertex()` 和 `SparseOptimizer.addEdge()`向一个图中添加顶点和边，最后调用 `SparseOptimizer.optimize()` 完成优化。

综上所述，在 g2o 中选择优化方法一共有三个步骤

*  步骤一： 选择线性方程  H Δ x = − b H \\Delta x=-b HΔx=−b的线性方程求解器，完全采用第三方的线性代数库，主要采用 Cholesky 分解和 PCG 迭代，具体可以选择的有Cholmod，CSparse （以上两者为比较著名的线性代数库），PCG （pre-conditioner is block Jacobi），Dense（dense Cholesky decomposition），或者 ORB-SLAM中使用的Eigen（sparse Cholesky decoposition from Eigen）
 *  步骤二： 块求解器 BlockSolver 构造线性方程求解器所需要的**矩阵块（H 和 b）**，需要用到边的雅克比 H = J T W J , b = J T W δ H=J^\{T\} W J, \\quad b=J^\{T\} W \\delta H=JTWJ,b=JTWδ  
    需要指定优化变量的维度，常见的有以下几种，其中 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>;

*  步骤三： 选择下降迭代策略，从 GN, LM, Doglog 中选择。

**结合前面 1.1 节，总结一下 g2o 的代码流程**

*  步骤一： 创建线性方程求解器，确定分解方法

// 每个误差项优化变量维度为3，误差值维度为1
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  
    // 创建一个线性求解器 LinearSolver，采用 dense cholesky 分解法
    Block::LinearSolverType* linearSolver 
        = new g2o::LinearSolverDense<Block::PoseMatrixType>();

要求解的增量方程为  H Δ x = − b H \\Delta x=-b HΔx=−b，通常想到的方法就是直接求逆 Δ x = − H − 1 ∗ b \\Delta x=-H^\{-1\} \* b Δx=−H−1∗b，对于较小维度的 H 矩阵可以直接求逆，但当 H 维度较大时采用下面的方法进行求逆：

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

*  步骤二： 构造线性方程的矩阵块，并用上面定义的线性求解器初始化

Block* solver_ptr = new Block( linearSolver );

BlockSolver 内部包含 LinearSolver，用上面定义的线性求解器 LinearSolver 来初始化，前面已经给定了优化变量的维度；

*  步骤三： 创建总求解器 solver，并从 GN, LM, DogLeg 中选一个，再用上述块求解器 BlockSolver 初始化

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

优化算法

g2o::OptimizationAlgorithmGaussNewton
    g2o::OptimizationAlgorithmLevenberg 
    g2o::OptimizationAlgorithmDogleg

*  步骤四： 创建稀疏优化器（SparseOptimizer）

g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 用前面定义好的求解器作为求解方法：
    optimizer.setVerbose( true );       // 打开调试输出

*  步骤五： 定义图的顶点和边，并添加到 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] );
        // 设置边的 id
        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 );
    }

*  步骤六： 设置优化参数，开始执行优化

optimizer.initializeOptimization();
    optimizer.optimize(100); // 迭代次数

## 2.g2o 的顶点（Vertex） ##

### 2.1.顶点的格式 ###

回到前面的类图上面的顶点部分的箭头，g2o 提供了一个比较通用的适合大多数情况的模板类 BaseVertex<D, T>（定义在 g2o/core/base\_vertex.h 中），其继承自OptimizableGraph::Vertex（定义在 g2o/core/optimizable\_graph.h 中），OptimizableGraph 又继承自 HyperGraph::Vertex（定义在 g2o/core/hyper\_graph.h 中），后两者都比较底层，一般使用第一个。

*  再来看看 BaseVertex<D, T> 类的模板参数 D，T

D 是 int 类型，表示顶点 Vertex 的最小维度，比如 3D 空间中旋转是 3 维的，那么这里 D=3；  
在源码注释中说 D 并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示(SO3->so3,SE3->se3).

static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space

T 是待估计 Vertex 的数据类型，比如用四元数表达三维旋转的话，T 就是 Quaternion 类型

typedef T EstimateType;
    EstimateType _estimate;

g2o 提供的常用的顶点类型，若没有的类型可参考自己定义

VertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta)
    VertexSE3 : public BaseVertex<6, SE3Quat>
    VertexSE3 : public BaseVertex<6, Isometry3>
    VertexPointXY : public BaseVertex<2, Eigen::Vector2d>
    VertexPointXYZ : public BaseVertex<3, Vector3>
    VertexSBAPointXYZ : public BaseVertex<3, Vector3d>//常用
    VertexLine2D : public BaseVertex<2, Line2D>
    VertexLine3D : public BaseVertex<4, Line3D>
    VertexSegment2D : public BaseVertex<4, Vector4>
    VertexCircle : public g2o::BaseVertex<3, Eigen::Vector3d>
    VertexPlane : public BaseVertex<3, Plane3D>
    VertexCam : public BaseVertex<6, SBACam>
    VertexPosition3D : public g2o::BaseVertex<3, Eigen::Vector3d>
    VertexPositionVelocity3D : public g2o::BaseVertex<6, Vector6d>
    VertexOdomDifferentialParams: public BaseVertex <3, Vector3>
    VertexCameraBAL : public BaseVertex<9, Eigen::VectorXd>
    VertexSim3Expmap : public BaseVertex<7, Sim3> //常用
    VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d>
    VertexSE3Expmap : public BaseVertex<6, SE3Quat> //常用
    VertexBaseline : public BaseVertex<1, double>

高博RGBD系列文章中：

//edge====继承自===>BaseBinaryEdge<6, Isometry3, VertexSE3, VertexSE3>
    //D：测量值的维度；E：测量值的数据类型；VertexSE3：顶点类型
    g2o::EdgeSE3* edge = new g2o::EdgeSE3();
    // 连接此边的两个顶点id
    edge->vertices() [0] = globalOptimizer.vertex( i-1 );
    edge->vertices() [1] = globalOptimizer.vertex( i );

顶点主要的成员函数（位于 g2o/core/base\_vertex.h 中）

// 返回优化之后顶点的值.
    const EstimateType& estimate() const {
         return _estimate;}

### 2.2.自定义顶点 ###

自定义一个顶点需要重写以下函数

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

*  read() 和 write() 读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅声明一下就可以了；
 *  setToOriginImpl() 是顶点重置函数，设置被优化变量的初始值；
 *  oplusImpl() 是顶点更新函数，主要用于优化过程中增量  Δ x \\Delta x Δ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 {
        }
    };

**举例二： 以李代数表示的位姿作为顶点，位于 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());
      }
    };

**举例三： 以三维向量表示的三维点作为顶点，位于 g2o/types/types\_sba.h 中**

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

class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>
    {
        
      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 double* update)
        {
        
          Eigen::Map<const Vector3d> v(update);
          _estimate += v;
        }
    };

### 2.3.将顶点添加到图中 ###

步骤：

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

**举例一：曲线拟合**

CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );

**举例二：三维坐标点**

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

## 3.g2o 中的边 ##

### 3.1.边的格式 ###

还是回到前面的类图，关注右上角的边部分，我们一般使用的类是 `BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge` 分别表示一元边，两元边，多元边（位于 g2o/g2o/core/base\_edge.h 中），类似于顶点，他们又继承自OptimizableGraph::Edge （位于 g2o/g2o/core/optimizable\_graph.h 中），hyper\_graph::Edge（位于g2o/g2o/core/hyper\_graph.h 中）。  
    
  一元边表示只连接一个顶点，二元边表示连接两个顶点，多元边表示连接 3 个或以上顶点。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ZiXzk0MTIxOQ_size_16_color_FFFFFF_t_70_pic_center 1]

**主要参数有：D, E, VertexXi, VertexXj**

*  D 是 int 型，表示测量值的维度；
 *  E 表示测量值的数据类型；
 *  VertexXi，VertexXj 分别表示不同顶点的类型。

**举例：用边表示三维点投影到图像平面的重投影误差**

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

*  首先顶点有两个是个，这是一个二元边；
 *  误差（测量值）的维度为 2，也就是像素坐标 u,v 的差值；
 *  误差（测量值）的数据类型为二维向量 Vector2D；
 *  两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ 和李群表示的相机位姿VertexSE3Expmap。

### 3.2.自定义边 ###

自定义边需要重写以下成员函数

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() 来定义协方差矩阵的逆

自定义边的格式：类似于定义顶点，但重点是 computeError()，linearizeOplus() 两个函数，也更复杂

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 中）
 *  cam->cam\_map(v1->estimate().map(v2->estimate())) 是利用相机内参将地图点从相机坐标系转换到图像坐标系。(cam\_map 操作定义在 g2o/types/sba/types\_six\_dof\_expmap.cpp 中）

关于雅克比矩阵的计算函数 linearizeOplus()：误差（观测相机方程）关于相机位姿和三维点的偏导数.  
 ∂ e ∂ δ ξ = − \[ f x Z ′ 0 − f x X ′ Z ′ 2 − f x X ′ Y ′ Z ′ 2 f x + f x X 2 Z ′ 2 − f x Y ′ Z ′ 0 f y Z ′ − f y Y ′ Z ′ 2 − f y − f y Y ′ 2 Z ′ 2 f y X ′ Y ′ Z ′ 2 f y X ′ Z ′ \] \\frac\{\\partial \\boldsymbol\{e\}\}\{\\partial \\delta \\boldsymbol\{\\xi\}\}=-\\left\[\\begin\{array\}\{ccccc\} \\frac\{f\_\{x\}\}\{Z^\{\\prime\}\} & 0 & -\\frac\{f\_\{x\} X^\{\\prime\}\}\{Z^\{\\prime 2\}\} & -\\frac\{f\_\{x\} X^\{\\prime\} Y^\{\\prime\}\}\{Z^\{\\prime 2\}\} & f\_\{x\}+\\frac\{f\_\{x\} X^\{2\}\}\{Z^\{\\prime 2\}\} & -\\frac\{f\_\{x\} Y^\{\\prime\}\}\{Z^\{\\prime\}\} \\\\ 0 & \\frac\{f\_\{y\}\}\{Z^\{\\prime\}\} & -\\frac\{f\_\{y\} Y^\{\\prime\}\}\{Z^\{\\prime 2\}\} & -f\_\{y\}-\\frac\{f\_\{y\} Y^\{\\prime 2\}\}\{Z^\{\\prime 2\}\} & \\frac\{f\_\{y\} X^\{\\prime\} Y^\{\\prime\}\}\{Z^\{\\prime 2\}\} & \\frac\{f\_\{y\} X^\{\\prime\}\}\{Z^\{\\prime\}\} \\end\{array\}\\right\] ∂δξ∂e=−\[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′−Z′2fxX′Y′−fy−Z′2fyY′2fx\+Z′2fxX2Z′2fyX′Y′−Z′fxY′Z′fyX′\] ∂ e ∂ P = ∂ e ∂ P ′ ∂ P ′ ∂ P \\frac\{\\partial e\}\{\\partial \\boldsymbol\{P\}\}=\\frac\{\\partial \\boldsymbol\{e\}\}\{\\partial \\boldsymbol\{P\}^\{\\prime\}\} \\frac\{\\partial \\boldsymbol\{P\}^\{\\prime\}\}\{\\partial \\boldsymbol\{P\}\} ∂P∂e=∂P′∂e∂P∂P′

上式分别对应下面代码中的 `_jacobianOplusXj` 和 `_jacobianOplusXi`（代码位于2o/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;
    }

### 3.3.将边添加到图中 ###

**举例一：一元边.**  
以曲线拟合为例（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 );
    }

**举例二：二元边。**  
位于 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]);

## 4.ORB-SLAM2 中的顶点 ##

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

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

### 4.1.相机位姿顶点 ###

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

### 4.2.地图点坐标顶点 ###

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

### 4.3.闭环时的 sim3 相机位姿 ###

与 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:
      };

## 5. ORB-SLAM2 中的边 ##

### 5.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;
    } // 优化相机位姿是重投影误差的雅克比矩阵.

### 5.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 优化时相对于相机位姿和地图点的一阶导）

### 5.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();
    };

### 5.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 优化中）

## 6.ORB-SLAM2 中的优化函数 ##

### 6.1.位姿优化函数 PoseOptimization() ###

函数描述：

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

顶点和边：

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(与特征点所在的尺度有关)

函数执行流程：

*  步骤一： 构造 g2o 优化器
 *  步骤二： 向优化器中添加顶点-当前帧的位姿
 *  步骤三： 向优化器中添加边（以单目重投影误差为例）

//步骤 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);
    //步骤 7： 将边添加到优化器中。

*  步骤四：开始优化，共优化 4 次，每次优化后将观测分为 outlier 和 inlier，outlier 不参与下次优化。  
    (1)但由于每次优化后是对所有的观测进行outlier和inlier判别，因此之前被判别为outlier有可能变成inlier，反之亦然。  
    (2)基于卡方检验计算异常值的阈值（假设测量有一个像素的偏差）。
 *  步骤五： 优化结束，用优化之后的位姿更新当前帧的位姿。

### 6.2.局部优化 LocalBundleAdjustment() ###

**函数描述:**

执行条件：在已经处理完队列中的最后一个关键帧之后，并且闭环检测线程没有请求停止局部建图线程，则开始对当前帧进行局部 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），优化的目标是让投影误差最小。

顶点与边

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(与特征点所在的尺度有关)

**函数执行流程**

*  步骤一：构造局部关键帧列表

//步骤 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);      // 迭代 5 次.

*  步骤十：检测 outlier，并设置下次不优化，同前面位姿优化时一样采用卡方检验计算阈值。
 *  步骤十一：排除误差较大的 outlier 后再次优化，迭代 10 次

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

*  步骤十二：在优化后重新计算误差，剔除连接误差比较大的关键帧和地图点。
 *  步骤十三：优化后更新关键帧位姿以及地图点坐标、平均观测方向等属性。

### 6.3.全局优化 GlobalBundleAdjustemnt() ###

**函数描述**  
全局 BA 优化在单目初始化和闭环矫正时执行。在全局优化中，所有的关键帧（除了第一帧）和三维点都参与优化，如下图所示：  
顶点与边

3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw)
    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(与特征点所在的尺度有关)

**函数执行流程**

*  步骤一：构造 g2o 优化器。
 *  步骤二：向优化器中添加顶点 VertexSE3Expmap - 关键帧的位姿。 遍历系统关键帧列表中的所有关键帧，依次添加顶点。

g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
    vSE3->setId(pKF->mnId);
    vSE3->setFixed(pKF->mnId==0); // 固定第一帧的位姿，其它帧可修正位姿
    optimizer.addVertex(vSE3);

*  步骤三：向优化器中添加顶点 VertexSBAPointXYZ - 地图点坐标。遍历所有关键帧观察到的地图点，依次添加为顶点

g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
    vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
    const int id = pMP->mnId+maxKFid+1;
    vPoint->setId(id);
    vPoint->setMarginalized(true);
    optimizer.addVertex(vPoint);

*  步骤四：向优化器中添加边 EdgeSE3ProjectXYZ - 重投影误差。

//步骤 1：实例化一条边
    g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
    //步骤 2：添加顶点
    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
    e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
    //步骤 3：添加测量值
    Eigen::Matrix<double,2,1> obs;
    obs << kpUn.pt.x, kpUn.pt.y;
    e->setMeasurement(obs);
    //步骤 4：设置信息矩阵
    const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
    e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
    //步骤 5：设置核函数
    //步骤 6：添加到优化器中

*  步骤五：开始优化，迭代 20 次。
 *  步骤六：更新优化结果（关键帧位姿、地图点坐标）。

### 6.4.闭环处的 Sim3 优化 OptimizeSim3() ###

**函数描述**

*  在用 RANSAC 求解过 Sim3，以及通过 Sim3 匹配更多的地图点后，对当前关键帧，闭环关键帧，以及匹配的地图点进行优化，获得更准确的 Sim3 变换，再进行下一步的闭环调整；
 *  当检测到闭环时，闭环连接的两个关键帧的位姿需要通过 Sim3 优化（以使得其尺度一致），在函数 Optimizer::OptimizeSim3() 中实现，优化求解两帧之间的相似变换矩阵，使得二维对应点的投影误差最小；
 *  如下图所示，Pos6 和 Pos2 为一个可能的闭环，则通过  ( u 4 , 2 , v 4 , 2 ) \\left(u\_\{4,2\}, v\_\{4,2\}\\right) (u4,2,v4,2)和 ( u 4 , 6 , v 4 , 6 ) \\left(u\_\{4,6\}, v\_\{4,6\}\\right) (u4,6,v4,6) 之间的投影误差来优化  S 6 , 2 S\_\{6,2\} S6,2。

**顶点与边:**

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(与特征点所在的尺度有关)

**函数执行流程**

*  步骤一： 初始化 g2o 优化器；
 *  步骤二： 添加顶点 VertexSim3Expmap - sim3 位姿，顶点的值为两个关键帧之间的 sim3 变换；

g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();    
    vSim3->_fix_scale=bFixScale;
    vSim3->setEstimate(g2oS12);     // 两个关键帧间的Sim3变换
    vSim3->setId(0);
    vSim3->setFixed(false);// 优化Sim3顶点
    vSim3->_principle_point1[0] = K1.at<float>(0,2); // 光心横坐标cx
    vSim3->_principle_point1[1] = K1.at<float>(1,2); // 光心纵坐标cy
    vSim3->_focal_length1[0] = K1.at<float>(0,0); // 焦距 fx
    vSim3->_focal_length1[1] = K1.at<float>(1,1); // 焦距 fy
    vSim3->_principle_point2[0] = K2.at<float>(0,2);
    vSim3->_principle_point2[1] = K2.at<float>(1,2);
    vSim3->_focal_length2[0] = K2.at<float>(0,0);
    vSim3->_focal_length2[1] = K2.at<float>(1,1);
    optimizer.addVertex(vSim3);

*  步骤三： 添加顶点 VertexSBAPointXYZ - 两帧的共视地图点，顶点的值为地图点的相机坐标；
 *  步骤四：添加边 - 顶点投影到当前帧的重投影误差 EdgeSim3ProjectXYZ 和投影到闭环帧的重投影误差 EdgeInverseSim3ProjectXYZ；
 *  步骤五： 开始优化，迭代 5 次；
 *  步骤六： 卡方检验剔除误差较大的边；
 *  步骤七： 再次优化剩余的边，迭代 5 或者 10 次；
 *  步骤八： 优化结束，返回两帧之间的 sim3 变换。

### 6.5.闭环后的 Sim3 位姿优化 OptimizeEssentialGraph() ###

**函数描述**

*  单目 SLAM 一般都会发生尺度（scale）漂移，因此 Sim3 上的优化是必要的，相对于SE3，Sim3 的自由度要多一个，而且优化的目标是矫正尺度因子，因此优化并没有加入更多的变量（如三维点）；
 *  在检测到闭环时在 sim3 上对所有的位姿进行一次优化，在函数 Optimizer::OptimizeEssentialGraph() 中执行，EssentialGraph 包括所有的关键帧顶点，但是优化边大大减少，包括 spanning tree（生成树），共视权重θ>100 的边，以及闭环连接边。用于闭环检测 Sim3 调整后优化。
 *  定义 sim3 上的残差为：  
     e i , j = log ⁡ sim ⁡ 3 ( S i j S j w S i w − 1 ) e\_\{i, j\}=\\log \_\{\\operatorname\{sim\} 3\}\\left(S\_\{i j\} S\_\{j w\} S\_\{i w\}^\{-1\}\\right) ei,j=logsim3(SijSjwSiw−1)  
    其中  S i w S\_\{i w\} Siw的初值是尺度为 1 的  p o s i pos\_i posi 相对于世界坐标系的变换矩阵， S j w S\_\{jw\} Sjw 同理； S i j S\_\{ij\} Sij表示 p o s i pos\_i posi 和  p o s j pos\_j posj 之间的相对位姿矩阵（sim3 优化之前），表示  S i w S\_\{iw\} Siw 和  S j w S\_\{jw\} Sjw 之间的测量；此处相当于认为局部的相对位姿是准确的，而全局位姿有累计误差，是不准确的.

**顶点与边:**

1. Vertex:
        - g2o::VertexSim3Expmap，Essential graph中关键帧的位姿
    2. Edge:
        - g2o::EdgeSim3()，BaseBinaryEdge
            + Vertex：关键帧的Tcw，MapPoint的Pw
            + measurement：经过CorrectLoop函数步骤2，Sim3传播校正后的位姿
            + InfoMatrix: 单位矩阵

**函数执行流程**

*  步骤一： 初始化 g2o 优化器；
 *  步骤二： 添加顶点 VertexSim3Expmap - 地图中所有的关键帧；
 *  步骤三： 添加边  
    闭环时因为 MapPoints 调整而出现的新关键帧之间的 sim3 变换误差  
    当前帧与闭环匹配成功的帧之间的误差
 *  步骤四： 开始优化，迭代 20 次；
 *  步骤五： 优化结束更新位姿 Sim3:\[sR t;0 1\] -> SE3:\[R t/s;0 1\]；
 *  步骤六： MapPoints 根据参考帧优化前后的相对关系调整自己的位置（地图点坐标不是直接从优化器中更新的）。

[ORB-SLAM2 _ 2_ _ g2o]: https://wym.netlify.app/2019-07-05-orb-slam2-optimization2/
[g2o_g2o]: https://www.cnblogs.com/gaoxiang12/p/5304272.html
[G2O_SLAM_Bundle Adjustment]: http://zhaoxuhui.top/blog/2018/04/10/g2o&bundle_adjustment.html
[g2o]: http://www.chenjianqu.com/show-97.html
[SLAM _ _g2o]: https://www.cnblogs.com/CV-life/archive/2019/03/13/10525579.html
[github]: https://github.com/wuxiaolang
[20200803104304250.png_pic_center]: /images/20230208/9f843567523145df931c6a92d19adc2d.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ZiXzk0MTIxOQ_size_16_color_FFFFFF_t_70_pic_center]: /images/20230208/a41055c568954c4dbaaead4bbf854c42.png
[20200803104627532.png_pic_center]: /images/20230208/9139ac8b96074a7fb0db3c20a4287228.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2ZiXzk0MTIxOQ_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20230208/9296f24683e94cc6a78675ac6bc5d69f.png