Pose graph优化:使用g2o自带的默认SE3顶点和边

#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表示位姿，是四元数和t来表示的.

• if (argc != 2) {
   cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl;
   return 1;
  

  }
• **********************************************/

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<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; // 顶点和边的数量
while (!fin.eof()) {
string name;
fin >> name;
if (name == “VERTEX_SE3:QUAT”) {
// SE3 顶点
g2o::VertexSE3 *v = new g2o::VertexSE3();//使用g2o默认的SE3顶点
int index = 0;
fin >> index;读取g2o文件为每个位姿设置的下标
v->setId(index);设置顶点ID，并把位姿数据读入顶点
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();/使用g2o默认的SE3边
int idx1, idx2; // 边所对应的关联的两个顶点ID
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;
}