lego-loam featureAssociation 源码注释(二)
咱们接着往下看initializationValue();!!!
FeatureAssociation():nh("~"){subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 1);pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 1);pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 1);pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 1);pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);pubOutlierCloudLast = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2);pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);initializationValue();}
initializationValue()
cloudCurvature = new float[N_SCAN*Horizon_SCAN];cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];cloudLabel = new int[N_SCAN*Horizon_SCAN];pointSelCornerInd = new int[N_SCAN*Horizon_SCAN];pointSearchCornerInd1 = new float[N_SCAN*Horizon_SCAN];pointSearchCornerInd2 = new float[N_SCAN*Horizon_SCAN];pointSelSurfInd = new int[N_SCAN*Horizon_SCAN];pointSearchSurfInd1 = new float[N_SCAN*Horizon_SCAN];pointSearchSurfInd2 = new float[N_SCAN*Horizon_SCAN];pointSearchSurfInd3 = new float[N_SCAN*Horizon_SCAN];cloudSmoothness.resize(N_SCAN*Horizon_SCAN);downSizeFilter.setLeafSize(0.2, 0.2, 0.2);segmentedCloud.reset(new pcl::PointCloud<PointType>());outlierCloud.reset(new pcl::PointCloud<PointType>());cornerPointsSharp.reset(new pcl::PointCloud<PointType>());cornerPointsLessSharp.reset(new pcl::PointCloud<PointType>());surfPointsFlat.reset(new pcl::PointCloud<PointType>());surfPointsLessFlat.reset(new pcl::PointCloud<PointType>());surfPointsLessFlatScan.reset(new pcl::PointCloud<PointType>());surfPointsLessFlatScanDS.reset(new pcl::PointCloud<PointType>());
首先,第一部分的初始化和loam的初始化几乎一样,可以看到提取的特征仍然是根据Curvature来计算的,也就是根据曲率来计算的。分别包括sharp、lesssharp、flat、lessflat四类。
timeScanCur = 0;timeNewSegmentedCloud = 0;timeNewSegmentedCloudInfo = 0;timeNewOutlierCloud = 0;newSegmentedCloud = false;newSegmentedCloudInfo = false;newOutlierCloud = false;systemInitCount = 0;systemInited = false;
当初始化完成后,systemInited会变成true。几个时间分别是当前扫描时间,特征提取的时间和找到离群点的时间。这些具体的步骤在后续的代码中详细介绍。
imuPointerFront = 0;imuPointerLast = -1;imuPointerLastIteration = 0;imuRollStart = 0; imuPitchStart = 0; imuYawStart = 0;cosImuRollStart = 0; cosImuPitchStart = 0; cosImuYawStart = 0;sinImuRollStart = 0; sinImuPitchStart = 0; sinImuYawStart = 0;imuRollCur = 0; imuPitchCur = 0; imuYawCur = 0;imuVeloXStart = 0; imuVeloYStart = 0; imuVeloZStart = 0;imuShiftXStart = 0; imuShiftYStart = 0; imuShiftZStart = 0;imuVeloXCur = 0; imuVeloYCur = 0; imuVeloZCur = 0;imuShiftXCur = 0; imuShiftYCur = 0; imuShiftZCur = 0;imuShiftFromStartXCur = 0; imuShiftFromStartYCur = 0; imuShiftFromStartZCur = 0;imuVeloFromStartXCur = 0; imuVeloFromStartYCur = 0; imuVeloFromStartZCur = 0;imuAngularRotationXCur = 0; imuAngularRotationYCur = 0; imuAngularRotationZCur = 0;imuAngularRotationXLast = 0; imuAngularRotationYLast = 0; imuAngularRotationZLast = 0;imuAngularFromStartX = 0; imuAngularFromStartY = 0; imuAngularFromStartZ = 0;for (int i = 0; i < imuQueLength; ++i){imuTime[i] = 0;imuRoll[i] = 0; imuPitch[i] = 0; imuYaw[i] = 0;imuAccX[i] = 0; imuAccY[i] = 0; imuAccZ[i] = 0;imuVeloX[i] = 0; imuVeloY[i] = 0; imuVeloZ[i] = 0;imuShiftX[i] = 0; imuShiftY[i] = 0; imuShiftZ[i] = 0;imuAngularVeloX[i] = 0; imuAngularVeloY[i] = 0; imuAngularVeloZ[i] = 0;imuAngularRotationX[i] = 0; imuAngularRotationY[i] = 0; imuAngularRotationZ[i] = 0;}skipFrameNum = 1;for (int i = 0; i < 6; ++i){transformCur[i] = 0;transformSum[i] = 0;}systemInitedLM = false;imuRollLast = 0; imuPitchLast = 0; imuYawLast = 0;imuShiftFromStartX = 0; imuShiftFromStartY = 0; imuShiftFromStartZ = 0;imuVeloFromStartX = 0; imuVeloFromStartY = 0; imuVeloFromStartZ = 0;
extern const int imuQueLength = 200;
transformCur包括三个平移向量和三个旋转角,roll,pitch,yaw。
laserCloudCornerLast.reset(new pcl::PointCloud<PointType>());laserCloudSurfLast.reset(new pcl::PointCloud<PointType>());laserCloudOri.reset(new pcl::PointCloud<PointType>());coeffSel.reset(new pcl::PointCloud<PointType>());kdtreeCornerLast.reset(new pcl::KdTreeFLANN<PointType>());kdtreeSurfLast.reset(new pcl::KdTreeFLANN<PointType>());laserOdometry.header.frame_id = "camera_init";laserOdometry.child_frame_id = "/laser_odom";laserOdometryTrans.frame_id_ = "camera_init";laserOdometryTrans.child_frame_id_ = "/laser_odom";isDegenerate = false;matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));frameCount = skipFrameNum;
skipFrameNum = 1; frameCount = 1;
在ROS中,frame_id和child_frame_id是用来指定坐标系的,它们定义了数据的参考点。
-
laserOdometry.header.frame_id = "camera_init";这行代码设置了激光里程计消息的frame_id为"camera_init"。frame_id通常用来指定消息的参考坐标系。在这里,"camera_init"可能是指相机初始化时的坐标系。 -
laserOdometry.child_frame_id = "/laser_odom";这行代码设置了激光里程计消息的child_frame_id为"/laser_odom"。child_frame_id通常用来指定相对于frame_id的子坐标系。在这里,"/laser_odom"可能是指激光里程计的坐标系。 -
laserOdometryTrans.frame_id_ = "camera_init";和laserOdometryTrans.child_frame_id_ = "/laser_odom";这两行代码看起来是在设置一个转换(Transform)消息,它将"camera_init"坐标系中的点转换到"/laser_odom"坐标系中。
相关文章:
lego-loam featureAssociation 源码注释(二)
咱们接着往下看initializationValue();!!! FeatureAssociation():nh("~"){subLaserCloud nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);s…...
Claude 3.5 的六大应用场景
Claude 3.5 的六大应用场景 随着人工智能技术的飞速发展,Claude 3.5 已经成为一款强大的语言模型工具,在多个领域展现了其卓越的应用潜力。本文将通过CSDN格式,介绍Claude 3.5在六大主要领域的实际应用场景,帮助开发者和企业更好…...
进程线程知识总结
1. 程序什么时候应该使用线程,什么时候单线程效率高 使用线程:在I/O密集型或高并发的场景,例如网络服务、文件读写等。通过多线程可以同时处理多个任务,提高利用率。单线程效率高:在CPU密集型任务中,当任务…...
Rsync数据复制/备份服务应用
文章目录 1. rsync概述1.1 什么是Rsync1.2 rsync的功能1.3 rsync 的功能特性1.4 Rsync 增量复制原理1.5 生产场景架构集群备份方案 2. Rsync工作方式介绍与实践2.1 本地数据传输模式2.1.1 本地数据传输模式语法2.1.2 本地数据传输模式实践 2.2 远程Shell 数据传输模式2.2.1 远程…...
如何为自己的跨境网站添加多国语言翻译功能及推荐起尔网定制与插件开发
如何为自己的跨境网站添加多国语言翻译功能及推荐起尔网定制与插件开发 在全球化的浪潮下,跨境电商成为越来越多企业拓展国际市场的重要途径。然而,语言障碍成为了一个不可忽视的问题。为了更好地服务全球用户,为自己的跨境网站添加多国语言…...
安全见闻(3)——开阔眼界,不做井底之蛙
内容预览 ≧∀≦ゞ 安全见闻三:脚本程序与病毒声明导语脚本语言BAT/PowerShell脚本木马与宏病毒脚本病毒BIOS病毒 结语 安全见闻三:脚本程序与病毒 声明 学习视频来自B站UP主 泷羽sec,如涉及侵权马上删除文章 笔记的只是方便各位师傅学习知识,以下网站只…...
MySQL 的意向锁(Intention Locks)原理详解
1. 背景:为什么需要意向锁? MySQL 中意向锁的主要作用是用于支持行级锁与表级锁的并存,特别是在 InnoDB 存储引擎中。InnoDB 提供了行级锁,而在某些场景下,数据库系统仍需要对整张表加锁,例如 LOCK TABLES …...
31个省份农业科技水平(农业技术创新或农业科技专利数据)2010-2022年
一、测算方式:参考C刊《湖北大学学报(哲学社会科学版)》张金鑫(2020)老师的做法,采用农业( 农林牧渔业) 三类专利总和来衡量农业技术创新 二、资料范围:31个省份,403个观测值,已经整理成面板数…...
Python代码执行失败问题及解决方案
目录 一、Python代码执行失败的原因 二、常见的Python错误类型 1. 语法错误(SyntaxError) 2. 运行时错误(RuntimeError) 3. 类型错误(TypeError) 4. 导入错误(ImportError) 5…...
Java 遗传算法
遗传算法(Genetic Algorithm, GA)是一种基于自然选择和遗传学原理的优化算法,用于求解复杂的搜索和优化问题。在Java中实现遗传算法通常包括以下几个步骤: 初始化种群:生成一组随机解作为初始种群。适应度评估&#x…...
C++ (一) 基础语法
基础语法:C的开胃小菜 欢迎来到C的世界,这里是编程的盛宴,也是逻辑的迷宫。别担心,我们不会一开始就让你啃硬骨头,而是从基础语法开始,让你慢慢品尝编程的美味。准备好了吗?让我们开始这场编程…...
Qt/C++路径轨迹回放/回放每个点信号/回放结束信号/拿到移动的坐标点经纬度
一、前言说明 在使用百度地图的路书功能中,并没有提供移动的信号以及移动结束的信号,但是很多时候都期望拿到移动的哪里了以及移动结束的信号,以便做出对应的处理,比如结束后需要触发一些对应的操作。经过搜索发现很多人都有这个…...
C 语言介绍及操作案例
C 语言是一种广泛使用的通用编程语言,具有高效、灵活和可移植性强等特点。 一、C 语言的基本特点 简洁高效 C 语言语法简洁,表达能力强。它提供了丰富的数据类型和运算符,可以方便地进行各种计算和操作。C 语言的代码执行效率高,能够直接访问硬件资源,适用于对性能要求较…...
Ivanti云服务被攻击事件深度解析:安全策略构建与未来反思
攻击事件背景 近期,威胁情报和研究机构Fortinet FortiGuard Labs发布了一份关于针对IT解决方案提供商Ivanti云服务设备(Ivanti Cloud Services Appliance,CSA)的复杂网络攻击的详细分析。 该攻击被怀疑是由国家级对手发起…...
如何做出正确选择编程语言:关于Delphi 与 C# 编程语言的优缺点对比
概述 为您的项目选择正确的技术可能是一项相当棘手的任务,尤其是当您以前从未需要做出这样的选择时。如今可用的选项范围非常广泛。虽然一些编程语言和工具有着相当悠久的历史,但其他一些则是刚刚开始赢得开发人员青睐的新手。 在这篇博文中࿰…...
39.3K Star,一个现代的数据库ORM工具,专为Node.js和TypeScript设计
大家好,今天给大家分享一个现代的数据库对象关系映射(Object-Relational Mapping,ORM)工具Prisma ORM,它旨在简化数据库操作,提高开发效率,并确保类型安全。 项目介绍 Prisma ORM适用于各种需要…...
Nginx和Mysql的基础命令
1.安装nginx brew install nginx 2.启动nginx brew services start nginx 3.查看nginx文件默认路径 brew info nginx 重装要先关闭nginx 4.nginx.conf 地址 nginx -t 5.nginx重启 brew services restart nginx 6.关闭nginx brew services stop nginx 7.卸载nginx brew uninstal…...
Docker之容器常见操作
docker 命令介绍 docker --help 管理命令: container 管理容器image 管理镜像network 管理网络命令: attach 介入到一个正在运行的容器build 根据 Dockerfile 构建一个镜像commit 根据容器的更改创建一个新的镜像cp 在本地文…...
猜数游戏(Fortran)
背景 学了两个月Fortran还没来一次正式练习 于是—— 代码 program gessnum! implicit none 不取消IN规则。integer::num,areal::Ncall random_seed()call random_number(N)aint(N*10)print*,"请输入您猜的数字:"read(*,*)numdo i1,3if (numa)thenpri…...
代码随想录 -- 贪心 -- 单调递增的数字
738. 单调递增的数字 - 力扣(LeetCode) 思路: 首先将正数n转化为字符串类型;定义一个flag:标记flag以及之后的位数都是9;从后向前遍历字符串n,如果当前的位数小于他上一位,将上一位…...
java调用dll出现unsatisfiedLinkError以及JNA和JNI的区别
UnsatisfiedLinkError 在对接硬件设备中,我们会遇到使用 java 调用 dll文件 的情况,此时大概率出现UnsatisfiedLinkError链接错误,原因可能有如下几种 类名错误包名错误方法名参数错误使用 JNI 协议调用,结果 dll 未实现 JNI 协…...
蓝牙 BLE 扫描面试题大全(2):进阶面试题与实战演练
前文覆盖了 BLE 扫描的基础概念与经典问题蓝牙 BLE 扫描面试题大全(1):从基础到实战的深度解析-CSDN博客,但实际面试中,企业更关注候选人对复杂场景的应对能力(如多设备并发扫描、低功耗与高发现率的平衡)和前沿技术的…...
DIY|Mac 搭建 ESP-IDF 开发环境及编译小智 AI
前一阵子在百度 AI 开发者大会上,看到基于小智 AI DIY 玩具的演示,感觉有点意思,想着自己也来试试。 如果只是想烧录现成的固件,乐鑫官方除了提供了 Windows 版本的 Flash 下载工具 之外,还提供了基于网页版的 ESP LA…...
Python如何给视频添加音频和字幕
在Python中,给视频添加音频和字幕可以使用电影文件处理库MoviePy和字幕处理库Subtitles。下面将详细介绍如何使用这些库来实现视频的音频和字幕添加,包括必要的代码示例和详细解释。 环境准备 在开始之前,需要安装以下Python库:…...
CMake 从 GitHub 下载第三方库并使用
有时我们希望直接使用 GitHub 上的开源库,而不想手动下载、编译和安装。 可以利用 CMake 提供的 FetchContent 模块来实现自动下载、构建和链接第三方库。 FetchContent 命令官方文档✅ 示例代码 我们将以 fmt 这个流行的格式化库为例,演示如何: 使用 FetchContent 从 GitH…...
Android Bitmap治理全解析:从加载优化到泄漏防控的全生命周期管理
引言 Bitmap(位图)是Android应用内存占用的“头号杀手”。一张1080P(1920x1080)的图片以ARGB_8888格式加载时,内存占用高达8MB(192010804字节)。据统计,超过60%的应用OOM崩溃与Bitm…...
Java多线程实现之Thread类深度解析
Java多线程实现之Thread类深度解析 一、多线程基础概念1.1 什么是线程1.2 多线程的优势1.3 Java多线程模型 二、Thread类的基本结构与构造函数2.1 Thread类的继承关系2.2 构造函数 三、创建和启动线程3.1 继承Thread类创建线程3.2 实现Runnable接口创建线程 四、Thread类的核心…...
【C++进阶篇】智能指针
C内存管理终极指南:智能指针从入门到源码剖析 一. 智能指针1.1 auto_ptr1.2 unique_ptr1.3 shared_ptr1.4 make_shared 二. 原理三. shared_ptr循环引用问题三. 线程安全问题四. 内存泄漏4.1 什么是内存泄漏4.2 危害4.3 避免内存泄漏 五. 最后 一. 智能指针 智能指…...
NPOI Excel用OLE对象的形式插入文件附件以及插入图片
static void Main(string[] args) {XlsWithObjData();Console.WriteLine("输出完成"); }static void XlsWithObjData() {// 创建工作簿和单元格,只有HSSFWorkbook,XSSFWorkbook不可以HSSFWorkbook workbook new HSSFWorkbook();HSSFSheet sheet (HSSFSheet)workboo…...
GO协程(Goroutine)问题总结
在使用Go语言来编写代码时,遇到的一些问题总结一下 [参考文档]:https://www.topgoer.com/%E5%B9%B6%E5%8F%91%E7%BC%96%E7%A8%8B/goroutine.html 1. main()函数默认的Goroutine 场景再现: 今天在看到这个教程的时候,在自己的电…...
