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画的。小绿只会这个土办法了):

(0)

相关推荐

  • PCL common中常见的基础功能函数

    pcl_common中主要是包含了PCL库常用的公共数据结构和方法,比如PointCloud的类和许多用于表示点,曲面,法向量,特征描述等点的类型,用于计算距离,均值以及协方差,角度转换以及几何变化的 ...

  • 机器学习入门

    首先我想说的是,欢迎批评.从纷杂的想法中总结出一点东西,是一个及其困难也非常有意思的工作,不可避免会犯错误.发现错误并且改正,同样是一个非常有意思的过程.我觉得不确定的用紫色标记. 机器学习,mach ...

  • 法向量、旋转矩阵计算(五十一)

    法向量、旋转矩阵计算(五十一)

  • g2o代码阅读 高翔Slambook第七讲:3d2d非线性优化

    在3d2d求取位姿的程序中同样也用到了g2o进行bundle adjustment来使用非线性优化的方式求取位姿.和第六讲的g2o进行曲线拟合的方式类似,进行3d2d位姿求取中使用g2o的过程可以完全 ...

  • 高翔Slambook第七讲代码解读(三角测量)

    在前面几期中,小绿简单的解读了第七讲的几个程序,运行这些程序或调用这些程序包装成的函数可以实现: 两帧图像中特征点的寻找与匹配: 两帧图像所对应的相机位姿变化的求解,包括2d-2d.3d-2d.3d- ...

  • 高翔Slambook第七讲代码解读(3d-3d位姿估计)

    上回咱们读完了pose_estimation_3d2d.cpp这个程序,也找到了3d-2d位姿估计与2d-2d位姿估计之间的联系与差别: 在2d-2d使用对极几何求解相机位姿时直接调用了OpenCV提 ...

  • 高翔Slambook第七讲代码解读(3d-2d位姿估计)

    上回咱们读完了pose_estimation_2d2d.cpp这个文件,基本上明白了通过对极几何计算相机位姿变换的过程,简单地说就是:你给我两帧图像,我给你算个R.t. 我们按部就班,跟着小绿来看一下 ...

  • 高翔Slambook第七讲代码解读(2d-2d位姿估计)

    大家好我是小绿,给自己起了英文名叫gREEN,为了方便大家记住我已经把它写在车牌上了.这里感谢福特公司,个人很喜欢猛禽,于是改了张图以后就作为小绿的封面了. 图源:百度 修图:gREEN 下面开始本期 ...

  • 高翔Slambook第七讲代码解读(特征点提取)

    大家好我是小绿. 作为一个视觉SLAM的入门学徒,高翔的书我看了一遍,视频也跟了一遍,代码在自己的电脑上也跑过,但总觉得跟啥都没学没有太大区别. 于是乎决定开始看代码.由于不是计算机专业的本科,看代码 ...

  • 第六讲:亲子阅读能够促进亲子关系

    邓兆行,中共党员,海珠区第二实验小学教育集团办公室主任.广州市"百千万人才培养工程"第四批名教师培养对象,海珠区骨干教师,海珠区小学语文青年教师成长联盟导师,广东省PBL项目式学习 ...

  • 张老师讲作文   阅读与理解(第六讲)

    张老师讲作文 &#160; 阅读与理解(第六讲)

  • 「周易登堂」第六讲 河出图、洛出书,五行之数现矣

    本节主要讲河图.洛书的概念及其与五行的关系. <周易>经文中只字未提河图.洛书,只在孔子传文中有只言片语.<易·系辞传·上>只在第十一章有一句"河出图,洛出书,圣人则 ...