g2o is an open-source C++ framework for optimizing graph-based nonlinear error functions. g2o has been designed to be easily extensible to a wide range of problems and a new problem typically can be specified in a few lines of code. The current implementation provides solutions to several variants of SLAM and BA.

一个简单的例子

现将通过一个直线拟合的例子理解G2O的工作流程。

假设方程\(y = kx + b\)， 给定n组观测数据\((x, y)\)求解系数\(X = [k, b]^T\).

令\(f(X) = y - kx -b\)N组数据可以组成一个大的非线性方程组

\[ \begin{bmatrix} y_1 - kx_1 -b \\ \vdots \\ y_n - kx_n -b \\ \end{bmatrix} \]

可以构建一个最小二乘问题：

\[ x = arg \, \min\limits_{x} \frac{1}{2} || f(x) ||^2 \]

要求解这个问题，根据推导部分可知，需要求解雅克比。

\[ J(X) = \begin{bmatrix} -k_1 & -1 \\ \cdots & \cdots \\ -k_n & -1 \\ \end{bmatrix} \]

/**
******************************************************************************
* @file
* @author  maky <chengwei920412@outlook.com>
* @version
* @date    2018-12-12 20:22:58
* @brief
******************************************************************************
* @attention
*
*
******************************************************************************
*/

#include <iostream>
#include <random>
#include <vector>
#include <memory>
#include <algorithm>
#include <Eigen/Eigen>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <opencv2/opencv.hpp>

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()
    {}
    virtual 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 void linearizeOplus() {
        const auto vertex = static_cast<const LineFittingVertex*> (_vertices[0]);
        const Eigen::Vector2d &estimate = vertex->estimate();
        /*
         * $$\begin{bmatrix} \frac{\partial y_1}{\partial x_1} & \frac{\partial y_2}{\partial x_2} \end{bmatrix} = \begin{bmatrix} -k & -1 \end{bmatrix}$$
         */
        _jacobianOplusXi(0, 0) = -_measurement(0, 0);
        _jacobianOplusXi(0, 1) = -1;
    }

    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 = 100.;
    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(true);
    // 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;
}

---

Reference

https://github.com/RainerKuemmerle/g2o

https://openslam-org.github.io/g2o.html

https://github.com/rosskidson/g2o_tutorial

Rainer & Grisetti g2o: A General Framework for Graph Optimization

Grisetti g2o：a general framework for(hyper) graph optimization