G2O使用步骤1——添加定点和边

前面介绍了G2O的整体框架与初始化求解器基本使用步骤，接下来介绍如何向优化器中添加定点和边

1. 顶点

我们从顶点的数据类型开始介绍，先来看看上图中和vertex有关的第1个类: HyperGraph::Vertex，然后我们看g2o 类结构图中第2个类,我们看到HyperGraph::Vertex 是通过类OptimizableGraph 来继承的；这个OptimizableGraph::Vertex 也非常底层,具体使用时一般都会进行扩展,因此g2o中提供了一个比较通用的适合大部分情况的模板。就是g2o 类结构图中 对应的第3个类:BaseVertex<D, T>。

这里的参数D是int 类型的，表示vertex的最小维度，比如3D空间中旋转是3维的，那么这里 D = 3；T是待估计vertex的数据类型，比如用四元数表达三维旋转的话，T就是Quaternion 类型，我们需要明确D，T的含义，D并非顶点的维度，而是流形空间的最小表示，T就是顶点的类型。

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

read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以

setToOriginImpl：顶点重置函数，设定被优化变量的原始值。

oplusImpl：顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。

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 {}
};
或者
// 第一个参数6 表示内部存储的优化变量维度,这是个6维的李代数
// 第二个参数是优化变量的类型,这里使用了g2o定义的相机位姿类型:SE3Quat。
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  VertexSE3Expmap();
  bool read(std::istream& is);
  bool write(std::ostream& os) const;
  virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map<const Vector6> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());        //更新方式
  }
};

接下来往图中添加定点

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

2.边

边有分为BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。

一元边你可以理解为一条边只连接一个顶点，两元边理解为一条边连接两个顶点，也就是我们常见的边啦，多元边理解为一条边可以连接多个（3个以上）顶点

与分析顶点类似，我们先看一下图中的几个参数的含义，具体有D, E, VertexXi, VertexXj四个参数

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

比如我们用边表示三维点投影到图像平面的重投影误差，就可以设置输入参数如下：

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

首先这个是个二元边。第1个2是说测量值是2维的，也就是图像像素坐标x,y的差值，对应测量值的类型是Vector2D，两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap

重定义边需要重写这几个函数

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

下面是一个复杂一点例子，3D-2D点的PnP 问题，也就是最小化重投影误差问题，这个问题非常常见，使用最常见的二元边

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

其实就是：误差 = 观测 - 投影

cam_map 函数功能是把相机坐标系下三维点（输入）用内参转换为图像坐标（输出），具体代码如下所示

Vector2  CameraParameters::cam_map(const Vector3 & trans_xyz) const {

  Vector2 proj = project2d(trans_xyz);

  Vector2 res;

  res[0] = proj[0]*focal_length + principle_point[0];

  res[1] = proj[1]*focal_length + principle_point[1];

  return res;

}

然后看 .map函数，它的功能是把世界坐标系下三维点变换到相机坐标系

      Vector3 map (const Vector3& xyz) const {

        return s*(r*xyz) + t;

      }

因此下面这个代码

v1->estimate().map(v2->estimate())

就是用V1估计的pose把V2代表的三维点，变换到相机坐标系下。

向图中添加边

需要明白添加的是那种类型的边，多元边，二元边等

比如：

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

        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++;

}

这里的setMeasurement函数里的p来自向量points_2d，也就是特征点的图像坐标(x,y)了

setVertex 有两个一个是 0 和 VertexSBAPointXYZ 类型的顶点，一个是1 和pose

void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;}

也就是说0和1是对应两个不同的顶点。

G2O的优化器基本上都是按照这种方法使用了。