非線性優化一個曲線擬合（ceres/g2o）

　　將代碼和實際理論結合起來才能更好的理解理論上是怎么實現的，參考用高博十四講的理論加實踐親手試一下，感覺公式和代碼才能結合起來。不能做到創新，至少做到了解和理解

曲線擬合問題：

　　考慮這樣一條曲線：$y = \exp (a{x^2} + bx + c) + w$，其中a,b,c為曲線的參數，w為高斯噪聲，滿足$w = (0,{\sigma ^2})$，假設有N個關於x,y的觀測數據點，想根據這些數據點求出曲線的參數，（誤差噪聲）最小二乘

\[\mathop {\min }\limits_{a,b,c} \frac{1}{2}\sum\limits_{i = 1}^N {{{\left\| {{y_i} - \exp (ax_i^2 + b{x_i} + c)} \right\|}^2}} \]

　　我們首先明確我們要估計的變量是a,b,c這三個系數，思路是先根據模型生成x,y的真值，然后在真值中加入高斯分布的噪聲。隨后使用高斯牛頓法從帶噪聲的數據擬合參數模型。定義誤差為：${e_i} = {y_i} - \exp (ax_i^2 + b{x_i} + c)$，求取雅克比矩陣：

\[{J_i} = \left[ {\begin{array}{*{20}{c}}
{\frac{{\partial {e_i}}}{{\partial a}} = - x_i^2\exp (ax_i^2 + b{x_i} + c)}\\
{\frac{{\partial {e_i}}}{{\partial b}} = - {x_i}\exp (ax_i^2 + b{x_i} + c)}\\
{\frac{{\partial {e_i}}}{{\partial c}} = - \exp (ax_i^2 + b{x_i} + c)}
\end{array}} \right]\]

高斯牛頓法的增量方程為：

\[\left( {\sum\limits_{i = 1}^{100} {{J_i}{{({\sigma ^2})}^{ - 1}}{J_i}^T} } \right)\Delta {x_k} = \sum\limits_{i = 1}^{100} { - {J_i}{{({\sigma ^2})}^{ - 1}}{e_i}} \]

#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace cv;
using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    double ar = 1.0, br = 2.0, cr = 1.0;         // 真實參數值
    double ae = 2.0, be = -1.0, ce = 5.0;        // 估計參數值,賦給一個初值，然后在這個初值的基礎上進行變化量的迭代
    int N = 100;                                 // 數據點
    double w_sigma = 1.0;                        // 噪聲Sigma值
    double inv_sigma = 1.0 / w_sigma;           // 后面噪聲1/（Sigma^2)值會用
    cv::RNG rng;                                 // OpenCV隨機數產生器

    vector<double> x_data, y_data;      // 用於擬合的觀測數據
    for (int i = 0; i < N; i++) {
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
    }

    // 開始Gauss-Newton迭代
    int iterations = 100;    // 迭代次數
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost最小二乘值，通過此值衡量迭代是否到位，進行終止

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//計算迭代時間用
    for (int iter = 0; iter < iterations; iter++) {

        Matrix3d H = Matrix3d::Zero();             // Hessian = J^T W^{-1} J in Gauss-Newton
        Vector3d b = Vector3d::Zero();             // bias
        cost = 0;

        for (int i = 0; i < N; i++) {
            double xi = x_data[i], yi = y_data[i];  // 第i個數據點
            double error = yi - exp(ae * xi * xi + be * xi + ce);
            Vector3d J; // 雅可比矩陣
            J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
            J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
            J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc

            H += inv_sigma * inv_sigma * J * J.transpose();//構造Hx=b
            b += -inv_sigma * inv_sigma * error * J;

            cost += error * error;//計算最小二乘結果，看是否是最小的
        }

        // 求解線性方程 Hx=b
        Vector3d dx = H.ldlt().solve(b);
        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastCost) {
            cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
            break;
        }

        ae += dx[0];//更新迭代量，繼續迭代尋找最優值
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
            "\t\testimated params: " << ae << "," << be << "," << ce << 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;

    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    waitKey(0);
    return 0;

}

 

•  同樣的問題用ceres庫進行曲線擬合，迭代和求解過程就可以通過ceres的模板庫來操作

#include <iostream>
#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維,也就是要要優化的參數塊，
        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,0,0};            // abc參數的估計值，這里初始化為0

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

 

•  圖優化(g2o)進行曲線擬合參照圖優化系列-g2o細細看，多看幾遍中間的來龍去脈

#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>
using namespace std; 

// 曲線模型的頂點，模板參數：優化變量維度和數據類型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _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 {}
};

// 誤差模型 模板參數：觀測值維度，類型，連接頂點類型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 計算曲線模型誤差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _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,1> > Block;  // 每個誤差項優化變量維度為3，誤差值維度為1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 線性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩陣塊求解器
    // 梯度下降方法，從GN, LM, DogLeg 中選
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 圖模型
    optimizer.setAlgorithm( solver );   // 設置求解器
    optimizer.setVerbose( true );       // 打開調試輸出
    
    // 往圖中增加頂點
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );
    
    // 往圖中增加邊
    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] );      // 觀測數值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩陣：協方差矩陣之逆
        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;
}