《LIO-SAM阅读笔记》3.后端优化

2023-12-29 10:12
文章标签 优化 笔记 阅读 后端 sam lio

本文主要是介绍《LIO-SAM阅读笔记》3.后端优化,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

前言:

LIO-SAM后端优化部分写在了mapOptimization.cpp文件中,本部分主要进行了激光帧的scan-to-map匹配,回环检测以及关键帧的因子图优化。本部分主要有两个环节同步进行,一个单独开辟了回环检测线程,另外一个是lidar数据的回调函数。

功能简介:

  • 1、scan-to-map匹配:提取当前激光帧特征点(角点、平面点),局部关键帧map的特征点,执行scan-to-map迭代优化,更新当前帧位姿;
  • 2、闭环检测:在历史关键帧中找距离相近,时间相隔较远的帧设为匹配帧,匹配帧周围提取局部关键帧map,同样执行scan-to-map匹配,得到位姿变换,构建闭环因子数据,加入因子图优化。
  • 3、关键帧因子图优化:关键帧加入因子图,添加激光里程计因子、GPS因子、闭环因子,执行因子图优化,更新所有关键帧位姿;

订阅:

  • 1、订阅当前激光帧点云信息,来自FeatureExtraction;
  • 2、订阅GPS里程计;
  • 3、订阅来自外部闭环检测程序提供的闭环数据,本程序没有提供,这里实际没用上。

发布:

  • 1、发布历史关键帧里程计;
  • 2、发布局部关键帧map的特征点云;
  • 3、发布激光里程计,rviz中表现为坐标轴;
  • 4、发布激光里程计;
  • 5、发布激光里程计路径,rviz中表现为载体的运行轨迹;
  • 6、发布地图保存服务;
  • 7、发布闭环匹配关键帧局部map;
  • 8、发布当前关键帧经过闭环优化后的位姿变换之后的特征点云;
  • 9、发布闭环边,rviz中表现为闭环帧之间的连线;
  • 10、发布局部map的降采样平面点集合;
  • 11、发布历史帧(累加的)的角点、平面点降采样集合;
  • 12、发布当前帧原始点云配准之后的点云。

一:scan-to-map匹配

scan-to-map匹配出现在雷达数据的回调函数laserCloudInfoHandler中,其是激光里程计最基本的位姿估计方法,其进行的操作有:当前帧位姿的初始化、组建局部地图、进行当前帧到局部地图的匹配和位姿优化、用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合。

1.位姿初始化

(1)如果是第一帧,用原始imu数据的RPY初始化当前帧位姿(旋转部分)

(2)后续帧,用imu里程计计算两帧之间的增量位姿变换,作用于前一帧的激光位姿(是因子图联合优化后的最优位姿),得到当前帧激光位姿 // note:利用imu里程计信息设置优化的初值

注意:此处的当前帧和上一帧之间的位姿变换 = 由上一帧的imu里程计位姿的逆*当前帧的imu里程计位姿。

2.局部地图map

(1)对最近的一帧关键帧,搜索时空维度上相邻的关键帧集合,降采样一下。这里采用的kdtree进行搜索,是将历史所有关键帧集合存入kdtree中进行搜索。

(2)对关键帧集合中的每一帧,提取对应的角点、平面点,加入局部map中。局部地图map中的点云超过一定数量时,就会清空下一帧的点云进行scan-to-map时进行重新创建。因此局部地图并不是每一帧点云到来时都进行重新,创建一次,只需要往点云地图中添加新的点云即可。

3.scan-to-map位姿优化

前面位姿初始化时,我们设置的当前帧的位姿主要是通过imu里程计得到,这里对上面的初始化位姿进行优化。

3.1 当前激光帧角点寻找局部map匹配点

(1)更新当前帧位姿,将当前帧角点坐标变换到map系下,在局部map中查找5个最近点,距离小于1m,且5个点构成直线(用距离中心点的协方差矩阵,特征值进行判断),则认为匹配上了。

(2)计算当前帧角点到直线的距离、垂线的单位向量,存储为角点参数。

3.2 当前激光帧平面点寻找局部map匹配点

(1)更新当前帧位姿,将当前帧平面点坐标变换到map系下,在局部map中查找5个最近点,距离小于1m,且5个点构成平面(最小二乘拟合平面),则认为匹配上了。

(2)计算当前帧平面点到平面的距离、垂线的单位向量,存储为平面点参数。

3.3 scan-to-map优化

上面的角点到直线的距离以及平面点到平面的距离,就是非线性优化中的约束项,并以此构建残差。

对匹配特征点计算Jacobian矩阵,观测值为特征点到直线、平面的距离,构建高斯牛顿方程,迭代优化当前位姿,存transformTobeMapped

3.4 与imu原始RPY数据加权融合

此处imu使用的是一个九轴的imu相比与六轴imu多出一个全局的imu位姿,此处imu原始RPY就是一个全局的imu位姿的roll、pitch和yaw角。

更新当前帧位姿的roll, pitch, z坐标;因为是小车,roll、pitch是相对稳定的,不会有很大变动,一定程度上可以信赖imu的数据,z是进行高度约束。

二:闭环检测线程

在此.cpp文件的节点中单独开辟了一个回环检测线程,在检测到回环后,在回环检测线程会对当前帧位姿进行一个icp的优化位姿。

1.选取候选闭环帧

在历史关键帧中查找与当前关键帧距离最近的关键帧集合,选择时间相隔较远的一帧作为候选闭环帧,此处同样使用的是kdtree的方法进行搜索。

这里时间的间隔设置的是30s,必须满足时间上超过一定的阈值,才认为是一个有效的回环。

2.提取当前帧特征点集合和组建候选帧区域map

提取当前关键帧特征点集合,降采样;提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合,降采样,目的是使用scan-to-map的方法进行优化位姿。

3.scan-to-map优化:icp

执行scan-to-map优化,调用icp方法,得到优化后位姿,构造闭环因子需要的数据,在因子图优化中一并加入更新位姿。

注意:

  • 前面也有scan-to-map优化,是基于非线性优化的方法来进行优化的;此处采用的icp点云配准的方法,这种ICP方法视为基于线性优化的变体。
  • 闭环的时候没有立即更新当前帧的位姿,而是添加闭环因子,让图优化去更新位姿。

通过调用PCL的icp库可以得到闭环帧到当前帧较为准确的位姿变换,用此位姿变换去矫正当前帧位姿,并作为约束因子加入到gtsam中,等待因子图优化环节一起优化。

三:因子图优化

因子图优化也是写在了雷达数据的回调函数laserCloudInfoHandler中,因子图优化联合了雷达里程计信息(包含IMU信息)、GPS信息和闭环信息一起进行优化。因子图优化是全局关键帧优化,前面的优化都是对当前帧的局部优化。

1.添加关键帧

计算当前帧与前一帧位姿变换,如果变化太小,不设为关键帧,反之设为关键帧,非关键帧则跳出,不进行因子图优化。

2.添加激光里程计因子

激光里程计因子是一个帧间约束,前一帧和当前帧(3.3中scan-to-map之后的位姿)的位姿变换。

3. 添加GPS因子

gps的数据会先被一个成熟的节点(在module_navsat.launch文件)处理,我们直接调用的gps的处理结果。

本项目中作者对gps的使用较为谨慎。

4.添加闭环因子

闭环因子就是就是闭环检测线程中计算的当前帧和闭环帧之间的位姿变换。

5.执行因子图优化

执行了因子图优化后,优化器中所有的关键帧位姿都进行了优化。

执行完因子图优化后,清空一下这一轮优化使用的因子图(里面的优化因子),历史数据不会清掉,ISAM保存起来了,只是清空因子图不是清空的优化器。

到此为之,不管是全局优化还是局部优化都完成了,有几个变量需要注意:

// note:当前位姿的一个最优估计(最准确的),通过以下两种方式更新:

// 1.如果这帧是普通帧,通过scan to map得到

// 2.若果这帧是关键帧,先通过scan to map更新,再通过因子图优化更新

transformTobeMapped[6]

// 历史关键帧位姿

// note:所有的雷达帧位姿会在这里更新 (如果是普通帧,则是因子图优化之前的san-map得到的位姿,如果是关键帧,则是因子图优化得到的位姿)

pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;

pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D;

pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D;

四:发布激光里程计

此处发布了一个增量式里程计信息,用于IMU预计分时使用。此外还有一个优化后的最优位姿,用于IMU里程计使用。

增量式里程计当前帧(scan to map 后的位姿,没经过回环因子图优化)与前一帧(因子图优化后的位姿)之间的位姿变换 (这是一个平滑的结果)。

这篇关于《LIO-SAM阅读笔记》3.后端优化的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



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

相关文章

Vue3 的 shallowRef 和 shallowReactive:优化性能

大家对 Vue3 的 ref 和 reactive 都很熟悉,那么对 shallowRef 和 shallowReactive 是否了解呢? 在编程和数据结构中,“shallow”(浅层)通常指对数据结构的最外层进行操作,而不递归地处理其内部或嵌套的数据。这种处理方式关注的是数据结构的第一层属性或元素,而忽略更深层次的嵌套内容。 1. 浅层与深层的对比 1.1 浅层(Shallow) 定义

HDFS—存储优化(纠删码)

纠删码原理 HDFS 默认情况下,一个文件有3个副本,这样提高了数据的可靠性,但也带来了2倍的冗余开销。 Hadoop3.x 引入了纠删码,采用计算的方式,可以节省约50%左右的存储空间。 此种方式节约了空间,但是会增加 cpu 的计算。 纠删码策略是给具体一个路径设置。所有往此路径下存储的文件,都会执行此策略。 默认只开启对 RS-6-3-1024k

使用opencv优化图片(画面变清晰)

文章目录 需求影响照片清晰度的因素 实现降噪测试代码 锐化空间锐化Unsharp Masking频率域锐化对比测试 对比度增强常用算法对比测试 需求 对图像进行优化,使其看起来更清晰,同时保持尺寸不变,通常涉及到图像处理技术如锐化、降噪、对比度增强等 影响照片清晰度的因素 影响照片清晰度的因素有很多,主要可以从以下几个方面来分析 1. 拍摄设备 相机传感器:相机传

JAVA智听未来一站式有声阅读平台听书系统小程序源码

智听未来,一站式有声阅读平台听书系统 🌟&nbsp;开篇:遇见未来,从“智听”开始 在这个快节奏的时代,你是否渴望在忙碌的间隙,找到一片属于自己的宁静角落?是否梦想着能随时随地,沉浸在知识的海洋,或是故事的奇幻世界里?今天,就让我带你一起探索“智听未来”——这一站式有声阅读平台听书系统,它正悄悄改变着我们的阅读方式,让未来触手可及! 📚&nbsp;第一站:海量资源,应有尽有 走进“智听

MySQL高性能优化规范

前言:      笔者最近上班途中突然想丰富下自己的数据库优化技能。于是在查阅了多篇文章后,总结出了这篇! 数据库命令规范 所有数据库对象名称必须使用小写字母并用下划线分割 所有数据库对象名称禁止使用mysql保留关键字(如果表名中包含关键字查询时,需要将其用单引号括起来) 数据库对象的命名要能做到见名识意,并且最后不要超过32个字符 临时库表必须以tmp_为前缀并以日期为后缀,备份

【学习笔记】 陈强-机器学习-Python-Ch15 人工神经网络(1)sklearn

系列文章目录 监督学习:参数方法 【学习笔记】 陈强-机器学习-Python-Ch4 线性回归 【学习笔记】 陈强-机器学习-Python-Ch5 逻辑回归 【课后题练习】 陈强-机器学习-Python-Ch5 逻辑回归(SAheart.csv) 【学习笔记】 陈强-机器学习-Python-Ch6 多项逻辑回归 【学习笔记 及 课后题练习】 陈强-机器学习-Python-Ch7 判别分析 【学

系统架构师考试学习笔记第三篇——架构设计高级知识(20)通信系统架构设计理论与实践

本章知识考点:         第20课时主要学习通信系统架构设计的理论和工作中的实践。根据新版考试大纲,本课时知识点会涉及案例分析题(25分),而在历年考试中,案例题对该部分内容的考查并不多,虽在综合知识选择题目中经常考查,但分值也不高。本课时内容侧重于对知识点的记忆和理解,按照以往的出题规律,通信系统架构设计基础知识点多来源于教材内的基础网络设备、网络架构和教材外最新时事热点技术。本课时知识

SWAP作物生长模型安装教程、数据制备、敏感性分析、气候变化影响、R模型敏感性分析与贝叶斯优化、Fortran源代码分析、气候数据降尺度与变化影响分析

查看原文>>>全流程SWAP农业模型数据制备、敏感性分析及气候变化影响实践技术应用 SWAP模型是由荷兰瓦赫宁根大学开发的先进农作物模型,它综合考虑了土壤-水分-大气以及植被间的相互作用;是一种描述作物生长过程的一种机理性作物生长模型。它不但运用Richard方程,使其能够精确的模拟土壤中水分的运动,而且耦合了WOFOST作物模型使作物的生长描述更为科学。 本文让更多的科研人员和农业工作者

从状态管理到性能优化:全面解析 Android Compose

文章目录 引言一、Android Compose基本概念1.1 什么是Android Compose?1.2 Compose的优势1.3 如何在项目中使用Compose 二、Compose中的状态管理2.1 状态管理的重要性2.2 Compose中的状态和数据流2.3 使用State和MutableState处理状态2.4 通过ViewModel进行状态管理 三、Compose中的列表和滚动

论文阅读笔记: Segment Anything

文章目录 Segment Anything摘要引言任务模型数据引擎数据集负责任的人工智能 Segment Anything Model图像编码器提示编码器mask解码器解决歧义损失和训练 Segment Anything 论文地址: https://arxiv.org/abs/2304.02643 代码地址:https://github.com/facebookresear