g2o代码阅读 高翔Slambook第六讲：曲线拟合

前些日子小绿做了一些高翔slam前端部分的代码解读，其中遇到g2o的部分基本上就黑箱化略过了。然而g2o是bundle adjustment中的关键，因此还是有必要对g2o进行一些系统的学习。

首先，为什么要使用g2o？

用我自己理解的话说，就是：代码化一个图模型的思想，用这个图模型去求解或者去优化需要求解或优化的变量。这里g2o的作用是：提供代码化图模型的工具——节点、边的定义，以及提供求解或者优化变量的途径——误差计算函数，因为要优化一些变量的实质就是要使计算得到的值与当前已知值之间的差异达到极小。

进而，如何使用g2o？

大致步骤可以分为：

1.在主程序运行之前：定义节点、边，包括内部的初始化函数、更新函数、误差计算函数、输入输出函数等等；

2.在主程序内部：实例化g2o求解器、选择迭代求解方式、实例化所使用的节点与边来逐步建立图模型、设置迭代次数并开始求解。

这里，网上也应该有比这更加全面细致的g2o讲解，在这里小绿就不加赘述了，不如一起拿高翔的代码作为例子来看一看。

我们可以以第6讲中，使用g2o进行曲线拟合的代码为例：

首先进行了所使用的节点与边的定义：

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
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 {}
};

这里所定义的节点类继承于g2o内置的类BaseVertex，所携带的待优化变量个数为3，存储形式为vector3d。这里3是由于所进行的曲线拟合需要确定三个未知数的量，因而待优化变量为3，而vector3d则是所选取的一种存储这三个变量的方式。进而定义了这个节点的重置函数与更新函数，以及最后的读写函数。这里重置函数、更新函数、读写函数都不是咱们用户自己调用的，而是在迭代优化过程中由g2o求解器去调用的，这里只要写出来摆在这就好（其中更新函数只是将当前值与更新增量进行加法，是迭代求解的思想）。

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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
};

这里定义的边是一个一元边，继承于BaseUnaryEdge。后面尖括号里的<1,double,CurveFittingVertex>则分别是：误差项维数、输入数据的变量形式、与这条边相连的节点类型。不难看出，这里误差就是Δy，是1维的，并且精确度是double，而与这个一元边相连的节点则是刚定义好的节点CurveFittingVertex。进而定义了这个边的一个带参构造函数，这里输入参数是一个double类型的变量x，输入后将被赋值给这个边类对象的一个内部变量_x中，来参与各种各样的运算。最后开始定义最关键的误差计算函数：

误差计算函数实则要计算这样一个误差：

具体到本章的问题上则是：

其中这里的abc则是待优化变量，也就是刚定义好的节点中存储的三个变量。

来看看误差计算函数中的具体语句：第一句是实例一个刚才定义好的节点类型的指针*v，用来调用这条边所连接的节点，由于是个一元边，所连接的节点就一个，也就是0号节点_verices[0]；第二句话则是掏出这个节点内部的待优化变量，也就是通过刚才的节点指针v去调用他指向的节点_verices[0]内部的estimate()函数，将他内部用来存储待优化变量的vector3d赋值给一个新定义的vector3d变量abc，此后便可以拿着这个abc进行相应的计算；第三句话则是完成上面公式的误差运算，其中yreal这个真实值是通过_measurement体现的（这个measurement值怎么进来的在后文会有解释）。

这里节点与边的定义中都出现了EIGEN_MAKE_ALIGNED_OPERATOR_NEW，这里是为了解决在new一个这样类型的对象时解决对齐问题。具体可以参考csdn上的blog，咱们在写的时候加上就好。

主函数中，通过以下代码来设定好带有误差的数据：

double a=1.0, b=2.0, c=1.0;         // 真实参数值
   int N=100;                          // 数据点
   double w_sigma=1.0;                 // 噪声Sigma值
   cv::RNG rng;                        // OpenCV随机数产生器
   double abc[3] = {0,0,0};            // abc参数的估计值

vector<double> x_data, y_data;      // 数据
   
   cout<<"generating data: "<<endl;
   for ( int i=0; i<N; i++ )
   {
       double x = i/100.0;
       x_data.push_back ( x );
       y_data.push_back (
           exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
       );
       cout<<x_data[i]<<" "<<y_data[i]<<endl;
   }

可以看出是一个带有高斯噪声的

而在这个问题中e的指数中关于x的三个系数被设置为待求量abc，下面的代码则是开始使用g2o去求解这三个变量：

// 构建图优化，先设定g2o
   typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
   Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
   Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) ); // 矩阵块求解器
   // 梯度下降方法，从GN, LM, DogLeg 中选
   g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr) );

g2o::SparseOptimizer optimizer;     // 图模型
   optimizer.setAlgorithm( solver );   // 设置求解器
   optimizer.setVerbose( true );       // 打开调试输出

这里建立了一个g2o求解器，并设置好所选用的各种参数：使用g2o块求解器（待优化变量有3个，误差维数为1）、选用线性求解器、迭代策略选用LM。

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

这里添加了图模型中所使用的顶点，并将其命名为顶点v。通过调用v中的setEstimate（）函数将待优化变量的初始值设定为000，并将这个顶点在整个求解器中的id设置为0，最后添加到求解器中。

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

这里开始往图模型中添加边。我们知道，所选取的边是一个一元边，那么和他连接的节点只有一个，并且这条边在计算误差时不仅需要这个节点所提供的参数估计值，也需要一个Y-real和用来和估计值一起联合计算Y-estimated的自变量值x。到现在就基本上已经清楚这条边的存在需要哪些量的支持了，那么每一句话就读得通了：第一行是在实例化这条边的同时，传入自变量值x；第二行是将这条边在整个求解器中设置编号为i；第三行是将这条边与节点v相连，并让v为这条边连接的第一个节点（从0开始计数）；第四行则是通过边内的函数setMeasurement（），将y_data[i]输入进去成为Y-real用来计算误差；第五行是信息矩阵，在此默认为一个1*1的矩阵，其值等于所选取的高斯噪声的方差平方分之一；最后一行是将设置好的边添加进入求解器。

// 执行优化
   cout<<"start optimization"<<endl;
   chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
   optimizer.initializeOptimization();
   optimizer.optimize(100);
   chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
   chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
   cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

最后开始让求解器执行优化，迭代次数设置为100。

// 输出优化值
   Eigen::Vector3d abc_estimate = v->estimate();
   cout<<"estimated model: "<<abc_estimate.transpose()<<endl;

优化得到的值将存放在节点中，再次通过节点内部的estimate()函数将其掏出，得到优化之后的结果。

所以说，通过本程序的代码设计，小绿更倾向于认为：节点确实是待优化变量，而边则是对待优化变量的限制，或者钳位。也就是说，不必硬要往图模型上靠，也就是边必须以节点为首末，边的存在其实只是为了通过误差计算来限制与其连接的节点的位置，在误差计算的过程中，一部分量是在定义边时初始化好的，一部分量是定义好边之后以setMeasurement进行输入的，再加上与这条边相连的节点所提供的待优化变量，这些量将通过预设好的误差计算算法来计算误差，并最后用来优化与这条边相连的节点所携带的待优化变量。

因此拟合曲线这个问题，我给出的“图模型”是这样的：

有点捞。但一元边要有一元边的样子，只连接一个节点就好了。

这里展示一下初始生成的带有噪声的函数曲线和优化之后的拟合曲线（将这个代码跑出来的数据存入txt，然后用matlab画的。小绿只会这个土办法了）：