不就是G2O嘛

从零开始一起学习SLAM | 理解图优化,一步步带你看懂g2o代码

SLAM的后端一般分为两种处理方法,一种是以扩展卡尔曼滤波(EKF)为代表的滤波方法,一种是以图优化为代表的非线性优化方法。不过,目前SLAM研究的主流热点几乎都是基于图优化的。

顺便总结下滤波方法的优缺点:

优点:在当时计算资源受限、待估计量比较简单的情况下,EKF为代表的滤波方法比较有效,经常用在激光SLAM中。

缺点:它的一个大缺点就是存储量和状态量是平方增长关系,因为存储的是协方差矩阵,因此不适合大型场景。而现在基于视觉的SLAM方案,路标点(特征点)数据很大,滤波方法根本吃不消,所以此时滤波的方法效率非常低

在SLAM里,图优化一般分解为两个任务:

1、构建图。机器人位姿作为顶点,位姿间关系作为边。

2、优化图。调整机器人的位姿(顶点)来尽量满足边的约束,使得误差最小。

g2o安装很简单,参考GitHub上官网:

https://github.com/RainerKuemmerle/g2o

1.顶点和边 

注意看 has-many 箭头,你看这个超图包含了许多顶点(HyperGraph::Vertex)和边(HyperGraph::Edge)。而这些顶点顶点继承自 Base Vertex,也就是OptimizableGraph::Vertex,而边可以继承自 BaseUnaryEdge(单边), BaseBinaryEdge(双边)或BaseMultiEdge(多边),它们都叫做OptimizableGraph::Edge

2.配置SparseOptimizer的优化算法和求解器

整个图的核心SparseOptimizer 包含一个优化算法(OptimizationAlgorithm)的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton(高斯牛顿法,简称GN), Levernberg-Marquardt(简称LM法), Powell's dogleg 三者中间选择一个(我们常用的是GN和LM)

3.如何求解 

OptimizationWithHessian 内部包含一个求解器(Solver),这个Solver实际是由一个BlockSolver组成的。这个BlockSolver有两个部分,一个是SparseBlockMatrix ,用于计算稀疏的雅可比和Hessian矩阵;一个是线性方程的求解器(LinearSolver),它用于计算迭代过程中最关键的一步HΔx=−b,LinearSolver有几种方法可以选择:PCG, CSparse, Choldmod,具体定义后面会介绍

高博在十四讲中g2o求解曲线参数的例子来说明,源代码地址

https://github.com/gaoxiang12/slambook/edit/master/ch6/g2o_curve_fitting/main.cpp

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3,误差值维度为1// 第1步:创建一个线性求解器LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 第2步:创建BlockSolver。并用上面定义的线性求解器初始化
Block* solver_ptr = new Block( linearSolver );      // 第3步:创建总求解器solver。并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );// 第4步:创建终极大boss 稀疏优化器(SparseOptimizer)
g2o::SparseOptimizer optimizer;     // 图模型
optimizer.setAlgorithm( solver );   // 设置求解器
optimizer.setVerbose( true );       // 打开调试输出// 第5步:定义图的顶点和边。并添加到SparseOptimizer中
CurveFittingVertex* v = new CurveFittingVertex(); //往图中增加顶点
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
for ( int i=0; i<N; i++ )    // 往图中增加边
{CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );edge->setId(i);edge->setVertex( 0, v );                // 设置连接的顶点edge->setMeasurement( y_data[i] );      // 观测数值edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵:协方差矩阵之逆optimizer.addEdge( edge );//设置迭代次数
}// 第6步:设置优化参数,开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100);

 1.线性求解器

LinearSolverCholmod :使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse:使用CSparse法。继承自LinearSolverCCS
LinearSolverPCG :使用preconditioned conjugate gradient 法,继承自LinearSolver
LinearSolverDense :使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen: 依赖项只有eigen,使用eigen中sparse Cholesky 求解,因此编译好后可以方便的在其他地方使用,性能和CSparse差不多。继承自LinearSolver

2.创建BlockSolver。并用上面定义的线性求解器初始化。

BlockSolver 内部包含 LinearSolver,用上面我们定义的线性求解器LinearSolver来初始化。

你点进去会发现 BlockSolver有两种定义方式,一种是指定的固定变量的solver,我们来看一下定义

using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

其中p代表pose的维度(注意一定是流形manifold下的最小表示),l表示landmark的维度

另一种是可变尺寸的solver,定义如下

using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

 这是因为在某些应用场景,我们的Pose和Landmark在程序开始时并不能确定,那么此时这个块状求解器就没办法固定变量,此时使用这个可变尺寸的solver,所有的参数都在中间过程中被确定另外你看block_solver.h的最后,预定义了比较常用的几种类型,如下所示:

BlockSolver_6_3 :表示pose 是6维,观测点是3维。用于3D SLAM中的BA
BlockSolver_7_3:在BlockSolver_6_3 的基础上多了一个scale
BlockSolver_3_2:表示pose 是3维,观测点是2维

3.创建总求解器solver。并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化

你点进去 GN、 LM、 Doglet算法内部,会发现他们都继承自同一个类OptimizationWithHessian,如下图所示,这也和我们最前面那个图是相符的。然后,我们点进去看 OptimizationAlgorithmWithHessian,发现它又继承自OptimizationAlgorithm,这也和前面的相符,总之,在该阶段,我们可以选则三种方法:

g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg 
g2o::OptimizationAlgorithmDogleg 

4.创建终极大boss 稀疏优化器(SparseOptimizer),并用已定义求解器作为求解方法。

创建稀疏优化器

g2o::SparseOptimizer    optimizer;

用前面定义好的求解器作为求解方法:

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

其中setVerbose是设置优化过程输出信息用的

SparseOptimizer::setVerbose(bool verbose)

5.定义图的顶点和边。并添加到SparseOptimizer中。

6.设置优化参数,开始执行优化。

设置SparseOptimizer的初始化、迭代次数、保存结果等。

SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)

设置迭代次数,然后就开始执行图优化了。

SparseOptimizer::optimize(int iterations, bool online)

https://www.jianshu.com/p/e16ffb5b265d

https://blog.csdn.net/heyijia0327/article/details/47686523

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/70936.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

【学习笔记】C++ 中 static 关键字的作用

目录 前言static 作用在变量上static 作用在全局变量上static 作用在局部变量上static 作用在成员变量上 static 作用在函数上static 作用在函数上static 作用在成员函数上 前言 在 C/C 中&#xff0c;关键字 static 在不同的应用场景下&#xff0c;有不同的作用&#xff0c;这…

老听说企业要做私域运营,那具体如何做呢?

以前企业获得新客户的方式是从各大流量平台进行引流&#xff0c;但现在这些公域平台人力投入和产出的比例不合理&#xff0c;或者费用太高而无法承担。因此&#xff0c;企业需要建立自己的私域流量池&#xff0c;无需付费、随时可接触的私域流量池。 那么&#xff0c;怎么做私域…

NIFI关于Parameter Contexts的使用

说明 nifi版本&#xff1a;1.23.2&#xff08;docker镜像&#xff09; 作用 Parameter Contexts&#xff08;参数上下文&#xff09;&#xff1a;参数上下文由 NiFi 实例全局定义/访问。访问策略可以应用于参数上下文&#xff0c;以确定哪些用户可以创建它们。创建后&#x…

自然语言处理(五):子词嵌入(fastText模型)

子词嵌入 在英语中&#xff0c;“helps”“helped”和“helping”等单词都是同一个词“help”的变形形式。“dog”和“dogs”之间的关系与“cat”和“cats”之间的关系相同&#xff0c;“boy”和“boyfriend”之间的关系与“girl”和“girlfriend”之间的关系相同。在法语和西…

如何让数据成为企业的生产力?

为什么有的企业投入大量的人力、物力、财力做数字化转型建设最终做了个寂寞&#xff01;企业领导没看到数字化的任何价值&#xff01; 如果要问企业数字化转型建设最核心的价值体现是什么&#xff0c;大部分人都会说是&#xff1a;数据&#xff01; 然而&#xff0c;不同的人…

微服务整合Seata1.5.2+Nacos2.2.1+SpringBoot

文章目录 一、Seata Server端1、下载seata server2、客户端配置-application.yml3、初始Mysql数据库4、导入初始配置到nacos5、启动测试 二、Seata Client端搭建1、为示例业务创建表2、业务代码集成 Seata 本文以seata-server-1.5.2&#xff0c;以配置中心、注册中心使用Nacos&…

百度王海峰披露飞桨生态最新成果 开发者数量已达800万

目录 前言文心大模型原生插件机制文心大模型超级助手飞桨开发者数已达800万 模型数超80万星河社区最后 前言 8月16日&#xff0c;由深度学习技术及应用国家工程研究中心举办的WAVE SUMMIT深度学习开发者大会上&#xff0c;位于北京举行。百度的首席技术官兼深度学习技术及应用…

德国金融监管机构网站遭遇大规模DDoS攻击后“瘫痪”

德国波恩的BaFin大楼 BaFin是负责监督和监管德国金融机构和市场的金融监管机构&#xff0c;其职责是确保德国金融体系的稳定性、完整性和透明度。 此外&#xff0c;BaFin 的网站还为企业和消费者提供银行、贷款和财产融资等方面的信息。它还提供消费者帮助热线和举报人信息共…

Java从入门到精通-流程控制(二)

习题讲解&#xff1a; 上次我们给大家留了一些流程控制的问题&#xff0c;这次给大家分析讲解一下&#xff1a; 条件语句练习&#xff1a; 1.编写Java程序&#xff0c;用于接受用户输入的数字&#xff0c;然后判断它是偶数还是奇数&#xff0c;并输出相应的消息。 import ja…

记录使用layui弹窗实现签名、签字

一、前言 本来项目使用的是OCX方式做签字的&#xff0c;因为项目需要转到国产化&#xff0c;不在支持OCX方式&#xff0c;需要使用前端进行签字操作 注&#xff1a;有啥问题看看文档&#xff0c;或者换着思路来&#xff0c;本文仅供参考&#xff01; 二、使用组件 获取jSign…

九章云极DataCanvas公司参与大模型重点项目合作签约,建设产业集聚区

9月3日&#xff0c;2023中国国际服务贸易交易会石景山国际开放合作论坛在石景山首钢园成功举办&#xff0c;北京市委常委、常务副市长夏林茂&#xff0c;商务部服务贸易和商贸服务业司司长王东堂&#xff0c;北京市石景山区委书记常卫出席论坛并致辞。论坛期间正式举行“石景山…

java8 新特性

1、lambda表达式 Lambda 是一个 匿名函数&#xff0c;我们可以把 Lambda 表达式理解为是 一段可以传递的代码&#xff08;将代码像数据一样进行传递&#xff09;。使用它可以写出更简洁、更灵活的代码。 &#xff08;1&#xff09;语法 Lambda 表达式&#xff1a;在Java 8 语…

STM32WB55开发(1)----套件概述

STM32WB55开发----1.套件概述 所用器件视频教学样品申请优势支持协议系统控制和生态系统访问功能示意图系统框图跳线设置开发板原理图 所用器件 所使用的器件是我们自行设计的开发板&#xff0c;该开发板是基于 STM32WB55 系列微控制器所构建。STM32WBXX_VFQFPN68 不仅是一款评…

【MyBatis篇】MyBatis框架基础知识笔记

目录 ORM思想&#xff08;对象关系映射思想&#xff09; 初识MyBatis 什么是MyBatis呢&#xff1f; JDBC VS MyBatis代码 获取数据库连接对比 对表格查询操作&#xff1a; JDBC弊端 MyBatis&#xff0c;JDBC对比 MyBatis进一步介绍以及本质分析 JDBC编程的劣势&…

VectorDraw Developer Framework 10.1004 Crack

VectorDraw 开发人员框架 (VDF) 是一个图形引擎库&#xff0c;开发人员可以使用它来可视化其应用程序。通过提供的功能&#xff0c;我们的客户可以轻松创建、编辑、管理、导出、导入和打印 2D 和 3D 绘图 - 图形文件。VDF 使用的强大格式称为 VDML&#xff08;以及与 VDML 相同…

ssprompt:一个LLM Prompt分发管理工具

阅读顺序 &#x1f31f;前言&#x1f514;ssprompt介绍命令介绍Metafile介绍版本依赖规则 &#x1f30a; PromptHubGitHub Token &#x1f680; Quick Install系统依赖pip安装Linux, macOS, Windows (WSL)Windows (Powershell) &#x1f6a9; Roadmap&#x1f30f; 项目交流讨论…

12.redis 持久化

redis 持久化 redis 持久化redis持久化策略RDB > Redis DataBase 定期备份rdb 文件处理rdb 优缺点 AOF > Append Only File 实时备份AOF 工作流程AOF 缓冲区刷新策略AOF 重写机制AOF 重写流程 混合持久化持久化流程总结 redis 持久化 redis 是一个内存数据库&#xff0c…

【大数据模型】让chatgpt为开发增速(开发专用提示词)

汝之观览&#xff0c;吾之幸也&#xff01;本文主要聊聊怎样才能更好的使用提示词&#xff0c;给开发提速&#xff0c;大大缩减我们的开发时间&#xff0c;比如在开发中使用生成表结构脚本的提示词&#xff0c;生成代码的提示词等等。 一、准备 本文主要根据Claude进行演示&am…

【产线故障】线上接口请求过慢如何排查?

文章目录 前言一、内存使用过高导致CPU满载案例代码分析思路 二、出现了类似死循环导致cpu负载案例代码分析思路 三、死锁案例代码分析思路 前言 首先线上接口变慢&#xff0c;原因可能有很多&#xff0c;有可能是网络&#xff0c;有可能是慢 SQL&#xff0c;有可能是服务本身…