ceres优化 简单函数拟合

参考博客：https://blog.csdn.net/liu502617169/article/details/88693442#commentBox

这篇博客写的很清楚

//使用ceres,根据50个含有高斯噪声的点拟合曲线 y = 5*sin(x) + 6*cos(x)
#include <iostream>
#include <opencv2/opencv.hpp>
#include <ceres/ceres.h>
#include <chrono>
#include <vector>
using namespace std;

const int DATA_SIZE = 50;

struct CURVE_FITTING_COST
{
    CURVE_FITTING_COST(double x, double y):_x(x),_y(y) {}
    template <typename T>
    bool operator()(
            const T* const a,const T* const b,T* residual
            ) const ;
    const double _x,_y;

};

template <typename T>
bool CURVE_FITTING_COST::operator()(const T *const a, const T *const b, T *residual) const {
    residual[0] = T(_y) - a[0]*sin(_x) - b[0]*cos(_x);
    return 1;
}

void generate_data(int data_size,std::vector<double>& ,std::vector<double>& );//坐标生成器

int main() {
    std::vector<double> point_x,point_y;
    generate_data(DATA_SIZE, point_x, point_y);
    for(int i =0;i<DATA_SIZE;i++){
        std::cout << "[" << i << "] " << point_x[i] << " " << point_y[i] << std::endl;
    }

    double a = 0,b = 0;//a b 参数的估计值
    ceres::Problem problem;
    ceres::CostFunction* costfunction;
    for(int i = 0; i<DATA_SIZE;i++)
    {
        costfunction = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,1,1>(new CURVE_FITTING_COST( point_x[i],point_y[i]));
        problem.AddResidualBlock(costfunction,NULL,&a,&b);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = 1;

    ceres::Solver::Summary summary;

    std::cout<<"Before Solve "<<"a= "<<a<<", "<<"b= "<<b<<endl;

    ceres::Solve(options,&problem,&summary);
    std::cout<<summary.BriefReport()<<std::endl;
    std::cout<<"a= "<<0<<" "<<"b= "<<0<<"--->";
    std::cout<<"a= "<<a<<" "<<"b= "<<b<<std::endl;

    return 0;
}

void generate_data (int data_size ,std::vector<double>& point_x,std::vector<double>& point_y) //将vector引用传递
{
    //create data with Gaussian noise

    for(double  i = 0;i<data_size;i++)
    {
        std::cout<<"generating data: "<<std::endl;
        double x = i/data_size;
        point_x.push_back(x);
        int a = 5,b = 6;
        cv::RNG rng;
        double sigma = 1.0;
        double y = a*sin(x)+b*cos(x) + rng.gaussian(sigma); //生成带有参数的坐标点
        point_y.push_back(y);

    }
}

 

注：生成点数据的时候函数中的形参是按照引用传递的

结果如下：

/home/s/slambook/ch6/ceres_study/cmake-build-debug/ceres_study
generating data: 
[0] 0 6
[1] 0.02 6.09879
[2] 0.04 6.19515
[3] 0.06 6.28902
[4] 0.08 6.38038
[5] 0.1 6.46919
[6] 0.12 6.55541
[7] 0.14 6.63901
[8] 0.16 6.71995
[9] 0.18 6.79821
[10] 0.2 6.87375
[11] 0.22 6.94653
[12] 0.24 7.01654
[13] 0.26 7.08374
[14] 0.28 7.14811
[15] 0.3 7.20962
[16] 0.32 7.26825
[17] 0.34 7.32396
[18] 0.36 7.37675
[19] 0.38 7.42659
[20] 0.4 7.47346
[21] 0.42 7.51734
[22] 0.44 7.55821
[23] 0.46 7.59606
[24] 0.48 7.63087
[25] 0.5 7.66262
[26] 0.52 7.69132
[27] 0.54 7.71693
[28] 0.56 7.73946
[29] 0.58 7.7589
[30] 0.6 7.77523
[31] 0.62 7.78845
[32] 0.64 7.79855
[33] 0.66 7.80554
[34] 0.68 7.8094
[35] 0.7 7.81014
[36] 0.72 7.80776
[37] 0.74 7.80225
[38] 0.76 7.79362
[39] 0.78 7.78188
[40] 0.8 7.76702
[41] 0.82 7.74906
[42] 0.84 7.72799
[43] 0.86 7.70384
[44] 0.88 7.6766
[45] 0.9 7.64629
[46] 0.92 7.61293
[47] 0.94 7.57652
[48] 0.96 7.53708
[49] 0.98 7.49462
Before Solve a= 0, b= 0
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.351118e+03    0.00e+00    3.08e+02   0.00e+00   0.00e+00  1.00e+04        0    2.01e-04    2.35e-04
   1  8.091447e-06    1.35e+03    2.20e-02   7.81e+00   1.00e+00  3.00e+04        1    2.15e-04    4.69e-04
   2  1.016677e-13    8.09e-06    1.18e-06   1.24e-03   1.00e+00  9.00e+04        1    1.96e-04    6.73e-04
   3  9.361101e-20    1.02e-13    4.85e-11   2.18e-07   1.00e+00  2.70e+05        1    1.94e-04    8.73e-04
Ceres Solver Report: Iterations: 4, Initial cost: 1.351118e+03, Final cost: 9.361101e-20, Termination: CONVERGENCE
a= 0 b= 0--->a= 5 b= 6

Process finished with exit code 0