Ceres

Ceres简介

Ceres是一个最小二乘问题求解库。其求解的最小二乘问题的一般形式如下：

\[min \frac{1}{2} \sum \rho_i(||f_i(x_{i1},...,x_{in})||^2)\\ s.t. l_j\leq x_j\leq u_j \]

在这个问题中，x_1,...x_n为优化变量，又称参数块,\(f_i\)称为代价函数也成为残差块，在SLAM中也可以理解为误差项。

一般步骤

• 定义每个参数据块。
• 定义残差块的计算方式。
• 定义残差块中雅可比的计算方式。
• 把所有参数块和残差块加入Ceres定义的Problem对象中，调用Solve函数求解。求解之前也可以传入一些配置信息（迭代次数、终止条件等）。

使用Ceres拟合曲线

#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{
            //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; //x,y数据
};
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;                 //噪声sigma
    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));
    }
    double abc[3]={ae,be,ce};
    
    //构建最小二乘问题
    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_NORMAL_CHOLESKY;//增量方程如何求解
    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<<"estimate a,b,c=";
    for(auto a:abc)cout<<a<<" ";
    cout<<endl;
    
    return 0;
}

CMakeLists.txt文件

cmake_minimum_required(VERSION 2.8)
project(ch6_2)

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

find_package(Ceres REQUIRED)
include_directories(${Ceres_INLCUDE_DIRS})

add_executable(ceresCurveFitting ceresCurveFitting.cpp)
target_link_libraries(ceresCurveFitting ${OpenCV_LIBS} ${CERES_LIBRARIES})