视觉slam十四讲ch6曲线拟合 代码注释（笔记版）
生活随笔
收集整理的這篇文章主要介紹了
视觉slam十四讲ch6曲线拟合 代码注释（笔记版）
小編覺得挺不錯的,現在分享給大家,幫大家做個參考.
1 #include <opencv2/core/core.hpp>
2 #include <ceres/ceres.h>
3 #include <chrono>
4
5 using namespace std;
6
7 // 代價函數的計算模型
8 struct CURVE_FITTING_COST
9 {
10 CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
11 // 殘差的計算
12 template <typename T>
13 bool operator() (
14 const T* const abc, // 模型參數，有3維 當沒有必要分類的時候 就用一個數組來存儲未知的系數,方便管理，而不是設3個變量，之后在()重載函數的形式參數個數變為3個
15 T* residual ) const // 殘差
16 {
17 residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
18 return true;
19 }
20 const double _x, _y; // x,y數據
21 };
22
23 int main ( int argc, char** argv )
24 {
25 double a=1.0, b=2.0, c=1.0; // 真實參數值
26 int N=100; // 數據點
27 double w_sigma=1.0; // 噪聲Sigma值(根號下方差)
28 cv::RNG rng; // OpenCV隨機數產生器
29 double abc[3] = {0.8,2.1,0.9}; // abc參數的估計值 (修改初始值 下面求解迭代過程會不同)
30
31 vector<double> x_data, y_data; // 數據
32
33 /*生成符合曲線的樣本*/
34 cout<<"generating data: "<<endl; //下面是從真實的曲線中取得樣本數據
35 for ( int i=0; i<N; i++ )
36 {
37 double x = i/100.0;
38 x_data.push_back ( x );
39 y_data.push_back (
40 exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
41 );
42 //cout<<x_data[i]<<" "<<y_data[i]<<endl;//輸出生成數據
43 }
44
45 // 構建最小二乘問題
46 ceres::Problem problem;
47 for ( int i=0; i<N; i++ )
48 {
49 /* 第一個參數 CostFunction* : 描述最小二乘的基本形式即代價函數 例如書上的116頁fi(.)的形式
50 * 第二個參數 LossFunction* : 描述核函數的形式 例如書上的ρi（.）
51 * 第三個參數 double* : 待估計參數（用數組存儲）
52 * 這里僅僅重載了三個參數的函數，如果上面的double abc[3]改為三個double a=0 ,b=0,c = 0;
53 * 此時AddResidualBlock函數的參數除了前面的CostFunction LossFunction 外后面就必須加上三個參數 分別輸入&a,&b,&c
54 * 那么此時下面的 ceres::AutoDiffCostFunction<>模板參數就變為了 <CURVE_FITTING_COST,1,1,1,1>后面三個1代表有幾類未知參數
55 * 我們修改為了a b c三個變量，所以這里代表了3類，之后需要在自己寫的CURVE_FITTING_COST類中的operator()函數中，
56 * 把形式參數變為了const T* const a, const T* const b, const T* const c ,T* residual
57 * 上面修改的方法與本例程實際上一樣，只不過修改的這種方式顯得亂，實際上我們在用的時候，一般都是殘差種類有幾個，那么后面的分類 就分幾類
58 * 比如后面講的重投影誤差，此事就分兩類 一類是相機9維變量，一類是點的3維變量，然而殘差項變為了2維
59 *
60 * （1）: 修改后的寫法（當然自己定義的代價函數要對應修改重載函數的形式參數，對應修改內部的殘差的計算）：
61 * ceres::CostFunction* cost_function
62 * = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 1 ,1 ,1>(
63 * new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
64 * problem.AddResidualBlock(cost_function,nullptr,&a,&b,&c);
65 * 修改后的代價函數的計算模型:
66 * struct CURVE_FITTING_COST
67 * {
68 * CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
69 * // 殘差的計算
70 * template <typename T>
71 * bool operator() (
72 * const T* const a,
73 * const T* const b,
74 * const T* const c,
75 * T* residual ) const // 殘差
76 * {
77 * residual[0] = T ( _y ) - ceres::exp ( a[0]*T ( _x ) *T ( _x ) + b[0]*T ( _x ) + c[0] ); // y-exp(ax^2+bx+c)
78 * return true;
79 * }
80 * const double _x, _y; // x,y數據
81 * };//代價類結束
82 *
83 *
84 * （2）: 本例程下面的語句通常拆開來寫(看起來方便些):
85 * ceres::CostFunction* cost_function
86 * = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
87 * new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
88 * problem.AddResidualBlock(cost_function,nullptr,abc)
89 * */
90 problem.AddResidualBlock ( // 向問題中添加誤差項
91 // 使用自動求導，模板參數：誤差類型，Dimension of residual(輸出維度 表示有幾類殘差，本例程中就一類殘差項目，所以為1)，輸入維度，維數要與前面struct中一致
92 /*這里1 代表*/
93 new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> (
94 new CURVE_FITTING_COST ( x_data[i], y_data[i] )// x_data[i], y_data[i] 代表輸入的獲得的試驗數據
95 ),
96 nullptr, // 核函數，這里不使用，為空 這里是LossFunction的位置
97 abc // 待估計參數3維
98 );
99 }
100
101 // 配置求解器ceres::Solver (是一個非線性最小二乘的求解器)
102 ceres::Solver::Options options; // 這里有很多配置項可以填Options類嵌入在Solver類中 ，在Options類中可以設置關于求解器的參數
103 options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解 這里的linear_solver_type 是一個Linear_solver_type的枚舉類型的變量
104 options.minimizer_progress_to_stdout = true; // 為真時 內部錯誤輸出到cout，我們可以看到錯誤的地方，默認情況下，會輸出到日志文件中保存
105
106 ceres::Solver::Summary summary; // 優化信息
107 chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//記錄求解時間間隔
108 //cout<<endl<<"求解前....."<<endl;
109 /*下面函數需要3個參數：
110 * 1、 const Solver::Options& options <----> optione
111 * 2、 Problem* problem <----> &problem
112 * 3、 Solver::Summary* summary <----> &summart (即使默認的參數也需要定義該變量 )
113 * 這個函數會輸出一些迭代的信息。
114 * */
115 ceres::Solve ( options, &problem, &summary ); // 開始優化
116 //cout<<endl<<"求解后....."<<endl;
117 chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
118 chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
119 cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
120
121 // 輸出結果
122 // BriefReport() : A brief one line description of the state of the solver after termination.
123 cout<<summary.BriefReport() <<endl;
124 cout<<"estimated a,b,c = ";
125 /*auto a:abc 或者下面的方式都可以*/
126 for ( auto &a:abc ) cout<<a<<" ";
127 cout<<endl;
128
129 return 0;
130 }
?
轉載于:https://www.cnblogs.com/newneul/p/8407365.html
創作挑戰賽新人創作獎勵來咯，堅持創作打卡瓜分現金大獎總結
以上是生活随笔為你收集整理的视觉slam十四讲ch6曲线拟合 代码注释（笔记版）的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 任天堂社长称Switch不会涨价：怕吓跑
- 下一篇： iOS sha1加密算法