SLAM——之G2O函数库

1. G2O示例

相较于Ceres而言，G2O函数库相对较为复杂，但是适用面更加广，可以解决较为复杂的重定位问题。Ceres库向通用的最小二乘问题的求解，定义优化问题，设置一些选项，可通过Ceres求解。而图优化，是把优化问题表现成图的一种方式，这里的图是图论意义上的图。一个图由若干个顶点,以及连着这些顶点的边组成。在这里，我们用顶点表示优化变量，而用边表示误差项。

为了使用g2o,首先要将曲线拟合问题抽象成图优化。这个过程中，只要记住节点为优化变量，边为误差项即可。曲线拟合的图优化问题可以画成以下形式：

G2O在数学上主要分为四个求解步骤：

1. 定义定点和边的类型
2. 构建图
3. 选择优化的算法及其参数
4. 调用g2o进行优化

在程序中的反应为：

1. 创建一个线性求解器LinearSolver。
2. 创建BlockSolver，并用上面定义的线性求解器初始化。
3. 创建总求解器solver，并从GN/LM/DogLeg 中选一个作为迭代策略，再用上述块求解器BlockSolver初始化。
4. 创建图优化的核心：稀疏优化器（SparseOptimizer）。
5. 定义图的顶点和边，并添加到SparseOptimizer中。
6. 设置优化参数，开始执行优化。

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std; 

// 曲线模型的顶点，模板参数：优化变量维度和数据类型(此处为自定义定点，具体可以参考下方的点定义)
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);//update强制类型转换为Vector3d
    }
    // 存盘和读盘：留空
    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
    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
};

int main( int argc, char** argv )
{ 
   
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = { 
   0,0,0};            // abc参数的估计值

    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 + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器。第一步：创建一个线性求解器LinearSolver。
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器。第二步：创建BlockSolver，并用上面定义的线性求解器初始化。
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );//第三步：创建BlockSolver，并用上面定义的线性求解器初始化。
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 图模型。第四步：创建图优化的核心：稀疏优化器（SparseOptimizer）。
    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose( true );       // 打开调试输出

    // 往图中增加顶点。第五步：定义图的顶点和边HyperGraph，并添加到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 );
    }

    // 执行优化。第六步：设置优化参数，开始执行优化。
    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::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;

    return 0;
}

2. G2O常见函数总结

对这个结构框图做一个简单介绍（注意图中三种箭头的含义（右上角注解））：

（1）$图的核心$：整个g2o框架可以分为上下两部分，两部分中间的连接点：SparseOpyimizer 就是整个g2o的核心部分。

（2）往上看，SparseOpyimizer其实是一个Optimizable Graph，从而也是一个超图（HyperGraph）。

（3）$顶点和边：$超图有很多顶点和边。
顶点继承自 Base Vertex，也即OptimizableGraph::Vertex；
而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge。

（4）$配置SparseOptimizer的优化算法和求解器：$往下看，SparseOptimizer包含一个优化算法部分OptimizationAlgorithm，它是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）、 Levernberg-Marquardt（简称LM法）、Powell’s dogleg 三者中间选择一个（常用的是GN和LM）。

（5）$如何求解：$对优化算法部分进行求解的时求解器solver，它实际由BlockSolver 组成。BlockSolver由两部分组成：一个是SparseBlockMatrix，它由于求解稀疏矩阵(雅克比和海塞)；另一个部分是LinearSolver，它用来求解线性方程 得到待求增量，因此这一部分是非常重要的，它可以从PCG/CSparse/Choldmod选择求解方法。

2.1. G2O创建一个线性求解器LinearSolver

我们要求的增量方程的形式是：H△X=-b，通常情况下想到的方法就是直接求逆，也就是△X=-H.inv*b。看起来好像很简单，但这有个前提，就是H的维度较小，此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。和Ceres类似，G2O需要一些特殊的方法对矩阵进行求逆。
这一步中我们可以选择不同的求解方式来求解线性方程，g2o中提供的求解方式主要有：

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

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

g2o/g2o/core/block_solver.h

你点进去会发现 BlockSolver有两种定义方式
一种是指定的固定变量的solver，我们来看一下定义

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

其中p代表pose的维度（注意一定是流形manifold下的最小表示），l表示landmark的维度
另一种是可变尺寸的solver，定义如下

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

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维

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

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

点进去 GN、 LM、 Doglet算法内部，会发现他们都继承自同一个类：OptimizationWithHessian，如下图所示，这也和我们最前面那个图是相符的

点进去看 OptimizationAlgorithmWithHessian，发现它又继承自OptimizationAlgorithm，这也和前面的相符

在该阶段可以选择三种方法：

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

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

/// 创建稀疏优化器
g2o::SparseOptimizer    optimizer;

/// 用前面定义好的求解器作为求解方法：
SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

/// 其中setVerbose是**设置优化过程输出信息**用的
SparseOptimizer::setVerbose(bool verbose)

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

$点Vertex$:
在g2o中定义Vertex有一个通用的类模板：BaseVertex。在结构框图中可以看到它的位置就是HyperGraph继承的根源。

同时在图中我们注意到BaseVertex具有两个参数D/T，这两个参数非常重要，我们来看一下：

D 是int 类型，表示vertex的最小维度，例如3D空间中旋转是3维的，则 D = 3
T 是待估计vertex的数据类型，例如用四元数表达三维旋转，则 T 就是Quaternion 类型

static const int Dimension = D; 
///< dimension of the estimate (minimal) in the manifold spacetypedef T EstimateType;EstimateType _estimate;

如何自己定义Vertex

在我们动手定义自己的Vertex之前，可以先看下g2o本身已经定义了一些常用的顶点类型：

VertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3> //Isometry3 是欧式变换矩阵T，实质是4*4矩阵//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>

但是！如果在使用中发现没有我们可以直接使用的Vertex，那就需要自己来定义了。一般来说定义Vertex需要重写这几个函数（注意注释）：

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;// 分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
virtual void oplusImpl(const number_t* update);//顶点更新函数
virtual void setToOriginImpl();//顶点重置函数，设定被优化变量的原始值。

请注意里面的oplusImpl函数，是非常重要的函数，主要用于优化过程中增量△x 的计算。根据增量方程计算出增量后，通过这个函数对估计值进行调整，因此该函数的内容要重视。

根据上面四个函数可以得到定义顶点的基本格式：

class myVertex: public g2o::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;
      }
  }

另外值得注意的是，优化变量更新并不是所有时候都可以像上面两个一样直接 += 就可以，这要看优化变量使用的类型（是否对加法封闭）。

2.6 向图中添加顶点

接着上面定义完的顶点，我们把它添加到图中：

CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) )；  // 设定初始值v->setId(0); // 定义节点编号
optimizer.addVertex( v );                  // 把节点添加到图中

2.6 $边Edge$

图优化中的边：BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。

顾名思义，一元边可以理解为一条边只连接一个顶点，两元边理解为一条边连接两个顶点（常见），多元边理解为一条边可以连接多个（3个以上）顶点。

以最常见的二元边为例分析一下他们的参数：D, E, VertexXi, VertexXj：

/// D 是 int 型，表示测量值的维度 （dimension）
/// E 表示测量值的数据类型
/// VertexXi，VertexXj 分别表示不同顶点的类型
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

上面这行代码表示二元边，参数1是说测量值是2维的；参数2对应测量值的类型是Vector2D，参数3和4表示两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap。

如何定义一个边
除了上面那行定义语句，还要复写一些重要的成员函数：

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;// 分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以virtual 
void computeError();// 非常重要，是使用当前顶点值计算的测量值与真实测量值之间的误差
virtual void linearizeOplus();// 非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是Jacobian矩阵

除了上面四个函数，还有几个重要的成员变量以及函数：

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

有了上面那些重要的成员变量和成员函数，就可以用来定义一条边了：

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
  }

向图中添加边
和添加点有一点类似，下面是添加一元边：

// 往图中增加边 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 );

但在SLAM中我们经常要使用的二元边（前后两个位姿），那么此时：

index = 1;
for ( const Point2f p:points_2d ){ 
   
    g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
    edge->setId ( index );  // 边的b编号 
    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++;}

2.7 G2O设置优化参数，开始执行优化

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

/// 初始化
SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)
/// 设置迭代次数，然后就开始执行图优化了。
SparseOptimizer::optimize(int iterations, bool online)

3. G2O 相机位姿优化

/** * BA Example * Author: Xiang Gao * Date: 2016.3 * Email: gaoxiang12@mails.tsinghua.edu.cn * * 在这个程序中，我们读取两张图像，进行特征匹配。然后根据匹配得到的特征，计算相机运动以及特征点的位置。这是一个典型的Bundle Adjustment，我们用g2o进行优化。 */

// for std
#include <iostream>
// for opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <boost/concept_check.hpp>
// for g2o
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/sba/types_six_dof_expmap.h>


using namespace std;

// 寻找两个图像中的对应点，像素坐标系
// 输入：img1, img2 两张图像
// 输出：points1, points2, 两组对应的2D点
int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 );

// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;

int main( int argc, char** argv )
{ 
   
    // 调用格式：命令 [第一个图] [第二个图]
    if (argc != 3)
    { 
   
        cout<<"Usage: ba_example img1, img2"<<endl;
        exit(1);
    }

    // 读取图像
    cv::Mat img1 = cv::imread( argv[1] );
    cv::Mat img2 = cv::imread( argv[2] );

    // 找到对应点
    vector<cv::Point2f> pts1, pts2;
    if ( findCorrespondingPoints( img1, img2, pts1, pts2 ) == false )
    { 
   
        cout<<"匹配点不够！"<<endl;
        return 0;
    }
    cout<<"找到了"<<pts1.size()<<"组对应特征点。"<<endl;
    // 构造g2o中的图
    // 先构造求解器
    g2o::SparseOptimizer    optimizer;
    // 使用Cholmod中的线性方程求解器
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
    // 6*3 的参数
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
    // L-M 下降
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );

    optimizer.setAlgorithm( algorithm );
    optimizer.setVerbose( false );

    // 添加节点
    // 两个位姿节点
    for ( int i=0; i<2; i++ )
    { 
   
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if ( i == 0)
            v->setFixed( true ); // 第一个点固定为零
        // 预设值为单位Pose，因为我们不知道任何信息
        v->setEstimate( g2o::SE3Quat() );
        optimizer.addVertex( v );
    }
    // 很多个特征点的节点
    // 以第一帧为准
    for ( size_t i=0; i<pts1.size(); i++ )
    { 
   
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        v->setId( 2 + i );
        // 由于深度不知道，只能把深度设置为1了
        double z = 1;
        double x = ( pts1[i].x - cx ) * z / fx;
        double y = ( pts1[i].y - cy ) * z / fy;
        v->setMarginalized(true);
        v->setEstimate( Eigen::Vector3d(x,y,z) );
        optimizer.addVertex( v );
    }

    // 准备相机参数
    g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
    camera->setId(0);
    optimizer.addParameter( camera );

    // 准备边
    // 第一帧
    vector<g2o::EdgeProjectXYZ2UV*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    { 
   
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(0)) );
        edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    // 第二帧
    for ( size_t i=0; i<pts2.size(); i++ )
    { 
   
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(1)) );
        edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0,0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }

    cout<<"开始优化"<<endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    cout<<"优化完毕"<<endl;

    //我们比较关心两帧之间的变换矩阵
    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
    Eigen::Isometry3d pose = v->estimate();
    cout<<"Pose="<<endl<<pose.matrix()<<endl;

    // 以及所有特征点的位置
    for ( size_t i=0; i<pts1.size(); i++ )
    { 
   
        g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
        cout<<"vertex id "<<i+2<<", pos = ";
        Eigen::Vector3d pos = v->estimate();
        cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl;
    }

    // 估计inlier的个数
    int inliers = 0;
    for ( auto e:edges )
    { 
   
        e->computeError();
        // chi2 就是 error*\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符
        if ( e->chi2() > 1 )
        { 
   
            cout<<"error = "<<e->chi2()<<endl;
        }
        else
        { 
   
            inliers++;
        }
    }

    cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl;
    optimizer.save("ba.g2o");
    return 0;
}


int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 )
{ 
   
    cv::ORB orb;
    vector<cv::KeyPoint> kp1, kp2;
    cv::Mat desp1, desp2;
    orb( img1, cv::Mat(), kp1, desp1 );
    orb( img2, cv::Mat(), kp2, desp2 );
    cout<<"分别找到了"<<kp1.size()<<"和"<<kp2.size()<<"个特征点"<<endl;

    cv::Ptr<cv::DescriptorMatcher>  matcher = cv::DescriptorMatcher::create( "BruteForce-Hamming");

    double knn_match_ratio=0.8;
    vector< vector<cv::DMatch> > matches_knn;
    matcher->knnMatch( desp1, desp2, matches_knn, 2 );
    vector< cv::DMatch > matches;
    for ( size_t i=0; i<matches_knn.size(); i++ )
    { 
   
        if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance )
            matches.push_back( matches_knn[i][0] );
    }

    if (matches.size() <= 20) //匹配点太少
        return false;

    for ( auto m:matches )
    { 
   
        points1.push_back( kp1[m.queryIdx].pt );
        points2.push_back( kp2[m.trainIdx].pt );
    }

    return true;
}

最后显示了特征点的数量，估计的位姿变换，以及各特征点的空间位置。最后，还显示了inliers的数量。

4. 李代数二维&三维转化

g2o::SE3Quat SE2ToSE3(const g2o::SE2& _se2)
{ 
   
    SE3Quat ret;
    ret.setTranslation(Eigen::Vector3d(_se2.translation()(0), _se2.translation()(1), 0));
    ret.setRotation(Eigen::Quaterniond(AngleAxisd(_se2.rotation().angle(), Vector3d::UnitZ())));
    return ret;
}

g2o::SE2 SE3ToSE2(const SE3Quat &_se3)
{ 
   
    Eigen::Vector3d eulers = g2o::internal::toEuler(_se3.rotation().matrix());
    return g2o::SE2(_se3.translation()(0), _se3.translation()(1), eulers(2));
}

5. 参考链接

视觉SLAM14讲 Ceres和G2O
https://www.guyuehome.com/34708

今天的文章SLAM——之G2O函数库分享到此就结束了，感谢您的阅读。