視覺SLAM（1）_非線性優化庫ceres 求解最小二乘問題

安裝Ceres:

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev libgoogle-glog-dev libgtest-dev 
然後cd 進cerse
 mkdir build
 cd build 
 cmake .. 
 make -j8 
 sudo make install

實踐：已知帶有噪聲的x,y, 我們想要求得參數a,b,c

/**********************
 * @FileName: main.cpp
 * @Author: Liu
 * @DateTime: 2019-06-26 09:20:10
 * @Description: the test for ceres curve fitting
 **********************/
#include<iostream>
#include<opencv2/core/core.hpp>
#include<ceres/ceres.h>
#include<chrono>
using namespace std;
struct CURVE_FITTING_COST//定義一個CostFunction模型
{
    CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
    template <typename T>
    bool operator()(const T* const abc,T* residual) const//模型參數：3維 ，殘差 
    {
        //y-exp(ax^2+bx+c)
        residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
        return true;
    }
    const double _x,_y;
};
int main(int argc,char** argv)
{
    double a=1.0,b=2.0,c=1.0;   //真實參數值
    int N=100;                  //100個數據點
    double w_sigma=1.0;         //噪聲sigma的值
    cv::RNG rng;                //opencv隨機數產生器
    double abc[3];              //a,b,c的參數估計值
    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;
    }
    //構建最小二乘問題
    /*
     problem.AddResidualBlock()將誤差項添加到目標函數中，由於優化需要梯度，有以下選擇：
    （1）使用Ceres自動求導（Auto Diff)
    （2）使用數值求導 （Numeric Diff）
    （3）自行推導解析的導數形式
    */
    ceres::Problem problem;
    for(int i=0;i<N;i++)
    {
        //向問題中添加誤差項 problem.AddResidualBlock()
        //使用自動求導ceres::AutoDiffCostFunction<>  需要指定誤差項和優化變量的維度，這裏的誤差是標量，維度爲1；優化的是a,b,c三個量，維度爲3.                                                                                  ，核函數，不使用，爲空；待估計參數
        problem.AddResidualBlock(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()<<" senconds."<<endl;

    //輸出結果
    cout<<summary.BriefReport()<<endl;
    cout<<"estimated a,b,c= ";
    for ( auto a:abc ) 
    {
        cout<<a<<" ";
    }
    cout<<endl;
}

CMakeLists文件

cmake_minimum_required(VERSION 2.8)
project(ceres)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
find_package(OpenCV REQUIRED)
#include_directories(${OpenCV_DIRS})

add_executable(ceres_node main.cpp)
target_link_libraries(ceres_node ${CERES_LIBRARIES} ${OpenCV_LIBS})

編譯與運行：

mkdir build
cd build
cmake ..
make 
./ceres_node

總結：
/*
使用Ceres求解非線性優化問題，一共分爲三個部分：
1、 第一部分：構建cost fuction，即代價函數，也就是尋優的目標式。這個部分需要使用擬函數（functor）這一技巧來實現，做法是定義一個cost function的結構體，在結構體內重載（）運算符，具體實現方法後續介紹。
2、 第二部分：通過代價函數構建待求解的優化問題。
3、 第三部分：配置求解器參數並求解問題，這個步驟就是設置方程怎麼求解、求解過程是否輸出等，然後調用一下Solve方法。
*/
第一部分：構造代價函數結構體

struct CURVE_FITTING_COST//定義一個CostFunction模型
{
    CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
    template <typename T>
    bool operator()(const T* const abc,T* residual) const//模型參數：3維 ，殘差 
    {
        //y-exp(ax^2+bx+c)
        residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
        return true;
    }
    const double _x,_y;
};

這裏的使用了仿函數的技巧，即在CostFunction結構體內，對（）進行重載，這樣的話，該結構體的一個實例就能具有類似一個函數的性質，在代碼編寫過程中就能當做一個函數一樣來使用。
關於仿函數，這裏再多說幾句，對結構體、類的一個實例，比如Myclass類的一個實例Obj1，如果Myclass裏對（）進行了重載，那Obj1被創建之後，就可以將Obj1這個實例當做函數來用，比如Obj（x）
ostFunction結構體中，對括號符號重載的函數中，傳入參數有兩個，一個是待優化的變量x，另一個是殘差residual，也就是代價函數的輸出。重載了（）符號之後，CostFunction就可以傳入AutoDiffCostFunction方法來構建尋優問題了。
第二部分：通過代價函數構建待求解的優化問題

    ceres::Problem problem;
    for(int i=0;i<N;i++)
    {
        //向問題中添加誤差項 problem.AddResidualBlock()
        //使用自動求導ceres::AutoDiffCostFunction<>  需要指定誤差項和優化變量的維度，這裏的誤差是標量，維度爲1；優化的是a,b,c三個量，維度爲3.                                                                                  ，核函數，不使用，爲空；待估計參數
        problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>(new CURVE_FITTING_COST(x_data[i],y_data[i])),nullptr,abc);
    }

這一部分就是待求解的優化問題的構建過程，使用之前結構體創建一個實例，由於使用了仿函數技巧，該實例在使用上可以當做一個函數。基於該實例new了一個CostFunction結構體，這裏使用的自動求導，將之前的代價函數結構體傳入，第一個1是輸出維度，即殘差的維度，第二個3是輸入維度，即待尋優參數abc的維度。分別對應之前結構體中的residual和x。
向問題中添加誤差項，本問題比較簡單，添加一次就行（有的問題要不斷多次添加ResidualBlock以構建最小二乘求解問題）。這裏的參數NULL是指不使用核函數，&x表示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()<<" senconds."<<endl;

    //輸出結果
    cout<<summary.BriefReport()<<endl;
    cout<<"estimated a,b,c= ";
    //for ( auto a:abc ) 
    for(int i=0;i<3;i++)
    {
        cout<<abc[i]<<" ";
    }

這一部分很好理解，創建一個Option，配置一下求解器的配置，創建一個Summary。最後調用Solve方法，求解。