大局观之整体流程

SLAM里，图优化一般可以分解为两个任务：

1. 构建图。机器人位姿作为顶点，位姿间关系作为边。
2. 优化图。调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。

这里以g2o源码说明文件中的类图为基础，自己做了一个流程说明。

讯享网蓝色框可以说明图是如何构成的，绿色框中则是说明了一些求解时的基本信息。小太阳是点出了整个类图最中心的部分SparseOptimizer，而橙色的箭头则是大概指出了写代码的流程。

理解了流程之后，就可以对照一下代码：

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1 // 第1步：创建一个线性求解器LinearSolver,有以下五种： // LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS // LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS // LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver // LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver // LinearSolverEigen： 依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 第2步：创建BlockSolver。并用上面定义的线性求解器初始化 // BlockSolver有两种定义方式，由使用场景决定 // 1)固定变量solver,定义为：using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >; // p表示pose在流形下的最小表示，l表示landmark的维度 // 2)可变尺寸solver，有时p和l不能一开始确定，定义为： // using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>; // 常用类型如下： // BlockSolver_6_3 ：表示pose 是6维，观测点是3维。用于3D SLAM中的BA // BlockSolver_7_3：在BlockSolver_6_3 的基础上多了一个scale // BlockSolver_3_2：表示pose 是3维，观测点是2维 Block* solver_ptr = new Block( linearSolver ); // 第3步：创建总求解器solver，选择一种优化算法，再用上述块求解器BlockSolver初始化 // 优化算法有3种： 高斯牛顿GN, LM, DogLeg g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr ); // 第4步：创建稀疏优化器（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); //100次迭代 

抠细节

顶点Vertex

关于顶点的内容主要按照顺序从三个方面逐步深入：

• 从源码入手理解顶点Vertex及其参数
• 怎么自己定义顶点？
• 怎么把顶点加入图中

A. 理解顶点Vertex及其参数

首先，我们可以从上面的类图中截取出关于顶点的部分，牢记is a箭头指向的是基类，也可以通过查看源码清晰的看到继承关系。
注意这里的BaseVertex<D,T>中参数的含义：

相当于说，顶点的基本类型是BaseVertex，我们之后的对顶点进行定义也是围绕这个类展开的。

B. 怎么定义顶点？

g2o中的预定义顶点

常用的一些顶点类型，可以直接用g2o内部的定义：

讯享网//2D pose Vertex, (x,y,theta) VertexSE2 : public BaseVertex<3, SE2> //6d vector (x,y,z,qx,qy,qz) 注意这里省略了四元数的实部w VertexSE3 : public BaseVertex<6, Isometry3> VertexPointXY : public BaseVertex<2, Vector2> VertexPointXYZ : public BaseVertex<3, Vector3> VertexSBAPointXYZ : public BaseVertex<3, Vector3> // SE3 Vertex 内部用一个转换矩阵(T)参数化，外部用该矩阵的指数映射参数化 VertexSE3Expmap : public BaseVertex<6, SE3Quat> // SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz)  // qw假定为正，否则用qx,qy,qz表示旋转时会导致模糊表示 VertexCam : public BaseVertex<6, SBACam> // Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) VertexSim3Expmap : public BaseVertex<7, Sim3> 

自定义顶点

重新定义顶点需要考虑重写(override) 下面的函数:

// 读/写函数，一般情况下不需要进行这两个操作，声明一下即可 virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; // 顶点更新函数，非常重要！主要用于增量△x的计算。 // 当根据增量方程算出增量后，就要通过这个函数对估计值进行调整 virtual void oplusImpl(const number_t* update); // 顶点重置函数，设定待优化量的初始值 virtual void setToOriginImpl(); 

自定义顶点，就相当于要以BaseVertex为基类，再写一个自它继承下来的新顶点类。先看具体的例子，然后再把这个具体的例子抽象成代码模板。

例子1：slam十四讲中的曲线拟合 ch6/g2o_curve_fitting/main.cpp

讯享网class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d> { 
    public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 把顶点的初值设置为0,0,0 virtual void setToOriginImpl() { 
    _estimate << 0,0,0; } // 更新，相当于x = x + △x, 这里顶点类型为Eigen::Vector3d可以直接相加 virtual void oplusImpl( const double* update ) { 
    _estimate += Eigen::Vector3d(update); } // 存盘和读盘 virtual bool read( istream& in ) { 
   } virtual bool write( ostream& out ) const { 
   } }; 

例子2：三维点作为待优化变量

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

例子3：g2o源码 g2o/types/sba/types_six_dof_expmap.h

讯享网/* brief SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map */ // 6是优化变量维度，即6维李代数 // 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()); } }; 

为什么此处更新不是直接相加？
因为变换矩阵不满足加法封闭。

为什么相机位姿顶点类的VertexSE3Expmap使用李代数表示相机位姿，而没有使用旋转矩阵和平移矩阵？
因为旋转矩阵是有约束的矩阵，它必须是正交矩阵且行列式为1。所以，如果要用它作为优化变量就会引入额外的约束条件，从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示后，就可以把位姿估计变成无约束的优化问题，求解难度降低。

抽象后的代码模板：

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*/; } } 

C. 怎么把顶点加入图中

上接slam十四讲中的曲线拟合的例子1：

讯享网// 往图中增加顶点 CurveFittingVertex* v = new CurveFittingVertex(); v->setEstimate( Eigen::Vector3d(0,0,0) ); //设置初始估计值 v->setId(0); //定义节点编号 optimizer.addVertex( v ); //optimizer是稀疏优化器的对象 

接着上面一点中优化三维点的例子2：

 int index = 1; // 遍历每一个landmark for ( const Point3f p:points_3d ) { 
    //每一次都新建一个顶点对象 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 ); } 

边Edge

边的内容也按照顺序从三个方面逐步深入：

• 理解边及其参数
• 怎么自己定义边？
• 怎么把边加入图中

A. 理解边及其参数

边的继承关系和顶点类似，只是这里有三个通用的模板：一元边、二元边、多元边。关于多元边的使用，参考github上的这个issue：#165
这里对几个参数进行说明：

举例：用边表示三维点投影到图像平面的重投影误差

讯享网// 二元边 // 测量值的维度为2，其数据类型是Vector2D， // 连接这条边的两个顶点类型，一个是三维点，一个是李群位姿 BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap> 

B. 怎么定义边？

和顶点类似，定义边的时候也要重写一些成员函数：

virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; // 使用当前顶点的值计算估计值和测量值之间的误差 virtual void computeError(); // 在当前顶点的值下，该误差对待优化变量的偏导数，即Jacobian virtual void linearizeOplus(); // 其他重要的成员变量和函数 _measurement：存储观测值 _error：存储computeError() 函数计算的误差 _vertices[]：存储顶点信息，如二元边的_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1） setId(int)：来定义边的编号（决定了在H矩阵中的位置） setMeasurement(type) 函数来定义观测值 setVertex(int, vertex) 来定义顶点 setInformation() 来定义协方差矩阵的逆 

这里同样先举具体的例子，再写出抽象的代码流程。

例子1：slam十四讲中的曲线拟合 ch6/g2o_curve_fitting/main.cpp

讯享网// 一元边 // 模板参数：观测值维度，测量值类型，连接顶点类型 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) ) ; } virtual bool read( istream& in ) { 
   } virtual bool write( ostream& out ) const { 
   } public: double _x; // x 值， y 值为 _measurement }; 

例子2：3D-2D点的PnP 问题，即最小化重投影误差问题 g2o/types/sba/types_six_dof_expmap.h

※这里定义的时候一定要特别注意，哪个顶点对应的_vertices[0],哪个对应的_vertices[1], 之后把边添加到图中时，即edge->setVertex()，就要遵循这个顺序！

//继承了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 注意是对应_vertices[1] const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]); // 顶点v2 注意是对应_vertices[0] 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); //cam_map()用于把相机坐标系下三维点用内参转换为图像坐标 //v1->estimate().map()表示把世界坐标系下三维点变换到相机坐标系 _error = obs-cam->cam_map(v1->estimate().map(v2->estimate())); } //3. 线性增量函数，也就是雅克比矩阵J的计算方法 // 详细内容参考slam14讲的第七讲 公式7.45-47 virtual void linearizeOplus(); //4. 相机参数 CameraParameters * _cam; bool read(std::istream& is); bool write(std::ostream& os) const; }; 

C. 向图添加边

例子1：曲线拟合添加一元边 slambook/ch6/g2o_curve_fitting/main.cpp

讯享网// 往图中增加边 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] ); // 观测数值就是实际得到的数据点y坐标 // 信息矩阵：协方差矩阵的逆 edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); optimizer.addEdge( edge ); } 

例子2：pnp优化添加二元边 slambook/ch7/pose_estimation_3d2d.cpp
对照B中的例子2一起看

index = 1; for ( const Point2f p:points_2d ) { 
    g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV(); edge->setId ( index ); //这里对边连接的两个顶点进行设置，v[0]是三维点，v[1]是位姿，不能调换顺序！要和之前定义的顺序统一 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++; } 

参考资料：
从零开始一起学习SLAM | 掌握g2o顶点编程套路
从零开始一起学习SLAM | 掌握g2o边的代码套路
从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码