视觉SLAM（1）_非线性优化库ceres 求解最小二乘问题

最新推荐文章于 2022-01-19 10:58:37 发布

什么都会一点儿的自动驾驶工程狮

最新推荐文章于 2022-01-19 10:58:37 发布

阅读量695

点赞数 1

分类专栏：视觉SLAM 基础

本文链接：https://blog.csdn.net/lzy6041/article/details/93854259

版权

视觉SLAM 基础专栏收录该内容

3 篇文章 1 订阅

订阅专栏

安装Ceres:

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev libgoogle-glog-dev libgtest-dev 
然后cd 进cerse
 mkdir build
 cd build 
 cmake .. 
 make -j8 
 sudo make install

实践：已知带有噪声的x,y, 我们想要求得参数a,b,c
在这里插入图片描述

/**********************
 * @FileName: main.cpp
 * @Author: Liu
 * @DateTime: 2019-06-26 09:20:10
 * @Description: the test for ceres curve fitting
 **********************/
#include<iostream>
#include<opencv2/core/core.hpp>
#include<ceres/ceres.h>
#include<chrono>
using namespace std;
struct CURVE_FITTING_COST//定义一个CostFunction模型
{
    CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
    template <typename T>
    bool operator()(const T* const abc,T* residual) const//模型参数：3维 ，残差 
    {
        //y-exp(ax^2+bx+c)
        residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
        return true;
    }
    const double _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];              //a,b,c的参数估计值
    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;
    }
    //构建最小二乘问题
    /*
     problem.AddResidualBlock()将误差项添加到目标函数中，由于优化需要梯度，有以下选择：
    （1）使用Ceres自动求导（Auto Diff)
    （2）使用数值求导 （Numeric Diff）
    （3）自行推导解析的导数形式
    */
    ceres::Problem problem;
    for(int i=0;i<N;i++)
    {
        //向问题中添加误差项 problem.AddResidualBlock()
        //使用自动求导ceres::AutoDiffCostFunction<>  需要指定误差项和优化变量的维度，这里的误差是标量，维度为1；优化的是a,b,c三个量，维度为3.                                                                                  ，核函数，不使用，为空；待估计参数
        problem.AddResidualBlock(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()<<" senconds."<<endl;

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

CMakeLists文件

cmake_minimum_required(VERSION 2.8)
project(ceres)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

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

add_executable(ceres_node main.cpp)
target_link_libraries(ceres_node ${CERES_LIBRARIES} ${OpenCV_LIBS})

编译与运行：

mkdir build
cd build
cmake ..
make 
./ceres_node

总结：
/*
使用Ceres求解非线性优化问题，一共分为三个部分：
1、第一部分：构建cost fuction，即代价函数，也就是寻优的目标式。这个部分需要使用拟函数（functor）这一技巧来实现，做法是定义一个cost function的结构体，在结构体内重载（）运算符，具体实现方法后续介绍。
2、第二部分：通过代价函数构建待求解的优化问题。
3、第三部分：配置求解器参数并求解问题，这个步骤就是设置方程怎么求解、求解过程是否输出等，然后调用一下Solve方法。
*/
第一部分：构造代价函数结构体

struct CURVE_FITTING_COST//定义一个CostFunction模型
{
    CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
    template <typename T>
    bool operator()(const T* const abc,T* residual) const//模型参数：3维 ，残差 
    {
        //y-exp(ax^2+bx+c)
        residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
        return true;
    }
    const double _x,_y;
};

这里的使用了仿函数的技巧，即在CostFunction结构体内，对（）进行重载，这样的话，该结构体的一个实例就能具有类似一个函数的性质，在代码编写过程中就能当做一个函数一样来使用。
关于仿函数，这里再多说几句，对结构体、类的一个实例，比如Myclass类的一个实例Obj1，如果Myclass里对（）进行了重载，那Obj1被创建之后，就可以将Obj1这个实例当做函数来用，比如Obj（x）
ostFunction结构体中，对括号符号重载的函数中，传入参数有两个，一个是待优化的变量x，另一个是残差residual，也就是代价函数的输出。重载了（）符号之后，CostFunction就可以传入AutoDiffCostFunction方法来构建寻优问题了。
第二部分：通过代价函数构建待求解的优化问题

    ceres::Problem problem;
    for(int i=0;i<N;i++)
    {
        //向问题中添加误差项 problem.AddResidualBlock()
        //使用自动求导ceres::AutoDiffCostFunction<>  需要指定误差项和优化变量的维度，这里的误差是标量，维度为1；优化的是a,b,c三个量，维度为3.                                                                                  ，核函数，不使用，为空；待估计参数
        problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>(new CURVE_FITTING_COST(x_data[i],y_data[i])),nullptr,abc);
    }

这一部分就是待求解的优化问题的构建过程，使用之前结构体创建一个实例，由于使用了仿函数技巧，该实例在使用上可以当做一个函数。基于该实例new了一个CostFunction结构体，这里使用的自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个3是输入维度，即待寻优参数abc的维度。分别对应之前结构体中的residual和x。
向问题中添加误差项，本问题比较简单，添加一次就行（有的问题要不断多次添加ResidualBlock以构建最小二乘求解问题）。这里的参数NULL是指不使用核函数，&x表示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()<<" senconds."<<endl;

    //输出结果
    cout<<summary.BriefReport()<<endl;
    cout<<"estimated a,b,c= ";
    //for ( auto a:abc ) 
    for(int i=0;i<3;i++)
    {
        cout<<abc[i]<<" ";
    }