标签：optimizer BA fin v3 顶点 g2o 位姿 优化

 

 

https://blog.csdn.net/heroybc/article/details/106355948

 

1.位姿图

 

 

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

BA优化中的计算量很大很大，为了减少计算量，我已经介绍过很多方法了，这里，我们可以对位姿图进行优化，要如何做到，其实也很简单，只需要把相机观察的三维点去掉，在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

　　

李代数的位姿图优化

//展示部分片段，完整参考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="">> 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 >> 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;
}

　　

 

标签：optimizer,BA,fin,v3,顶点,g2o,位姿,优化
From： https://www.cnblogs.com/gooutlook/p/16646439.html