非线性优化学习笔记

CMakeLists文件

cmake_minimum_required( VERSION 2.8 )
project( ceres_curve_fitting )set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )# 寻找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} )

g2o库

流程：

1. 定义顶点和边的类型

        顶点和边：CurveFittingVertex和CurveFittingEdge

        顶点就是待优化的参数，边是误差项。这两个类分别派生自BaseVertex和BaseUnaryEdge，重写以下重要虚函数

（1）顶点的更新函数oplusImpl，增量更新

（2）顶点的重置函数

（3）边的误差计算函数，取出边所连接顶点的当前估计值，并与观测值进行比较

（4）边的雅克比计算函数：linearizeOplus

（5）存盘和读盘函数

2. 构建图

3. 选择优化算法

4. 使用g2o进行优化

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std; // 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWvirtual 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_NEWCurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}// 计算曲线模型误差void computeError(){const CurveFittingVertex* v = static_cast (_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 x_data, y_data;      // 数据cout<<"generating data: "< > Block;  // 每个误差项优化变量维度为3，误差值维度为1Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense(); // 线性方程求解器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; isetId(i);edge->setVertex( 0, v );                // 设置连接的顶点edge->setMeasurement( y_data[i] );      // 观测数值edge->setInformation( Eigen::Matrix::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆optimizer.addEdge( edge );}// 执行优化cout<<"start optimization"< time_used = chrono::duration_cast>( t2-t1 );cout<<"solve time cost = "<estimate();cout<<"estimated model: "<

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
)

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！