Code

#include <iostream>
#include <opencv2/opencv.hpp>
#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>

class LineFittingVertex : public g2o::BaseVertex<2, Eigen::Vector2d>
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl()
    {
        _estimate << 0, 0;
    }

    virtual void oplusImpl(const double* update)
    {
        _estimate += Eigen::Vector2d(update);
    }

    virtual bool read(std::istream& in) { return true; }
    virtual bool write(std::ostream& out) const { return true; }
};

class LineFittingEdge : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, LineFittingVertex>
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    LineFittingEdge()
    : BaseUnaryEdge()
    {}
    void computeError()
    {
        const LineFittingVertex* vertex = static_cast<const LineFittingVertex*> (_vertices[0]);
        const Eigen::Vector2d &estimate = vertex->estimate();
        // error = y - (kx + b)
        _error(0, 0) = _measurement(1, 0) - (estimate(0, 0) * _measurement(0, 0) + estimate(1, 0));
    }
    virtual bool read(std::istream& in) { return true; }
    virtual bool write(std::ostream& out) const { return true; }
};


int main(int argc, const char **argv)try
{
    std::srand((unsigned int)std::time(NULL));
    cv::RNG random;
    double x_range = 3.;
    std::uint64_t inlier_total = 1000;
    double sigma = 1.;

    double k = (double)(std::rand() % 20000 - 10000) / (10000. / 5.);
    double b = (double)(std::rand() % 20000 - 10000) / (10000. / 3.);

    std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> points_obs;
    for (auto pos = 0; pos < inlier_total; pos++) {
        Eigen::Vector2d point;
        point.x() = (double)(std::rand() % 20000 - 10000) / (10000. / x_range);
        point.y() = k * point.x() + b + random.gaussian(sigma);
        points_obs.push_back(point);
    }

    // fitting
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>> BlockSolverType;
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<g2o::LinearSolverDense<BlockSolverType::PoseMatrixType>>()));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(algorithm);
    optimizer.setVerbose(false);
    // vertex
    {
        LineFittingVertex* vertex = new LineFittingVertex();
        vertex->setEstimate(Eigen::Vector2d(0., 0.));
        vertex->setId(0);
        optimizer.addVertex(vertex);
    }
    // edge
    for (auto pos = 0; pos < points_obs.size(); pos++)
    {
        auto vertex = optimizer.vertices().find(0);
        if (vertex == optimizer.vertices().end()) {
            std::cout << "can not find vertext: " << 0 << std::endl;
            continue;
        }
        LineFittingEdge* edge = new LineFittingEdge();
        edge->setId(pos);
        edge->setVertex(0, vertex->second);
        edge->setMeasurement(points_obs.at(pos));
        edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (sigma*sigma));
        optimizer.addEdge(edge);
    }

    optimizer.initializeOptimization();
    optimizer.optimize(100);

    auto vertex = optimizer.vertices().find(0);
    if (vertex == optimizer.vertices().end()) {
        std::cout << "can not find vertext: " << 0 << std::endl;
        return -1;
    }
    Eigen::Vector2d estimate = ((LineFittingVertex *)(vertex->second))->estimate();
    std::cout << "true: " << k << " " << b << " " << "estimated: " << estimate.transpose() << std::endl;

    return 0;
}
catch (const std::exception &exp) {
    std::printf("exception: %s.", exp.what());
    return -1;
}
catch (...) {
    std::printf("exception: unknown exception.");
    return -1;
}