SLAM练习题（十）—— 图优化G2O

SLAM学习笔记

g20框架

在SLAM领域，基于图优化的一个用的非常广泛的库就是g2o，它是General Graphic Optimization 的简称，是一个用来优化非线性误差函数的c++框架。

g2o官方github
g2o论文

求解过程：

先指定求解方法：OptimizationWithHessian 内部包含一个求解器（Solver），这个Solver实际是由一个BlockSolver组成的。==这个BlockSolver有两个部分，一个是SparseBlockMatrix ，用于计算稀疏的雅可比和Hessian矩阵；一个是线性方程的求解器（LinearSolver），==它用于计算迭代过程中最关键的一步HΔx=−b，LinearSolver有几种方法可以选择：PCG, CSparse, Choldmod。

再添加顶点和边：注意看 has-many 箭头，这个超图包含了许多顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）。而这些顶点顶点继承自 Base Vertex，也就是OptimizableGraph::Vertex，而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge。添加好定点和边后，设置优化参数，开始执行优化。

步骤：

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

求解步骤

1创建一个线性求解器

我们要求的增量方程的形式是：H△X=-b，通常情况下想到的方法就是直接求逆，也就是△X=-H.inv*b。看起来好像很简单，但这有个前提，就是H的维度较小，此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。此时我们就需要一些特殊的方法对矩阵进行求逆。

g2o提供的几种线性求解方法：

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

矩阵分解参考矩阵分解

PCG算法参考PCG算法实现

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

可变尺寸的solver:

在某些应用场景，我们的Pose和Landmark在程序开始时并不能确定，那么此时这个块状求解器就没办法固定变量，此时使用这个可变尺寸的solver，所有的参数都在中间过程中被确定。

3创建总求解器solver

创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化。

在g2o/g2o/core/ 目录下，发现Solver的优化方法有三种：分别是高斯牛顿（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法，他们都继承自同一个类：OptimizationWithHessian，该类又继承自OptimizationAlgorithm。

三种迭代策略：

g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg 
g2o::OptimizationAlgorithmDogleg

4创建稀疏优化器（SparseOptimizer）

g2o::SparseOptimizer    optimizer;	// 创建稀疏优化器
SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)	// 用前面定义好的求解器作为求解方法
SparseOptimizer::setVerbose(bool verbose)// 设置优化过程输出信息

5定义顶点和边并添加

顶点

对于顶点，g2o中提供了一个比较通用的适合大部分情况的模板：BaseVertex（参考上面的g2o框架图），它在g2o/core/base_vertex.h：链接(这个文件相对来说较小，直接放上来了)

#ifndef G2O_BASE_VERTEX_H
#define G2O_BASE_VERTEX_H

#include "optimizable_graph.h"
#include "creators.h"
#include "g2o/stuff/macros.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/StdVector>
#include <stack>

namespace g2o {

/**
 * \brief Templatized BaseVertex
 *
 * Templatized BaseVertex
 * D  : minimal dimension of the vertex, e.g., 3 for rotation in 3D
 * T  : internal type to represent the estimate, e.g., Quaternion for rotation in 3D
 */
  template <int D, typename T>
  class BaseVertex : public OptimizableGraph::Vertex {
    public:
    typedef T EstimateType;
    typedef std::stack<EstimateType, 
                       std::vector<EstimateType,  Eigen::aligned_allocator<EstimateType> > >
    BackupStackType;

    static const int Dimension = D;           ///< dimension of the estimate (minimal) in the manifold space

    typedef Eigen::Map<Eigen::Matrix<number_t, D, D, Eigen::ColMajor>, Eigen::Matrix<number_t, D, D, Eigen::ColMajor>::Flags & Eigen::PacketAccessBit ? Eigen::Aligned : Eigen::Unaligned >  HessianBlockType;

  public:
    BaseVertex();

    virtual const number_t& hessian(int i, int j) const { assert(i<D && j<D); return _hessian(i,j);}
    virtual number_t& hessian(int i, int j)  { assert(i<D && j<D); return _hessian(i,j);}
    virtual number_t hessianDeterminant() const {return _hessian.determinant();}
    virtual number_t* hessianData() { return const_cast<number_t*>(_hessian.data());}

    inline virtual void mapHessianMemory(number_t* d);

    virtual int copyB(number_t* b_) const {
      memcpy(b_, _b.data(), Dimension * sizeof(number_t));
      return Dimension; 
    }

    virtual const number_t& b(int i) const { assert(i < D); return _b(i);}
    virtual number_t& b(int i) { assert(i < D); return _b(i);}
    virtual number_t* bData() { return _b.data();}

    inline virtual void clearQuadraticForm();

    //! updates the current vertex with the direct solution x += H_ii\b_ii
    //! @returns the determinant of the inverted hessian
    inline virtual number_t solveDirect(number_t lambda=0);

    //! return right hand side b of the constructed linear system
    Eigen::Matrix<number_t, D, 1, Eigen::ColMajor>& b() { return _b;}
    const Eigen::Matrix<number_t, D, 1, Eigen::ColMajor>& b() const { return _b;}
    //! return the hessian block associated with the vertex
    HessianBlockType& A() { return _hessian;}
    const HessianBlockType& A() const { return _hessian;}

    virtual void push() { _backup.push(_estimate);}
    virtual void pop() { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();}
    virtual void discardTop() { assert(!_backup.empty()); _backup.pop();}
    virtual int stackSize() const {return _backup.size();}

    //! return the current estimate of the vertex
    const EstimateType& estimate() const { return _estimate;}
    //! set the estimate for the vertex also calls updateCache()
    void setEstimate(const EstimateType& et) { _estimate = et; updateCache();}

  protected:
    HessianBlockType _hessian;
    Eigen::Matrix<number_t, D, 1, Eigen::ColMajor> _b;
    EstimateType _estimate;
    BackupStackType _backup;
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

#include "base_vertex.hpp"

} // end namespace g2o


#endif

可以看到，BaseVertex是个抽象类，我们在使用的时候需要重写其中的虚函数。

D并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示，这里一定要区别开，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 myVertex: public g2::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*/;
      }
  }

举个栗子

• 曲线拟合：

$$y=exp(a{x}^2+bx=c)+w$$

其中a,b,c为曲线的参数，w为高斯噪声，假设我们有N个关于x,y的观测数据点，想根据这些数据点求出曲线的参数。那么可以用下面的最小二乘问题以估计曲线参数：
$$mi{n}_{a,b,c}\frac{1}{2}\sum _{i=1}^N||y_i-exp(a{x}_i^2+b{x}_i+c)|{|}^2$$
在这个问题中，待估计值是a,b,c，误差为一维标量，转化成图优化问题就是顶点为三维，边为一维。

class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;	//设初始值为(0 ，0 ，0）
    }

    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);	//update为增量△m 迭代的时候，估计值m=m + △m, m是向量（a,b,c）
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

因为优化变量是向量，可以直接通过加法来进行更新。但是对于李代数表示位姿VertexSE3Expmap，就不能直接通过加法进行更新。

• 李代数表示位姿VertexSE3Expmap

来自g2o官网，在这里g2o/types/sba/types_six_dof_expmap.h

它内部使用了四元数表达旋转，然后加上位移来存储位姿，同时支持李代数上的运算，比如对数映射（log函数）、李代数上增量（update函数）等操作。

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());        //更新方式 类似于左扰动？
  }
};

对于更新函数里面的Map操作有点疑惑，不太清楚它的作用，文档是这样描述的：Constructor in the dynamic-size matrix case.即动态矩阵的构造函数，查了它的定义：

inline Map(PointerArgType dataPtr, Index rows, Index cols, const StrideType& stride = StrideType())
      : Base(cast_to_pointer_type(dataPtr), rows, cols), m_stride(stride)
    {
      PlainObjectType::Base::_check_template_params();
    }

可以看到Map调用了_check_template_params(),也就是检查模板参数，所以猜想其是 用来检查参数的。

• 空间三维点VertexPointXYZ

这个也是g2o预定义的类型：

class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    
    VertexSBAPointXYZ();
    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;
    virtual void setToOriginImpl() {
      _estimate.fill(0);
    }

    virtual void oplusImpl(const number_t* update)
    {
      Eigen::Map<const Vector3> v(update);
      _estimate += v;	// 更新直接相加
    }
};

添加顶点

还是举几个例子：

• 曲线拟合：

// 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );

主要就是给Vertex添加估计值，id，然后把该顶点添加到优化器optimizer里面去。

• 空间点VertexSBAPointXYZ

也就是优化变量路标点landmark

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 );	// true => this node should be marginalized out during the optimization	 
        optimizer.addVertex ( point );
    }

由于我们要优化每一个路标点，所以有多少个路标点，就要添加多少个顶点。

G2O 中对路标点设置边缘化(Point->setMarginalized(true))是为了 在计算求解过程中，先消去路标点变量，实现先求解相机位姿，然后再利用求解出来的相机 位姿反过来计算路标点的过程，目的是为了加速求解，并非真的将路标点给边缘化掉;参考vins-mono的边缘化分析

边

在g2o官方GitHub上可以看到其继承关系，主要在这些文件里面：

g2o/g2o/core/hyper_graph.h
g2o/g2o/core/optimizable_graph.h
g2o/g2o/core/base_edge.h

边的类型：
BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。

多元边就是一条边连接多个顶点（>=3）。

在这里只放一下一元边的源码：

#ifndef G2O_BASE_UNARY_EDGE_H
#define G2O_BASE_UNARY_EDGE_H

#include <iostream>
#include <cassert>
#include <limits>

#include "g2o/config.h"
#include "g2o/stuff/misc.h"
#include "base_edge.h"
#include "robust_kernel.h"

namespace g2o {


  template <int D, typename E, typename VertexXi>
  class BaseUnaryEdge : public BaseEdge<D,E>
  {
    public:
      static const int Dimension = BaseEdge<D, E>::Dimension;
      typedef typename BaseEdge<D,E>::Measurement Measurement;
      typedef VertexXi VertexXiType;
      typedef typename Eigen::Matrix<number_t, D, VertexXiType::Dimension, D==1?Eigen::RowMajor:Eigen::ColMajor>::AlignedMapType JacobianXiOplusType;
      typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
      typedef typename BaseEdge<D,E>::InformationType InformationType;

      BaseUnaryEdge() : BaseEdge<D,E>(),
        _jacobianOplusXi(0, D, VertexXiType::Dimension)
      {
        _vertices.resize(1, nullptr);
      }

      virtual void resize(size_t size);

      virtual bool allVerticesFixed() const;

      virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);

      virtual OptimizableGraph::Vertex* createVertex(int i);

      /**
       * Linearizes the oplus operator in the vertex, and stores
       * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj
       */
      virtual void linearizeOplus();

      //! returns the result of the linearization in the manifold space for the node xi
      const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;}

      virtual void constructQuadraticForm();

      virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to);

      virtual void mapHessianMemory(number_t*, int, int, bool) {assert(0 && "BaseUnaryEdge does not map memory of the Hessian");}

      using BaseEdge<D,E>::resize;
      using BaseEdge<D,E>::computeError;

    protected:
      using BaseEdge<D,E>::_measurement;
      using BaseEdge<D,E>::_information;
      using BaseEdge<D,E>::_error;
      using BaseEdge<D,E>::_vertices;
      using BaseEdge<D,E>::_dimension;

      JacobianXiOplusType _jacobianOplusXi;

    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

#include "base_unary_edge.hpp"

} // end namespace g2o

#endif

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

自定义边的一般格式：

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
  }

最重要的就是computeError()，linearizeOplus()两个函数了。

举个栗子

• 曲线拟合：

还是上面的曲线拟合问题。

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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) ) ;
    }
    
    // 定义雅克比矩阵 即误差对优化变量的导数，在这里为对a,b,c的偏导数
    virtual void linearizeOplus() override {
        const CurveFittingVertex ∗v = static_cast<const CurveFittingVertex ∗> (_vertices[0]);
        const Eigen::Vector3d abc = v−>estimate();
        double y = exp(abc[0] ∗ _x ∗ _x + abc[1] ∗ _x + abc[2]);
        _jacobianOplusXi[0] = −_x ∗ _x ∗ y;
        _jacobianOplusXi[1] = −_x ∗ y;
        _jacobianOplusXi[2] = −y;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

雅克比矩阵是1*3的的矩阵（雅克比矩阵的维度为D误差×D优化变量）
$$\frac{\phi y}{\phi a}=-y{x}^2,\frac{\phi y}{\phi b}=-yx,\frac{\phi y}{\phi c}=-y$$

• 3D-2D点的PnP 问题

也就是最小化重投影误差问题，这个问题非常常见，使用最常见的二元边

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

对于computeError()的理解：

cam_map 函数功能是把相机坐标系下三维点（输入）用内参转换为图像坐标（输出），它的定义在
g2o/types/sba/types_six_dof_expmap.cpp:

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函数，它的功能是把世界坐标系下三维点变换到相机坐标系，函数在
g2o/types/sim3/sim3.h：

Vector3 map (const Vector3& xyz) const {
        return s*(r*xyz) + t;
      }

所以误差 = 观测 - 投影（计算）

_error的类型和边的类型一致……
对于linearizeOplus()的理解

还有一个很重要的函数就是linearizeOplus()，用来定义雅克比矩阵：

一些常用类型的顶点和边就在这里面定义：g2o/g2o/types/sba/types_six_dof_expmap.cpp

源码中的EdgeProjectXYZ2UV的linearizeOplus()的定义如下：

void EdgeProjectXYZ2UV::linearizeOplus() {
  // 从位姿顶点v1 取得位姿估计值T
  VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
  SE3Quat T(vj->estimate());
  // 从空间点 顶点v0 取得路标点在世界坐标系下的坐标估计值 xyz
  VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
  Vector3 xyz = vi->estimate();
  Vector3 xyz_trans = T.map(xyz);	// 将空间点xyz转换到相机坐标系下

  number_t x = xyz_trans[0];
  number_t y = xyz_trans[1];
  number_t z = xyz_trans[2];	
  number_t z_2 = z*z;				

  const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0));
 //	误差关于空间点P的导数 是一个2*3的矩阵
  Matrix<number_t,2,3,Eigen::ColMajor> tmp;
  tmp(0,0) = cam->focal_length;	
  tmp(0,1) = 0;
  tmp(0,2) = -x/z*cam->focal_length;

  tmp(1,0) = 0;
  tmp(1,1) = cam->focal_length;
  tmp(1,2) = -y/z*cam->focal_length;
   
  _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();
// 误差关于位姿se3的导数 是一个2*6的矩阵
  _jacobianOplusXj(0,0) =  x*y/z_2 *cam->focal_length;
  _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *cam->focal_length;
  _jacobianOplusXj(0,2) = y/z *cam->focal_length;
  _jacobianOplusXj(0,3) = -1./z *cam->focal_length;
  _jacobianOplusXj(0,4) = 0;
  _jacobianOplusXj(0,5) = x/z_2 *cam->focal_length;

  _jacobianOplusXj(1,0) = (1+y*y/z_2) *cam->focal_length;
  _jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length;
  _jacobianOplusXj(1,2) = -x/z *cam->focal_length;
  _jacobianOplusXj(1,3) = 0;
  _jacobianOplusXj(1,4) = -1./z *cam->focal_length;
  _jacobianOplusXj(1,5) = y/z_2 *cam->focal_length;
}

该雅克比矩阵和14讲上164页的推导结果一致。成员变量_jacobianOplusXi是误差到空间点的导数，_jacobianOplusXj是误差到相机位姿的导数，以李代数的左乘扰动表达。稍有差别的是，g2o相机里用f统一描述fx,fy，并且李代数定义顺序不同（g2o是旋转在前，平移在后，书中是平移在前，旋转在后），所以矩阵前三列和后三列与书中的定义是相反的，此外都一致。

添加边

还是举几个栗子

• 曲线拟合

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

比较简单，不再赘述

• 3d-2dPnp

其实还是那个最小化重投影误差问题。我们优化了位姿和空间点，有两个顶点，对应的边为二元边：

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 );	// 这里的 0 1与我们定义边的时候指定的顶点顺序有关？
        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)(像素坐标u,v)

_vertices[0] 对应的是 VertexSBAPointXYZ 类型的顶点，也就是三维点，_vertices[1] 对应的是VertexSE3Expmap 类型的顶点，也就是位姿pose。因此前面 1 对应的就应该是 pose，0对应的 应该就是三维点。

6设置优化参数并开始执行优化

设置SparseOptimizer的初始化、迭代次数、保存结果等

SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)	// 初始化
SparseOptimizer::optimize(int iterations, bool online)	// 设置迭代次数，然后就可以进行图优化了

参考：
视觉SLAM14讲第二版
从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码