視覺SLAM十四講ch6曲線擬合　代碼註釋

ceres

// ceres 版本
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代價函數的計算模型
struct CURVE_FITTING_COST
{
    CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
    // 殘差的計算
    template <typename T>
    bool operator() (
        const T* const abc,     // 模型參數，有3維 當沒有必要分類的時候 就用一個數組來存儲未知的係數,方便管理，而不是設3個變量，之後在()重載函數的形式參數個數變爲3個
        T* residual ) const     // 殘差
    {
        residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c)
        return true;
    }
    const double _x, _y;    // x,y數據
};

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.8,2.1,0.9};      // 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++ )
    {
        /* 第一個參數 CostFunction* : 描述最小二乘的基本形式即代價函數 例如書上的116頁fi(.)的形式
         * 第二個參數 LossFunction* : 描述核函數的形式 例如書上的ρi（.）
         * 第三個參數 double* :       待估計參數（用數組存儲）
         * 這裏僅僅重載了三個參數的函數，如果上面的double abc[3]改爲三個double a=0 ,b=0,c = 0;
         * 此時AddResidualBlock函數的參數除了前面的CostFunction LossFunction 外後面就必須加上三個參數 分別輸入&a,&b,&c
         * 那麼此時下面的 ceres::AutoDiffCostFunction<>模板參數就變爲了 <CURVE_FITTING_COST,1,1,1,1>後面三個1代表有幾類未知參數
         * 我們修改爲了a b c三個變量，所以這裏代表了3類，之後需要在自己寫的CURVE_FITTING_COST類中的operator()函數中，
         * 把形式參數變爲了const T* const a, const T* const b, const T* const c ,T* residual
         * 上面修改的方法與本例程實際上一樣，只不過修改的這種方式顯得亂，實際上我們在用的時候，一般都是殘差種類有幾個，那麼後面的分類 就分幾類
         * 比如後面講的重投影誤差，此事就分兩類 一類是相機9維變量，一類是點的3維變量，然而殘差項變爲了2維
         *
         * （1）: 修改後的寫法（當然自己定義的代價函數要對應修改重載函數的形式參數，對應修改內部的殘差的計算）：
         *      ceres::CostFunction* cost_function
         *              = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 1 ,1 ,1>(
         *                  new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
         *      problem.AddResidualBlock(cost_function,nullptr,&a,&b,&c);
         * 修改後的代價函數的計算模型:
         *   struct CURVE_FITTING_COST
         *   {
         *       CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
         *       // 殘差的計算
         *       template <typename T>
         *       bool operator() (
         *          const T* const a,
         *          const T* const b,
         *          const T* const c,
         *          T* residual   ) const     // 殘差
         *       {
         *           residual[0] = T ( _y ) - ceres::exp ( a[0]*T ( _x ) *T ( _x ) + b[0]*T ( _x ) + c[0] ); // y-exp(ax^2+bx+c)
         *           return true;
         *       }
         *       const double _x, _y;    // x,y數據
         *   };//代價類結束
         *
         *
         * （2）: 本例程下面的語句通常拆開來寫(看起來方便些):
         * ceres::CostFunction* cost_function
         *              = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
         *                  new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
         * problem.AddResidualBlock(cost_function,nullptr,abc)
         * */
        problem.AddResidualBlock (     // 向問題中添加誤差項
        // 使用自動求導，模板參數：誤差類型，Dimension of residual(輸出維度 表示有幾類殘差，本例程中就一類殘差項目，所以爲1)，輸入維度，維數要與前面struct中一致
                /*這裏1 代表*/
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> ( 
                new CURVE_FITTING_COST ( x_data[i], y_data[i] )// x_data[i], y_data[i] 代表輸入的獲得的試驗數據
            ),
            nullptr,            // 核函數，這裏不使用，爲空  這裏是LossFunction的位置
            abc                 // 待估計參數3維
        );
    }

    // 配置求解器ceres::Solver (是一個非線性最小二乘的求解器)
    ceres::Solver::Options options;     // 這裏有很多配置項可以填Options類嵌入在Solver類中 ，在Options類中可以設置關於求解器的參數
    options.linear_solver_type = ceres::DENSE_QR;  // 增量方程如何求解 這裏的linear_solver_type 是一個Linear_solver_type的枚舉類型的變量
    options.minimizer_progress_to_stdout = true;   // 爲真時 內部錯誤輸出到cout，我們可以看到錯誤的地方，默認情況下，會輸出到日誌文件中保存

    ceres::Solver::Summary summary;                // 優化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//記錄求解時間間隔
    //cout<<endl<<"求解前....."<<endl;
    /*下面函數需要3個參數：
     * 1、 const Solver::Options& options <----> optione
     * 2、 Problem* problem               <----> &problem
     * 3、 Solver::Summary* summary       <----> &summart (即使默認的參數也需要定義該變量 )
     * 這個函數會輸出一些迭代的信息。
     * */
    ceres::Solve ( options, &problem, &summary );  // 開始優化
    //cout<<endl<<"求解後....."<<endl;
    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;

    // 輸出結果
    // BriefReport() : A brief one line description of the state of the solver after termination.
    cout<<summary.BriefReport() <<endl;
    cout<<"estimated a,b,c = ";
    /*auto a:abc  或者下面的方式都可以*/
    for ( auto &a:abc ) cout<<a<<" ";
    cout<<endl;

    return 0;
}

g20

#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
#include <memory>
using namespace std; 

// 曲線模型的頂點，模板參數：優化變量維度和數據類型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW  //表示在利用Eigen庫的數據結構時new的時候 需要對齊，所以加入EIGEN特有的宏定義即可實現
    //下面幾個虛函數都是覆蓋了基類的對應同名同參數的函數
    virtual void setToOriginImpl() // 重置 這個虛函數override 覆蓋了Vertex類的對應函數 函數名字和參數都是一致的，是多態的本質
    {
        _estimate << 0,0,0;//輸入優化變量初始值
    }
    
    virtual void oplusImpl( const double* update ) // 更新 對於擬合曲線這種問題，這裏更新優化變量僅僅是簡單的加法，
                                                    // 但是到了位姿優化的時候，旋轉矩陣更新是左乘一個矩陣 此時這個更新函數就必須要重寫了
    {   //更新參數估計值
        _estimate += Eigen::Vector3d(update);
    }
    // 存盤和讀盤：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

// 誤差模型 模板參數：觀測值維度，類型，連接頂點類型  //這裏觀測值維度是1維，如果是108頁6.12式，則觀測值維度是2
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    //自己添加explicit 防止隱式轉換
    explicit CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 計算曲線模型誤差
    void computeError()
    {
/*       _vertices是std::vector<Vertex *>類型的變量，我們這裏把基類指針_vertices【0】強制轉換成const CurveFittingVertex* 自定義子類的常量指針
        這裏的轉換是上行轉換（子類指針轉換到基類），對於static_cast 和dynamic_cast兩種的結果都是一樣的，但是對於這種下行轉換則dynamic_cast比static_cast多了類型檢查功能
        更安全些，但是dynamic_cast只能用在類類型的指針 引用，static_cast則不限制,即可以用在類型也可以用在其他類型，所以這裏應該更改爲dynamic_cast
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
*/
        //修改後
        const CurveFittingVertex* v = dynamic_cast<const CurveFittingVertex*> (_vertices[0]);
        //獲取此時待估計參數的當前更新值 爲下面計算誤差項做準備
        const Eigen::Vector3d abc = v->estimate();
        //這裏的error是1x1的矩陣，因爲誤差項就是1個 _measurement是測量值yi
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值爲 _measurement
};

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;
    }

    // 構建圖優化，先設定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,3> > Block;  // 每個誤差項優化變量維度爲3，誤差值維度爲1後面的那個參數與誤差變量無關 僅僅表示路標點的維度 這裏因爲沒有用到路標點 所以爲什麼值都可以

/*
原版錯誤方式 : 這樣會出錯
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 線性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩陣塊求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );//LM法
*/

/*第一種解決方式: 將普通指針強制轉換成智能指針 需要注意的是 轉化之後 原來的普通指針指向的內容會有變化
 普通指針可以強制轉換成智能指針，方式是通過智能指針的一個構造函數來實現的， 比如下面的Block( std::unique_ptr<Block::LinearSolverType>( linearSolver ) );
 這裏面就是將linearSolver普通指針作爲參數用智能指針構造一個臨時的對象，此時原來的普通指針就無效了，一定不要再次用那個指針了，否則會有意想不到的錯誤，如果還想保留原來的指針
 那麼就可以利用第二種方式 定義的時候就直接用智能指針就好，但是就如第二種解決方案那樣，也會遇到類型轉換的問題。詳細見第二種方式說明
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();    // 線性方程求解器
    Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>( linearSolver ) );      // 矩陣塊求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::unique_ptr<g2o::Solver>(solver_ptr) );//LM法
*/

/*第二種解決方案: 定義變量時就用智能指針 需要注意的是 需要std::move移動
 *下面可以這樣做 std::make_unique<>是在c++14中引進的 而std::make_shared<>是在c++11中引進的,都是爲了解決用new爲智能指針賦值的操作。這種更安全。
 *  對於（2）將linearSovler智能指針的資源利用移動構造函數轉移到新建立的Block中，此時linearSolver這個智能指針默認不能夠訪問以及使用了。
 *  對於(3)來說，因爲solver_ptr是一個指向Block類型的智能指針，但是g2o::OptimizationAlgorithmLevenberg 構造函數接受的是std::unique_ptr<Solver>的參數，引起衝突，但是智能指針指向不同的類型時，
 *  不能夠通過強制轉換，所以此時應該用一個std::move將一個solver_ptr變爲右值，然後調用std::unique_ptr的移動構造函數，而這個函數的本身並沒有限制指針
 *  指向的類型，只要是std::unique_ptr類的對象，我們就可以調用智能指針的移動構造函數進行所屬權的移動。
 *
 * */
    std::unique_ptr<Block::LinearSolverType>linearSolver( new g2o::LinearSolverDense<Block::PoseMatrixType>() );// 線性方程求解器(1)
    std::unique_ptr<Block> solver_ptr ( new  Block( std::move(linearSolver) ) );// 矩陣塊求解器 (2)
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::move(solver_ptr) );//(3) LM法

    // 梯度下降方法，從GN, LM, DogLeg 中選(下面的兩種方式要按照上面的兩種解決方案對應修改，否則會編譯出錯 )
     //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::move(solver_ptr) );
     //g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( std::move(solver_ptr) );

    g2o::SparseOptimizer optimizer;     // 圖模型
    optimizer.setAlgorithm( solver );   // 設置求解器
    optimizer.setVerbose( true );       // 打開調試輸出
    
    // 往圖中增加頂點
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );//增加頂點的初始值，如果是位姿 則初始值是用ICP PNP來提供初始化值
    v->setId(0);//增加頂點標號 多個頂點要依次增加編號
    optimizer.addVertex( v );//將新增的頂點加入到圖模型中
    
    // 往圖中增加邊 N個
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // 設置連接的頂點
        edge->setMeasurement( y_data[i] );      // 觀測數值 經過高斯噪聲的
        //這裏的信息矩陣可以參考:http://www.cnblogs.com/gaoxiang12/p/5244828.html 裏面有說明
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩陣：協方差矩陣之逆 這裏爲1表示加權爲1
        optimizer.addEdge( edge );
    }
    
    // 執行優化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    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;
    
    // 輸出優化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
    
    return 0;
}