《LIO-SAM阅读笔记》1.IMU预积分模块

前言:

        LIO-SAM是一个多传感器融合的紧耦合SLAM框架,融合的传感器类型有雷达、IMU和GPS,其中雷达和IMU在LIO-SAM框架中必须使用的。LIO-SAM的优化策略采用了GTSAM库,GTSAM库采用了因子图的优化方法,其提供了一些列C++的外部接口,以便用户方便传入参数等进行优化。特别的是GTSAM库专门单独设计关于IMU计算与优化的接口。

        IMU预积分模块在LIO-SAM源码中写在了imuPreintegration.cpp文件中,其中预积分模块的功能使用class IMUPreintegration来实现,IMUPreintegration类中构造函数中最主要的两个部分分别是imu的回调函数imuHandler和odom的回调函数odometryHandler。

一:IMUPreintegration类

1.IMU回调函数imuHandler

在imuHandler中主要分为了3个部分,分别是将imu数据放入缓存队列,计算imu里程计,发布imu里程计。

1.1 将imu数据放入缓存队列

在回调函数中,首先将接受到的imu原始数据存放进两个队列中,第一个队列用来预积分与优化,第二个队列用来更新imu状态量(进行优化后的位姿传播),这两个队列都是在odom的回调函数odometryHandler中使用。

1.2 计算imu里程计

       此部分是将2.3部分联系在一起的,利用最近一次的odom里程计作为起点利用imu预积分来计算每一帧imu数据时刻的位姿。

        2.3小节只是将当前帧的odom里程计作为起点加入到imu预积分器中,在此小节中是加入每帧imu信息到预积分器,以及预积分器的推理。

1.3 发布imu里程计

imu里程计位姿转到lidar系,发布里程计。

2.odom回调函数odometryHandler

在odometryHandler回调函数中主要进行了imu数据和lidar的里程计数据联合进行因子图优化的操作。

2.1 odometryHandler中进行的主要操作
  • Step 0. 系统初始化,第一帧
  • Step 1. 计算前一帧与当前帧之间的imu预积分量,用前一帧状态施加预积分量得到当前帧初始状态估计,添加来自mapOptimization的当前帧位姿,进行因子图优化,更新当前帧状态
  • Step 2. 优化之后,执行重传播;优化更新了imu的偏置,用最新的偏置重新计算当前激光里程计时刻之后的imu预积分,这个预积分用于计算每时刻位姿。
  • Step 3. 每隔100帧激光里程计,重置ISAM2优化器,保证优化效率
2.2 因子图优化的步骤
  • 1.添加imu预积分因子
// 上面imu预积分的结果
const gtsam::PreintegratedImuMeasurements &preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements &>(*imuIntegratorOpt_);
// 参数:前一帧位姿,前一帧速度,当前帧位姿,当前帧速度,前一帧偏置,预计分量   //?:此处的当前帧位姿和当前帧速度是哪里得到的?此处是否是待求量?
gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
graphFactors.add(imu_factor);
  • 2.添加imu偏置因子,前一帧偏置,当前帧偏置,观测值,噪声协方差;deltaTij()是积分段的时间
graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
                                                                            gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
  • 3.添加位姿因子
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
graphFactors.add(pose_factor);
// 用前一帧的状态、偏置,施加imu预计分量,得到当前帧的状态  // note: 前一帧的状态是经过上一次优化后的结果
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
  • 4.变量节点赋初值
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);
  • 5.优化
optimizer.update(graphFactors, graphValues);
optimizer.update();
graphFactors.resize(0);
graphValues.clear();
// 优化结果
gtsam::Values result = optimizer.calculateEstimate();

注意: 每优化完成一次后,就会清空因子图和变量,优化器是每100帧重置一次。因此每次向优化器内添加的因子图和变量是一一对应的。

  • 6.利用优化结果更新状态量
// 更新当前帧位姿、速度
prevPose_ = result.at<gtsam::Pose3>(X(key));
prevVel_ = result.at<gtsam::Vector3>(V(key));
// 更新当前帧状态
prevState_ = gtsam::NavState(prevPose_, prevVel_);
// 更新当前帧imu偏置
prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
  • 7.重置预积分器,设置新的偏置,这样下一帧激光里程计进来的时候,预积分量就是两帧之间的增量
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
2.3 因子图优化之后的重传播步骤

        这里用一张示意图来表达,这一部操作最主要的原因是:imu接受数据的频率大于odom里程计的数据,因此每来一个odom数据,队列中已经有多个imu数据,而因子图优化的频率是按照odom里程计的频率来进行的,因此如果想要得到每一个imu数据时刻的位姿估计就要以最近的odom时刻的位姿为初始值,通过每个imu数据时刻的预积分进行位姿的传播。

效果展示:此处一小段粉红色的轨迹就是通过经过因子图优化后的重传播(IMU预积分)预测出的轨迹,前面蓝色的轨迹是因子图优化得到的轨迹。

二:TransformFusion类

TransformFusion类主要功能是订阅激光里程计(来自MapOptimization优化后的激光里程计)和IMU里程计(IMU预积分中),根据前一时刻激光里程计,和该时刻到当前时刻的IMU里程计变换增量,计算当前时刻IMU里程计;rviz展示IMU里程计轨迹(局部)。

概括:TransformFusion类是为了将以增量式里程计为起点的IMU里程计转化到,以后端优化后的激光里程计为起点的IMU里程计并发布出出去。

重点:此处发布的imu里程计和IMU预积分中发布的IMU里程计是不同的,MapOptimization中会发送两个激光里程计话题。其中一个是增量式激光里程计,在IMU预积分订阅并使用。另外一个是后端优化后的激光里程计,在TransformFusion订阅并使用。最终我们发布的轨迹信息是基于优化后激光里程计信息+IMU里程计信息。之所以还发布增量式的激光里程计信息是为了方便IMU预积分环节使用。

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

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

K8S理论

kubernetes&#xff1a;8个字母省略&#xff0c;就是k8s 自动部署自动扩展和管理容器化部署的应用程序的一个开源系统 k8s是负责自动化运维管理多个容器化程序的集群&#xff0c;是一个功能强大的容器编排工具 分布式和集群化的方式进行容器化管理 版本有1.15 .1.18 .1.20 …

力扣-收集足够苹果的最小花园周长[思维+组合数]

题目链接 题意&#xff1a; 给你一个用无限二维网格表示的花园&#xff0c;每一个 整数坐标处都有一棵苹果树。整数坐标 (i, j) 处的苹果树有 |i| |j| 个苹果。 你将会买下正中心坐标是 (0, 0) 的一块 正方形土地 &#xff0c;且每条边都与两条坐标轴之一平行。 给你一个整…

【flink番外篇】7、flink的State(Keyed State和operator state)介绍及示例 - 完整版

Flink 系列文章 一、Flink 专栏 Flink 专栏系统介绍某一知识点&#xff0c;并辅以具体的示例进行说明。 1、Flink 部署系列 本部分介绍Flink的部署、配置相关基础内容。 2、Flink基础系列 本部分介绍Flink 的基础部分&#xff0c;比如术语、架构、编程模型、编程指南、基本的…

dpdk原理概述及核心源码剖析

dpdk原理 1、操作系统、计算机网络诞生已经几十年了&#xff0c;部分功能不再能满足现在的业务需求。如果对操作系统做更改&#xff0c;成本非常高&#xff0c;所以部分问题是在应用层想办法解决的&#xff0c;比如前面介绍的协程、quic等&#xff0c;都是在应用层重新开发的框…

docker 私有仓库

Docker 私有仓库 一、私有仓库搭建 # 1、拉取私有仓库镜像 docker pull registry # 2、启动私有仓库容器 docker run -id --nameregistry -p 5000:5000 registry # 3、打开浏览器 输入地址http://私有仓库服务器ip:5000/v2/_catalog&#xff0c;看到{"repositories&quo…

Linux操作系统——进程(三) 进程优先级

进程优先级 首先呢&#xff0c;我们知道一个进程呢&#xff08;或者也可以叫做一个任务&#xff09;&#xff0c;它呢有时候要在CPU的运行队列中排队&#xff0c;要么有时候阻塞的时候呢又要在设备的等待队列中排队&#xff0c;其实我们排队的本质就是&#xff1a;确认优先级。…

【数据结构】什么是二叉树?

&#x1f984;个人主页:修修修也 &#x1f38f;所属专栏:数据结构 ⚙️操作环境:Visual Studio 2022 目录 &#x1f4cc;二叉树的定义 &#x1f4cc;二叉树的特点 &#x1f4cc;特殊二叉树 &#x1f4cc;二叉树的性质 &#x1f4cc;二叉树的存储结构 &#x1f4cc;二叉树…

大语言模型说明书

在浩瀚的信息宇宙中&#xff0c;大语言模型如同一颗璀璨的星星正在熠熠生辉。21世纪以来&#xff0c;人工智能可谓是飞速发展&#xff0c;从简单的神经网络到大语言模型、生成式AI&#xff0c;这并非仅仅是一种技术的进步&#xff0c;更是人类智慧的飞跃。大语言模型不仅仅是语…

opencv入门到精通——形态学转换

目录 目标 理论 1. 侵蚀 2. 扩张 3. 开运算 4. 闭运算 5. 形态学梯度 6. 顶帽 7. 黑帽 结构元素 目标 在这一章当中&#xff0c; 我们将学习不同的形态学操作&#xff0c;例如侵蚀&#xff0c;膨胀&#xff0c;开运算&#xff0c;闭运算等。我们将看到不同的功能&…

期末复习【计算机图像处理】

期末复习【计算机图像处理】 前言版权推荐期末复习期末考点内容期末考试题型一、填空 2分*10 概念二、简答 5分*2三、计算 10分*6四、绘图 10分*1 测验二测验三最后 前言 2023-12-25 15:09:07 昨天没有睡好 0点~3点看B站 Google模拟面试 3点~5点没睡着 5~6点睡了一会 6~12点终…

isp代理/双isp代理/数据中心代理的区别?如何选择?

本文我们来详细科普一下几种不同的代理类型&#xff1a;isp代理/双isp代理/数据中心代理&#xff0c;了解他们的区别&#xff0c;选择更适合自己的代理类型。 在讲述这几种代理类型之前&#xff0c;我们先复习一下代理大类有哪几种。 一、机房代理和非机房代理 在做代理ip选…

《试题与研究》期刊发表投稿方式

《试题与研究》杂志是面向全国公开发行的国家CN级权威教育期刊。创刊以来一直以服务教育服务学生为办刊宗旨&#xff0c;以优秀的内容质量和编校质量深受广大读者好评&#xff0c;其权威性、导向性、针对性、实用性在全国教育期刊中独树一帜。为推动教育科研事业的发展&#xf…

20231225使用亿佰特的蓝牙模块dongle协议分析仪E104-2G4U04A抓取BLE广播数据

20231225使用亿佰特的蓝牙模块dongle协议分析仪E104-2G4U04A抓取BLE广播数据 结论&#xff1a;硬件蓝牙分析仪 不一定比 手机端的APK的效果好&#xff01; 亿佰特E104-2G4U04A需要3片【单通道】&#xff0c;电脑端的UI为全英文的。 BLE-AnalyzerPro WCH升级版BLE-PRO蓝牙分析仪…

DRF视图组件

【1】两个视图基类 APIView APIView是 Django REST Framework 提供的一个基类&#xff0c;用于创建基于函数或基于类的视图。使用 APIView 可以根据需求自定义请求处理逻辑&#xff0c;对于简单的接口逻辑&#xff0c;可以直接继承APIView类。 GenericAPIView GenericAPIVi…

如何使用 YOLOv8 做对象检测

介绍 对象检测是一项计算机视觉任务&#xff0c;涉及识别和定位图像或视频中的对象。它是许多应用的重要组成部分&#xff0c;例如自动驾驶汽车、机器人和视频监控。 多年来&#xff0c;已经开发了许多方法和算法来查找图像中的对象及其位置。卷积神经网络对于此类任务有着非…

Dbeaver如何连接Oceanbase?

Dbeaver & Oceanbase 一、新增驱动二、连接数据库 一、新增驱动 1、新建驱动 点击数据库 -> 驱动管理器 -> 新建 2、设置驱动 驱动名称可随意填写注意驱动类型要是Generichost:port填写实际的host和port 库中新增下载的oceanbase驱动jar包 二、连接数据库 1、找…

linux 系统重启 Redis 服务

先 打开服务器 执行 sudo systemctl stop redis暂停Redis服务 然后 执行 sudo systemctl start redis启动 redis 服务 然后可以执行 sudo systemctl status redis查看 redis 状态

PyAV 使用浅谈

背景&#xff1a; PyAV是一个用于音频和视频处理的Python库&#xff0c;它提供了一个简单而强大的接口&#xff0c;用于解码、编码、处理和分析各种音频和视频格式。PyAV基于FFmpeg多媒体框架&#xff0c;它本质上是FFmpeg 的Python绑定&#xff0c;因此可以利用FFmpeg的功能来…

移动Web

文章目录 移动 Web一、平面转换1. 平移2. 旋转3. 渐变 二、空间转换1. 平移2. 旋转3. 动画 三、移动适配1. 谷歌模拟器2. rem3. less4. vw 四、响应式布局1. 媒体查询2. Bootstrap 移动 Web 一、平面转换 作用&#xff1a;为元素添加动态效果&#xff0c;一般与过渡配合使用 …

如何使用PatchaPalooza对微软每月的安全更新进行全面深入的分析

关于PatchaPalooza PatchaPalooza是一款针对微软每月安全更新的强大分析工具&#xff0c;广大研究人员可以直接使用该工具来对微软每月定期推送的安全更新代码进行详细、全面且深入的安全分析。 PatchaPalooza使用了微软MSRC CVRF API的强大功能来获取、存储和分析安全更新数…