https://blog.csdn.net/heroybc/article/details/106355948
1.位姿图
当把BA优化问题中的位姿图的问题简化要怎么做?
BA优化中的计算量很大很大,为了减少计算量,我已经介绍过很多方法了,这里,我们可以对位姿图进行优化,要如何做到,其实也很简单,只需要把相机观察的三维点去掉,在g2o优化问题上,只使用相机顶点,以及相邻相机顶点的关系描述即可。
这样,我们就大大简化了计算量,为啥这就简化计算量了呢,因为我们只考虑相机顶点,以及相邻相机顶点的关系,不考虑三维点了,我们知道,三维点的数量一定远远大于相机位姿顶点的数量。
数学
2.位姿优化
我们已经学习过李群李代数的知识了,我们知道优化问题的相关数学如下:
相对的位姿关系:
我们通过对积几何(平面投影)得到位姿变化1,又通过上式得到位姿变化2,我们要做的,就是减少位姿变化1与位姿变化1的误差,这是一个最小二乘问题。
![](/i/l/?n=22&i=blog/1008336/202209/1008336-20220901143527525-937548477.png)
为了优化两个位姿,求其偏导数:
得到雅可比矩阵后,就是用梯度法、牛顿法求解啦,相信大家到这里对于最小二乘问题已经轻车熟路了。
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