g2o图优化简单入门

Jiqiang_z

已于 2023-12-05 20:29:45 修改

阅读量484

点赞数 1

分类专栏： SLAM学习笔记 ORB-SLAM系列阅读笔记文章标签： linux c++

于 2023-03-21 16:53:26 首次发布

本文链接：https://blog.csdn.net/jjjqqq123321/article/details/129692332

版权

SLAM学习笔记同时被 2 个专栏收录

32 篇文章 0 订阅

订阅专栏

ORB-SLAM系列阅读笔记

9 篇文章 0 订阅

订阅专栏

前言：

g2o中用顶点表示优化变量，用边表示误差项。

一条边连接几个顶点就是就是几元边（就是误差项中有几个优化变量，两元边最常见）

情景1：只优化位姿，不优化地图点时

1.顶点就是位姿，边的误差项一般为：A到B为位姿×B到A的位姿=E，log(E)=0的原理。

2.顶点是位姿和地图点，但是地图点不参与优化。边的误差项一般为重投影误差。

顶点的预测值为优化前位姿（3d），边的观测值为特征点坐标（2d）

顶点的个数为优化的位姿数量，如果只优化当前帧则个数为1,如果优化多个关键帧，则顶点个数为多个。

边的个数为匹配成功的特征点个数，如果只优化当前帧则为当前帧成功匹配到的特征点个数。

情景2：同时优化位姿和地图点时

顶点就是位姿和地图点，边的误差项一般为重投影误差。

g2o的使用主要有以下几个步骤：

1.定义顶点和边的类型。

2.构建图。

3.选择优化算法。

4.调用g2o进行优化，返回结果。

g2o编程流程流程图：

示例：


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

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

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

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

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

// 第5步：定义图的顶点和边。并添加到SparseOptimizer中
CurveFittingVertex* v = new CurveFittingVertex(); //往图中增加顶点
v->setEstimate( Eigen::Vector3d(0,0,0) );  //顶点的预测值
v->setId(0);
optimizer.addVertex( v );
for ( int i=0; i<N; i++ )    // 往图中增加边
{
  CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
  edge->setId(i);
  edge->setVertex( 0, v );                // 设置连接的顶点
  edge->setMeasurement( y_data[i] );      // 观测数值
  edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
  optimizer.addEdge( edge );
}

// 第6步：设置优化参数，开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100);

g2o顶点编程套路：

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


VertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3>  //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexPointXY : public BaseVertex<2, Vector2>
VertexPointXYZ : public BaseVertex<3, Vector3>
VertexSBAPointXYZ : public BaseVertex<3, Vector3>

// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
VertexSE3Expmap : public BaseVertex<6, SE3Quat>

// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
// qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation
VertexCam : public BaseVertex<6, SBACam>

// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
VertexSim3Expmap : public BaseVertex<7, Sim3>

如果内部定义的顶点类型不满足需求，重新定义顶点一般需要考虑重写如下函数：


virtual bool read(std::istream& is);    //读盘函数
virtual bool write(std::ostream& os) const; //存盘函数
virtual void oplusImpl(const number_t* update); //顶点的更新函数
virtual void setToOriginImpl(); //顶点的重置函数

向图中添加顶点：


    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        // 创建一个顶点，指向g2o库中的VertexSBAPointXYZ类型的对象
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        // 为顶点设置ID，该ID为index+1
        point->setId ( index++ );
        // 设置优化变量的初始估计值，将点的x、y、z坐标作为Vector3d类型传入
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        // 将该顶点设为边缘化变量，表示该点变量可以被边缘化掉，以加速优化过程
        point->setMarginalized ( true );
        // 将该顶点变量添加到优化器中
        optimizer.addVertex ( point );
    }

g2o边的代码套路：

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


virtual bool read(std::istream& is);  //read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
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() 来定义协方差矩阵的逆

定义二元边示例：


//继承了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;
};

向图中添加二元边：


    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        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 ) );//p来自向量points_2d，也就是特征点的图像坐标(x,y)
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

g2o与ceres的比较：

g2o库：

公开的API说明比较少，对入门小白不太友好，看起来会头大。

使用固定的模板，不太灵活

提供了预先定义好的接口，需要做的工作比较简单，比较傻瓜

ceres库：

本身是针对一般优化问题的库，更通用

给了开发者很大的空间和自由度去自己设计优化策略

当然需要做的工作也比较多，需要自己定义顶点边等接口

总之，g2o就像一个傻瓜相机，预先设置好了很多参数，你只能从固定模式中选择

ceres就像一个专业相机，需要你懂参数，然后自己根据需要调节参数，适用范围更广

g2o的输出信息：