SLAM算法与工程实践——SLAM基本库的安装与使用（6）：g2o优化库（3）构建g2o顶点

2023-12-26 08:04
文章标签 算法 安装 使用 构建 实践 优化 基本 slam 工程 顶点 g2o

本文主要是介绍SLAM算法与工程实践——SLAM基本库的安装与使用（6）：g2o优化库（3）构建g2o顶点，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

SLAM算法与工程实践系列文章

下面是SLAM算法与工程实践系列文章的总链接，本人发表这个系列的文章链接均收录于此

SLAM算法与工程实践系列文章链接

下面是专栏地址：

SLAM算法与工程实践系列专栏

文章目录

  • SLAM算法与工程实践系列文章
    • SLAM算法与工程实践系列文章链接
    • SLAM算法与工程实践系列专栏
  • 前言
  • SLAM算法与工程实践——SLAM基本库的安装与使用（6）：g2o优化库（3）
    • 如何自己定义顶点
      • 一般格式
      • 在曲线拟合中自定义顶点的例子
      • SE(3)位姿示例
      • 三维点示例
    • 如何向图中添加顶点

前言

这个系列的文章是分享SLAM相关技术算法的学习和工程实践

SLAM算法与工程实践——SLAM基本库的安装与使用（6）：g2o优化库（3）

在图6-2中，涉及顶点的3个类是 HyperGraph::Vertex、OptimizableGraph::Vertex 和 BaseVertex。

先来看第1个类——HyperGraph::Vertex，它是一个抽象顶点类，必须通过派生来使用。下面是其定义中的说明。

// hyper graph.h
class G2O CORE API HyperGraph
{public:// ......//! abstract Vertex,your types must derive from that oneclass G2O_CORE_API Vertex : public yperGraphElement{// ......}
}

然后我们看第2个类——OptimizableGraph::Vertex，查看定义可以发现它继承自HyperGraph::Vertex，如下所示。

// optimizable graph.h
struct G2O_CORE_API OptimizableGraph : public HyperGraph
{// ......//A general case Vertex for optimizationclass G2O_CORE_API Vertex : public HyperGraph::Vertex,public
HyperGraph::DataContainer{// ......}
}

不过，OptimizableGraph::Vertex 也是非常底层的类，在具体使用时一般都会进行扩展，因此 g2o 提供了一个比较通用的适合大部分情况的模板，也就是第 3个类一BaseVertex。

我们找到源码中关于 BaseVertex 的定义，可以发现 BaseVertex 继承 OptimizableGraph::Vertex。

以上 3 个类的关系和 图 6-2 中显示的完全一致。

// g2o/core/base vertex.h
namespace g2o
{
#define G2O_VERTEX_DIM ((D == Eigen:Dynamic) ? _dimension : D)
/**
*	\brief Templatized Basevertex
*	Templatized Basevertex
*	D : minimal dimension of the vertex,e.g.,3 for rotation in 3D.-1 means dynamically assigned at runtime.
*	T : internal type to represent the estimate,e.g.,Quaternion for rotation in 3D
*/template <int D,typename T>class Basevertex : public OptimizableGraph::Vertex{static const int Dimension D;// dimension of the estimate (minimal)in the manifold space// ......}
}

最后我们来看上述代码中的模板参数D和T。

D是 int 类型的，表示 Vertex 的最小维度，比如在3D空间中旋转是三维的，那么这里D=3。

T是待估计 Vertex 的数据类型，比如用四元数表达三维旋转，那么 T 就是 Quaternion 类型的。

如何自己定义顶点

g2o内部定义了一些常用的顶点类型

// g2o 定义好的常用顶点类型
// 2D位姿顶点(x,y,theta)
VertexSE2 : public Basevertex<3,SE2>// 六维向量(x,y,z,gx,qy,gz),省略了四元数中的qw
VertexSE3 : public Basevertex<6,Isometry3>// 二维点和三维点
VertexPointXY : public BaseVertex<2,Vector2>
VertexPointXYZ : public Basevertex<3,Vector3>
VertexSBAPointXYZ : public Basevertex<3,Vector3>// SE(3)顶点，内部用变换矩阵参数化，外部用指数映射参数化
VertexSE3Expmap : public BaseVertex<6,SE3Quat>// SBACam顶点
VertexCam : public Basevertex<6,SBACam>// Sim(3)顶点
Vertexsim3Expmap : public Basevertex<7,Sim3>

自己定义。重新定义顶点一般需要考虑重写如下函数

// 读/写函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
virtual bool read(std:istream& is);
virtual bool write(std:ostream& os) const;// 顶点更新函数。这是一个非常重要的函数，主要用于优化过程中增量 △× 的计算。
// 计算出增量后，就是通过这个函数对估计值进行调整的
virtual void oplusImpl(const number_t* update);// 设定被优化顶点的初始值
virtual void setToOriginImpl();

一般格式

一般用下面的格式自定义g2o顶点

// 自定义 g2o 顶点通用格式
class myVertex:public g2o::BaseVertex<Dim,Type>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWmyvertex(){}virtual void read(std:istream& is){}virtual void write(std:ostream& os) const {}virtual void setoriginImpl(){_estimate = Type();}virtual void oplusImpl(const double* update)override{_estimate += /*更新*/；}
}

在曲线拟合中自定义顶点的例子

// 在曲线拟合中自定义顶点
class CurveFittingVertex:public g2o::BaseVertex<3,Eigen:Vector3d>
{public:EIGEN MAKE ALIGNED OPERATOR NEW// 设定被优化顶点的初始值为0virtual void setToOriginImpl(){_estimate << 0,0,0;}// 顶点更新函数virtual void oplusImpl(const double* update){_estimate +=Eigen::Vector3d(update);}// 存盘和读盘：留空virtual bool read(istream& in ){}virtual bool write(ostream& out) const {}
};

SE(3)位姿示例

以SE(3)位姿作为顶点的定义示例。更新函数用的就是乘法

// 以SE(3)位姿作为顶点的定义示例
// g2o/types/sba/types_six_dof_expmap.h
class g2o TYPES SBA API VertexSE3Expmap : public Basevertex<6,SE3Quat>{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexSE3Expmap();bool read(std:istream& is);bool write(std:ostream& os) const;//设定被优化顶点的初始值virtual void setToOriginImpl(){_estimate = SE3Quat ()}//顶点更新函数virtual void oplusImpl(const number_t* update_ ){Eigen::Map<const Vector6> update(update_ )//乘法更新setEstimate(SE3Quat::exp(update)*estimate());}
};

第一行代码中的参数如何理解？

第一个参数“6”表示内部存储的优化变量维度，这是一个六维的李代数。

第二个参数是优化变量的类型，这里使用了g2o 定义的相机位姿类型 SE3Quat。SE3Quat内部使用了四元数表达旋转，然后加上位移来存储SE(3)位姿。

为什么这里更新时没有直接做加法呢？

这里不用加法来更新位姿是因为SE(3)位姿不满足加法封闭性，但它对乘法是封闭的。

三维点示例

以三维点作为顶点的定义示例

// 以三维点作为顶点的定义示例
class g2o_TYPES_SBA_API VertexSBAPointXYZ : public Basevertex<3,Vector3>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexSBAPointXYZ();virtual bool read(std::istream& is);virtual bool write(std::ostream& os) const;// 设定被优化顶点的初始值virtual void setTooriginImpl(){_estimate.fill(0);}// 顶点更新函数virtual void oplusImpl(const number_t*update){Eigen:Map<const Vector3> v(update);_estimate += v;		// 加法更新}
}

如何向图中添加顶点

// 在曲线拟合中向图中添加顶点示例
// 新建顶点
CurveFittingVertex* v = new CurveFittingvertex();
//设定估计值
v->setEstimate(Eigen::Vector3d(0,0,0));
//设置顶点编号ID
v->setId(0);
//将顶点添加到优化器中
optimizer.addVertex( v );

下面是添加 VertexSBAPointXYZ 顶点的例子。

// 顶点初始 ID 为 1
int index = 1;
// 循环添加所有三维点并作为顶点
for (const Point3f p:points_3d)
{// 新建顶点g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();// 设定顶点编号，由于循环添加多个顶点，因此编号需要自增point->setId(index++);// 设定估计值point->setEstimate(Eigen::Vector3d (p.x,p.y,p.z ));// 设定需要边缘化point->setMarginalized( true );// 将顶点添加到优化器中optimizer.addVertex( point );
}

这篇关于SLAM算法与工程实践——SLAM基本库的安装与使用（6）：g2o优化库（3）构建g2o顶点的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/538652。

相关文章

Java中流式并行操作parallelStream的原理和使用方法

Java中流式并行操作parallelStream的原理和使用方法

《Java中流式并行操作parallelStream的原理和使用方法》本文详细介绍了Java中的并行流（parallelStream）的原理、正确使用方法以及在实际业务中的应用案例,并指出在使用并行流... 目录Java中流式并行操作parallelStream0. 问题的产生1. 什么是parallelS
阅读更多...
Linux join命令的使用及说明

Linux join命令的使用及说明

《Linuxjoin命令的使用及说明》`join`命令用于在Linux中按字段将两个文件进行连接,类似于SQL的JOIN,它需要两个文件按用于匹配的字段排序,并且第一个文件的换行符必须是LF,`jo... 目录一. 基本语法二. 数据准备三. 指定文件的连接key四.-a输出指定文件的所有行五.-o指定输出
阅读更多...
Linux jq命令的使用解读

Linux jq命令的使用解读

《Linuxjq命令的使用解读》jq是一个强大的命令行工具,用于处理JSON数据,它可以用来查看、过滤、修改、格式化JSON数据,通过使用各种选项和过滤器,可以实现复杂的JSON处理任务... 目录一. 简介二. 选项2.1.2.2-c2.3-r2.4-R三. 字段提取3.1 普通字段3.2 数组字段四.
阅读更多...
Linux kill正在执行的后台任务 kill进程组使用详解

Linux kill正在执行的后台任务 kill进程组使用详解

《Linuxkill正在执行的后台任务kill进程组使用详解》文章介绍了两个脚本的功能和区别,以及执行这些脚本时遇到的进程管理问题,通过查看进程树、使用`kill`命令和`lsof`命令,分析了子... 目录零. 用到的命令一. 待执行的脚本二. 执行含子进程的脚本，并kill2.1 进程查看2.2 遇到的
阅读更多...
JDK21对虚拟线程的几种用法实践指南

JDK21对虚拟线程的几种用法实践指南

《JDK21对虚拟线程的几种用法实践指南》虚拟线程是Java中的一种轻量级线程,由JVM管理,特别适合于I/O密集型任务,：本文主要介绍JDK21对虚拟线程的几种用法,文中通过代码介绍的非常详细,... 目录一、参考官方文档二、什么是虚拟线程三、几种用法1、Thread.ofVirtual().start(
阅读更多...
详解SpringBoot+Ehcache使用示例

详解SpringBoot+Ehcache使用示例

《详解SpringBoot+Ehcache使用示例》本文介绍了SpringBoot中配置Ehcache、自定义get/set方式,并实际使用缓存的过程,文中通过示例代码介绍的非常详细,对大家的学习或者... 目录摘要概念内存与磁盘持久化存储：配置灵活性：编码示例引入依赖：配置ehcache.XML文件：配置
阅读更多...
Java 虚拟线程的创建与使用深度解析

Java 虚拟线程的创建与使用深度解析

《Java虚拟线程的创建与使用深度解析》虚拟线程是Java19中以预览特性形式引入,Java21起正式发布的轻量级线程,本文给大家介绍Java虚拟线程的创建与使用,感兴趣的朋友一起看看吧... 目录一、虚拟线程简介1.1 什么是虚拟线程？1.2 为什么需要虚拟线程？二、虚拟线程与平台线程对比代码对比示例：三
阅读更多...
从基础到高级详解Go语言中错误处理的实践指南

从基础到高级详解Go语言中错误处理的实践指南

《从基础到高级详解Go语言中错误处理的实践指南》Go语言采用了一种独特而明确的错误处理哲学,与其他主流编程语言形成鲜明对比,本文将为大家详细介绍Go语言中错误处理详细方法,希望对大家有所帮助... 目录1 Go 错误处理哲学与核心机制1.1 错误接口设计1.2 错误与异常的区别2 错误创建与检查2.1 基础
阅读更多...
k8s按需创建PV和使用PVC详解

k8s按需创建PV和使用PVC详解

《k8s按需创建PV和使用PVC详解》Kubernetes中,PV和PVC用于管理持久存储,StorageClass实现动态PV分配,PVC声明存储需求并绑定PV,通过kubectl验证状态,注意回收... 目录1.按需创建 PV（使用 StorageClass）创建 StorageClass2.创建 PV
阅读更多...
Redis 基本数据类型和使用详解

Redis 基本数据类型和使用详解

《Redis基本数据类型和使用详解》String是Redis最基本的数据类型,一个键对应一个值,它的功能十分强大,可以存储字符串、整数、浮点数等多种数据格式,本文给大家介绍Redis基本数据类型和... 目录一、Redis 入门介绍二、Redis 的五大基本数据类型2.1 String 类型2.2 Hash
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2