当前位置: 七九推 > IT编程>软件设计>设计模式 > SLAM——之G2O函数库

SLAM——之G2O函数库

2022年09月07日 设计模式 我要评论
1. G2O示例 相较于Ceres而言,G2O函数库相对较为复杂,但是适用面更加广,可以解决较为复杂的重定位问题。Ceres库向通用的最小二乘问题的求解&#x

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) 图 的 核 心 \color{#4285f4}{图的核心} :整个g2o框架可以分为上下两部分,两部分中间的连接点:SparseOpyimizer 就是整个g2o的核心部分。

(2)往上看,SparseOpyimizer其实是一个Optimizable Graph,从而也是一个超图(HyperGraph)。

(3) 顶 点 和 边 : \color{#4285f4}{顶点和边:} 超图有很多顶点和边。
顶点继承自 Base Vertex,也即OptimizableGraph::Vertex;
而边可以继承自 BaseUnaryEdge(单边), BaseBinaryEdge(双边)或BaseMultiEdge(多边),它们都叫做OptimizableGraph::Edge。

(4) 配 置 S p a r s e O p t i m i z e r 的 优 化 算 法 和 求 解 器 : \color{#4285f4}{配置SparseOptimizer的优化算法和求解器:} SparseOptimizer往下看,SparseOptimizer包含一个优化算法部分OptimizationAlgorithm,它是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton(高斯牛顿法,简称GN)、 Levernberg-Marquardt(简称LM法)、Powell’s dogleg 三者中间选择一个(常用的是GN和LM)。

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

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

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

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

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中

点 V e r t e x \color{#4285f4}{点 Vertex} 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 边 E d g e \color{#4285f4}{边 Edge} 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

(0)
打赏 微信扫一扫 微信扫一扫

相关文章:

  • Kubernetes(二)Pod详解及容器设计模式

    Kubernetes(二)Pod详解及容器设计模式

    题外话:推荐大家学习k8s,看一下张磊的《深入剖析Kubernetes》这本书。 1. 为什么需要 Pod 1.1 容器的基本概念 我们知道 Po... [阅读全文]
  • 参考文献格式生成器(GB/T 7714-2015)

    参考文献格式生成器(GB/T 7714-2015)

    写在前面 这是一个因为懒诞生的小项目,发出来是感觉也花了一些精力就自己用太亏了。 因为懒得对着引用文献标准一个个敲字,文献管理软件的... [阅读全文]
  • 软件开发基础之设计模式概述

    软件开发基础之设计模式概述

    成为一名优秀的软件开发工程师,设计模式的重要性不言而喻,本章节是对设计模式的前置知识概述,涉及概念性较大,读者可在设计模式学习过程中参阅本文档。在第一章节,主要... [阅读全文]
  • 详解Go语言设计模式之单例模式

    详解Go语言设计模式之单例模式

    单例模式的概念单例模式很容易记住。就像名称一样,它只能提供对象的单一实例,保证一个类只有一个实例,并提供一个全局访问该实例的方法。在第一次调用该实例时被创建,然... [阅读全文]
  • pytorch dataset实战案例之读取数据集的代码

    pytorch dataset实战案例之读取数据集的代码

    概述最近在跑一篇图像修复论文的代码,配置好环境之后开始运行,发现数据一直加载不进去。害,还是得看人家代码咋写的,一句一句看逻辑,准能找出问题。通读dataset... [阅读全文]
  • Visitor设计模式及发送pod创建请求实现详解

    Visitor设计模式及发送pod创建请求实现详解

    确立目标理解kubectl的核心实现之一:visitor design pattern访问者模式理解发送pod创建请求的细节visitor design pat... [阅读全文]

版权声明:本文内容由互联网用户贡献,该文观点仅代表作者本人。本站仅提供信息存储服务,不拥有所有权,不承担相关法律责任。 如发现本站有涉嫌抄袭侵权/违法违规的内容, 请发送邮件至 2386932994@qq.com 举报,一经查实将立刻删除。

发表评论

验证码:
Copyright © 2017-2023  七九推 保留所有权利. 粤ICP备17035492号
站长QQ:2386932994 | 联系邮箱:2386932994@qq.com