视觉SLAM笔记(56) 位姿图优化


1. g2o 原生位姿图

下面来演示使用 g2o 进行位姿图优化
首先,用 g2o_viewer 打开预先生成的仿真位姿图sphere.g2o

$ g2o_viewer sphere.g2o

如图所示:

在这里插入图片描述
该位姿图是由 g2o 自带的 create sphere 程序仿真生成的
它的真实轨迹为一个球,由从下往上的多个层组成
每层为一个正圆形,很多个大小不一的圆形层组成了一个完整的球体,共包含 2500 个位姿节点
,可以看成一个转圈上升的过程
在这里插入图片描述
然后,仿真程序生成了 t − 1 到 t 时刻的边,称为 odometry 边(里程计)
此外,又生成层与层之间的边,称为 loop closure(回环)
随后,在每条边上添加观测噪声,并根据里程计边的噪声,重新设置节点的初始值
这样,就得到了带累计误差的位姿图数据
它局部看起来像球体的一部分,但整体形状与球体相差甚远

现在从这些带噪声的边和节点初始值出发,尝试优化整个位姿图,得到近似真值的数据
当然,实际当中的机器人肯定不会出现这样正球形的运动轨迹,以及如此完整的里程计与回环观测数据
仿真成正球的好处,是能够直观地看到优化结果是否正确(只要看它各个角度圆不圆就行了)

可以点击 g2o_viewer 中的 optimize 函数,可以看到每步的优化结果和收敛的过程
另一方面, sphere.g2o 也是一个文本文件,可以用文本编辑器打开,查看它里面的内容
文件前半部分由节点组成,后半部分则是边:

VERTEX_SE3:QUAT 0 -0.125664 -1.53894e-17 99.9999 0.706662 4.32706e-17 0.707551 -4.3325e-17 
...
EDGE_SE3:QUAT 2449 2499 -0.186998 -0.0161808 -6.29417 -0.00401022 0.0316215 -0.00624077 0.999472 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 40000 0 0 40000 0 40000 

可以看到,节点类型是 VERTEX_SE3,表达一个相机位姿
g2o 默认使用四元数和平移向量表达位姿
所以后面的字段意义为: ID, tx, ty, tz, qx, qy, qz, qw
前三个为平移向量元素,后四个为表示旋转的单位四元数

同样,边的信息为:两个节点的 ID,tx, ty, tz, qx, qy, qz, qw
信息矩阵的右上角(由于信息矩阵为对称阵,只需保存一半即可)
可以看到这里把信息矩阵设成了对角阵

为了优化该位姿图,可以使用 g2o 默认的顶点和边,它们是由四元数表示的
由于仿真数据也是 g2o 生成的
所以用 g2o 本身优化就无需多做什么工作了,只需配置一下优化参数即可

程序 pose_graph_g2o_SE3.cpp 使用 L-M算法对该位姿图进行优化,并把结果存储至 result.g2o 文件中

#include <iostream>
#include <fstream>
#include <string>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
using namespace std;

int main(int argc, char** argv)
{
    if (argc != 2)
    {
        cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl;
        return 1;
    }
    ifstream fin(argv[1]);
    if (!fin)
    {
        cout << "file " << argv[1] << " does not exist." << endl;
        return 1;
    }

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> Block;  // 6x6 BlockSolver
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<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::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm(solver);   // 设置求解器

    int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量
    while (!fin.eof())
    {
        string name;
        fin >> name;
        if (name == "VERTEX_SE3:QUAT")
        {
            // SE3 顶点
            g2o::VertexSE3* v = new g2o::VertexSE3();
            int index = 0;
            fin >> index;
            v->setId(index);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            if (index == 0)
                v->setFixed(true);
        }
        else if (name == "EDGE_SE3:QUAT")
        {
            // SE3-SE3 边
            g2o::EdgeSE3* e = new g2o::EdgeSE3();
            int idx1, idx2;     // 关联的两个顶点
            fin >> idx1 >> idx2;
            e->setId(edgeCnt++);
            e->setVertex(0, optimizer.vertices()[idx1]);
            e->setVertex(1, optimizer.vertices()[idx2]);
            e->read(fin);
            optimizer.addEdge(e);
        }
        if (!fin.good()) break;
    }

    cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;

    cout << "prepare optimizing ..." << endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    cout << "calling optimizing ..." << endl;
    optimizer.optimize(30);

    cout << "saving optimization results ..." << endl;
    optimizer.save("result.g2o");

    return 0;
}

选择了 6×6 的块求解器,使用 L-M 下降方式,迭代次数选择 30 次
调用此程序对位姿图进行优化:

$ ./pose_graph_g2o_SE3 ../sphere.g2o

在这里插入图片描述
然后,用 g2o_viewer 打开 result.g2o 查看结果

$ g2o_viewer result.g2o

在这里插入图片描述
结果从一个不规则的形状优化成了一个看起来完整的球
这件事情过程实质上和点击 g2o_viewer 上的 Optimize 按钮没有区别


2. 李代数上的位姿图优化

下面,根据前面的李代数推导,来实现一下李代数上的优化
来试试把 Sophus 用到 g2o 中,在pose_graph_g2o_lie_algebra.cpp中定义自己的顶点和边

#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Core>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_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 <g2o/solvers/cholmod/linear_solver_cholmod.h>

#include <sophus/se3.h>
#include <sophus/so3.h>
using namespace std;
using Sophus::SE3;
using Sophus::SO3;

typedef Eigen::Matrix<double, 6, 6> Matrix6d;

// 给定误差求J_R^{-1}的近似
Matrix6d JRInv(SE3 e)
{
    Matrix6d J;
    J.block(0, 0, 3, 3) = SO3::hat(e.so3().log());
    J.block(0, 3, 3, 3) = SO3::hat(e.translation());
    J.block(3, 0, 3, 3) = Eigen::Matrix3d::Zero(3, 3);
    J.block(3, 3, 3, 3) = SO3::hat(e.so3().log());
    J = J * 0.5 + Matrix6d::Identity();
    return J;
}
// 李代数顶点
typedef Eigen::Matrix<double, 6, 1> Vector6d;
class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        bool read(istream& is)
    {
        double data[7];
        for (int i = 0; i < 7; i++)
            is >> data[i];
        setEstimate(SE3(
            Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
            Eigen::Vector3d(data[0], data[1], data[2])
        ));
    }

    bool write(ostream& os) const
    {
        os << id() << " ";
        Eigen::Quaterniond q = _estimate.unit_quaternion();
        os << _estimate.translation().transpose() << " ";
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << endl;
        return true;
    }

    virtual void setToOriginImpl()
    {
        _estimate = Sophus::SE3();
    }
    // 左乘更新
    virtual void oplusImpl(const double* update)
    {
        Sophus::SE3 up(
            Sophus::SO3(update[3], update[4], update[5]),
            Eigen::Vector3d(update[0], update[1], update[2])
        );
        _estimate = up * _estimate;
    }
};

// 两个李代数节点之边
class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, SE3, VertexSE3LieAlgebra, VertexSE3LieAlgebra>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        bool read(istream& is)
    {
        double data[7];
        for (int i = 0; i < 7; i++)
            is >> data[i];
        Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
        q.normalize();
        setMeasurement(
            Sophus::SE3(q, Eigen::Vector3d(data[0], data[1], data[2]))
        );
        for (int i = 0; i < information().rows() && is.good(); i++)
            for (int j = i; j < information().cols() && is.good(); j++)
            {
                is >> information() (i, j);
                if (i != j)
                    information() (j, i) = information() (i, j);
            }
        return true;
    }
    bool write(ostream& os) const
    {
        VertexSE3LieAlgebra* v1 = static_cast<VertexSE3LieAlgebra*> (_vertices[0]);
        VertexSE3LieAlgebra* v2 = static_cast<VertexSE3LieAlgebra*> (_vertices[1]);
        os << v1->id() << " " << v2->id() << " ";
        SE3 m = _measurement;
        Eigen::Quaterniond q = m.unit_quaternion();
        os << m.translation().transpose() << " ";
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << " ";
        // information matrix 
        for (int i = 0; i < information().rows(); i++)
            for (int j = i; j < information().cols(); j++)
            {
                os << information() (i, j) << " ";
            }
        os << endl;
        return true;
    }

    // 误差计算与书中推导一致
    virtual void computeError()
    {
        Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
        Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
        _error = (_measurement.inverse()*v1.inverse()*v2).log();
    }

    // 雅可比计算
    virtual void linearizeOplus()
    {
        Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
        Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
        Matrix6d J = JRInv(SE3::exp(_error));
        // 尝试把J近似为I?
        _jacobianOplusXi = -J * v2.inverse().Adj();
        _jacobianOplusXj = J * v2.inverse().Adj();
    }
};

为了实现从 g2o 文件的存储和读取,实现了 read 和 write 函数
并且“伪装”成 g2o 内置的 SE3 顶点,使得 g2o_viewer 能够认识并渲染它
事实上,除了内部使用 Sophus 的李代数表示之外,从外部看起来没有什么区别
值得注意的是这里雅可比的计算过程
有若干种选择:

  1. 不提供雅可比计算函数,让 g2o 自动计算数值雅可比
  2. 提供完整或近似的雅可比计算过程

这里用JRInv() 函数提供近似的 Jr-1
可以尝试把它近似为 I,或者干脆注释掉 oplusImpl 函数,看看结果会有什么区别
之后调用 g2o 进行优化问题:

$ ./pose_graph_g2o_lie ../sphere.g2o

在这里插入图片描述

发现,迭代 23 次后,总体误差保持不变,事实上可以让优化算法停止了
在这里插入图片描述

而上一个实验中用满了 30 次迭代后误差仍在下降
在这里插入图片描述
在调用优化后,查看 result_lie.g2o 观察它的结果,从肉眼上看不出任何区别
在这里插入图片描述

如果在这个 g2o_viewer 界面按下 Optimize 按钮
g2o 将使用它自带的 SE3 顶点进行优化,可以在下方文本框中看到:
在这里插入图片描述
整体误差在 SE3 边的度量下为 44360,略小于之前 30 次迭代时的 44811
这说明使用李代数进行优化后,在更少的迭代次数下得到了更好的结果


3. 关于后端优化

球的例子是一个比较有代表性的案例
它具有和实际中相似的里程计边(Odometry)和回环边(Loop Closure)
这也正是实际 SLAM 中,一个位姿图中可能有的东西
同时,“球”也具有一定的计算规模:总共有 2,500 个位姿节点和近 10,000 条边
发现优化它费了不少时间(相对于实时性要求很强的前端来说)

另一方面,一般认为位姿图是结构最简单的图之一
在不假设机器人如何运动的前提下,很难再进一步讨论它的稀疏性了
因为机器人可能会直线往前运动,形成带状的位姿图,是稀疏的
也可能是“左手右手一个慢动作”,形成了大量的小型回环需要优化(Loopy motion)
从而变成像“球”那样比较稠密的位姿图

无论如何,在没有进一步的信息之前,似乎无法再利用位姿图的求解结构了

自从 PTAM 提出以来,人们就已经意识到,后端的优化没必要实时地响应前端的图像数据
人们倾向于把前端和后端分开,运行于两个独立线程之中
历史上称为跟踪(Tracking)和建图(Mapping)
虽然如此叫,建图部分主要是指后端的优化内容
通俗地说,前端需要实时响应视频的速度,例如每秒 30Hz
而优化可以慢悠悠地运行,只要在优化完成时把结果返回给前端即可
所以通常不会对后端优化提出很高的速度要求


参考:

《视觉SLAM十四讲》


相关推荐:

视觉SLAM笔记(55) 位姿图
视觉SLAM笔记(54) Ceres 操作后端优化
视觉SLAM笔记(53) g2o 操作后端优化
视觉SLAM笔记(52) BA 与图优化
视觉SLAM笔记(51) 非线性系统和 EKF


谢谢!

发布了217 篇原创文章 · 获赞 294 · 访问量 289万+

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie

分享到微信朋友圈

×

扫一扫,手机浏览