G2O非线性优化

2/22/2017来源:ASP.NET技巧人气:1808

上一篇文章介绍最小二乘法,本文介绍g2o实现最小二乘法。 g2o即General Graph Optimization,它是一个基于图优化的库。至于图优化理论,参照半闲居士的博客 接着上回曲线拟合问题,拟合y=Asin(Bx)+Ccos(Dx),已知N组数据(xi,yi),i=0,1,⋯N−1,待优化变量V=[A,B,C,D],优化问题即为: V=argmin(∑i=0N−1(y−Asin(BX)−Ccos(Dx))2) 整个图中只有一个顶点,即待优化变量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)); // 误差函数:观测量减去估计量 } // Jacobin void 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∂[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》