7.請更改曲線擬合實驗中的曲線模型，並用Ceres和g2o進行優化實驗。例如，可以使用更多的引數和更復雜的模型

　　Ceres：以使用更多的引數為例：y-exp(ax^3+bx^2+cx+d)

　　僅僅是在程式中將模型引數增加到4維，沒什麼創新而言

 1 #include <iostream>
 2 #include <opencv2/core/core.hpp>
 3 #include <ceres/ceres.h>
 4 #include <chrono>
 5 
 6 using namespace std;
 7 
 8 
 9
 
 //cost function的計算模型
10 struct CURVE_FITTING_COST
11 {
12     CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
13     //殘差的計算
14     template <typename T>
15     bool operator()(
16             const T* const abcd,//引數模型，有3維
17             T* residual) const //殘差
18     {
19         //y-exp(ax^3+bx^2+cx+d)
 

20         residual[0]=T(_y)-ceres::exp(abcd[0]*T(_x)*T(_x)*T(_x)+abcd[1]*T(_x)*T(_x)+
21                 abcd[2]*T(_x)+abcd[3]);
22         return true;
23     }
24     const double _x,_y;//x,y資料
25 };
26 
27 
28 int main(int argc, char *argv[])
29 {
30     double a=1.0,b=2.0,c=1.0,d=1.0;//真實引數值
31     int N=100
 
; //資料點
32     double w_sigma=1.0; //噪聲Sigma值
33     cv::RNG rng; //opencv隨機數產生器
34     double abcd[4]={0,0,0,0}; //abc引數的估計值
35     vector<double> x_data,y_data; //資料
36     
37     cout<<"generating data: "<<endl;
38     for(int i=0;i<N;++i)
39     {
40         double x=i/100.0;
41         x_data.push_back(x);
42         y_data.push_back(
43                     exp(a*x*x*x+b*x*x+c*x+d)+rng.gaussian(w_sigma)
44                     );
45         cout<<x_data[i]<<"  "<<y_data[i]<<endl;
46     }
47     
48     //構建最小二乘問題
49     ceres::Problem problem;
50     for(int i=0;i<N;++i){
51         problem.AddResidualBlock(//向問題中新增誤差項
52                           //使用自動求導，模板引數：誤差型別，輸出維度，輸入維度，數值參照前面struct中寫法
53                     new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,4>(
54                                       new CURVE_FITTING_COST(x_data[i],y_data[i])
55                                       ),
56                                   nullptr,//核函式，這裡不使用，為空
57                                   abcd   //待估計引數
58                     );
59     }
60     
61     //配置求解器
62     ceres::Solver::Options options;//這裡有很多配置項可以填
63     options.linear_solver_type=ceres::DENSE_QR;//增量方程如何求解
64     options.minimizer_progress_to_stdout=true;//輸出到out
65     
66     ceres::Solver::Summary summary;//優化資訊
67     chrono::steady_clock::time_point t1=chrono::steady_clock::now();
68     ceres::Solve(options,&problem,&summary);//開始優化
69     chrono::steady_clock::time_point t2=chrono::steady_clock::now();
70     chrono::duration<double> time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
71     cout<<"solve time cost= "<<time_used.count()<<" seconds."<<endl;
72     
73     //輸出結果
74     cout<<summary.BriefReport()<<endl;
75     cout<<"eastimated a,b,c,d= ";
76     for(auto a:abcd) cout<<a<<" ";
77     cout<<endl;
78     return 0;
79 }

執行結果為：

　　generating data:
0  2.71828
0.01  2.90429
0.02  2.07451
0.03  2.37759
0.04  4.07721
0.05  2.59433
0.06  2.25946
0.07  3.25026
0.08  3.49916
0.09  1.88001
0.1  3.83777
0.11  3.06639
0.12  4.46577
0.13  1.24944
0.14  1.36149
0.15  2.77749
0.16  2.623
0.17  5.32702
0.18  3.7666
0.19  2.1773
0.2  5.16208
0.21  2.91042
0.22  1.29655
0.23  2.30167
0.24  2.56549
0.25  3.95411
0.26  5.459
0.27  5.00078
0.28  4.03768
0.29  3.88333
0.3  6.34615
0.31  4.99392
0.32  6.03929
0.33  4.23159
0.34  4.14403
0.35  6.21883
0.36  5.33838
0.37  5.16594
0.38  5.45064
0.39  5.40612
0.4  7.00321
0.41  6.61634
0.42  6.23023
0.43  7.57696
0.44  6.02186
0.45  6.39285
0.46  7.03393
0.47  8.66677
0.48  5.80718
0.49  8.76548
0.5  8.15641
0.51  8.7939
0.52  9.30043
0.53  8.56226
0.54  10.4322
0.55  10.0204
0.56  12.2858
0.57  10.7917
0.58  10.7625
0.59  12.79
0.6  13.2231
0.61  13.899
0.62  13.3477
0.63  13.9156
0.64  15.3254
0.65  14.9943
0.66  15.7969
0.67  18.7825
0.68  19.1731
0.69  19.872
0.7  19.3818
0.71  23.0033
0.72  24.1526
0.73  25.5962
0.74  25.0404
0.75  25.9559
0.76  29.7316
0.77  30.8304
0.78  31.2814
0.79  33.627
0.8  36.1912
0.81  37.6664
0.82  41.6295
0.83  43.9126
0.84  46.742
0.85  48.8838
0.86  54.1265
0.87  58.2142
0.88  60.2013
0.89  65.8682
0.9  72.3178
0.91  77.7578
0.92  82.4311
0.93  86.7493
0.94  94.6651
0.95  98.7412
0.96  109.823
0.97  117.395
0.98  128.15
0.99  135.634
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  6.898490e+04    0.00e+00    2.14e+03   0.00e+00   0.00e+00  1.00e+04        0    1.17e-04    2.15e-04
   1  7.950822e+100   -7.95e+100    0.00e+00   5.63e+02  -1.17e+96  5.00e+03        1    1.61e-04    4.42e-04
   2  3.478360e+99   -3.48e+99    0.00e+00   4.95e+02  -5.12e+94  1.25e+03        1    8.29e-05    5.58e-04
   3  3.566582e+95   -3.57e+95    0.00e+00   3.09e+02  -5.30e+90  1.56e+02        1    5.68e-05    6.41e-04
   4  1.183153e+89   -1.18e+89    0.00e+00   1.51e+02  -1.78e+84  9.77e+00        1    5.18e-05    7.13e-04
   5  3.087066e+73   -3.09e+73    0.00e+00   7.00e+01  -4.91e+68  3.05e-01        1    5.72e-05    7.89e-04
   6  4.413641e+31   -4.41e+31    0.00e+00   2.13e+01  -1.04e+27  4.77e-03        1    5.10e-05    8.59e-04
   7  6.604687e+04    2.94e+03    4.98e+03   6.39e-01   1.65e+00  1.43e-02        1    1.23e-04    1.00e-03
   8  5.395798e+04    1.21e+04    1.59e+04   8.07e-01   2.02e+00  4.29e-02        1    1.14e-04    1.13e-03
   9  3.089338e+04    2.31e+04    3.19e+04   5.71e-01   1.62e+00  1.29e-01        1    1.13e-04    1.26e-03
  10  8.430982e+03    2.25e+04    3.21e+04   3.74e-01   1.30e+00  3.86e-01        1    1.12e-04    1.39e-03
  11  8.852002e+02    7.55e+03    1.29e+04   1.77e-01   1.08e+00  1.16e+00        1    1.11e-04    1.52e-03
  12  2.313901e+02    6.54e+02    2.59e+03   4.89e-02   1.01e+00  3.48e+00        1    1.11e-04    1.65e-03
  13  1.935710e+02    3.78e+01    8.37e+02   2.72e-02   1.01e+00  1.04e+01        1    1.11e-04    1.77e-03
  14  1.413188e+02    5.23e+01    6.05e+02   6.43e-02   1.01e+00  3.13e+01        1    1.11e-04    1.90e-03
  15  8.033187e+01    6.10e+01    3.36e+02   1.08e-01   1.01e+00  9.39e+01        1    1.11e-04    2.03e-03
  16  5.660145e+01    2.37e+01    7.69e+01   9.43e-02   9.99e-01  2.82e+02        1    1.11e-04    2.15e-03
  17  5.390796e+01    2.69e+00    1.52e+01   5.86e-02   9.97e-01  8.45e+02        1    1.18e-04    2.29e-03
  18  5.233724e+01    1.57e+00    9.31e+00   9.73e-02   9.96e-01  2.53e+03        1    1.12e-04    2.42e-03
  19  5.125192e+01    1.09e+00    3.58e+00   1.21e-01   9.98e-01  7.60e+03        1    1.11e-04    2.54e-03
  20  5.098190e+01    2.70e-01    1.08e+00   9.37e-02   1.00e+00  2.28e+04        1    1.25e-04    2.68e-03
  21  5.086440e+01    1.18e-01    6.49e-01   1.47e-01   1.00e+00  6.84e+04        1    1.28e-04    2.84e-03
  22  5.070258e+01    1.62e-01    4.62e-01   2.86e-01   1.00e+00  2.05e+05        1    1.12e-04    2.97e-03
  23  5.059978e+01    1.03e-01    2.40e-01   3.30e-01   1.00e+00  6.16e+05        1    1.11e-04    3.09e-03
  24  5.058282e+01    1.70e-02    7.40e-02   1.68e-01   1.00e+00  1.85e+06        1    1.11e-04    3.22e-03
  25  5.058233e+01    4.94e-04    1.16e-02   3.17e-02   1.00e+00  5.54e+06        1    1.25e-04    3.36e-03
solve time cost= 0.00346683 seconds.
Ceres Solver Report: Iterations: 26, Initial cost: 6.898490e+04, Final cost: 5.058233e+01, Termination: CONVERGENCE
eastimated a,b,c,d= 0.796567 2.2634 0.969126 0.969952

與我們設定的真值a=1,b=2,c=1,d=1相差不多