SLAM图优化g2o
SLAM图优化g2o
图优化g2o框架
图优化的英文是 graph optimization 或者 graph-based optimization, “图”其实是数据结构中的graph。凸优化的英文是 convex optimization,这里的“凸”其实是凸函数的意思,所以单从英文就能区分开。
图优化有什么优势?
SLAM的后端一般分为两种处理方法,一种是以扩展卡尔曼滤波(EKF)为代表的滤波方法,一种是以图优化为代表的非线性优化方法。SLAM研究的主流热点几乎都是基于图优化。
滤波方法尤其是EKF方法,在SLAM发展很长的一段历史中一直占据主导地位,早期的大神们研究了各种各样的滤波器来改善滤波效果,SLAM,EKF是必须要掌握的。滤波方法的优缺点:
优点:在计算资源受限、待估计量比较简单的情况下,EKF为代表的滤波方法比较有效,经常用在激光SLAM中。
缺点:存储量和状态量是平方增长关系,存储的是协方差矩阵,不适合大型场景。基于视觉的SLAM方案,路标点(特征点)数据很大,滤波方法根本吃不消,所以此时滤波的方法效率非常低。
图优化在视觉SLAM中效率很高吗?
以前都是用滤波方法,因为在图优化里,Bundle Adjustment(后面简称BA)起到了核心作用。大量特征点和相机位姿的BA计算量其实很大,根本没办法实时。
在视觉SLAM中,虽然包含大量特征点和相机位姿,其实BA是稀疏的,稀疏的就好办了,就可以加速了啊!比较代表性的就是2009年,几个大神发表了自己的研究成果《SBA:A software package for generic sparse bundle adjustment》,而且计算机硬件发展也很快,因此基于图优化的视觉SLAM也可以实时了!
图优化是什么?
图优化里的图就是数据结构里的图,一个图由若干个顶点(vertex),以及连接这些顶点的边(edge)组成。
比如一个机器人在房屋里移动,在某个时刻 t 的位姿(pose)就是一个顶点,这个也是待优化的变量。而位姿之间的关系就构成了一个边,比如时刻 t 和时刻 t+1 之间的相对位姿变换矩阵就是边,边通常表示误差项。
在SLAM里,图优化一般分解为两个任务:
1、构建图。机器人位姿作为顶点,位姿间关系作为边。
2、优化图。调整机器人的位姿(顶点)来尽量满足边的约束,使得误差最小。
根据机器人位姿来作为图的顶点,这个位姿可以来自机器人的编码器,也可以是ICP匹配得到的,图的边就是位姿之间的关系。由于误差的存在,实际上机器人建立的地图是不准的,如下图左。通过设置边的约束,使得图优化向着满足边约束的方向优化,最后得到了一个优化后的地图(如下图中所示),与真正的地图(下图右)非常接近。
g2o 框架
图优化如何编程实现呢?
在SLAM领域,基于图优化的一个用的非常广泛的库就是g2o,是General Graphic Optimization 的简称,是一个用来优化非线性误差函数的c++框架。
这个g2o怎么用呢?
先安装,其实g2o安装很简单,参考GitHub上官网:
https://github.com/RainerKuemmerle/g2o
按照步骤来安装就行了。需要注意的是安装之前确保电脑上已经安装好了第三方依赖。
第一次接触g2o,确实有这种感觉,而且官网文档写的也比较“不通俗不易懂”,不过如果理顺了框架,再去看代码,应该很快能够入手了。
g2o实现了很多内部的算法,在进行构造的时候,需要遵循一些规则,毕竟一个程序不可能满足所有的要求,g2o的使用中还是应该多看多记,这样才能更好的使用这个库。
首先看一下下面这个图,是g2o的基本框架结构。看图的时候要注意箭头类型。
1、图的核心
要知道这个图中哪个最重要,就去看看箭头源头在哪里
最左侧的SparseOptimizer?SparseOptimizer是整个图的核心,右上角的 is-a 实心箭头,这个SparseOptimizer是一个Optimizable Graph,从而也是一个超图(HyperGraph)。
2、顶点和边
先来看上面的结构。has-many 箭头,这个超图包含了许多顶点(HyperGraph::Vertex)和边(HyperGraph::Edge)。而这些顶点继承自 Base Vertex,也就是OptimizableGraph::Vertex,而边可以继承自 BaseUnaryEdge(单边), BaseBinaryEdge(双边)或BaseMultiEdge(多边),都称为OptimizableGraph::Edge
顶点和边在编程中很重要的,看底部的结构。
3、配置SparseOptimizer的优化算法和求解器
整个图的核心SparseOptimizer 包含一个优化算法(OptimizationAlgorithm)的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton(高斯牛顿法,简称GN), Levernberg-Marquardt(简称LM法), Powell's dogleg 三者中间选择一个(常用的是GN和LM)
GN和LM就是非线性优化方法中常用的两种。
4、如何求解
OptimizationWithHessian 内部包含一个求解器(Solver),这个Solver实际是由一个BlockSolver组成的。这个BlockSolver有两个部分,一个是SparseBlockMatrix ,用于计算稀疏的雅可比和Hessian矩阵;一个是线性方程的求解器(LinearSolver),用于计算迭代过程中最关键的一步HΔx=−b,LinearSolver有几种方法可以选择:PCG, CSparse, Choldmod,上面图的一个简单理解。
g2o编程流程
从底层开始搭建框架一直到顶层。g2o的整个框架就是按照下图中标的这个顺序来写的。
SLAM十四讲中g2o求解曲线参数的例子来说明,源代码地址
https://github.com/gaoxiang12/slambook/edit/master/ch6/g2o_curve_fitting/main.cpp
为了方便理解,重新加了注释。如下所示,
typedefg2o::BlockSolver< g2o::BlockSolverTraits<
3,
1> > Block;
// 每个误差项优化变量维度为3,误差值维度为1
// 第1步:创建一个线性求解器LinearSolver
Block::LinearSolverType* linearSolver =
newg2o::LinearSolverDense<Block::PoseMatrixType>();
// 第2步:创建BlockSolver。并用上面定义的线性求解器初始化
Block* solver_ptr =
newBlock( linearSolver );
// 第3步:创建总求解器solver。并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver =
newg2o::OptimizationAlgorithmLevenberg( solver_ptr );
// 第4步:创建终极大boss 稀疏优化器(SparseOptimizer)
g2o::SparseOptimizer optimizer;
// 图模型
optimizer.setAlgorithm( solver );
// 设置求解器
optimizer.setVerbose(
true);
// 打开调试输出
// 第5步:定义图的顶点和边。并添加到SparseOptimizer中
CurveFittingVertex* v =
newCurveFittingVertex();
//往图中增加顶点
v->setEstimate( Eigen::Vector3d(
0,
0,
0) );
v->setId(
0);
optimizer.addVertex( v );
for(
inti=
0; i<N; i++ )
// 往图中增加边
{
CurveFittingEdge* edge =
newCurveFittingEdge( 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、创建一个线性求解器LinearSolver
增量方程的形式是:H△X=-b,通常的方法就是直接求逆,也就是△X=-H.inv*b。看起来好像很简单,但这有个前提,就是H的维度较小,此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时,矩阵求逆变得很困难,求解问题也变得很复杂。
需要一些特殊的方法对矩阵进行求逆,看下图是GitHub上g2o相关部分的代码
可以分别查看每个方法的解释。
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>;
为何会有可变尺寸的solver呢?
这是因为在某些应用场景, 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初始化
g2o/g2o/core/ 目录下, Solver的优化方法有三种:分别是高斯牛顿(GaussNewton)法,LM(Levenberg–Marquardt)法、Dogleg法,如下图所示,也和前面的图相匹配
上图最后那个OptimizationAlgorithmWithHessian 是干嘛的?
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(
boolverbose)
看一下定义
5、定义图的顶点和边。并添加到SparseOptimizer中。
这部分比较复杂,不展开介绍。
6、设置优化参数,开始执行优化。
设置SparseOptimizer的初始化、迭代次数、保存结果等。
初始化
SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)
设置迭代次数,然后就开始执行图优化了。
SparseOptimizer::optimize(
intiterations,
boolonline)
SLAM图优化g2o的更多相关文章
- 从零开始一起学习SLAM | 理解图优化,一步步带你看懂g2o代码
首发于公众号:计算机视觉life 旗下知识星球「从零开始学习SLAM」 这可能是最清晰讲解g2o代码框架的文章 理解图优化,一步步带你看懂g2o框架 小白:师兄师兄,最近我在看SLAM的优化算法,有种 ...
- 视觉SLAM漫淡(二):图优化理论与g2o的使用
视觉SLAM漫谈(二):图优化理论与g2o的使用 1 前言以及回顾 各位朋友,自从上一篇<视觉SLAM漫谈>写成以来已经有一段时间了.我收到几位热心读者的邮件.有的希望我介绍一下当前 ...
- 深入理解图优化与g2o:图优化篇
前言 本节我们将深入介绍视觉slam中的主流优化方法——图优化(graph-based optimization).下一节中,介绍一下非常流行的图优化库:g2o. 关于g2o,我13年写过一个文档,然 ...
- g2o:一种图优化的C++框架
转载自 Taylor Guo g2o: A general framework for graph optimization 原文发表于IEEE InternationalConference on ...
- 深入理解图优化与g2o:g2o篇
内容提要 讲完了优化的基本知识,我们来看一下g2o的结构.本篇将讨论g2o的代码结构,并带着大家一起写一个简单的双视图bundle adjustment:从两张图像中估计相机运动和特征点位置.你可以把 ...
- Ubuntu16.04安装视觉SLAM环境(g2o)
1.首先在github上下载g2o图优化库 git clone https://github.com/RainerKuemmerle/g2o.git 2.运行安装以下依赖库 sudo apt-get ...
- CF 291E. Tree-String Problem [dfs kmp trie图优化]
CF291E 题意:一棵树,每条边上有一些字符,求目标串出现了多少次 直接求目标串的fail然后一边dfs一边跑kmp 然后就被特殊数据卡到\(O(n^2)\)了... 因为这样kmp复杂度分析的基础 ...
- 『The Captain 最短路建图优化』
The Captain(BZOJ 4152) Description 给定平面上的n个点,定义(x1,y1)到(x2,y2)的费用为min(|x1-x2|,|y1-y2|),求从1号点走到n号点的最小 ...
- TVM图优化(以Op Fusion为例)
首先给出一个TVM 相关的介绍,这个是Tianqi Chen演讲在OSDI18上用的PPThttps://files.cnblogs.com/files/jourluohua/Tianqi-Chen- ...
随机推荐
- 976. Largest Perimeter Triangle
Given an array A of positive lengths, return the largest perimeter of a triangle with non-zero area, ...
- 869. Reordered Power of 2
Starting with a positive integer N, we reorder the digits in any order (including the original order ...
- 【DB宝50】Oracle异构平台迁移之完全可传输导出导入(Full Transportable Export & Import)
目录 一.简介 1.1.使用场景 1.2.限制条件 二.完全可传输操作步骤 三.案例演示 3.1.环境 3.2.源库操作 3.2.1.将需要传输的用户表空间设置为RO状态 3.2.2.使用Data P ...
- 多指灵巧手MoveIt!与Gazebo联合仿真框架搭建
至于为什么叫框架,一是因为灵巧手的3维模型没有按照基本的设计要求画,正常来说,设计机器人机构之前应该设计好机构需要多少个自由度/DOF,每个自由度是旋转/revolute类型还是滑移/prismati ...
- Maven不扫描java文件夹下的配置文件解决办法
<build> <resources> <resource> <directory>src/main/java</directory> &l ...
- 基于queue的python多进程日志管理
在我们的异常检测应用中,需要对每组IoT设备分别训练一个模型,每个模型对一组设备的指标数据进行实时异常检测.方案采用master-worker+消息队列的方式实现模型对外服务,但是每个worker的日 ...
- VBO、VAO和EBO
Vertex Buffer Object 对于经历过fixed pipeline的我来讲,VBO的出现对于渲染性能提升让人记忆深刻.完了,暴露年龄了~ //immediate mode glBegin ...
- Spark大数据处理框架入门(单机版)
导读 引言 环境准备 安装步骤 1.下载地址 2.开始下载 3.解压spark 4.配置环境变量 5.配置 spark-env.sh 6.启动spark服务 7.测试spark stay hungry ...
- CRM系统全方位管理企业
您在选择一款CRM系统的时候,首先要考虑销售团队的感受和意见.让CRM系统在帮助销售团队优化工作流程的同时,更好地对销售团队进行管理.销售人员每卖出一件商品,要从寻找筛选商机开始,经过沟通客户需求.满 ...
- 通过Dapr实现一个简单的基于.net的微服务电商系统(十二)——istio+dapr构建多运行时服务网格
多运行时是一个非常新的概念.在 2020 年,Bilgin Ibryam 提出了 Multi-Runtime(多运行时)的理念,对基于 Sidecar 模式的各种产品形态进行了实践总结和理论升华.那到 ...