视觉slam14讲ch6

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]);      //残差初始值
         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;                    //数据点
    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++)          //以0.1的步长增加x
    {
        double x = i/100.0;             
        x_data.push_back(x);            //x坐标点入栈
        y_data.push_back(exp(a*x*x + b*x + c) + rng.gaussian(w_sigma));     //(函数+高斯噪声)生成随机y坐标点
        cout << "["<< x_data[i] << "," << y_data[i] <<"]"<< endl;           //x,y数据打印
    }

    //构建最小二乘问题
    ceres::Problem problem;
    for(int i = 0; i < N; i++)
    {
        //向问题中添加误差项
        problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>     //使用自动求导(模板参数：误差类型，输出维度)，数值参照前面struct中写法
        (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拟合曲线

#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>            //CurveFittingVertex子类继承模板类BaseVertex
{
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 );      //LM方法求解
    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;
}