第6讲 非线性优化
6.3 实践Ceres
环境配置
https://blog.csdn.net/qq_39236499/article/details/122547508
代码详解
cmake_minimum_required( VERSION 2.8 ) project( ceres_curve_fitting ) set( CMAKE_BUILD_TYPE "Release" ) set( CMAKE_CXX_FLAGS "-std=c++11 -O3" ) # 添加cmake模块以使用ceres库 list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules ) # 寻找Ceres库并添加它的头文件 find_package( Ceres REQUIRED ) include_directories( ${CERES_INCLUDE_DIRS} ) # OpenCV find_package( OpenCV REQUIRED ) include_directories( ${OpenCV_DIRS} ) add_executable( curve_fitting main.cpp ) # 与Ceres和OpenCV链接 target_link_libraries( curve_fitting ${CERES_LIBRARIES} ${OpenCV_LIBS} )
#include <iostream> #include <opencv2/core/core.hpp> #include <ceres/ceres.h> #include <chrono> using namespace std; // 代价函数的计算模型 struct CURVE_FITTING_COST { CURVE_FITTING_COST (double x, double y) : _x(x), _y(y) {} // 残差的计算 template <typename T> bool operator() ( const T* const abc, // 模型参数，有3维 T* residual ) const // 残差 { residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T (_x)+abc[2]); // y-exp(ax^2+bx+c) return true; } const double _x, _y; // x,y数据 }; int main ( int argc, char** argv ) { double a=1.0, b=2.0, c=1.0; // 真实参数值 int N=100; // 数据点 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 << "生成数据: " << 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; } cout << endl; // 构建最小二乘问题 ceres::Problem problem; for (int i=0; i<N; i++){ problem.AddResidualBlock ( // 向问题中添加误差项 // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致 new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> ( new CURVE_FITTING_COST ( x_data[i], y_data[i] ) ), nullptr, // 核函数，这里不使用，为空 abc // 待估计参数 ); } // 配置求解器 ceres::Solver::Options options; // 这里有很多配置项可以填 options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解 options.minimizer_progress_to_stdout = true; // 输出到cout ceres::Solver::Summary summary; // 优化信息 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); ceres::Solve ( options, &problem, &summary ); // 开始优化 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 ); cout << endl; cout << "solve time cost = " << time_used.count() << " seconds. " << endl; // 输出结果 cout << summary.BriefReport() << endl; cout << "estimated a,b,c = "; for (auto i : abc){ cout << i << " "; } cout << endl; return 0; }
6.4 实践：g2o
环境配置
https://blog.csdn.net/qq_39236499/article/details/122790033
代码详解
CMakeLists.txt
cmake_minimum_required( VERSION 2.8 ) project( g2o_curve_fitting ) set( CMAKE_BUILD_TYPE "Release" ) set( CMAKE_CXX_FLAGS "-std=c++11 -O3" ) # 添加cmake模块以使用ceres库 list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules ) # 寻找G2O find_package( G2O REQUIRED ) include_directories( ${G2O_INCLUDE_DIRS} "/usr/include/eigen3" ) # OpenCV find_package( OpenCV REQUIRED ) include_directories( ${OpenCV_DIRS} ) add_executable( curve_fitting main.cpp ) # 与G2O和OpenCV链接 target_link_libraries( curve_fitting ${OpenCV_LIBS} g2o_core g2o_stuff )
g2o_curve_fitting.cpp
#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> using namespace std; // 曲线模型的顶点，模板参数：优化变量维度和数据类型 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 {} }; // 误差模型 模板参数：观测值维度，类型，连接顶点类型 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 }; 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; } cout << endl; // 构建图优化，先设定g2o typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1 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::Vector3d(0,0,0) ); v->setId(0); optimizer.addVertex( v ); // 往图中增加边 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 ); } // 执行优化 cout << endl; cout << "开始优化：" << 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; }
该小节遇到的问题
运行./curve_fitting报错如下：
./curve_fitting: error while loading shared libraries: libg2o_core.so: cannot open shared object file: No such file or directory
解决方法参考：https://blog.csdn.net/stm32F103vc/article/details/98874108
流程如下：
- 终端输入：
sudo gedit /etc/ld.so.conf
- 添加如下代码：
/usr/local/lib
- 运行：
sudo ldconfig
- 再次终端输入运行
./curve_fitting
- 成功
