G2O非線性優(yōu)化

2019-11-08 19:24:41

字體：大中小

供稿：網(wǎng)友

上一篇文章介紹最小二乘法，本文介紹g2o實(shí)現(xiàn)最小二乘法。 g2o即General Graph Optimization，它是一個(gè)基于圖優(yōu)化的庫(kù)。至于圖優(yōu)化理論，參照半閑居士的博客接著上回曲線擬合問(wèn)題，擬合y=Asin(Bx)+Ccos(Dx) $y=Asin(Bx)+Ccos(Dx)$ ，已知N組數(shù)據(jù)(xi,yi),i=0,1,?N?1 $(x_i, y_i), i = 0, 1, /cdots N-1$ ，待優(yōu)化變量V=[A,B,C,D] $V = [A, B, C, D]$ ，優(yōu)化問(wèn)題即為： 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)$ 整個(gè)圖中只有一個(gè)頂點(diǎn)，即待優(yōu)化變量V，連接只有一個(gè)頂點(diǎn)的邊即一元邊（Unary Edge），使用g2o主要有以下幾個(gè)步驟： 1. 定義頂點(diǎn)和邊的類型； 2. 選擇優(yōu)化算法； 3. 構(gòu)建圖； 4. 調(diào)用 g2o 進(jìn)行優(yōu)化，返回結(jié)果。

由于g2o中沒(méi)有本例類型頂點(diǎn)和邊，因此首先需要自己定義

// 曲線模型的頂點(diǎn)，模板參數(shù)：優(yōu)化變量維度和數(shù)據(jù)類型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 {}};// 頂點(diǎn)構(gòu)造函數(shù)CurveFittingVertex::CurveFittingVertex() : BaseVertex<4, Eigen::Vector4d>() { }// 頂點(diǎn)更新函數(shù)void CurveFittingVertex::oplusImpl(const double* update_) { Eigen::Map<const Vector4d> up(update_); _estimate += up;}

頂點(diǎn)的定義主要需要定義_estimate的更新函數(shù)oplusImpl和setToOriginImpl。

// 誤差模型模板參數(shù)：觀測(cè)值維度，觀測(cè)量類型，連接頂點(diǎn)類型class CurveFittingEdge: public g2o::BaseUnaryEdge<1, double, CurveFittingVertex>{public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CurveFittingEdge(); // 計(jì)算曲線模型誤差 void computeError(); virtual void linearizeOplus(); bool read(std::istream& is) {} bool write(std::ostream& os) const {}public: double _x;};// 邊的構(gòu)造函數(shù)CurveFittingEdge::CurveFittingEdge() : g2o::BaseUnaryEdge<1, double, CurveFittingVertex>(){}// 誤差函數(shù)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)); // 誤差函數(shù)：觀測(cè)量減去估計(jì)量}// 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]; // 誤差項(xiàng)對(duì)待優(yōu)化變量的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);}

誤差函數(shù)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)]$ 邊的定義主要需要定義誤差函數(shù)和計(jì)算Jacobin。當(dāng)然邊的Jacobin函數(shù)可以不定義，g2o會(huì)采用如上篇博客的數(shù)值求導(dǎo)方法。如果我們能夠推導(dǎo)出雅可比矩陣的解析形式并告訴優(yōu)化庫(kù)，就可以避免數(shù)值求導(dǎo)中的諸多問(wèn)題。

然后是圖的構(gòu)造和求解了

#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; // 真實(shí)參數(shù)值 int N = 100; double w_sigma = 2.0; // 噪聲值Sigma cv::RNG rng; // 隨機(jī)數(shù)產(chǎn)生器OpenCV double abcd[4] = {0, 0, 0, 0}; // 參數(shù)的估計(jì)值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; } // 構(gòu)建圖優(yōu)化，先設(shè)定g2o // 矩陣塊：每個(gè)誤差項(xiàng)優(yōu)化變量維度為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 ); // 設(shè)置求解器 optimizer.setVerbose(true); // 打開調(diào)試輸出 // 往圖中增加頂點(diǎn) CurveFittingVertex *v = new CurveFittingVertex(); // 設(shè)置優(yōu)化初始估計(jì)值 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); // 設(shè)置連接的頂點(diǎn) edge->setMeasurement( y_data[i] ); // 觀測(cè)數(shù)值 // 信息矩陣：協(xié)方差矩陣之逆 edge->setInformation( Eigen::Matrix<double, 1, 1>::Identity() * 1 /(w_sigma* w_sigma) ); edge->_x = x_data[i]; optimizer.addEdge( edge ); } // 執(zhí)行優(yōu)化 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; // 輸出優(yōu)化值 Eigen::Vector4d abcd_estimate = v->estimate(); cout << "estimated module: " << endl << abcd_estimate << endl; return 0;}

另外g2o中設(shè)置核函數(shù)、邊緣化等等后續(xù)再更新。

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

上一篇：PAT甲級(jí)1119

下一篇：HDU3118-利用二進(jìn)制狀態(tài)和二分圖的性質(zhì)（好題）