非线性优化学习笔记

好好仔仔

已于 2022-06-05 19:44:54 修改

阅读量474

点赞数 1

文章标签：学习 c++ 算法性能优化

于 2022-06-05 19:28:53 首次发布

本文链接：https://blog.csdn.net/luoyihao123456/article/details/125132318

版权

最小二乘法的引出

在什么位姿下最可能产生当前观测到的数据”：求位姿x，使得概率P(x)最大。由于x满足高斯分布，由高斯分布的概率密度函数形式可知，最小化噪声项（误差）的一个二次型相当于使P(x)最大。通常各个时刻的输入u和观测z是独立的，因此该问题等价于最小化所有时刻位姿以及观测的估计值与实际值之间的马氏距离。

高斯牛顿法和列文伯格-马夸尔特法

非线性优化求解最小二乘问题：给定一个目标函数，求解当x为多少时目标函数值最小。牛顿法是将目标函数在xk处泰勒展开，保留二阶梯度信息，用泰勒展开项代替原函数求导取极值。高斯牛顿法也是泰勒展开，但并不直接展开目标函数，而是一阶泰勒展开不带平方的函数，进而近似出目标函数，在求导取极值时有效避免了海塞矩阵的计算。

列文伯格-马夸尔特法：高斯牛顿法中用一阶展开只在展开点附近有较好的近似结果，所以限定一个展开范围，认为只有在该范围内才是有效搜索。当实际值下降的速度大于预测速度时，说明此时下降效果好，放大搜索半径。当x比较小时，LM近似于高斯牛顿法，当x比较大时，LM近似于最速下降法。最速下降法精度和收敛速度都比较高，但H矩阵计算困难。

在视觉slam十四讲中，使用了三种方式进行非线性优化的求解，分别是手写高斯牛顿法、Ceres库、g2o库。

手写高斯牛顿法

流程：

1. 初始化H和b

2. 根据假设的带求参数初始值，计算每一个点的H和b，并进行累加

3. 求解线性方程Hx=b

4. 解出的x是带求参数的增量，累加到带求参数中并重新进行第2步流程

源代码如下：

#include<iostream>
#include<opencv2/opencv.hpp>
#include<Eigen/Core>
#include<Eigen/Dense>
#include<chrono>
// #include<vector>
using namespace std;
using namespace Eigen;

int main(int argc,char **argv)
{
  double ar=1.0,br=2.0,cr=1.0;
  double ae=2.0,be=-1.0,ce=5.0;
  int N=100;
  double w_sigma=1.0;
  double inv_sigma=1.0/w_sigma;
  cv::RNG rng; //opencv产生随机数据

  vector<double> x_data,y_data;
  for (int i = 0; i < N; i++)
  {
    double x=i/100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma*w_sigma));
  }
  
  //开始迭代
  int iterations=100;
  double cost=0,lastCost=0;
  chrono::steady_clock::time_point t1=chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++)
  {
    Matrix3d H=Matrix3d::Zero();
    Vector3d b=Vector3d::Zero();
    cost=0;

    for (int i = 0; i < N; i++)
    {
      double xi=x_data[i],yi=y_data[i];
      double error=yi-exp(ae*xi*xi+be*xi+ce);
      Vector3d J;
      J[0]=-xi*xi*exp(ae*xi*xi+be*xi+ce);
      J[1]=-xi*exp(ae*xi*xi+be*xi+ce);
      J[2]=-exp(ae*xi*xi+be*xi+ce);

      H+=inv_sigma*inv_sigma*J*J.transpose();
      b+=-inv_sigma*inv_sigma*error*J;

      cost+=error*error;//cost是所有误差的平方和
    }

    Vector3d dx=H.ldlt().solve(b);
    if(isnan(dx[0]))
    {
      cout<<"result is nan"<<endl;
      break;
    }

    if(iter>0&&cost>=lastCost)
    {
      cout<<"cost:"<<cost<<">=last cost:"<<lastCost<<",break."<<endl;
      break;
    }

    ae+=dx[0];
    be+=dx[1];
    ce+=dx[2];
    lastCost=cost;
    cout<<"total cost:"<<cost<<",\t\tupdate:"<<dx.transpose()<<"\t\testimated params:"<<ae<<","<<be<<","<<ce<<endl;
  }
  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;
  cout<<"estimated abc="<<ae<<","<<be<<","<<ce<<endl;
  return 0;
}

CMakeLists文件如下：

cmake_minimum_required( VERSION 2.8 )
project(gaussNewton )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# Eigen
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIRS})

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable( gaussNewton gaussNewton.cpp )
# 和OpenCV链接
target_link_libraries( gaussNewton ${OpenCV_LIBS} )

Ceres库

流程：

1. 定义残差块的类，并在类中定义带模板参数的（）运算符，使该类称为仿函数

2. 定义参数块和残差块。double abc[3]即为参数块，残差块通过构造CURVE_FITTING_COST对象，调用 ceres::AutoDiffCostFunction将误差项添加到目标函数中

3. 设置自动求导误差项和优化变量的维度

4. 使用Solve函数进行求解

#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;                          // 数据点
    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;
    }

    // 构建最小二乘问题
    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<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

    // 输出结果
    cout<<summary.BriefReport() <<endl;
    cout<<"estimated a,b,c = ";
    for ( auto a:abc ) cout<<a<<" ";
    cout<<endl;

    return 0;
}

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 <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;
    }
    
    // 构建图优化，先设定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<<"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;
}

CMakeLists

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
)