后端2-学习笔记

滑动窗口法

为了控制BA优化的规模，需要选择那些应该被优化的帧的位姿和地图点，orbslam2中采用的是一种共视图的结构，例如，仅优化与当前帧有20个以上共视路标点的关键帧。

不难想到滑动窗口分为两步：

1. 新增一个关键帧以及它观测到的路标点

2. 删除旧的关键帧，如果旧的关键帧观测到的路标点没有被其他帧观测到，连同路标点一并删除

后端优化中要求解方程Hx=b，研究发现H是一个稀疏矩阵，于是对该矩阵分块，使用Schur消元也就是边缘化的方法，先求出了关键帧的位姿，再求解路标点的坐标。

边缘化某个关键帧就是固定当前帧的位姿估计值，求其他变量以这个帧的位姿估计值为条件的条件概率

边缘化某个关键帧会导致右下角路标点矩阵不再是对角矩阵，这个过程也叫填入，使得BA无法按照原先的稀疏方式进行求解，这也导致了EKF无法处理较大规模的滑动窗口。

位姿图优化

舍弃对路标点的优化，只保留对位姿的优化。也就是固定地图点，求位姿。

优化前：

优化后：

#include 
#include 
#include 

#include 
#include 
#include 
#include 

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

    // 设定g2o
    typedef g2o::BlockSolver> BlockSolverType;
    typedef g2o::LinearSolverEigen LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique(g2o::make_unique()));
    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;
}