《视觉SLAM十四讲》课后习题—ch6
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相差不多
