SLAM高翔十四讲（十）第十讲 后段优化（滑动窗口和位姿图）

一、滑动窗口和位姿图

1.1 问题引入

带有相机位姿和空间点的图优化称为BA，能有效求解大规模的定位与建图问题。但是，我们需要控制BA的计算规模。
这里有两种方法？
1）滑动窗口法
2）位姿图

1.2 滑动窗口法

BA固定在一个时间窗口，离开这个窗口就被丢弃。当窗口结构发生变化时，我们需要如何做？

1. 新增一个关键帧和路标点
   由N个变量变成N+1个，按照正常的BA流程处理。对所有点边缘化，即可得到这N+1个关键帧的高斯分布参数。
2. 删除旧的关键帧
   删除旧的关键帧会导致整个问题不再稀疏，因此，需要对边缘化的过程进行改造，使其保持滑动窗口BA的稀疏性：1）边缘化旧的关键帧时，同时边缘化它观测到的路标点；2）要边缘化旧的关键帧，要看它观测到的路标点是否在新的关键帧中找到，若可则边缘化，否则不边缘。

滑动窗口法比较适合VO系统，不适合大规模建图的系统。

1.3 位姿图法

构建一个只有轨迹的图优化，位姿节点的边可通过关键帧的特征匹配之后的运动估计来给定初始值，之后不再考虑路标点，只关心所有相机位姿的联系。省去了大量特征点优化的计算，指标六了关键帧的轨迹，从而构建出位姿图。
位姿图这里同样是构造误差的最小二乘问题，求出雅可比矩阵，然后利用G-L、L-M方法求解。这里除了采用李代数表示位姿，其余都是相似的。至于误差模型和雅可比矩阵公式，在课本P271-272，请自行查看。

二、实践

2.1 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(pose_graph)

set(CMAKE_BUILD_TYPE "Debug")
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++14 -msse4")

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

# Eigen
include_directories("/usr/include/eigen3")

# sophus 
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# g2o 
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

add_executable(pose_graph_g2o_SE3 pose_graph_g2o_SE3.cpp)
target_link_libraries(pose_graph_g2o_SE3
        g2o_core g2o_stuff g2o_types_slam3d ${CHOLMOD_LIBRARIES}
        ${FMT_LIBRARIES} fmt
        )

add_executable(pose_graph_g2o_lie pose_graph_g2o_lie_algebra.cpp)
target_link_libraries(pose_graph_g2o_lie
        g2o_core g2o_stuff
        ${CHOLMOD_LIBRARIES}
        ${Sophus_LIBRARIES}
        ${FMT_LIBRARIES} fmt
        )

2.2 使用g2o自带的位姿图

这里的误差模型和雅可比矩阵公式，使用g2o自带的。

1. 代码演示

#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/solvers/eigen/linear_solver_eigen.h>

using namespace std;

/************************************************
 * 本程序演示如何用g2o solver进行位姿图优化
 * 使用列文伯格-马夸尔特方法对位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 这里使用g2o/types/slam3d/中的SE3表示位姿，它实质上是四元数而非李代数.
 * **********************************************/

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

    // 构建图优化，先设定g2o
    // 每个误差项优化变量维度为6，误差值维度为6
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
    // 线性方程求解器
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    // 选择优化算法，从GN, LM, DogLeg 中选，这里使用列文伯格-马夸尔特方法对位姿图优化
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm(solver);   // 设置求解器
    optimizer.setVerbose(true);       // 打开调试输出

    // 往图中增加顶点和边
    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 << "optimizing ..." << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);

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

    return 0;
}

2. 实验结果

read total 2500 vertices, 9799 edges.
optimizing ...
iteration= 0	 chi2= 1023011093.967642	 time= 0.319439	 cumTime= 0.319439	 edges= 9799	 schur= 0	 lambda= 805.622433	 levenbergIter= 1
iteration= 1	 chi2= 385118688.233188	 time= 0.292632	 cumTime= 0.612071	 edges= 9799	 schur= 0	 lambda= 537.081622	 levenbergIter= 1
......
iteration= 28	 chi2= 45095.174401	 time= 0.293314	 cumTime= 10.2802	 edges= 9799	 schur= 0	 lambda= 0.003127	 levenbergIter= 1
saving optimization results ...
iteration= 29	 chi2= 44811.248507	 time= 0.572786	 cumTime= 10.853	 edges= 9799	 schur= 0	 lambda= 0.003785	 levenbergIter= 2

2.3 使用李代数自定义g2o的顶点和边的位姿图（这里使用Sophus表示李代数）

这里的误差模型和雅可比矩阵公式，使用的是本章讲的。

1. 代码演示

#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/solvers/eigen/linear_solver_eigen.h>

#include <sophus/se3.hpp>

using namespace std;
using namespace Eigen;
using Sophus::SE3d;
using Sophus::SO3d;

/************************************************
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 本节使用李代数表达位姿图，节点和边的方式为自定义
 * **********************************************/

typedef Matrix<double, 6, 6> Matrix6d;

// 给定误差求J_R^{-1}的近似
Matrix6d JRInv(const SE3d &e) {
    Matrix6d J;
    J.block(0, 0, 3, 3) = SO3d::hat(e.so3().log());
    J.block(0, 3, 3, 3) = SO3d::hat(e.translation());
    J.block(3, 0, 3, 3) = Matrix3d::Zero(3, 3);
    J.block(3, 3, 3, 3) = SO3d::hat(e.so3().log());
    // J = J * 0.5 + Matrix6d::Identity();
    J = Matrix6d::Identity();    // try Identity if you want
    return J;
}

// 李代数顶点
typedef Matrix<double, 6, 1> Vector6d;

class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual bool read(istream &is) override {
        double data[7];
        for (int i = 0; i < 7; i++)
            is >> data[i];
        setEstimate(SE3d(
            Quaterniond(data[6], data[3], data[4], data[5]),
            Vector3d(data[0], data[1], data[2])
        ));
        return true;// 服了，原代码没有这个return报错，找了好久
    }

    virtual bool write(ostream &os) const override {
        os << id() << " ";
        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() override {
        _estimate = SE3d();
    }

    // 左乘更新
    virtual void oplusImpl(const double *update) override {
        Vector6d upd;
        upd << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate = SE3d::exp(upd) * _estimate;
    }
};

// 两个李代数节点之边
class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, SE3d, VertexSE3LieAlgebra, VertexSE3LieAlgebra> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    virtual bool read(istream &is) override {
        double data[7];
        for (int i = 0; i < 7; i++)
            is >> data[i];
        Quaterniond q(data[6], data[3], data[4], data[5]);
        q.normalize();
        setMeasurement(SE3d(q, 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;
    }

    virtual bool write(ostream &os) const override {
        VertexSE3LieAlgebra *v1 = static_cast<VertexSE3LieAlgebra *> (_vertices[0]);
        VertexSE3LieAlgebra *v2 = static_cast<VertexSE3LieAlgebra *> (_vertices[1]);
        os << v1->id() << " " << v2->id() << " ";
        SE3d 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() override {
        SE3d v1 = (static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate();
        SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate();
        _error = (_measurement.inverse() * v1.inverse() * v2).log();
    }

    // 雅可比计算
    virtual void linearizeOplus() override {
        SE3d v1 = (static_cast<VertexSE3LieAlgebra *> (_vertices[0]))->estimate();
        SE3d v2 = (static_cast<VertexSE3LieAlgebra *> (_vertices[1]))->estimate();
        Matrix6d J = JRInv(SE3d::exp(_error));  // 这里使用JRInv()函数提供近似的雅可比计算
        // 尝试把J近似为I？
        _jacobianOplusXi = -J * v2.inverse().Adj();  //P272 式10.9
        _jacobianOplusXj = J * v2.inverse().Adj();   //P272 式10.10
    }
};

int main(int argc, char **argv) {
    //if (argc != 2) {
    //    cout << "Usage: pose_graph_g2o_SE3_lie sphere.g2o" << endl;
    //    return 1;
    //}
    ifstream fin("/home/robot/桌面/slambook2-master/ch10/sphere.g2o");
    //if (!fin) {
    //    cout << "file " << argv[1] << " does not exist." << endl;
    //    return 1;
    //}

    // 构建图优化，先设定g2o
    // 每个误差项优化变量维度为6，误差值维度为6
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
    // 线性方程求解器
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    // 选择优化算法，从GN, LM, DogLeg 中选，这里使用列文伯格-马夸尔特方法对位姿图优化
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm(solver);   // 设置求解器
    optimizer.setVerbose(true);       // 打开调试输出

    // 往图中增加顶点和边
    int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量

    vector<VertexSE3LieAlgebra *> vectices;
    vector<EdgeSE3LieAlgebra *> edges;
    while (!fin.eof()) {
        string name;
        fin >> name;
        if (name == "VERTEX_SE3:QUAT") {
            // 顶点
            VertexSE3LieAlgebra *v = new VertexSE3LieAlgebra();
            int index = 0;
            fin >> index;
            v->setId(index);
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            vectices.push_back(v);
            if (index == 0)
                v->setFixed(true);
        } else if (name == "EDGE_SE3:QUAT") {
            // SE3-SE3 边
            EdgeSE3LieAlgebra *e = new EdgeSE3LieAlgebra();
            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);
            edges.push_back(e);
        }
        if (!fin.good()) break;
    }

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

    // 执行优化
    cout << "optimizing ..." << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    cout << "saving optimization results ..." << endl;

    // 因为用了自定义顶点且没有向g2o注册，这里保存自己来实现
    // 伪装成 SE3 顶点和边，让 g2o_viewer 可以认出
    ofstream fout("result_lie.g2o");
    for (VertexSE3LieAlgebra *v:vectices) {
        fout << "VERTEX_SE3:QUAT ";
        v->write(fout);
    }
    for (EdgeSE3LieAlgebra *e:edges) {
        fout << "EDGE_SE3:QUAT ";
        e->write(fout);
    }
    fout.close();
    return 0;
}

2. 实验结果
   这里迭代到27次就收敛，显然比上一个实验收敛的快。

read total 2500 vertices, 9799 edges.
optimizing ...
iteration= 0	 chi2= 674837160.579968	 time= 0.316748	 cumTime= 0.316748	 edges= 9799	 schur= 0	 lambda= 6658.554263	 levenbergIter= 1
iteration= 1	 chi2= 234706314.970484	 time= 0.287529	 cumTime= 0.604277	 edges= 9799	 schur= 0	 lambda= 2219.518088	 levenbergIter= 1
......
iteration= 26	 chi2= 127578.573840	 time= 2.50943	 cumTime= 13.126	 edges= 9799	 schur= 0	 lambda= 311923526733.601257	 levenbergIter= 9
saving optimization results ...
iteration= 27	 chi2= 127578.573840	 time= 4.99682	 cumTime= 18.1228	 edges= 9799	 schur= 0	 lambda= 11238229430124300558502199296.000000	 levenbergIter= 10