G2O 与ceres风格不同，它是专门用于进行图优化的库，提供了大量预先定义的节点和边供用户调用。

曲线拟合的例子其实不太适合用来理解g2o，它涉及的顶点只有一个，边是一元边，有很多个。还是用图优化的例子更容易理解。

TEB算法的base_teb_edges.h, g2o_types\vertex_pose.h 和 g2o_types\vertex_timediff.h很有学习价值，以及edge_obstacle.h, edge_shortest_path.h, edge_velocity.h

顶点

最基础的BaseVertex类，template <int D, typename T> class BaseVertex，

• D: minimal dimension of the vertex, 例如 3 for rotation in 3D. D并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示
• T: 节点估计值的类型,例如 Quaternion for rotation in 3D

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) (注意不管四元数的 w)
• VertexPointXY : public BaseVertex<2, Vector2>
• VertexPointXYZ : public BaseVertex<3, Vector3>
• VertexSBAPointXYZ : public BaseVertex<3, Vector3>
• VertexSE3Expmap : public BaseVertex<6, SE3Quat>

如果我们需要的顶点类型这里面没有，就得自己定义了

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

# 读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下，作为空函数
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();

setToOriginImpl 和 _estimate

setToOriginImpl：顶点重置函数，设定被优化变量的原始值。有时可以不用实现。具体来说就是初始化成员变量 _estimate， _estimate的类型就是继承 BaseVertex时的模板参数

如果_estimate是Eigen::Vector3d，那么setToOriginImpl的内容可以是: _estimate << 0,0,0;。 如果是PoseSE2， _estimate的赋值可以这样：

_estimate.position() = position;
_estimate.x() = x;
_estimate.y() = y;
_estimate.theta() = theta;

virtual void setToOriginImpl()
{
    _estimate.setZero();
}

oplusImpl

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

比较简单的类型是这样实现：

// double 也可以换成 number_t
virtual void oplusImpl( const double* update ) // 更新
{
     // update为增量△m 迭代的时候，估计值 m= m + △m, m是向量（a,b,c）
    _estimate += Eigen::Vector3d(update);  
}

virtual void oplusImpl(const double* update)
{
    _estimate.plus(update);
}

李代数的类VertexSE3Expmap，上面两个函数是

virtual void setToOriginImpl() {    // 重置
    _estimate = SE3Quat();
}

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

边

g2o自带的边

• EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>
• EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>
• EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>

仅优化一个变量的时候选择单边

以一元边为例template <int D, typename E, typename VertexXi> class BaseUnaryEdge : public BaseEdge<D,E>。

• D 表示测量值的维度
• E 表示测量值的数据类型
• VertexXi，VertexXj 分别表示不同顶点的类型。（一元边只有一个顶点）

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

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

最重要的就是computeError()，linearizeOplus()两个函数了。如果我们没有给出雅可比，g2o也会进行数值求导，但是会比重写的雅可比慢。 雅克比矩阵的维度为 误差维度 × 优化变量的维度 ，两个函数一般是这样的形式

virtual void computeError() override
{
  // ...
    _error = _measurement - Something;
}
virtual void linearizeOplus() override
{
    _jacobianOplusXi(pos, pos) = something;
    // ...

    /*
    _jocobianOplusXj(pos, pos) = something;
    ...
    */
}      

雅格比矩阵要针对每个顶点求解，上面说的维度是每个顶点求解的维度。比如边的类型继承

class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose>

由于是一元边，那么只针对一个顶点求解，误差维度是边的第一个模板类型2，优化变量的维度是顶点的第一个模板参数6.

TEB算法中的class EdgeVelocity : public BaseTebMultiEdge<2, double>，是三元边，两个VertexPose顶点，类型PoseSE2。顶点VertexTimeDiff，类型double。对于前者，雅格比矩阵是 2x3，对后者是 2x1，也就是如下

_jacobianOplus[0].resize(2,3); // conf1
_jacobianOplus[1].resize(2,3); // conf2
_jacobianOplus[2].resize(2,1); // deltaT

对于边class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>，这是二元边，雅格比包含两部分，维度分别为 2x3 和 2x6. 后一个顶点的类型是李群。可以这样写

// 相对 _jacobianOplus[2] 的另一种写法
_jacobianOplusXi(0,0) =
......
_jacobianOplusXj(0,0) = 
......

removeEdge()

removeEdge 和 removeVertex只有在ORB-SLAM中看到。

After running optimize() once, I am finding a high number of outliers. Is there any way to delete a single edge out of the graph and run optimize again, or do I have to construct it again?

The method removeEdge() removes the edge from a graph and unlinks it from all of the vertices it was attached to. After you call it on your edges, I think you need to call initializeOptimization() to reset all of g2o’s internal data structures to the new graph configuration. You should then be able to call optimize()

// vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;
// 剔除一些误差大的边
// Check inliers
int nBad=0;
for(size_t i=0; i<vpEdges12.size();i++)
{
    g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
    g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
    if(!e12 || !e21)
        continue;

    if(e12->chi2()>th2 || e21->chi2()>th2)
    {
        size_t idx = vnIndexEdge[i];
        vpMatches1[idx]=static_cast<MapPoint*>(NULL);
        optimizer.removeEdge(e12);
        optimizer.removeEdge(e21);
        vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
        vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
        nBad++;
    }
}

setLevel(int level)

也是在ORB-SLAM中用到

优化完成后，对每一条边都进行检查，剔除误差较大的边（认为是错误的边），并设置setLevel为0，即下次不再对该边进行优化

setLevel(int ) is useful when you call optimizer.initializeOptimization(int ). If you assign initializeOptimization(0), the optimizer will include all edges up to level 0 in the optimization, and edges set to level >=1 will not be included

// 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
// 第二个判断条件，用于检查构成该边的MapPoint在该相机坐标系下的深度是否为正？
if(e->chi2()>5.991 || !e->isDepthPositive())
{
    e->setLevel(1);// 不优化
}