1. 程式人生 > >從零開始學SLAM: Ceres求解優化問題

從零開始學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})