《视觉SLAM十四讲》课后习题—ch6

最新推荐文章于 2023-11-25 10:06:16 发布

转载最新推荐文章于 2023-11-25 10:06:16 发布 · 492 阅读

0 ·

CC 4.0 BY-SA版权

原文链接：http://www.cnblogs.com/cc111/p/9357119.html

文章标签：

#人工智能

本文展示了一个使用Ceres库进行复杂曲线模型拟合的例子，通过优化多项式模型的参数来逼近一组带有高斯噪声的数据点。实验使用了指数型多项式模型y-exp(ax^3+bx^2+cx+d)，并通过最小二乘法求解参数，最终得到的结果接近于预设的真实参数。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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相差不多