【SLAM】位姿图优化的入门与实践

1.位姿图

当把BA优化问题中的位姿图的问题简化要怎么做？

BA优化中的计算量很大很大，为了减少计算量，我已经介绍过很多方法了，这里，我们可以对位姿图进行优化，要如何做到，其实也很简单，只需要把相机观察的三维点去掉，在g2o优化问题上，只使用相机顶点，以及相邻相机顶点的关系描述即可。

这样，我们就大大简化了计算量，为啥这就简化计算量了呢，因为我们只考虑相机顶点，以及相邻相机顶点的关系，不考虑三维点了，我们知道，三维点的数量一定远远大于相机位姿顶点的数量。

这就是位姿图，在g2o优化问题上，只使用相机顶点，以及相邻相机顶点的关系。

2.位姿优化

我们已经学习过李群李代数的知识了，我们知道优化问题的相关数学如下：

相对的位姿关系：

我们通过对积几何（平面投影）得到位姿变化1，又通过上式得到位姿变化2，我们要做的，就是减少位姿变化1与位姿变化1的误差，这是一个最小二乘问题。

为了优化两个位姿，求其偏导数：

得到雅可比矩阵后，就是用梯度法、牛顿法求解啦，相信大家到这里对于最小二乘问题已经轻车熟路了。

3.g2o图优化与位姿优化

我们先安装g2o并运行g2o。

//安装

git clone https://github.com/RainerKuemmerle/g2o/
git log |grep 8ba8a*
git checkout 8ba8a03f7863e1011e3270bb73c8ed9383ccc2a2
sudo apt-get install libqt4-dev
sudo apt-get install qt4-qmake
sudo apt-get install libqglviewer-dev
mkdir build
cd build
cmake ../
make -j8

打开我们的可视化软件：

可以操作一下：

4.李代数的位姿图优化

//展示部分片段，完整参考GITHUB

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "Usage: pose_graph_g2o_SE3_lie 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<g2o::blocksolvertraits<6, 6="">&gt; BlockSolverType;
    typedef g2o::LinearSolverEigen<blocksolvertype::posematrixtype> LinearSolverType;
    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 &gt;&gt; name;
        if (name == "VERTEX_SE3:QUAT") {
            // 顶点
            VertexSE3LieAlgebra *v = new VertexSE3LieAlgebra();
            int index = 0;
            fin &gt;&gt; index;
            v-&gt;setId(index);
            v-&gt;read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            vectices.push_back(v);
            if (index == 0)
                v-&gt;setFixed(true);
        } else if (name == "EDGE_SE3:QUAT") {
            // SE3-SE3 边
            EdgeSE3LieAlgebra *e = new EdgeSE3LieAlgebra();
            int idx1, idx2;     // 关联的两个顶点
            fin &gt;&gt; idx1 &gt;&gt; idx2;
            e-&gt;setId(edgeCnt++);
            e-&gt;setVertex(0, optimizer.vertices()[idx1]);
            e-&gt;setVertex(1, optimizer.vertices()[idx2]);
            e-&gt;read(fin);
            optimizer.addEdge(e);
            edges.push_back(e);
        }
        if (!fin.good()) break;
    }

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

    cout &lt;&lt; "optimizing ..." &lt;&lt; endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);

    cout &lt;&lt; "saving optimization results ..." &lt;&lt; endl;

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

5.小结

这里我们就介绍完了后端优化的部分，在这一部分呢，主要以全局BA优化和位姿优化为主，在当前的研究领域当中，还有很多新颖的办法，如果还有可能，我会继续讲解，SLAM系统的前端、后端部分已经结束，还要一部分，就是回环检测，这将是非常重要但是相对好理解的一部分内容。