视觉十四讲：第六讲

g2o是一个基于图优化的库，图优化是把优化问题表现为一种图的方式。一个图由若干个顶点和边组成。

顶点表示优化变量，边表示误差项。

g2o的使用步骤：

1.定义顶点和边的类型；

2.构建图；

3.选择优化算法；

4.调用g2o进行优化

#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/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() override {

    _estimate << 0, 0, 0;

  }

  // 更新估计值

  virtual void 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 abc = 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 abc = 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;  // x 值， y 值为 _measurement

};

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;

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

  }

  // 构建图优化，先设定g2o

  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1

  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

  // 梯度下降方法，可以从GN, LM, DogLeg 中选

  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 *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(10);   //迭代次数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 << "solve time cost = " << time_used.count() << " seconds. " << endl;

  // 输出优化值

  Eigen::Vector3d abc_estimate = v->estimate();

  cout << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;

}

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8)

project(ch6)

set(CMAKE_BUILD_TYPE Release)

set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

# OpenCV

find_package(OpenCV REQUIRED)

include_directories(${OpenCV_INCLUDE_DIRS})

# Ceres

find_package(Ceres REQUIRED)

include_directories(${CERES_INCLUDE_DIRS})

# g2o

find_package(G2O REQUIRED)

include_directories(${G2O_INCLUDE_DIRS})

# Eigen

include_directories("/usr/include/eigen3")

add_executable(gaussNewton gaussNewton.cpp)

target_link_libraries(gaussNewton ${OpenCV_LIBS})

add_executable(ceresCurveFitting ceresCurveFitting.cpp)

target_link_libraries(ceresCurveFitting ${OpenCV_LIBS} ${CERES_LIBRARIES})

add_executable(g2oCurveFitting g2oCurveFitting.cpp)

target_link_libraries(g2oCurveFitting ${OpenCV_LIBS} g2o_core g2o_stuff)

g2o需要新建一个cmake文件，建立一个FindG2O.cmake的文件：

# Find the header files

FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h

  ${G2O_ROOT}/include

  $ENV{G2O_ROOT}/include

  $ENV{G2O_ROOT}

  /usr/local/include

  /usr/include

  /opt/local/include

  /sw/local/include

  /sw/include

  NO_DEFAULT_PATH

  )

# Macro to unify finding both the debug and release versions of the

# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY

# macro.

MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)

  FIND_LIBRARY("${MYLIBRARY}_DEBUG"

    NAMES "g2o_${MYLIBRARYNAME}_d"

    PATHS

    ${G2O_ROOT}/lib/Debug

    ${G2O_ROOT}/lib

    $ENV{G2O_ROOT}/lib/Debug

    $ENV{G2O_ROOT}/lib

    NO_DEFAULT_PATH

    )

  FIND_LIBRARY("${MYLIBRARY}_DEBUG"

    NAMES "g2o_${MYLIBRARYNAME}_d"

    PATHS

    ~/Library/Frameworks

    /Library/Frameworks

    /usr/local/lib

    /usr/local/lib64

    /usr/lib

    /usr/lib64

    /opt/local/lib

    /sw/local/lib

    /sw/lib

    )

  FIND_LIBRARY(${MYLIBRARY}

    NAMES "g2o_${MYLIBRARYNAME}"

    PATHS

    ${G2O_ROOT}/lib/Release

    ${G2O_ROOT}/lib

    $ENV{G2O_ROOT}/lib/Release

    $ENV{G2O_ROOT}/lib

    NO_DEFAULT_PATH

    )

  FIND_LIBRARY(${MYLIBRARY}

    NAMES "g2o_${MYLIBRARYNAME}"

    PATHS

    ~/Library/Frameworks

    /Library/Frameworks

    /usr/local/lib

    /usr/local/lib64

    /usr/lib

    /usr/lib64

    /opt/local/lib

    /sw/local/lib

    /sw/lib

    )

  IF(NOT ${MYLIBRARY}_DEBUG)

    IF(MYLIBRARY)

      SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})

    ENDIF(MYLIBRARY)

  ENDIF( NOT ${MYLIBRARY}_DEBUG)

ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)

# Find the core elements

FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)

FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)

# Find the CLI library

FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)

# Find the pluggable solvers

FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)

FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)

FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)

FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)

FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)

FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)

FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)

FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)

# Find the predefined types

FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)

FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)

FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)

FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)

FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)

FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)

FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)

# G2O solvers declared found if we found at least one solver

SET(G2O_SOLVERS_FOUND "NO")

IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)

  SET(G2O_SOLVERS_FOUND "YES")

ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)

# G2O itself declared found if we found the core libraries and at least one solver

SET(G2O_FOUND "NO")

IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)

  SET(G2O_FOUND "YES")

ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)

巴特西

视觉十四讲：第六讲_g2o图优化

最新文章

热门文章