gtsam初探以及结合LIO-SAM算法的一些理解

概述

GTSAM(Georgia Tech Smoothing and Mapping)是基于因子图的C++库,本篇基于GTSAM对因子图优化做一个简单了解和梳理,并以LIO-SAM为例进一步分析因子图优化在SLAM中的应用。

参考链接:
[0]gtsam官方文档
[1]https://blog.csdn.net/QLeelq/article/details/111368277
[2]https://zhuanlan.zhihu.com/p/621999120

1.基本概念

  • 因子图:因子图是一种无向图,由变量和因子组成
  • 变量:待优化的状态,可以有多个变量,如机器人位姿、IMU偏置等,每个变量包含了多帧的信息,如位姿X={x1, x2, ... , xn}
  • 因子:对应图优化中的边, 即两个变量或者观测与变量之间的约束,如两帧之间的位姿变换、3D点到相机图像的投影关系
  • 先验因子:基于GPS或者其他模块得到的观测信息或者位姿初值
  • 值:指变量的具体数值

在这里插入图片描述

如下图所示,{x1,x2,x3}为变量
黑色方块为因子,其中连接两个变量的为二元因子,如o12, o23;
连接一个变量的为一元因子,如f1, f2, f3, p1,其中p1为先验因子,f1, f2, f3为观测因子

2.官方例子说明

2.1 机器人运动模型

下图为gtsam官方文档中最简单的一个例子,一个基于马尔可夫链的机器人运动模型,即当前时刻的机器人位姿只由机器人上一时刻位姿决定。
在这里插入图片描述

如上图所示:

  • x1,x2,x3为变量,表示机器人的位姿
  • f0为一元先验因子,可以是初始位姿
  • f1,f2为二元因子,表示机器人之间的运动,可以是IMU或激光SLAM的里程计信息

2.1.1建立因子图

对于某个问题,在建立关于该问题的数学模型后,就可以基于数学模型建立因子图。gtsam为不同的数学模型内置了现成的模板,在创建因子图时只需要根据不同的模型选择不同的模板即可。
上述机器人运动模型因子图创建过程如下所示:

// step1:构建一个空的非线性因子图
NonlinearFactorGraph graph;// step2:为因子图添加因子并连接到变量
// [1]构建先验因子,也就是图中的f_0, 这里使用二维姿态(x,y,theta)简化问题
Pose2 priorMean(0.0, 0.0, 0.0);
// [2]初始化高斯噪声,代表我们对该因子的不确定性
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
// [3]将先验因子加入因子图, 其中的1表示该因子连接到第1个变量
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));// [1]构建里程计因子,也就是图中的f_1,f_2, 往前移动2米,y轴不便,theta不变
Pose2 odometry(2.0, 0.0, 0.0);
// [2]初始化高斯噪声
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// [3]将里程计因子加入因子图, (1,2)(2,3)分别代表该里程计约束是从变量1到变量2,从变量2到变量3
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));// step3:设置各个变量的初始值
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));// step4:调用优化器并使用设定好的初始值对因子图优化
Values result = LevenberMarquardtOptimizer(graph, initial).optimize();

2.3机器人运动建模

进一步的,为上述运动模型添加观测模型,这里以GPS的测量为例
在这里插入图片描述

如上图所示:

  • x1,x2,x3为变量,表示机器人的位姿
  • o12,o23为二元因子,表示机器人之间的运动
  • f1,f2,f3为一元观测因子,即GPS的观测值

2.3.1自定义观测因子

通过NoiseModelFactor1<T>模板类定义新的一元因子,其中T为一元因子所对应变量的类型。

需要针对新因子定义一下几个信息:
(1)有关传感器观测值的私有成员变量。
(2)有关因子初始化的构造函数。
(3)用于计算评估误差的误差函数,功能包括:返回变量和观测值之间的残差信息、计算用于非线性优化的雅克比矩阵。

以定义一元GPS因子为例
(1)定义GPS的测量值mx_, my_
(2)定义GPS因子的构造函数,在使用时通过该函数实例化因子。用给定的x,y值初始化mx_, my_观测值,并给定与该因子连接的变量代号j以及观测的噪声协方差矩阵model
(3)重载用于计算评估误差的函数evaluateError:残差 = 变量值 - 观测值
公式定义如下:
在这里插入图片描述

自定义因子代码如下:

// 自定义因子是Pose2类型的,Pose2对应2D位姿的李群变换矩阵,即3x3的一个矩阵,包括2x2的旋转和2x1的平移
class UnaryFactor: public NoiseModelFactor1<Pose2> {// 观测的状态包括x和y两个值 double mx_, my_;public:// 成员函数, 在参数列表中初始化所关联变量的Id, 噪声模型, 测量值x和yUnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y){}// 残差函数模型,残差 = 变量值 - 观测值// 输入:当前时刻的位姿q,对误差求导的雅格比矩阵H// 位姿q=[qx,qy,angle]为三维,误差为两维,因此误差函数对位姿求导的雅格比矩阵为2x3Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const{// 如果存在雅格比矩阵,对其进行更新if(H){   // Jac是误差函数对位姿求导的雅格比矩阵,因为观测函数设置的比较简单,因此雅格比矩阵也很简单,具体的公式推导见下图gtsam::Matrix Jac = (Matrix(2,3)<< 1.0,0.0,0.0, 0.0,1.0,0.0).finished();;(*H) = Jac;}return (Vector(2) << q.x() - mx_, q.y() - my_).finished();}
};

创建因子图

// step1:构建一个空的非线性因子图
NonlinearFactorGraph graph;
// step2:为因子图添加因子并连接到变量
// 建立观测的噪声模型
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
// 加入自定义因子
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));// step3:设置各个变量的初始值
Values initial;
initial.insert(1, 0.1, 0.0);
initial.insert(2, 2.0, -0.1);
initial.insert(3, 4.1, 0.1);// step4:调用优化器并使用设定好的初始值对因子图优化
Values result = LevenberMarquardtOptimizer(graph, initial).optimize();

3.LIO-SAM中的因子图应用

3.1IMU因子

在LIO-SAM的imageProjection模块主要对IMU数据进行积分,使用gtsam的IMU预积分模块。该模块订阅雷达里程计的位姿,将该位姿作为初始积分的先验,然后通过gtsam的IMU预积分模块得到IMU高频里程计,同时更新偏置信息,因子图如下:
在这里插入图片描述

在这个过程中:

  • 变量:位姿(旋转和平移)、速度、偏置
  • 先验因子:位姿的先验因子为雷达里程计的位姿,速度和偏置的先验因子为0
  • 观测因子:IMU因子,每个时刻IMU的角速度加速度,通过观测方程转化为观测值即位姿

1.创建因子图并添加先验因子

// 声明非线性因子图
gtsam::NonlinearFactorGraph graphFactors;// [1]加入雷达里程计先验因子
// 将雷达里程计位姿转换到IMU坐标系作为先验
prevPose_ = lidarPose.compose(lidar2Imu);
// 创建因子,X(0)表示先验因子连接到X第一个变量,prevPose_为先验因子具体的值,priorPoseNoise为之前定义的噪声
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
// 添加因子
graphFactors.add(priorPose);// [2]加入速度先验因子
prevVel_ = gtsam::Vector3(0, 0, 0);// 初始化为0
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);// 创建
graphFactors.add(priorVel);// 加入// [3]加入Bias先验因子
prevBias_ = gtsam::imuBias::ConstantBias();// 初始化为0
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);// 创建
graphFactors.add(priorBias);// 添加// 将初始状态设置为因子图变量的初始值
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);// 使用优化器对变量进行更新
optimizer.update(graphFactors, graphValues);

2.添加IMU因子
通过gtsam自带的积分器,已经将一段时间内(两帧激光之间)的IMU进行了连续积分,积分角速度得到旋转,积分加速度得到线速度和位姿

// 使用IMU预积分的结果构建IMU因子,并加入因子图中
// 将IMU的连续积分结果封装为gtsam::PreintegratedImuMeasurements格式
const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
// 从preint_imu中拿出一次积分结果即IMU因子,如上图这个因子连接了相邻两个时刻的位姿、速度和上一时刻的Bias
gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
graphFactors.add(imu_factor);// imuIntegratorOpt_->predict输入之前时刻的状态和偏差,预测当前时刻的状态
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
// 将IMU预积分的结果作为当前时刻因子图变量的初值
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);// 更新一次优化器
optimizer.update(graphFactors, graphValues);
optimizer.update();// 从优化器中获取当前经过优化后估计值
gtsam::Values result = optimizer.calculateEstimate();
prevPose_  = result.at<gtsam::Pose3>(X(key));
prevVel_   = result.at<gtsam::Vector3>(V(key));
prevState_ = gtsam::NavState(prevPose_, prevVel_);
prevBias_  = result.at<gtsam::imuBias::ConstantBias>(B(key));

2.地图优化

在LIO-SAM的地图优化模块,只在添加关键帧时进行了因子图优化。因子图如下所示:
在这里插入图片描述

  • 变量:关键帧位姿
  • 因子:激光里程计因子、GPS因子、回环检测因子

下面一段代码是关于添加激光里程计因子的过程,对于第一帧添加的是PriorFactor类型的先验因子,对于后续帧添加的是BetweenFactor类型的二元因子,过程和上述一样。

void addOdomFactor()
{if (cloudKeyPoses3D->points.empty()){noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*metergtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));}else{noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());gtsam::Pose3 poseTo   = trans2gtsamPose(transformTobeMapped);gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);}
}

总结

综上来看,基于gtsam的因子图优化代码结构是比较清晰的,只不过在使用之前需要根据自己的误差模型选择合适的模板,针对里程计类型、GPS类型等gtsam都提供了相应的模板,对于gtsam库中没有的模板可以自行定义。

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

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

相关文章

Android各版本对应的SDK及JDK版本要求

1、Android Gradle 插件版本说明 | Android 开发者 | Android Developers 2、 3、Android Gradle 插件 7.3.0&#xff08;2022 年 9 月&#xff09; | Android 开发者 | Android Developers 4、 5、参考 Android中Gradle版本和Gradle插件版本 - 简书

Spring IOC - ConfigurationClassPostProcessor源码解析

上文提到Spring在Bean扫描过程中&#xff0c;会手动将5个Processor类注册到beanDefinitionMap中&#xff0c;其中ConfigurationClassPostProcessor就是本文将要讲解的内容&#xff0c;该类会在refresh()方法中通过调用invokeBeanFactoryPosstProcessors(beanFactory)被调用。 5…

68 内网安全-域横向PTHPTKPTT哈希票据传递

目录 演示案例:域横向移动PTH传递-Mimikatz域横向移动PTK传递-Mimikatz域横向移动PTT传递-MS14068&kekeo&local国产Ladon内网杀器测试验收-信息收集,连接等 涉及资源: PTH(pass the hash) #利用lm或ntlm的值进行的渗透测试 PTT(pass the ticket) #利用的票据凭证TGT进行…

C++ Qt 学习(二):常用控件使用与界面布局

1. Qt 布局详解 ui 设计器设计界面很方便&#xff0c;为什么还要手写代码&#xff1f; 更好的控制布局更好的设置 qss代码复用 完全不会写 Qt 布局&#xff0c;很麻烦&#xff0c;怎么学会手写布局&#xff1f; 看 Qt 自己怎么写改良 Qt 的布局写法 1.1 水平布局 #include …

LVGL库入门 02 - 布局

1、简单布局 可以使用 lv_obj_set_pos(obj, x, y) 调整一个控件的位置&#xff08;或者使用类似的函数单独调整一个方向的坐标&#xff09;&#xff0c;将它放在相对父容器左上角的合适位置。不过这种布局方式非常死板&#xff0c;因为绝对坐标一旦设定就不能自动调整&#xf…

fastapi-参数

路径参数 你可以使用与 Python 格式化字符串相同的语法来声明路径"参数"或"变量"&#xff1a; from fastapi import FastAPIapp FastAPI()app.get("/items/{item_id}") async def item_details(item_id: int):return {"item_id": i…

竖版视频怎么做二维码?扫码播放竖版视频的方法

当我们在将视频转二维码图片展示的时候&#xff0c;一般横版视频在手机展示不会有影响&#xff0c;但是竖版视频会默认用横版的方式播放就会导致无法清晰的看到画面的内容&#xff0c;那么如何将竖版视频生成二维码是很多小伙伴头疼的一个问题。那么下面教大家使用二维码生成器…

Android 如何在Android studio中快速创建raw和assets文件夹

一 方案 1. 创建raw文件夹 切成project浏览模式——>找到res文件粘贴要放入raw文件夹下的文件。 当然此时raw文件还没有&#xff0c;直接在右侧输入框中出现的路径~\res后面加上\raw即可。 2. 创建assets文件夹 同理在main文件夹下粘贴要放入assets文件夹的文件&#xff0…

Java精品项目62基于Springboot+Vue实现的大学生在线答疑平台(编号V62)

Java精品项目62基于SpringbootVue实现的大学生在线答疑平台(编号V62) 大家好&#xff0c;小辰今天给大家介绍一个基于SpringbootVue实现的大学生在线答疑平台(编号V62)&#xff0c;演示视频公众号&#xff08;小辰哥的Java&#xff09;对号查询观看即可 文章目录 Java精品项目…

云尘-Node1 js代码

继续做题 拿到就是基本扫一下 nmap -sP 172.25.0.0/24 nmap -sV -sS -p- -v 172.25.0.13 然后顺便fscan扫一下咯 nmap: fscan: 还以为直接getshell了 老演员了 其实只是302跳转 所以我们无视 只有一个站 直接看就行了 扫出来了两个目录 但是没办法 都是要跳转 说明还是需要…

tomcat必要的配置

tomcat要配置两个&#xff0c;不然访问不了localhost:8080 名&#xff1a;CATALINA_HOME 值&#xff1a;D:\software\computer_software\Tomcat\tomcat8.5.66

taro全局配置页面路由和tabBar页面跳转

有能力可以看官方文档&#xff1a;Taro 文档 页面路由配置&#xff0c;配置在app.config.ts里面的pages里&#xff1a; window用于设置小程序的状态栏、导航条、标题、窗口背景色&#xff0c;其配置项如下&#xff1a; tabBar配置&#xff1a;如果小程序是一个多 tab 应用&…

你一般会什么时候使用CHATGPT?

在当今数字时代&#xff0c;人们对于人工智能&#xff08;AI&#xff09;的依赖程度日益增加&#xff0c;而ChatGPT作为一种强大的自然语言处理工具&#xff0c;吸引了人们的广泛关注和应用。那么&#xff0c;人一般在什么时候会想要使用ChatGPT呢&#xff1f;这个问题涵盖了多…

【原创】java+swing+mysql志愿者管理系统设计与实现

摘要&#xff1a; 志愿者管理系统是一个用于管理志愿者以及活动报名的系统&#xff0c;提高志愿者管理的效率&#xff0c;同时为志愿者提供更好的服务和体验。本文主要介绍如何使用javaswingmysql去实现一个志愿者管理系统。 功能分析&#xff1a; 系统主要提供给管理员和志…

什么是块存储、文件存储、对象存储?

我们都知道&#xff0c;存储设备就是为数据提供空间。 U盘、硬盘和固态硬盘都是存储最终的存储设备。而块存储、文件存储和对象存储也可以简单地理解是不同类型的存储设备&#xff0c;它们是根据使用介质存储数据的手段或方法不同来划分的。 首先我们来看下块存储&#xff1a;…

160. 相交链表、Leetcode的Python实现

博客主页&#xff1a;&#x1f3c6;看看是李XX还是李歘歘 &#x1f3c6; &#x1f33a;每天分享一些包括但不限于计算机基础、算法等相关的知识点&#x1f33a; &#x1f497;点关注不迷路&#xff0c;总有一些&#x1f4d6;知识点&#x1f4d6;是你想要的&#x1f497; ⛽️今…

Unity Perception合成数据生成、标注与ML模型训练

在线工具推荐&#xff1a; Three.js AI纹理开发包 - YOLO合成数据生成器 - GLTF/GLB在线编辑 - 3D模型格式在线转换 - 3D场景编辑器 任何训练过机器学习模型的人都会告诉你&#xff0c;模型是从数据得到的&#xff0c;一般来说&#xff0c;更多的数据和标签会带来更好的性能。 …

垃圾分类箱通过工业4G路由器实现无人值守远程管理

据今年发布的相关数据统计&#xff0c;人们日常生活中每人每天至少能制造1.2kg垃圾&#xff0c;在环保事业中日常垃圾处理已经成为一项紧迫且不可忽视的任务。为了实现城市清洁和环境保护&#xff0c;越来越多的地区开始引入垃圾分类箱。传统的垃圾分类箱管理方式存在着一些不便…

【APP】go-musicfox - 一款网易云音乐命令行客户端, 文件很小Mac版本只有16.5M

go-musicfox 是用 Go 写的又一款网易云音乐命令行客户端&#xff0c;支持各种音质级别、UnblockNeteaseMusic、Last.fm、MPRIS 和 macOS 交互响应&#xff08;睡眠暂停、蓝牙耳机连接断开响应和菜单栏控制等&#xff09;等功能特性。 预览 启动 启动界面 主界面 主界面 通…

网络工程师-入门基础课:华为HCIA认证课程介绍

【微/信/公/众/号&#xff1a;厦门微思网络】 华为HCIA试听课程&#xff1a;超级实用&#xff0c;华为VRP系统文件详解 华为HCIA试听课程&#xff1a;不会传输层协议&#xff0c;HCIA都考不过 华为HCIA试听课程&#xff1a;网络工程师的基本功&#xff1a;网络地址转换NAT 一…