从零入门激光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

相关文章

IntelliJ IDEA 中配置 Spring MVC 环境的详细步骤及问题解决

《IntelliJIDEA中配置SpringMVC环境的详细步骤及问题解决》:本文主要介绍IntelliJIDEA中配置SpringMVC环境的详细步骤及问题解决,本文分步骤结合实例给大... 目录步骤 1:创建 Maven Web 项目步骤 2:添加 Spring MVC 依赖1、保存后执行2、将新的依赖

Golang HashMap实现原理解析

《GolangHashMap实现原理解析》HashMap是一种基于哈希表实现的键值对存储结构,它通过哈希函数将键映射到数组的索引位置,支持高效的插入、查找和删除操作,:本文主要介绍GolangH... 目录HashMap是一种基于哈希表实现的键值对存储结构,它通过哈希函数将键映射到数组的索引位置,支持

如何为Yarn配置国内源的详细教程

《如何为Yarn配置国内源的详细教程》在使用Yarn进行项目开发时,由于网络原因,直接使用官方源可能会导致下载速度慢或连接失败,配置国内源可以显著提高包的下载速度和稳定性,本文将详细介绍如何为Yarn... 目录一、查询当前使用的镜像源二、设置国内源1. 设置为淘宝镜像源2. 设置为其他国内源三、还原为官方

最详细安装 PostgreSQL方法及常见问题解决

《最详细安装PostgreSQL方法及常见问题解决》:本文主要介绍最详细安装PostgreSQL方法及常见问题解决,介绍了在Windows系统上安装PostgreSQL及Linux系统上安装Po... 目录一、在 Windows 系统上安装 PostgreSQL1. 下载 PostgreSQL 安装包2.

Python使用getopt处理命令行参数示例解析(最佳实践)

《Python使用getopt处理命令行参数示例解析(最佳实践)》getopt模块是Python标准库中一个简单但强大的命令行参数处理工具,它特别适合那些需要快速实现基本命令行参数解析的场景,或者需要... 目录为什么需要处理命令行参数?getopt模块基础实际应用示例与其他参数处理方式的比较常见问http

Python利用ElementTree实现快速解析XML文件

《Python利用ElementTree实现快速解析XML文件》ElementTree是Python标准库的一部分,而且是Python标准库中用于解析和操作XML数据的模块,下面小编就来和大家详细讲讲... 目录一、XML文件解析到底有多重要二、ElementTree快速入门1. 加载XML的两种方式2.

Java的栈与队列实现代码解析

《Java的栈与队列实现代码解析》栈是常见的线性数据结构,栈的特点是以先进后出的形式,后进先出,先进后出,分为栈底和栈顶,栈应用于内存的分配,表达式求值,存储临时的数据和方法的调用等,本文给大家介绍J... 目录栈的概念(Stack)栈的实现代码队列(Queue)模拟实现队列(双链表实现)循环队列(循环数组

java解析jwt中的payload的用法

《java解析jwt中的payload的用法》:本文主要介绍java解析jwt中的payload的用法,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录Java解析jwt中的payload1. 使用 jjwt 库步骤 1:添加依赖步骤 2:解析 JWT2. 使用 N

Python中__init__方法使用的深度解析

《Python中__init__方法使用的深度解析》在Python的面向对象编程(OOP)体系中,__init__方法如同建造房屋时的奠基仪式——它定义了对象诞生时的初始状态,下面我们就来深入了解下_... 目录一、__init__的基因图谱二、初始化过程的魔法时刻继承链中的初始化顺序self参数的奥秘默认

MySql match against工具详细用法

《MySqlmatchagainst工具详细用法》在MySQL中,MATCH……AGAINST是全文索引(Full-Textindex)的查询语法,它允许你对文本进行高效的全文搜素,支持自然语言搜... 目录一、全文索引的基本概念二、创建全文索引三、自然语言搜索四、布尔搜索五、相关性排序六、全文索引的限制七