// ceres 版本
1 #include <opencv2/core/core.hpp> 2 #include <ceres/ceres.h> 3 #include <chrono> 4 5 using namespace std; 6 7 // 代價函數的計算模型 8 struct CURVE_FITTING_COST 9 { 10 CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {} 11 // 殘差的計算 12 template <typename T> 13 bool operator() ( 14 const T* const abc, // 模型參數，有3維 當沒有必要分類的時候 就用一個數組來存儲未知的系數,方便管理，而不是設3個變量，之后在()重載函數的形式參數個數變為3個 15 T* residual ) const // 殘差 16 { 17 residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c) 18 return true; 19 } 20 const double _x, _y; // x,y數據 21 }; 22 23 int main ( int argc, char** argv ) 24 { 25 double a=1.0, b=2.0, c=1.0; // 真實參數值 26 int N=100; // 數據點 27 double w_sigma=1.0; // 噪聲Sigma值(根號下方差) 28 cv::RNG rng; // OpenCV隨機數產生器 29 double abc[3] = {0.8,2.1,0.9}; // abc參數的估計值 (修改初始值 下面求解迭代過程會不同) 30 31 vector<double> x_data, y_data; // 數據 32 33 /*生成符合曲線的樣本*/ 34 cout<<"generating data: "<<endl; //下面是從真實的曲線中取得樣本數據 35 for ( int i=0; i<N; i++ ) 36 { 37 double x = i/100.0; 38 x_data.push_back ( x ); 39 y_data.push_back ( 40 exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma ) 41 ); 42 //cout<<x_data[i]<<" "<<y_data[i]<<endl;//輸出生成數據 43 } 44 45 // 構建最小二乘問題 46 ceres::Problem problem; 47 for ( int i=0; i<N; i++ ) 48 { 49 /* 第一個參數 CostFunction* : 描述最小二乘的基本形式即代價函數 例如書上的116頁fi(.)的形式 50 * 第二個參數 LossFunction* : 描述核函數的形式 例如書上的ρi（.） 51 * 第三個參數 double* : 待估計參數（用數組存儲） 52 * 這里僅僅重載了三個參數的函數，如果上面的double abc[3]改為三個double a=0 ,b=0,c = 0; 53 * 此時AddResidualBlock函數的參數除了前面的CostFunction LossFunction 外后面就必須加上三個參數 分別輸入&a,&b,&c 54 * 那么此時下面的 ceres::AutoDiffCostFunction<>模板參數就變為了 <CURVE_FITTING_COST,1,1,1,1>后面三個1代表有幾類未知參數 55 * 我們修改為了a b c三個變量，所以這里代表了3類，之后需要在自己寫的CURVE_FITTING_COST類中的operator()函數中， 56 * 把形式參數變為了const T* const a, const T* const b, const T* const c ,T* residual 57 * 上面修改的方法與本例程實際上一樣，只不過修改的這種方式顯得亂，實際上我們在用的時候，一般都是殘差種類有幾個，那么后面的分類 就分幾類 58 * 比如后面講的重投影誤差，此事就分兩類 一類是相機9維變量，一類是點的3維變量，然而殘差項變為了2維 59 * 60 * （1）: 修改后的寫法（當然自己定義的代價函數要對應修改重載函數的形式參數，對應修改內部的殘差的計算）： 61 * ceres::CostFunction* cost_function 62 * = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 1 ,1 ,1>( 63 * new CURVE_FITTING_COST ( x_data[i], y_data[i] ) ); 64 * problem.AddResidualBlock(cost_function,nullptr,&a,&b,&c); 65 * 修改后的代價函數的計算模型: 66 * struct CURVE_FITTING_COST 67 * { 68 * CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {} 69 * // 殘差的計算 70 * template <typename T> 71 * bool operator() ( 72 * const T* const a, 73 * const T* const b, 74 * const T* const c, 75 * T* residual ) const // 殘差 76 * { 77 * residual[0] = T ( _y ) - ceres::exp ( a[0]*T ( _x ) *T ( _x ) + b[0]*T ( _x ) + c[0] ); // y-exp(ax^2+bx+c) 78 * return true; 79 * } 80 * const double _x, _y; // x,y數據 81 * };//代價類結束 82 * 83 * 84 * （2）: 本例程下面的語句通常拆開來寫(看起來方便些): 85 * ceres::CostFunction* cost_function 86 * = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>( 87 * new CURVE_FITTING_COST ( x_data[i], y_data[i] ) ); 88 * problem.AddResidualBlock(cost_function,nullptr,abc) 89 * */ 90 problem.AddResidualBlock ( // 向問題中添加誤差項 91 // 使用自動求導，模板參數：誤差類型，Dimension of residual(輸出維度 表示有幾類殘差，本例程中就一類殘差項目，所以為1)，輸入維度，維數要與前面struct中一致 92 /*這里1 代表*/ 93 new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> ( 94 new CURVE_FITTING_COST ( x_data[i], y_data[i] )// x_data[i], y_data[i] 代表輸入的獲得的試驗數據 95 ), 96 nullptr, // 核函數，這里不使用，為空 這里是LossFunction的位置 97 abc // 待估計參數3維 98 ); 99 } 100 101 // 配置求解器ceres::Solver (是一個非線性最小二乘的求解器) 102 ceres::Solver::Options options; // 這里有很多配置項可以填Options類嵌入在Solver類中 ，在Options類中可以設置關於求解器的參數 103 options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解 這里的linear_solver_type 是一個Linear_solver_type的枚舉類型的變量 104 options.minimizer_progress_to_stdout = true; // 為真時 內部錯誤輸出到cout，我們可以看到錯誤的地方，默認情況下，會輸出到日志文件中保存 105 106 ceres::Solver::Summary summary; // 優化信息 107 chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//記錄求解時間間隔 108 //cout<<endl<<"求解前....."<<endl; 109 /*下面函數需要3個參數： 110 * 1、 const Solver::Options& options <----> optione 111 * 2、 Problem* problem <----> &problem 112 * 3、 Solver::Summary* summary <----> &summart (即使默認的參數也需要定義該變量 ) 113 * 這個函數會輸出一些迭代的信息。 114 * */ 115 ceres::Solve ( options, &problem, &summary ); // 開始優化 116 //cout<<endl<<"求解后....."<<endl; 117 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 118 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 ); 119 cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl; 120 121 // 輸出結果 122 // BriefReport() : A brief one line description of the state of the solver after termination. 123 cout<<summary.BriefReport() <<endl; 124 cout<<"estimated a,b,c = "; 125 /*auto a:abc 或者下面的方式都可以*/ 126 for ( auto &a:abc ) cout<<a<<" "; 127 cout<<endl; 128 129 return 0; 130 }
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 <opencv2/core/core.hpp> #include <cmath> #include <chrono> #include <memory> using namespace std; // 曲線模型的頂點，模板參數：優化變量維度和數據類型 class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW //表示在利用Eigen庫的數據結構時new的時候 需要對齊，所以加入EIGEN特有的宏定義即可實現 //下面幾個虛函數都是覆蓋了基類的對應同名同參數的函數 virtual void setToOriginImpl() // 重置 這個虛函數override 覆蓋了Vertex類的對應函數 函數名字和參數都是一致的，是多態的本質 { _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 {} }; // 誤差模型 模板參數：觀測值維度，類型，連接頂點類型 //這里觀測值維度是1維，如果是108頁6.12式，則觀測值維度是2 class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW //自己添加explicit 防止隱式轉換 explicit CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {} // 計算曲線模型誤差 void computeError() { /* _vertices是std::vector<Vertex *>類型的變量，我們這里把基類指針_vertices【0】強制轉換成const CurveFittingVertex* 自定義子類的常量指針 這里的轉換是上行轉換（子類指針轉換到基類），對於static_cast 和dynamic_cast兩種的結果都是一樣的，但是對於這種下行轉換則dynamic_cast比static_cast多了類型檢查功能 更安全些，但是dynamic_cast只能用在類類型的指針 引用，static_cast則不限制,即可以用在類型也可以用在其他類型，所以這里應該更改為dynamic_cast const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]); */ //修改后 const CurveFittingVertex* v = dynamic_cast<const CurveFittingVertex*> (_vertices[0]); //獲取此時待估計參數的當前更新值 為下面計算誤差項做准備 const Eigen::Vector3d abc = v->estimate(); //這里的error是1x1的矩陣，因為誤差項就是1個 _measurement是測量值yi _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 }; int main( int argc, char** argv ) { 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; } // 構建圖優化，先設定g2o typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,3> > Block; // 每個誤差項優化變量維度為3，誤差值維度為1后面的那個參數與誤差變量無關 僅僅表示路標點的維度 這里因為沒有用到路標點 所以為什么值都可以 /* 原版錯誤方式 : 這樣會出錯 Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 線性方程求解器 Block* solver_ptr = new Block( linearSolver ); // 矩陣塊求解器 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );//LM法 */ /*第一種解決方式: 將普通指針強制轉換成智能指針 需要注意的是 轉化之后 原來的普通指針指向的內容會有變化 普通指針可以強制轉換成智能指針，方式是通過智能指針的一個構造函數來實現的， 比如下面的Block( std::unique_ptr<Block::LinearSolverType>( linearSolver ) ); 這里面就是將linearSolver普通指針作為參數用智能指針構造一個臨時的對象，此時原來的普通指針就無效了，一定不要再次用那個指針了，否則會有意想不到的錯誤，如果還想保留原來的指針 那么就可以利用第二種方式 定義的時候就直接用智能指針就好，但是就如第二種解決方案那樣，也會遇到類型轉換的問題。詳細見第二種方式說明 Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 線性方程求解器 Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>( linearSolver ) ); // 矩陣塊求解器 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::unique_ptr<g2o::Solver>(solver_ptr) );//LM法 */ /*第二種解決方案: 定義變量時就用智能指針 需要注意的是 需要std::move移動 *下面可以這樣做 std::make_unique<>是在c++14中引進的 而std::make_shared<>是在c++11中引進的,都是為了解決用new為智能指針賦值的操作。這種更安全。 * 對於（2）將linearSovler智能指針的資源利用移動構造函數轉移到新建立的Block中，此時linearSolver這個智能指針默認不能夠訪問以及使用了。 * 對於(3)來說，因為solver_ptr是一個指向Block類型的智能指針，但是g2o::OptimizationAlgorithmLevenberg 構造函數接受的是std::unique_ptr<Solver>的參數，引起沖突，但是智能指針指向不同的類型時， * 不能夠通過強制轉換，所以此時應該用一個std::move將一個solver_ptr變為右值，然后調用std::unique_ptr的移動構造函數，而這個函數的本身並沒有限制指針 * 指向的類型，只要是std::unique_ptr類的對象，我們就可以調用智能指針的移動構造函數進行所屬權的移動。 * * */ std::unique_ptr<Block::LinearSolverType>linearSolver( new g2o::LinearSolverDense<Block::PoseMatrixType>() );// 線性方程求解器(1) std::unique_ptr<Block> solver_ptr ( new Block( std::move(linearSolver) ) );// 矩陣塊求解器 (2) g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::move(solver_ptr) );//(3) LM法 // 梯度下降方法，從GN, LM, DogLeg 中選(下面的兩種方式要按照上面的兩種解決方案對應修改，否則會編譯出錯 ) //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::move(solver_ptr) ); //g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( std::move(solver_ptr) ); g2o::SparseOptimizer optimizer; // 圖模型 optimizer.setAlgorithm( solver ); // 設置求解器 optimizer.setVerbose( true ); // 打開調試輸出 // 往圖中增加頂點 CurveFittingVertex* v = new CurveFittingVertex(); v->setEstimate( Eigen::Vector3d(0,0,0) );//增加頂點的初始值，如果是位姿 則初始值是用ICP PNP來提供初始化值 v->setId(0);//增加頂點標號 多個頂點要依次增加編號 optimizer.addVertex( v );//將新增的頂點加入到圖模型中 // 往圖中增加邊 N個 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] ); // 觀測數值 經過高斯噪聲的 //這里的信息矩陣可以參考:http://www.cnblogs.com/gaoxiang12/p/5244828.html 里面有說明 edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩陣：協方差矩陣之逆 這里為1表示加權為1 optimizer.addEdge( edge ); } // 執行優化 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; // 輸出優化值 Eigen::Vector3d abc_estimate = v->estimate(); cout<<"estimated model: "<<abc_estimate.transpose()<<endl; return 0; }
歡迎大家關注我的微信公眾號「佛系師兄」，里面有關於 Ceres 以及 OpenCV 等更多技術文章。
比如
「反復研究好幾遍，我才發現關於 CMake 變量還可以這樣理解！」
更多好的文章會優先在里面不定期分享！打開微信客戶端，掃描下方二維碼即可關注！
