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。

相关文章

使用Python将JSON,XML和YAML数据写入Excel文件

使用Python将JSON,XML和YAML数据写入Excel文件

《使用Python将JSON,XML和YAML数据写入Excel文件》JSON、XML和YAML作为主流结构化数据格式,因其层次化表达能力和跨平台兼容性,已成为系统间数据交换的通用载体,本文将介绍如何... 目录如何使用python写入数据到Excel工作表用Python导入jsON数据到Excel工作表用
阅读更多...
鸿蒙中@State的原理使用详解(HarmonyOS 5)

鸿蒙中@State的原理使用详解(HarmonyOS 5)

《鸿蒙中@State的原理使用详解(HarmonyOS5)》@State是HarmonyOSArkTS框架中用于管理组件状态的核心装饰器,其核心作用是实现数据驱动UI的响应式编程模式,本文给大家介绍... 目录一、@State在鸿蒙中是做什么的？二、@Spythontate的基本原理1. 依赖关系的收集2.
阅读更多...
Python基础语法中defaultdict的使用小结

Python基础语法中defaultdict的使用小结

《Python基础语法中defaultdict的使用小结》Python的defaultdict是collections模块中提供的一种特殊的字典类型,它与普通的字典（dict）有着相似的功能,本文主要... 目录示例1示例2python的defaultdict是collections模块中提供的一种特殊的字
阅读更多...
C++ Sort函数使用场景分析

C++ Sort函数使用场景分析

《C++Sort函数使用场景分析》sort函数是algorithm库下的一个函数,sort函数是不稳定的,即大小相同的元素在排序后相对顺序可能发生改变,如果某些场景需要保持相同元素间的相对顺序,可使... 目录C++ Sort函数详解一、sort函数调用的两种方式二、sort函数使用场景三、sort函数排序
阅读更多...
Java String字符串的常用使用方法

Java String字符串的常用使用方法

《JavaString字符串的常用使用方法》String是JDK提供的一个类,是引用类型,并不是基本的数据类型,String用于字符串操作,在之前学习c语言的时候,对于一些字符串,会初始化字符数组表... 目录一、什么是String二、如何定义一个String1. 用双引号定义2. 通过构造函数定义三、St
阅读更多...
Python Faker库基本用法详解

Python Faker库基本用法详解

《PythonFaker库基本用法详解》Faker是一个非常强大的库,适用于生成各种类型的伪随机数据,可以帮助开发者在测试、数据生成、或其他需要随机数据的场景中提高效率,本文给大家介绍PythonF... 目录安装基本用法主要功能示例代码语言和地区生成多条假数据自定义字段小结Faker 是一个 python
阅读更多...
如何解决mmcv无法安装或安装之后报错问题

如何解决mmcv无法安装或安装之后报错问题

《如何解决mmcv无法安装或安装之后报错问题》：本文主要介绍如何解决mmcv无法安装或安装之后报错问题,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录mmcv无法安装或安装之后报错问题1.当我们运行YOwww.chinasem.cnLO时遇到2.找到下图所示这里3.
阅读更多...
Python通过模块化开发优化代码的技巧分享

Python通过模块化开发优化代码的技巧分享

《Python通过模块化开发优化代码的技巧分享》模块化开发就是把代码拆成一个个“零件”,该封装封装,该拆分拆分,下面小编就来和大家简单聊聊python如何用模块化开发进行代码优化吧... 目录什么是模块化开发如何拆分代码改进版：拆分成模块让模块更强大：使用 __init__.py你一定会遇到的问题模www.
阅读更多...
Pydantic中Optional 和Union类型的使用

Pydantic中Optional 和Union类型的使用

《Pydantic中Optional和Union类型的使用》本文主要介绍了Pydantic中Optional和Union类型的使用,这两者在处理可选字段和多类型字段时尤为重要,文中通过示例代码介绍的... 目录简介Optional 类型Union 类型Optional 和 Union 的组合总结简介Pyd
阅读更多...
Vue3使用router,params传参为空问题

Vue3使用router,params传参为空问题

《Vue3使用router,params传参为空问题》：本文主要介绍Vue3使用router,params传参为空问题,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐... 目录vue3使用China编程router,params传参为空1.使用query方式传参2.使用 Histo
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2