g2o图优化的使用

水深无声 2022-04-23 16:38 178阅读 0赞

[g2o class 的描述手册][g2o class]  
[好的链接][Link 1]

### 文章目录 ###

*   *  图优化介绍
     *   *  结构图
     *  举例
     *  常用的边和点

## 图优化介绍 ##

*  节点：优化变量
 *  边：误差项
 *  步骤：
    
    1.  定义顶点
    2.  定义边
    3.  配置BlockSolver(LinerSolverType)
    4.  配置OptimizationAlgorithm
    5.  配置Optimizer
    6.  添加顶点
    7.  添加边
    8.  启动优化

### 结构图 ###

![在这里插入图片描述][20190711155258654.png]

--------------------

## 举例 ##

*  进行曲线拟合 y = e x p ( a x 2 + b x + c ) + w y=exp(ax^2+bx+c)+w y=exp(ax2\+bx\+c)\+w  
     a   b   c a \\space b \\space c a b c是曲线参数， w w w是高斯噪声，假设我们有Ｎ=100个带噪声的数据点(a=1,b=2,c=3，加噪声)，拟合曲线，求曲线参数：  
     min ⁡ a b c 1 2 ∑ i = 1 N ∣ ∣ y i − e x p ( a x i 2 + b x i + c ) ∣ ∣ 2 \\min \\limits\_\{abc\} \\frac\{1\}\{2\}\\sum\_\{i=1\}^\{N\}||y\_i-exp(ax\_i^2+bx\_i+c)||^2 abcmin21i=1∑N∣∣yi−exp(axi2\+bxi\+c)∣∣2

#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 <opencv2/opencv.hpp>
    #include <cmath>
    #include <chrono>
    #include <Eigen/Core>
    using namespace std;
    //1.定义顶点
    
    class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> //优化变量的<维数、和数据类型>
    { 
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW  //Eigen 库数据结构内存对齐使用
        virtual void setToOriginImpl()　
        { 
            _estimate << 0, 0, 0;　　// 优化变量重置为０
        }
    
        virtual void oplusImpl(const double* update)　//重写虚函数：定点(优化变量)跟新, update相当于　增量
        { 
            _estimate += Eigen::Vector3d(update);
        }
    
        virtual bool read(istream& in) { }
        virtual bool write(ostream& out){ }
    
    };
    
    
    
    //2.定义边 
    
    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));  // measurement 是观测值，相当于yi
        }
    
    
        virtual bool read(istream& in) { }
        virtual bool write(ostream& out)const { }
    
    public:
        double _x;
    };
    
    
    int main(void)
    { 
    
        double a=1.0, b=2.0, c=1.0;
        int N=100;
        double w_sigma = 1.0;
    
        cv::RNG rng;
        double abc[3] = { 0,0,0};
    
        vector<double> x_data, y_data;
    
        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));
        }
        
    
    //3. 配置BlockSolver(LinerSolverType)
        typedef g2o::BlockSolver<g2o::BlockSolverTraits<3,1>> Block;   // 优化变量维度　3, 误差维度　１
        Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();　//线性方程求解器
        Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver));　　// 矩阵块求解器
    
    
    
    
    //4.配置OptimizationAlgorithm,可选其一
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(std::unique_ptr<Block>(solver_ptr));
    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr)); 
        g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(std::unique_ptr<Block>(solver_ptr));
    
    
    
    
    //５.配置Optimizer 
        g2o::SparseOptimizer optimizer; // 图模型
        optimizer.setAlgorithm(solver); //配置求解器
        optimizer.setVerbose(true);   // 打开调试输出
    
    
    //6. 向图中添加定点
        CurveFittingVertex* v= new CurveFittingVertex();
        v->setEstimate(Eigen::Vector3d(0, 0, 0));
        v->setId(0);
        optimizer.addVertex(v);
    
    
    
    //7.向图中添加边
        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);
        }
    
    
    
    //8.启动优化
        optimizer.initializeOptimization();
        optimizer.optimize(100);
    
    
        Eigen::Vector3d abc_estimate = v->estimate();　　
    	cout << abc_estimate.transpose() << endl;　//输出优化值

结果：和真实值（1,2,1）相差不大  
![在这里插入图片描述][20190411094010160.png]

## 常用的边和点 ##

<table> 
 <thead> 
  <tr> 
   <th>顶点</th> 
   <th>头文件</th> 
   <th>描述</th> 
  </tr> 
 </thead> 
 <tbody> 
  <tr> 
   <td>VertexSE3Expmap</td> 
   <td>g2o/types/sba/types_six_dof_expmap.h</td> 
   <td>李代数表示位姿</td> 
  </tr> 
  <tr> 
   <td>VertexSBAPointXYZ</td> 
   <td>g2o/types/sba/types_sba.h</td> 
   <td>空间点XYZ</td> 
  </tr> 
  <tr> 
   <td></td> 
   <td></td> 
   <td></td> 
  </tr> 
  <tr> 
   <td></td> 
   <td></td> 
   <td></td> 
  </tr> 
 </tbody> 
</table>

<table> 
 <thead> 
  <tr> 
   <th>边</th> 
   <th>头文件</th> 
   <th>描述</th> 
  </tr> 
 </thead> 
 <tbody> 
  <tr> 
   <td>EdgeProjectXYZ2UV</td> 
   <td>g2o/types/sba/types_six_dof_expmap.h</td> 
   <td>空间点投影和像素的误差</td> 
  </tr> 
  <tr> 
   <td></td> 
   <td></td> 
   <td></td> 
  </tr> 
  <tr> 
   <td></td> 
   <td></td> 
   <td></td> 
  </tr> 
  <tr> 
   <td></td> 
   <td></td> 
   <td></td> 
  </tr> 
 </tbody> 
</table>

[g2o class]: http://docs.ros.org/fuerte/api/re_vision/html/namespaceg2o.html
[Link 1]: https://blog.csdn.net/yuyuntan/article/category/8554257
[20190711155258654.png]: /images/20220224/45fff11364f44890b00499ff0d3239ed.png
[20190411094010160.png]: /images/20220224/6d013209026d4ea1b59ac3b53501dcac.png