从零入门激光SLAM(十三)——LeGo-LOAM源码超详细解析4

2024-05-10 09:36

本文主要是介绍从零入门激光SLAM(十三)——LeGo-LOAM源码超详细解析4,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

大家好呀,我是一个SLAM方向的在读博士,深知SLAM学习过程一路走来的坎坷,也十分感谢各位大佬的优质文章和源码。随着知识的越来越多,越来越细,我准备整理一个自己的激光SLAM学习笔记专栏,从0带大家快速上手激光SLAM,也方便想入门SLAM的同学和小白学习参考,相信看完会有一定的收获。如有不对的地方欢迎指出,欢迎各位大佬交流讨论,一起进步。博主创建了一个科研互助群****Q:772356582,欢迎大家加入讨论。

一、整体框架

1.1 目的

主要根据里程计获得的先验位姿进行后端优化,闭环检测和图优化

1.2 输入

//接收相机坐标系下的点和里程计
//上一帧角点
subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, &mapOptimization::laserCloudCornerLastHandler, this); 
//上一帧面点
subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, &mapOptimization::laserCloudSurfLastHandler, this);
//上一帧无效点
subOutlierCloudLast = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2, &mapOptimization::laserCloudOutlierLastHandler, this); 
//里程计位姿
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5, &mapOptimization::laserOdometryHandler, this);  
//IMU数据
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 50, &mapOptimization::imuHandler, this); 

1.3 输出

//机器人关键帧在全局坐标系下的位置信息,轨迹
pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("/key_pose_origin", 2);
//机器人周围激光雷达点云数据
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 2);
//经过位姿图优化和点云配准后的里程计信息
pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 5);
//机器人历史轨迹的点云数据
pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/history_cloud", 2);
//经过ICP配准后的机器人激光雷达点云数据
pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/corrected_cloud", 2);
//机器人最近获取的点云数据
pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("/recent_cloud", 2);
//经过位姿图优化和点云配准后的机器人点云数据用于建图
pubRegisteredCloud = nh.advertise<sensor_msgs::PointCloud2>("/registered_cloud", 2);

主函数

主要的功能是在run函数里面

int main(int argc, char** argv)
{ros::init(argc, argv, "lego_loam");ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");mapOptimization MO;// 1.进行闭环检测与闭环的功能std::thread loopthread(&mapOptimization::loopClosureThread, &MO);// 2.将数据发布到ros中,可视化std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);ros::Rate rate(200);while (ros::ok()){ros::spinOnce();MO.run(); //进入执行run函数rate.sleep();}loopthread.join();visualizeMapThread.join();return 0;
}
// 3.run函数
void run(){if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {timeLastProcessing = timeLaserOdometry;transformAssociateToMap(); //转换到map坐标系下extractSurroundingKeyFrames(); //提取周围的关键帧downsampleCurrentScan(); //下采样当前帧// 当前扫描进行边缘优化,图优化以及进行LM优化的过程scan2MapOptimization();saveKeyFramesAndFactor(); //保存关键帧和因子correctPoses(); //校正位姿publishTF(); //发布坐标变换publishKeyPosesAndFrames(); //发布关键帧和因子clearCloud();}}} //清除点云

二、函数解析

2.1 transformAssociateToMap

  • 作用:将坐标转移到世界坐标系下,得到可用于建图的Lidar坐标
  • 输入:transformBefMapped[] 前一帧在世界坐标系的位姿
  •  transformSum  当前帧的位姿
    
  • 输出:transformTobeMapped当前帧在世界坐标系的位置
  • 代码:
 void transformAssociateToMap(){float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);float y1 = transformBefMapped[4] - transformSum[4];float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);float x2 = x1;float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;// 计算平移增量transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;transformIncre[5] = z2;……x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4];y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2])* transformIncre[4];z1 = transformIncre[5];x2 = x1;y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;transformTobeMapped[3] = transformAftMapped[3] - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);transformTobeMapped[4] = transformAftMapped[4] - y2;transformTobeMapped[5] = transformAftMapped[5] - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);}

详情请见。。。
https://www.guyuehome.com/46822

这篇关于从零入门激光SLAM(十三)——LeGo-LOAM源码超详细解析4的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



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

相关文章

将Mybatis升级为Mybatis-Plus的详细过程

《将Mybatis升级为Mybatis-Plus的详细过程》本文详细介绍了在若依管理系统(v3.8.8)中将MyBatis升级为MyBatis-Plus的过程,旨在提升开发效率,通过本文,开发者可实现... 目录说明流程增加依赖修改配置文件注释掉MyBATisConfig里面的Bean代码生成使用IDEA生

使用Jackson进行JSON生成与解析的新手指南

《使用Jackson进行JSON生成与解析的新手指南》这篇文章主要为大家详细介绍了如何使用Jackson进行JSON生成与解析处理,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录1. 核心依赖2. 基础用法2.1 对象转 jsON(序列化)2.2 JSON 转对象(反序列化)3.

Linux系统配置NAT网络模式的详细步骤(附图文)

《Linux系统配置NAT网络模式的详细步骤(附图文)》本文详细指导如何在VMware环境下配置NAT网络模式,包括设置主机和虚拟机的IP地址、网关,以及针对Linux和Windows系统的具体步骤,... 目录一、配置NAT网络模式二、设置虚拟机交换机网关2.1 打开虚拟机2.2 管理员授权2.3 设置子

Springboot @Autowired和@Resource的区别解析

《Springboot@Autowired和@Resource的区别解析》@Resource是JDK提供的注解,只是Spring在实现上提供了这个注解的功能支持,本文给大家介绍Springboot@... 目录【一】定义【1】@Autowired【2】@Resource【二】区别【1】包含的属性不同【2】@

Linux系统中卸载与安装JDK的详细教程

《Linux系统中卸载与安装JDK的详细教程》本文详细介绍了如何在Linux系统中通过Xshell和Xftp工具连接与传输文件,然后进行JDK的安装与卸载,安装步骤包括连接Linux、传输JDK安装包... 目录1、卸载1.1 linux删除自带的JDK1.2 Linux上卸载自己安装的JDK2、安装2.1

Spring Boot + MyBatis Plus 高效开发实战从入门到进阶优化(推荐)

《SpringBoot+MyBatisPlus高效开发实战从入门到进阶优化(推荐)》本文将详细介绍SpringBoot+MyBatisPlus的完整开发流程,并深入剖析分页查询、批量操作、动... 目录Spring Boot + MyBATis Plus 高效开发实战:从入门到进阶优化1. MyBatis

SpringCloud动态配置注解@RefreshScope与@Component的深度解析

《SpringCloud动态配置注解@RefreshScope与@Component的深度解析》在现代微服务架构中,动态配置管理是一个关键需求,本文将为大家介绍SpringCloud中相关的注解@Re... 目录引言1. @RefreshScope 的作用与原理1.1 什么是 @RefreshScope1.

Java并发编程必备之Synchronized关键字深入解析

《Java并发编程必备之Synchronized关键字深入解析》本文我们深入探索了Java中的Synchronized关键字,包括其互斥性和可重入性的特性,文章详细介绍了Synchronized的三种... 目录一、前言二、Synchronized关键字2.1 Synchronized的特性1. 互斥2.

Java使用Curator进行ZooKeeper操作的详细教程

《Java使用Curator进行ZooKeeper操作的详细教程》ApacheCurator是一个基于ZooKeeper的Java客户端库,它极大地简化了使用ZooKeeper的开发工作,在分布式系统... 目录1、简述2、核心功能2.1 CuratorFramework2.2 Recipes3、示例实践3

Python实现无痛修改第三方库源码的方法详解

《Python实现无痛修改第三方库源码的方法详解》很多时候,我们下载的第三方库是不会有需求不满足的情况,但也有极少的情况,第三方库没有兼顾到需求,本文将介绍几个修改源码的操作,大家可以根据需求进行选择... 目录需求不符合模拟示例 1. 修改源文件2. 继承修改3. 猴子补丁4. 追踪局部变量需求不符合很