g2oCurveFitting.cpp

// We can think of the problem as graph optimzation problem and solve likewise

#include <iostream>
#include <g2o/core/g2o_core_api.h>
#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/solvers/csparse/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/csparse/optimization_algorithm_dogleg.h>
#include <g2o/solvers/cholmod/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::Vecgtor3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual void setToOriginImpl() override
    {
        _estimate << 0, 0, 0;
    }

    virtualvoid oplusImpl(const double *update) override
    {
        _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) {}

    virtual void computeError() override
    {
        const CurveFittingVertex *v = static_cast<const CurveFittingVertex *>(_vertices[0]);
        const Eigen::Vector3d abd = v->estimate();
        _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * __x + abc(1, 0) * _x + abc(2, 0));
    }

    virtual void linearizeOplus() override
    {
        const CurveFittingVertex *v = static_cast<const CurveFittingVertex *>(_vertices[0]);
        const Eigen::Vector3d abd = v->estimate();
        double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
        _jacobianOplusXi[0] = _x * _x * y;
        _jacobianOplusXi[1] = _x * y;
        _jacobianOplusXi[2] = y;
    }

    virtual bool read(istream &in) {}
    virtual bool write(ostream &out) const {}

public:
    double _x;
};

int main(int argc, char **argv)
{
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;

    auto solver = new g2o::OptimizationAlgorithmGaussNewton(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    )

    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(true);

    CurveFittingVertex *v = new CurveFittingVertex();
    v->setEstimate(Eigen::Vector3d(ae, be, ce));
    v->setId(0);
    optimizer.addVertex(v);

    for (int i = 0; i < N; i++)
    {
        CurveFittingEdge *e = new CurveFittingEdge(x_data[i]);
        e->setVertex(0, v);
        e->setMeasurement(y_data[i]);
        e->setId(i);
        e->setMeasurement(y_data[i]);
        e->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1(w_sigma * w_sigma));
        optimizer.addEdge(e);
    }

    cout << "start optimization" << endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optimization costs time: " << time_used.count() << " seconds." << endl;

    Eigen::Vector3d abc_estimate = v->estimate();
    cout << "estimated abc: " << abc_estimate.transpose() << endl;

    return 0;
}