從零開始學SLAM: Ceres求解優化問題
從《視覺SLAM十四講》和ceres的tutorial開始學起,同時複習一下C++中的語法。
struct CURVE_FITTING_COST { CURVE_FITTING_COST ( double x,double y): _x(x),_y(y) {} template <typename T> bool operator()( const T* const abc, T* residual )const { residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]); return true; } const double _x,_y; }
要使用Ceres,首先要定義求解問題的cost function<CURVE_FITTING_COST>。 在結構體中首先構造建構函式:
CURVE_FITTING_COST ( double x,double y): _x(x),_y(y) {} //結構體的建構函式初始化列表
//相當於CURVE_FITTING_COST ( double x,double y) {_x=x; _y=y}
接下來,定義模板template
template <typename T>
template的使用是為了簡化不同型別的函式和類的重複定義,模版例項化時可以替換任意型別,不僅包括內建型別(int等),也包括自定義型別class。編譯器在例項化之後才知道的資料的型別。
bool operator()( const T* const abc, T* residual )const
{
residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
return true;
}
過載()符號,仿函式的小技巧,使結構體的一個例項具有類似一個函式的性質,在程式碼編寫過程中能當做一個函式一樣來使用。 對結構體、類的一個例項,比如Myclass類的一個例項Obj1,如果Myclass裡對()進行了過載,那Obj1被建立之後,就可以將Obj1這個例項當做函式來用,比如Obj(x)。在這裡CURVE_FITTING_COST()中共傳入了兩個變數,包括3維引數向量abc以及殘差residual。
int main ( int argc, char** argv )
{
double a=1.0, b=2.0, c=1.0; // 真實引數值
int N=100; // 資料點
double w_sigma=1.0; // 噪聲Sigma值
cv::RNG rng; // OpenCV隨機數產生器
double abc[3] = {0,0,0}; // abc引數的估計值
vector<double> x_data, y_data; // 資料
cout<<"generating data: "<<endl;
for ( int i=0; i<N; i++ )
{
double x = i/100.0;
x_data.push_back ( x );
y_data.push_back (
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
);
cout<<x_data[i]<<" "<<y_data[i]<<endl;
}
// 構建最小二乘問題
ceres::Problem problem;
for ( int i=0; i<N; i++ )
{
problem.AddResidualBlock ( // 向問題中新增誤差項
// 使用自動求導,模板引數:誤差型別,輸出維度,輸入維度,維數要與前面struct中一致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> (
new CURVE_FITTING_COST ( x_data[i], y_data[i] )
),
nullptr, // 核函式,這裡不使用,為空
abc // 待估計引數
);
}
// 配置求解器
ceres::Solver::Options options; // 這裡有很多配置項可以填
options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
options.minimizer_progress_to_stdout = true; // 輸出到cout
ceres::Solver::Summary summary; // 優化資訊
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve ( options, &problem, &summary ); // 開始優化
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
// 輸出結果
cout<<summary.BriefReport() <<endl;
cout<<"estimated a,b,c = ";
for ( auto a:abc ) cout<<a<<" ";
cout<<endl;
return 0;
}
這裡主要對之前我們構建的costfunction是怎麼使用的進行講解,也就是下面的程式碼
problem.AddResidualBlock (
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> (
new CURVE_FITTING_COST ( x_data[i], y_data[i] )
),
nullptr,
abc
);
這一段程式碼可以寫成另外一種簡單的形式
CostFunction* cost_function =
new AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST( x_data[i], y_data[i] ));
problem.AddResidualBlock (cost_function, nullptr, abc );
其中,我們第一段程式碼new CURVE_FITTING_COST( x_data[i], y_data[i] )呼叫了結構體的建構函式,而<CURVE_FITTING_COST, 1, 3>中,1代表殘差維度,3代表待優化的引數維度。回顧之前的建構函式,過載()時傳入的引數分別是優化變數和殘差(殘差就是代價函式的輸出)。然後將CURVE_FITTING_COST傳入AutoDiffCostFunction方法來構建尋優問題。 第二段是向問題中新增誤差項,nullptr表示空指標(相較於NULL,優點在於在編譯器進行解釋程式時,NULL會被直接解釋成0,變為一個數字,而nullptr不會),abc為待尋優引數。
for ( auto a:abc )
表示的則是基於範圍的for迴圈,例如
#include<iostream>
#include<stdlib.h>
#include<string>
using namespace std;
int main()
{
int arr[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
for (auto val : arr)
{ cout << val << " "; }
system("pause");
return 0;
}
//輸出結果: 1 2 3 4 5 6 7 8 9 10
CMakeList程式碼為
cmake_minimum_required(VERSION 2.8) project(ceres) find_package(Ceres REQUIRED) include_directories(${CERES_INCLUDE_DIRS}) add_executable(use_ceres use_ceres.cpp) target_link_libraries(use_ceres ${CERES_LIBRARIES})