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

2023-12-24 19:30

本文主要是介绍《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。

1.IMU回调函数imuHandler

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预积分)预测出的轨迹,前面蓝色的轨迹是因子图优化得到的轨迹。

这篇关于《LIO-SAM阅读笔记》1.IMU预积分模块的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/532870

相关文章

Python logging模块使用示例详解

《Pythonlogging模块使用示例详解》Python的logging模块是一个灵活且强大的日志记录工具,广泛应用于应用程序的调试、运行监控和问题排查,下面给大家介绍Pythonlogging模... 目录一、为什么使用 logging 模块?二、核心组件三、日志级别四、基本使用步骤五、快速配置(bas

Python datetime 模块概述及应用场景

《Pythondatetime模块概述及应用场景》Python的datetime模块是标准库中用于处理日期和时间的核心模块,本文给大家介绍Pythondatetime模块概述及应用场景,感兴趣的朋... 目录一、python datetime 模块概述二、datetime 模块核心类解析三、日期时间格式化与

Python如何调用指定路径的模块

《Python如何调用指定路径的模块》要在Python中调用指定路径的模块,可以使用sys.path.append,importlib.util.spec_from_file_location和exe... 目录一、sys.path.append() 方法1. 方法简介2. 使用示例3. 注意事项二、imp

Python中模块graphviz使用入门

《Python中模块graphviz使用入门》graphviz是一个用于创建和操作图形的Python库,本文主要介绍了Python中模块graphviz使用入门,具有一定的参考价值,感兴趣的可以了解一... 目录1.安装2. 基本用法2.1 输出图像格式2.2 图像style设置2.3 属性2.4 子图和聚

Python的time模块一些常用功能(各种与时间相关的函数)

《Python的time模块一些常用功能(各种与时间相关的函数)》Python的time模块提供了各种与时间相关的函数,包括获取当前时间、处理时间间隔、执行时间测量等,:本文主要介绍Python的... 目录1. 获取当前时间2. 时间格式化3. 延时执行4. 时间戳运算5. 计算代码执行时间6. 转换为指

Python正则表达式语法及re模块中的常用函数详解

《Python正则表达式语法及re模块中的常用函数详解》这篇文章主要给大家介绍了关于Python正则表达式语法及re模块中常用函数的相关资料,正则表达式是一种强大的字符串处理工具,可以用于匹配、切分、... 目录概念、作用和步骤语法re模块中的常用函数总结 概念、作用和步骤概念: 本身也是一个字符串,其中

Python中的getopt模块用法小结

《Python中的getopt模块用法小结》getopt.getopt()函数是Python中用于解析命令行参数的标准库函数,该函数可以从命令行中提取选项和参数,并对它们进行处理,本文详细介绍了Pyt... 目录getopt模块介绍getopt.getopt函数的介绍getopt模块的常用用法getopt模

python logging模块详解及其日志定时清理方式

《pythonlogging模块详解及其日志定时清理方式》:本文主要介绍pythonlogging模块详解及其日志定时清理方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地... 目录python logging模块及日志定时清理1.创建logger对象2.logging.basicCo

Qt spdlog日志模块的使用详解

《Qtspdlog日志模块的使用详解》在Qt应用程序开发中,良好的日志系统至关重要,本文将介绍如何使用spdlog1.5.0创建满足以下要求的日志系统,感兴趣的朋友一起看看吧... 目录版本摘要例子logmanager.cpp文件main.cpp文件版本spdlog版本:1.5.0采用1.5.0版本主要

利用Python快速搭建Markdown笔记发布系统

《利用Python快速搭建Markdown笔记发布系统》这篇文章主要为大家详细介绍了使用Python生态的成熟工具,在30分钟内搭建一个支持Markdown渲染、分类标签、全文搜索的私有化知识发布系统... 目录引言:为什么要自建知识博客一、技术选型:极简主义开发栈二、系统架构设计三、核心代码实现(分步解析