《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

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

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

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

相关文章

SpringSecurity安全框架 ——认证与授权

目录 一、简介 1.1 什么是Spring Security 1.2 工作原理 1.3 为什么选择Spring Security 1.4 HttpSecurity 介绍&#x1f31f; 二、用户认证 2.1 导入依赖与配置 2.2 用户对象UserDetails 2.3 业务对象UserDetailsService 2.4 SecurityConfig配置 2.4.1 BCryptPasswo…

第11章 GUI Page436 使用缓冲DC, wxBufferedPaintDC

所谓“缓冲DC”&#xff0c;是指将所有图元都先划到一个人眼看不到的“设备上下文”之上&#xff0c;最后再一次性复制到真正的屏幕DC之上&#xff0c;这样我们就看不到中间画的过程了&#xff0c;也就不会感到闪烁了。 注意&#xff0c;这时不能解除ScrolledWindow1的背景擦除…

Golang实现JAVA虚拟机-运行时数据区

一、运行时数据区概述 JVM学习&#xff1a; JVM-运行时数据区 运行时数据区可以分为两类&#xff1a;一类是多线程共享的&#xff0c;另一类则是线程私有的。 多线程共享的运行时数据区需要在Java虚拟机启动时创建好&#xff0c;在Java虚拟机退出时销毁。对象实例存储在堆区类信…

探索 Vue3 (四) keep-alive缓存组件

keep-alive 的作用 官网介绍&#xff1a;KeepAlive | Vue.js keep-alive为抽象组件&#xff0c;主要用于缓存内部组件数据状态。可以将组件缓存起来并在需要时重新使用&#xff0c;而不是每次重新创建。这可以提高应用的性能和用户体验&#xff0c;特别是在需要频繁切换组件时…

【Linux】僵尸与孤儿 进程等待

目录 一&#xff0c;僵尸进程 1&#xff0c;僵尸进程 2&#xff0c;僵尸进程的危害 二&#xff0c;孤儿进程 1&#xff0c;孤儿进程 三&#xff0c;进程等待 1&#xff0c;进程等待的必要性 2&#xff0c;wait 方法 3&#xff0c;waitpid 方法 4&#xff0c;回收小结…

vue场景 无分页列表条件过滤,子组件多选来自父组件的列表

日常开发中&#xff0c;经常会遇到下面场景&#xff1a; 页面加载一个无分页列表&#xff0c;同时工具栏设置多个条件可对列表过滤的场景(典型的就是关键字模糊查询)父组件传给子组件列表&#xff0c;子组件中需要多选列表多选&#xff0c;选择结果返回父组件 1 无分页列表过…

Tg5032smn:高稳定性105℃高温

TG5032SMN是一款频率范围10MHz ~ 54MHz,具有高稳定的TCXO晶振&#xff0c;可与CMOS或限幅正弦输出。外部尺寸5.0 3.2 1.45mm&#xff0c;超小型,质地轻。该系列晶振的额定工作范围-40℃~&#xfe62;105C内可高稳定性工作&#xff0c;使得信号频率的误差很小。TG5032SMN与其他…

Android Termux安装SSH结合内网穿透实现远程SFTP文件传输

文章目录 1. 安装openSSH2. 安装cpolar3. 远程SFTP连接配置4. 远程SFTP访问4. 配置固定远程连接地址 SFTP&#xff08;SSH File Transfer Protocol&#xff09;是一种基于SSH&#xff08;Secure Shell&#xff09;安全协议的文件传输协议。与FTP协议相比&#xff0c;SFTP使用了…

Linux的安装及管理程序

一、如何在linux安装卸载软件 1. 编译安装 灵活性较高 难度较大 可以安装较新的版本 2. rpm安装&#xff08;redhat&#xff09; linux 包安装 查软件信息&#xff1a;是否安装&#xff0c;文件列表 rpm 软件名 3. yum yum是RPM升级版本&#xff0c;解决rpm的弊端 安装软件 首…

9.独立看门狗IWDG窗口看门狗WWDG编码思路

前言&#xff1a; 看门狗是维护系统稳定性的一向技术&#xff0c;可以让代码跑飞及时复位&#xff0c;在产品中非常常用&#xff0c;俗话说&#xff0c;重启能解决90%的问题&#xff0c;作为产品来说&#xff0c;你总不能因为一次bug就让程序卡死不动了&#xff0c;肯定要试着重…

WEB 3D技术 three.js 集合体 讲解三角形构建图形 顶点概念 顶点值重用

那么 本文 我们来说一下集合体 我们要创建物体 都要先创建一个集合体 集合体也决定了我们元素的形态 材质 决定了我们的外观 我们结合体 其实基础都是三角形 我们编写如下代码 import ./style.css import * as THREE from "three"; import { OrbitControls } from …

截断整型提升算数转换

文章目录 &#x1f680;前言&#x1f680;截断&#x1f680;整型提升✈️整型提升是怎样的 &#x1f680;算术转换 &#x1f680;前言 大家好啊&#xff01;这里阿辉补一下前面操作符遗漏的地方——截断、整型提升和算数转换 看这一篇要先会前面阿辉讲的数据的存储否则可能看不…