视觉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， t_x, t_y, t_z, q_x, q_y, q_z, q_w
前三个为平移向量元素，后四个为表示旋转的单位四元数

同样，边的信息为：两个节点的 ID，t_x, t_y, t_z, q_x, q_y, q_z, q_w
信息矩阵的右上角（由于信息矩阵为对称阵，只需保存一半即可）
可以看到这里把信息矩阵设成了对角阵

为了优化该位姿图，可以使用 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() 函数提供近似的 J_r^-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

---

谢谢！