.g2o文件数据格式

本文档详细解析了在高博slambook中遇到的.g2o文件内容,包括顶点(VERTEX_SE3:QUAT)的数据结构,如点的索引、坐标和四元数位姿,以及边(EDGE_SE3:QUAT)的信息,如从节点ID、到节点ID、相对位置和四元数。同时,介绍了信息矩阵的上三角元素,这对于理解slam中的 pose graph 构建至关重要。
摘要由CSDN通过智能技术生成

在学习高博slambook/ch11/pose_graph_gtsam.cpp时不清楚其调用的sphere.g2o文件中每项数据是什么,导致看不懂代码,查阅资料后明白了其各项含义,现在记录一下.

1.顶点

VERTEX_SE3:QUAT

点的索引值                        点的坐标                                  四元数形式的相机位姿

point id            position.x     positiion.y    position.z            qx               qy                qz                 qw

2499                 1.0318         85.323        -76.0615      0.779315    0.0718127    -0.620437    -0.0506864 

2.边

EDGE_SE3:QUAT

idFrom    idTo       position.x     position.y     position.z          qx                   qy                      qz               qw

    0           1        -0.0187953   0.0328449   -0.125146    0.0634648     -0.000250128    0.00237634    0.997981

信息矩阵的上三角元素

10000  0  0  0  0  0      10000  0  0  0  0      10000  0  0  0      40000  0  0    40000  0     40000 

(此信息矩阵为

  • 3
    点赞
  • 8
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
g2o的源码比较复杂,主要分为以下几个部分: 1. core:核心部分,包括图模型(Graph)、顶点(Vertex)、边(Edge)、优化算法(OptimizationAlgorithm)等。 2. solver:求解器部分,包括线性求解器(LinearSolver)、稀疏矩阵(SparseBlockMatrix)、稠密矩阵(DenseBlockMatrix)等。 3. types:类型部分,包括各种图优化问题相关的数据类型,如向量(Vector)、旋转矩阵(SE3)、四元数(Quaternion)等。 4. stuff:工具部分,包括文件读写、时间计算、随机数生成、命令行参数解析等。 下面以求解一个简单的图优化问题为例,简单介绍g2o的源码结构和使用方法。 首先,我们需要定义一个图模型,并向模型中添加顶点和边。比如,我们可以定义如下的图模型: ```c++ #include <g2o/core/sparse_optimizer.h> #include <g2o/core/block_solver.h> #include <g2o/core/optimization_algorithm_levenberg.h> #include <g2o/solvers/eigen/linear_solver_eigen.h> typedef g2o::BlockSolver<g2o::BlockSolverTraits<2,1>> BlockSolverType; typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType; typedef g2o::OptimizationAlgorithmLevenberg OptimizationAlgorithmType; int main(int argc, char** argv) { // 构建图优化模型 g2o::SparseOptimizer optimizer; optimizer.setVerbose(true); // 设置线性求解器和块求解器 LinearSolverType *linearSolver = new LinearSolverType(); BlockSolverType *blockSolver = new BlockSolverType(linearSolver); OptimizationAlgorithmType *optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver); // 设置求解算法和优化器 optimizer.setAlgorithm(optimizationAlgorithm); // 添加顶点 g2o::VertexSE2 *v1 = new g2o::VertexSE2(); v1->setId(0); v1->setEstimate(g2o::SE2(0,0,0)); optimizer.addVertex(v1); g2o::VertexSE2 *v2 = new g2o::VertexSE2(); v2->setId(1); v2->setEstimate(g2o::SE2(1,0,0)); optimizer.addVertex(v2); // 添加边 g2o::EdgeSE2 *e = new g2o::EdgeSE2(); e->setVertex(0, v1); e->setVertex(1, v2); e->setMeasurement(g2o::SE2(1,0,0)); e->setInformation(Eigen::Matrix3d::Identity()); optimizer.addEdge(e); // 执行优化 optimizer.initializeOptimization(); optimizer.optimize(10); // 输出结果 std::cout << v1->estimate().toVector().transpose() << std::endl; std::cout << v2->estimate().toVector().transpose() << std::endl; return 0; } ``` 在上面的代码中,我们首先定义了一个g2o::SparseOptimizer对象,然后设置了线性求解器和块求解器,以及Levenberg-Marquardt优化算法。接着,我们向模型中添加了两个顶点和一条边,其中边的测量值为(1,0,0),信息矩阵为单位矩阵。最后,我们执行了10次优化,并输出了优化后的顶点估计值。 g2o的源码结构比较复杂,但是其设计思想清晰,代码风格规范,易于扩展和修改。使用g2o库可以快速高效地解决各种图优化问题,是图优化领域不可或缺的工具。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值