《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 后的位姿,没经过回环因子图优化)与前一帧(因子图优化后的位姿)之间的位姿变换 (这是一个平滑的结果)。

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

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

相关文章

Vue2面试题:说一下什么是路由守卫?

是路由跳转前、后的一些钩子函数 分类&#xff1a; 全局守卫&#xff1a;【写在main.js中 或 router文件夹下的index.js中】 beforeEach: 进入路由之前的验证&#xff08;常用&#xff0c;如&#xff1a;判断用户是否登录&#xff09; afterEach: 路由进入之后的验证&#xff0…

[云原生] Go web工作流程

web工作流程 Web服务器的工作原理可以简单地归纳为 客户机通过TCP/IP协议建立到服务器的TCP连接客户端向服务器发送HTTP协议请求包&#xff0c;请求服务器里的资源文档服务器向客户机发送HTTP协议应答包&#xff0c;如果请求的资源包含有动态语言的内容&#xff0c;那么服务器…

06|调用模型:使用OpenAI API还是微调开源Llama2/ChatGLM?

06&#xff5c;调用模型&#xff1a;使用OpenAI API还是微调开源Llama2/ChatGLM&#xff1f; 让我们带着下面的问题来开始这一节课的学习。大语言模型&#xff0c;不止 ChatGPT 一种。调用 OpenAI 的 API&#xff0c;当然方便且高效&#xff0c;不过&#xff0c;如果我就是想用…

解决Java编译出现类名不匹配报错

当遇到Java编译错误时&#xff0c;很多时候都是由于类名与文件名不匹配导致的。在Java中&#xff0c;公共类的名称必须与其所在的文件名一致&#xff0c;包括大小写敏感。下面是我在编译第一个Java程序"HelloWorld"时遇到的问题和解决办法的记录。 问题描述 在终端…

HCIA-Datacom题库(自己整理分类的)——OSPF协议多选

ospf的hello报文功能是 邻居发现 同步路由器的LSDB 更新LSA信息 维持邻居关系 下列关于OSPF区域描述正确的是 在配置OSPF区域正确必须给路由器的loopback接配置IP地址 所有的网络都应在区域0中宣告 骨干区域的编号不能为2 区域的编号范围是从0.0.0.0到255.255.255.255…

第十二章 Sleuth分布式请求链路跟踪

Sleuth分布式请求链路跟踪 gitee:springcloud_study: springcloud&#xff1a;服务集群、注册中心、配置中心&#xff08;热更新&#xff09;、服务网关&#xff08;校验、路由、负载均衡&#xff09;、分布式缓存、分布式搜索、消息队列&#xff08;异步通信&#xff09;、数…

VisualStudio项目中多个同名文件处理

问题 在vs2019环境中&#xff0c;现在开发一个较大的C项目&#xff0c;它的源文件在多个文件夹中组织(在文件系统上)。在其中两个文件夹中&#xff0c;有几个同名的文件。如&#xff1a; .\ConsoleApplication1\JAMGEN_CONFIG_x4_test\firls.c .\ConsoleApplication1\NoiseCo…

YOLOv8训练自定义数据集和运行参数解读

1、YOLOv8深度学习环境搭建及安装 1.1. Yolov8介绍 设置操作类型 YOLOv8模型可用于各种任务&#xff0c;包括检测、分割和分类。这些任务的不同之处在于它们产生的输出类型和它们要解决的特定问题。 **检测:**检测任务涉及识别和定位图像或视频中感兴趣的对象或区域。YOLO模…

多模态大模型-CogVLm 论文阅读笔记

多模态大模型-CogVLm 论文阅读笔记 COGVLM: VISUAL EXPERT FOR LARGE LANGUAGEMODELS 论文地址 :https://arxiv.org/pdf/2311.03079.pdfcode地址 : https://github.com/THUDM/CogVLM时间 : 2023-11机构 : zhipuai,tsinghua关键词: visual language model效果:&#xff08;2023…

一键访问多功能数据解决方案:如何轻松构建和管理APITable智能多维表格

文章目录 前言1. 部署APITable2. cpolar的安装和注册3. 配置APITable公网访问地址4. 固定APITable公网地址 前言 vika维格表作为新一代数据生产力平台&#xff0c;是一款面向 API 的智能多维表格。它将复杂的可视化数据库、电子表格、实时在线协同、低代码开发技术四合为一&am…

【芯片DFX】Arm调试架构篇

【芯片DFX】万字长文带你搞懂JTAG的门门道道【芯片DFX】ARM:CoreSight、ETM、PTM、ITM、HTM、ETB等常用术语解析

《数据库开发实践》之触发器

一、什么是触发器&#xff1f; 1.概念&#xff1a; 简单来说触发器就是一种特殊的存储过程&#xff0c;在数据库服务器触发事件的时候会自动执行其SQL语句集。 2.构成四要素&#xff1a; &#xff08;1&#xff09;名称&#xff1a;要符合标识符命名规则 &#xff08;2&am…

element ui级连选择,lazyLoad选择地区

ui文档上直接给了一函数 先试试看效果是什么&#xff0c;加上let id0;不然会报错 props: {lazy: true,lazyLoad (node, resolve) {let id 0;const { level } node;setTimeout(() > {const nodes Array.from({ length: level 1 })//创建一个新数组&#xff0c;数组长度l…

Bug升级记

2023.12.28 &#xff08;1) 小程序session_key泄露隐患 核心&#xff1a;session_key这个字段及对应值不应该传到小程序客户端等服务器外的环境 错误操作&#xff1a;直接在小程序调用https://api.weixin.qq.com/sns/jscode2session并将session_key作为参数进行明文传输 正确操…

【javaweb】Tomcat 9.0中的HttpServletRequest

2023年12月28日&#xff0c;周四晚上 目录 什么是HttpServletRequest HttpServletRequest对象是由谁产生的 什么是HttpServletRequest HttpServletRequest 是 Java Servlet 中的接口&#xff0c;用于封装客户端请求的信息。它提供了访问客户端请求的方法&#xff0c;包括请求…

【六袆 - Framework】Flowable 引擎的介绍(1)

Flowable 框架的介绍 什么是 Flowable for by https://www.flowable.com/open-source/docs/bpmn/ch02-GettingStarted 什么是 Flowable Flowable 的介绍&#xff1a; 轻量级业务流程引擎&#xff1a; Flowable是一款用Java编写的轻量级业务流程引擎。其核心功能包括部署BPMN…

鸿蒙WebGL开发入门

WebGL的全称为Web Graphic Library(网页图形库),主要用于交互式渲染2D图形和3D图形。目前HarmonyOS中使用的WebGL是基于OpenGL裁剪的OpenGL ES,可以在HTML5的canvas元素对象中使用,无需使用插件,支持跨平台。WebGL程序是由JavaScript代码组成的,其中使用的API可以利用用户…

使用nodejs对接arXiv文献API

GPT4.0国内站点: 海鲸AI-支持GPT(3.5/4.0)&#xff0c;文件分析&#xff0c;AI绘图 要使用 Node.js 对接 arXiv 的 API&#xff0c;你可以使用 axios 库或者 Node.js 的内置 http 模块来发送 HTTP 请求。以下是一个简单的例子&#xff0c;展示了如何使用 axios 来获取 arXiv 上…

[OCR]Python 3 下的文字识别CnOCR

目录 1 CnOCR 2 安装 3 实践 1 CnOCR CnOCR 是 Python 3 下的文字识别&#xff08;Optical Character Recognition&#xff0c;简称OCR&#xff09;工具包。 工具包支持简体中文、繁体中文&#xff08;部分模型&#xff09;、英文和数字的常见字符识别&#xff0c;支持竖…

基于Java+SpringBoot+vue实现图书借阅管理系统

基于JavaSpringBootvue实现图书借阅和销售商城一体化系统 &#x1f345; 作者主页 程序设计 &#x1f345; 欢迎点赞 &#x1f44d; 收藏 ⭐留言 &#x1f4dd; &#x1f345; 文末获取源码联系方式 &#x1f4dd; 文章目录 基于JavaSpringBootvue实现图书借阅和销售商城一体化…