小言_互联网的博客

视觉SLAM笔记（29） g2o

2020-04-01 15:16 471人阅读  评论(0)

视觉SLAM笔记（29） g2o

1. 图优化

图优化是一种将非线性优化与图论结合起来的理论
因此在使用它之前，需要花一点篇幅介绍一个图优化理论

在 视觉SLAM笔记（27） 非线性最小二乘 介绍了非线性最小二乘的求解方式
它们是由很多个误差项之和组成的

然而，仅有一组优化变量和许多个误差项，并 不清楚它们之间的关联
比方说，某一个优化变量 x x j 存在于多少个误差项里？
能保证对它的优化是有意义的吗？
进一步，希望能够直观地看到该优化问题长什么样
于是，就说到了图优化，就是把优化问题表现成图（Graph） 的一种方式

这里的图是图论意义上的图
一个图由若干个顶点（Vertex） ，以及连接着这些节点的边（Edge） 组成
进而，用顶点表示优化变量，用边表示误差项
于是，对任意一个上述形式的非线性最小二乘问题，可以构建与之对应的一个图

一个简单的图优化例子：

用三角形表示相机位姿节点，用圆形表示路标点，它们构成了图优化的顶点
同时，蓝色线表示相机的运动模型，红色虚线表示观测模型，它们构成了图优化的边

此时，虽然整个问题的数学形式仍是：


但现在可以直观地看到问题的结构了
如果希望，也可以做去掉孤立顶点或优先优化边数较多（或按图论的术语，度数较大）的顶点这样的改进
但是最基本的图优化，是 用图模型来表达一个非线性最小二乘的优化问题
而可以利用图模型的某些性质，做更好的优化

2. g2o 的编译与安装

通用图形优化 g2o （General Graphic Optimization， G2O）为 SLAM 提供了图优化所需的内容
在使用一个库之前，需要对它进行编译和安装
应该已经体验很多次这个过程了，它们基本都是大同小异的
关于 g2o，可以从 github 下载： https://github.com/RainerKuemmerle/g2o
在 /3rdparty 下也附带了 g2o 库

解压代码包后，会看到 g2o 库的所有源码，它也是一个 CMake 工程
先来安装它的依赖项（部分依赖项与 Ceres 有重合）：

sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev libcxsparse3.1.4 libcholmod3.0.6

然后，按照 cmake 的方式对 g2o 进行编译安装即可

$ mkdir build
$ cd build
$ cmake ..
$ make
$ sudo make install

安装完成后， g2o 的头文件将在 /usr/local/g2o 下，库文件在 /usr/local/lib/ 下
现在，重新考虑 视觉SLAM笔记（28） Ceres 中的曲线拟合实验，在 g2o 中实验一遍

3. 拟合曲线

为了使用 g2o，首先要做的是将曲线拟合问题抽象成图优化
这个过程中，只要记住节点为优化变量，边为误差项即可
因此，曲线拟合的图优化问题可以画成：

在曲线拟合问题中，整个问题只有一个顶点：曲线模型的参数 a; b; c
而每个带噪声的数据点，构成了一个个误差项，也就是图优化的边
但这里的边与平时想的边不太一样，它们是 一元边（Unary Edge）
即 只连接一个顶点——因为整个图只有一个顶点

所以在图中，就只能把它画成自己连到自己的样子了
事实上，图优化中一条边可以连接一个、两个或多个顶点
这主要反映在每个误差与多少个优化变量有关
在稍微有些玄妙的说法中，把它叫做 超边（Hyper Edge），整个图叫做 超图（Hyper Graph）

弄清了这个图模型之后，接下来就是在 g2o 中建立该模型，进行优化了
作为 g2o 的用户，要做的事主要有以下几个步骤：

  1. 定义顶点和边的类型
  2. 构建图
  3. 选择优化算法
  4. 调用 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>
{
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(std::unique_ptr<Block::LinearSolverType>(linearSolver));       // 矩阵块求解器

    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::unique_ptr<Block>(solver_ptr) );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( std::unique_ptr<Block>(solver_ptr) );
    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;
}

在这个程序中，从 g2o 派生出了用于曲线拟合的图优化顶点和边：
CurveFittingVertex 和 CurveFittingEdge，这实质上是扩展了 g2o 的使用方式
在这两个派生类中，重写了重要的虚函数：

  1. 顶点的更新函数： oplusImpl
    优化过程最重要的是增量 ∆ x x 的计算，而该函数处理的是 x x k+1 = x x k + ∆ x x 的过程
    这仅仅是个简单的加法而已，为什么 g2o 不直接完成？
    在曲线拟合过程中，由于曲线参数本身位于向量空间中，这个更新计算确实就是简单的加法
    但是，当优化变量不处于向量空间中时，比方说 x x 是相机位姿，它本身不一定有加法运算
    这时，就需要重新定义增量如何加到现有的估计上的行为了
    这时可能使用左乘更新或右乘更新，而不是直接的加法
  2. 顶点的重置函数： setToOriginImpl
    这是平凡的，把估计值置零即可
  3. 边的误差计算函数： computeError
    该函数需要取出边所连接的顶点的当前估计值
    根据曲线模型，与它的观测值进行比较
    这和最小二乘问题中的误差模型是一致的
  4. 存盘和读盘函数： read, write
    由于我们并不想进行读写操作，就留空了
    定义了顶点和边之后，在 main 函数里声明了一个图模型，然后按照生成的噪声数据
    往图模型中添加顶点和边，最后调用优化函数进行优化

编译完成后执行出现以下错误，参考 静态库路径配置g2o

一切正常的话，g2o 给出优化的结果：

使用 L-M 方法进行梯度下降，在迭代了 16 次后，最后优化结果与 Ceres 实验中相差无几

参考：

《视觉SLAM十四讲》

相关推荐：

视觉SLAM笔记（28） Ceres
视觉SLAM笔记（27） 非线性最小二乘
视觉SLAM笔记（26） 状态估计问题
视觉SLAM笔记（25） 拼接点云
视觉SLAM笔记（24） 图像基础操作

谢谢！


转载：https://blog.csdn.net/qq_32618327/article/details/102403457
查看评论
* 以上用户言论只代表其个人观点，不代表本网站的观点或立场