G2O非线性优化

2019-11-08 19:24:41

字体：大中小

来源：转载

供稿：网友

上一篇文章介绍最小二乘法，本文介绍g2o实现最小二乘法。 g2o即General Graph Optimization，它是一个基于图优化的库。至于图优化理论，参照半闲居士的博客接着上回曲线拟合问题，拟合y=Asin(Bx)+Ccos(Dx) $y=Asin(Bx)+Ccos(Dx)$ ，已知N组数据(xi,yi),i=0,1,⋯N−1 $(x_i, y_i), i = 0, 1, /cdots N-1$ ，待优化变量V=[A,B,C,D] $V = [A, B, C, D]$ ，优化问题即为： V=argmin(∑i=0N−1(y−Asin(BX)−Ccos(Dx))2) $V = {argmin} /left(/sum_{i=0}^{N-1}(y-Asin(BX)-Ccos(Dx))^2 /right)$ 整个图中只有一个顶点，即待优化变量V，连接只有一个顶点的边即一元边（Unary Edge），使用g2o主要有以下几个步骤： 1. 定义顶点和边的类型； 2. 选择优化算法； 3. 构建图； 4. 调用 g2o 进行优化，返回结果。

由于g2o中没有本例类型顶点和边，因此首先需要自己定义

// 曲线模型的顶点，模板参数：优化变量维度和数据类型class CurveFittingVertex: public g2o::BaseVertex<4, Vector4d>{public: EIGEN_MAKE_ALIGNED_OperaTOR_NEW CurveFittingVertex(); virtual void setToOriginImpl() // 重置 { _estimate << 0, 0, 0, 0; } virtual void oplusImpl(const double* update_);// 更新 //输入输出可以不定义 bool read(std::istream& is) {} bool write(std::ostream& os) const {}};// 顶点构造函数CurveFittingVertex::CurveFittingVertex() : BaseVertex<4, Eigen::Vector4d>() { }// 顶点更新函数void CurveFittingVertex::oplusImpl(const double* update_) { Eigen::Map<const Vector4d> up(update_); _estimate += up;}

顶点的定义主要需要定义_estimate的更新函数oplusImpl和setToOriginImpl。

// 误差模型模板参数：观测值维度，观测量类型，连接顶点类型class CurveFittingEdge: public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>{public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CurveFittingEdge(); // 计算曲线模型误差 void computeError(); virtual void linearizeOplus(); bool read(std::istream& is) {} bool write(std::ostream& os) const {}public: double _x;};// 边的构造函数CurveFittingEdge::CurveFittingEdge() : g2o::BaseUnaryEdge<1, double, CurveFittingVertex>(){}// 误差函数void CurveFittingEdge::computeError(){ const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>( _vertices[0]); const Vector4d abcd = v->estimate(); double A = abcd[0], B = abcd[1], C = abcd[2], D = abcd[3]; _error(0,0) = _measurement - (A * sin(B*_x) + C * cos(D*_x)); // 误差函数：观测量减去估计量}// Jacobinvoid CurveFittingEdge::linearizeOplus(){ CurveFittingVertex *vi = static_cast<CurveFittingVertex *>(_vertices[0]); Vector4d abcd = vi->estimate(); double A = abcd[0], B = abcd[1], C = abcd[2], D = abcd[3]; // 误差项对待优化变量的Jacobin _jacobianOplusXi(0,0) = -sin(B*_x); _jacobianOplusXi(0,1) = -A*_x*cos(B*_x); _jacobianOplusXi(0,2) = -cos(D*_x); _jacobianOplusXi(0,3) = C*_x*sin(D*_x);}

误差函数e=y−Asin(Bx)−Ccos(Dx) $e = y - Asin(Bx)-Ccos(Dx)$ ,因此 ∂e∂[ABCD]=[−sin(Bx) −Axcos(Bx) −cos(Dx) Cxsin(Dx)] $/frac{/partial e}{/partial [ABCD]} = [-sin(Bx) / / -Axcos(Bx) / / -cos(Dx) / / Cxsin(Dx)]$ 边的定义主要需要定义误差函数和计算Jacobin。当然边的Jacobin函数可以不定义，g2o会采用如上篇博客的数值求导方法。如果我们能够推导出雅可比矩阵的解析形式并告诉优化库，就可以避免数值求导中的诸多问题。

然后是图的构造和求解了

#include <iostream>#include <g2o/core/base_vertex.h>#include <g2o/core/base_unary_edge.h>#include <g2o/core/block_solver.h>#include <g2o/core/optimization_algorithm_levenberg.h>#include <g2o/core/optimization_algorithm_gauss_newton.h>#include <g2o/core/optimization_algorithm_dogleg.h>#include <g2o/solvers/dense/linear_solver_dense.h>#include <Eigen/Core>#include <Eigen/Dense>#include <opencv2/core/core.hpp>#include <cmath>#include <chrono>using namespace std;using namespace Eigen;int main(){ double a = 5.0, b = 1.0, c = 10.0, d = 2.0; // 真实参数值 int N = 100; double w_sigma = 2.0; // 噪声值Sigma cv::RNG rng; // 随机数产生器OpenCV double abcd[4] = {0, 0, 0, 0}; // 参数的估计值abc vector<double> x_data, y_data; cout << "generate random data" << endl; for(int i = 0; i < N; i++) { //generate a random variable [-10 10] double x = rng.uniform(-10., 10.); double y = a * sin(b*x) + c * cos(d *x) + rng.gaussian(w_sigma); x_data.push_back(x); y_data.push_back(y); cout << x_data[i] << " , " << y_data[i] << endl; } // 构建图优化，先设定g2o // 矩阵块：每个误差项优化变量维度为4 ，误差值维度为1 typedef g2o::BlockSolver< g2o::BlockSolverTraits<4, 1> > Block; // 线性方程求解器：稠密的增量方程 Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); Block* solver_ptr = new Block(linearSolver); // 矩阵块求解器 // 梯度下降方法，从GN, LM, DogLeg 中选 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr ); // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr ); g2o::SparSEOptimizer optimizer; // 图模型 optimizer.setAlgorithm( solver ); // 设置求解器 optimizer.setVerbose(true); // 打开调试输出 // 往图中增加顶点 CurveFittingVertex *v = new CurveFittingVertex(); // 设置优化初始估计值 v->setEstimate( Eigen::Vector4d(1.6, 1.4, 6.2, 1.7)); v->setId(0); v->setFixed(false); optimizer.addVertex(v); // 往图中增加边 for(int i = 0; i < N; i++) { CurveFittingEdge* edge = new CurveFittingEdge(); edge->setId(i+1); edge->setVertex(0, v); // 设置连接的顶点 edge->setMeasurement( y_data[i] ); // 观测数值 // 信息矩阵：协方差矩阵之逆 edge->setInformation( Eigen::Matrix<double, 1, 1>::Identity() * 1 /(w_sigma* w_sigma) ); edge->_x = x_data[i]; optimizer.addEdge( edge ); } // 执行优化 cout << "strat 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; // 输出优化值 Eigen::Vector4d abcd_estimate = v->estimate(); cout << "estimated module: " << endl << abcd_estimate << endl; return 0;}

另外g2o中设置核函数、边缘化等等后续再更新。

引用: [1] 《g2o: A General Framework for Graph Optimization》 [2] 《slambook by gaoxiang》

上一篇：PAT甲级1119

下一篇：HDU3118-利用二进制状态和二分图的性质（好题）