当前位置: 首页 > news >正文

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_idchild_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();&#xff01;&#xff01;&#xff01; FeatureAssociation():nh("~"){subLaserCloud nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);s…...

Claude 3.5 的六大应用场景

Claude 3.5 的六大应用场景 随着人工智能技术的飞速发展&#xff0c;Claude 3.5 已经成为一款强大的语言模型工具&#xff0c;在多个领域展现了其卓越的应用潜力。本文将通过CSDN格式&#xff0c;介绍Claude 3.5在六大主要领域的实际应用场景&#xff0c;帮助开发者和企业更好…...

进程线程知识总结

1. 程序什么时候应该使用线程&#xff0c;什么时候单线程效率高 使用线程&#xff1a;在I/O密集型或高并发的场景&#xff0c;例如网络服务、文件读写等。通过多线程可以同时处理多个任务&#xff0c;提高利用率。单线程效率高&#xff1a;在CPU密集型任务中&#xff0c;当任务…...

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 远程…...

如何为自己的跨境网站添加多国语言翻译功能及推荐起尔网定制与插件开发

如何为自己的跨境网站添加多国语言翻译功能及推荐起尔网定制与插件开发 在全球化的浪潮下&#xff0c;跨境电商成为越来越多企业拓展国际市场的重要途径。然而&#xff0c;语言障碍成为了一个不可忽视的问题。为了更好地服务全球用户&#xff0c;为自己的跨境网站添加多国语言…...

安全见闻(3)——开阔眼界,不做井底之蛙

内容预览 ≧∀≦ゞ 安全见闻三&#xff1a;脚本程序与病毒声明导语脚本语言BAT/PowerShell脚本木马与宏病毒脚本病毒BIOS病毒 结语 安全见闻三&#xff1a;脚本程序与病毒 声明 学习视频来自B站UP主 泷羽sec,如涉及侵权马上删除文章 笔记的只是方便各位师傅学习知识,以下网站只…...

MySQL 的意向锁(Intention Locks)原理详解

1. 背景&#xff1a;为什么需要意向锁&#xff1f; MySQL 中意向锁的主要作用是用于支持行级锁与表级锁的并存&#xff0c;特别是在 InnoDB 存储引擎中。InnoDB 提供了行级锁&#xff0c;而在某些场景下&#xff0c;数据库系统仍需要对整张表加锁&#xff0c;例如 LOCK TABLES …...

31个省份农业科技水平(农业技术创新或农业科技专利数据)2010-2022年

一、测算方式&#xff1a;参考C刊《湖北大学学报(哲学社会科学版)》张金鑫&#xff08;2020&#xff09;老师的做法&#xff0c;采用农业( 农林牧渔业) 三类专利总和来衡量农业技术创新 二、资料范围&#xff1a;31个省份&#xff0c;403个观测值&#xff0c;已经整理成面板数…...

Python代码执行失败问题及解决方案

目录 一、Python代码执行失败的原因 二、常见的Python错误类型 1. 语法错误&#xff08;SyntaxError&#xff09; 2. 运行时错误&#xff08;RuntimeError&#xff09; 3. 类型错误&#xff08;TypeError&#xff09; 4. 导入错误&#xff08;ImportError&#xff09; 5…...

Java 遗传算法

遗传算法&#xff08;Genetic Algorithm, GA&#xff09;是一种基于自然选择和遗传学原理的优化算法&#xff0c;用于求解复杂的搜索和优化问题。在Java中实现遗传算法通常包括以下几个步骤&#xff1a; 初始化种群&#xff1a;生成一组随机解作为初始种群。适应度评估&#x…...

C++ (一) 基础语法

基础语法&#xff1a;C的开胃小菜 欢迎来到C的世界&#xff0c;这里是编程的盛宴&#xff0c;也是逻辑的迷宫。别担心&#xff0c;我们不会一开始就让你啃硬骨头&#xff0c;而是从基础语法开始&#xff0c;让你慢慢品尝编程的美味。准备好了吗&#xff1f;让我们开始这场编程…...

Qt/C++路径轨迹回放/回放每个点信号/回放结束信号/拿到移动的坐标点经纬度

一、前言说明 在使用百度地图的路书功能中&#xff0c;并没有提供移动的信号以及移动结束的信号&#xff0c;但是很多时候都期望拿到移动的哪里了以及移动结束的信号&#xff0c;以便做出对应的处理&#xff0c;比如结束后需要触发一些对应的操作。经过搜索发现很多人都有这个…...

C 语言介绍及操作案例

C 语言是一种广泛使用的通用编程语言,具有高效、灵活和可移植性强等特点。 一、C 语言的基本特点 简洁高效 C 语言语法简洁,表达能力强。它提供了丰富的数据类型和运算符,可以方便地进行各种计算和操作。C 语言的代码执行效率高,能够直接访问硬件资源,适用于对性能要求较…...

Ivanti云服务被攻击事件深度解析:安全策略构建与未来反思

攻击事件背景 近期&#xff0c;威胁情报和研究机构Fortinet FortiGuard Labs发布了一份关于针对IT解决方案提供商Ivanti云服务设备&#xff08;Ivanti Cloud Services Appliance&#xff0c;CSA&#xff09;的复杂网络攻击的详细分析。 该攻击被怀疑是由国家级对手发起&#xf…...

如何做出正确选择编程语言:关于Delphi 与 C# 编程语言的优缺点对比

概述 为您的项目选择正确的技术可能是一项相当棘手的任务&#xff0c;尤其是当您以前从未需要做出这样的选择时。如今可用的选项范围非常广泛。虽然一些编程语言和工具有着相当悠久的历史&#xff0c;但其他一些则是刚刚开始赢得开发人员青睐的新手。 在这篇博文中&#xff0…...

39.3K Star,一个现代的数据库ORM工具,专为Node.js和TypeScript设计

大家好&#xff0c;今天给大家分享一个现代的数据库对象关系映射&#xff08;Object-Relational Mapping&#xff0c;ORM&#xff09;工具Prisma ORM&#xff0c;它旨在简化数据库操作&#xff0c;提高开发效率&#xff0c;并确保类型安全。 项目介绍 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 管理网络命令&#xff1a; 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*,"请输入您猜的数字&#xff1a;"read(*,*)numdo i1,3if (numa)thenpri…...

代码随想录 -- 贪心 -- 单调递增的数字

738. 单调递增的数字 - 力扣&#xff08;LeetCode&#xff09; 思路&#xff1a; 首先将正数n转化为字符串类型&#xff1b;定义一个flag&#xff1a;标记flag以及之后的位数都是9&#xff1b;从后向前遍历字符串n&#xff0c;如果当前的位数小于他上一位&#xff0c;将上一位…...

C++_核心编程_多态案例二-制作饮品

#include <iostream> #include <string> using namespace std;/*制作饮品的大致流程为&#xff1a;煮水 - 冲泡 - 倒入杯中 - 加入辅料 利用多态技术实现本案例&#xff0c;提供抽象制作饮品基类&#xff0c;提供子类制作咖啡和茶叶*//*基类*/ class AbstractDr…...

Xshell远程连接Kali(默认 | 私钥)Note版

前言:xshell远程连接&#xff0c;私钥连接和常规默认连接 任务一 开启ssh服务 service ssh status //查看ssh服务状态 service ssh start //开启ssh服务 update-rc.d ssh enable //开启自启动ssh服务 任务二 修改配置文件 vi /etc/ssh/ssh_config //第一…...

Appium+python自动化(十六)- ADB命令

简介 Android 调试桥(adb)是多种用途的工具&#xff0c;该工具可以帮助你你管理设备或模拟器 的状态。 adb ( Android Debug Bridge)是一个通用命令行工具&#xff0c;其允许您与模拟器实例或连接的 Android 设备进行通信。它可为各种设备操作提供便利&#xff0c;如安装和调试…...

uni-app学习笔记二十二---使用vite.config.js全局导入常用依赖

在前面的练习中&#xff0c;每个页面需要使用ref&#xff0c;onShow等生命周期钩子函数时都需要像下面这样导入 import {onMounted, ref} from "vue" 如果不想每个页面都导入&#xff0c;需要使用node.js命令npm安装unplugin-auto-import npm install unplugin-au…...

laravel8+vue3.0+element-plus搭建方法

创建 laravel8 项目 composer create-project --prefer-dist laravel/laravel laravel8 8.* 安装 laravel/ui composer require laravel/ui 修改 package.json 文件 "devDependencies": {"vue/compiler-sfc": "^3.0.7","axios": …...

JavaScript 数据类型详解

JavaScript 数据类型详解 JavaScript 数据类型分为 原始类型&#xff08;Primitive&#xff09; 和 对象类型&#xff08;Object&#xff09; 两大类&#xff0c;共 8 种&#xff08;ES11&#xff09;&#xff1a; 一、原始类型&#xff08;7种&#xff09; 1. undefined 定…...

GO协程(Goroutine)问题总结

在使用Go语言来编写代码时&#xff0c;遇到的一些问题总结一下 [参考文档]&#xff1a;https://www.topgoer.com/%E5%B9%B6%E5%8F%91%E7%BC%96%E7%A8%8B/goroutine.html 1. main()函数默认的Goroutine 场景再现&#xff1a; 今天在看到这个教程的时候&#xff0c;在自己的电…...

破解路内监管盲区:免布线低位视频桩重塑停车管理新标准

城市路内停车管理常因行道树遮挡、高位设备盲区等问题&#xff0c;导致车牌识别率低、逃费率高&#xff0c;传统模式在复杂路段束手无策。免布线低位视频桩凭借超低视角部署与智能算法&#xff0c;正成为破局关键。该设备安装于车位侧方0.5-0.7米高度&#xff0c;直接规避树枝遮…...

Spring Boot + MyBatis 集成支付宝支付流程

Spring Boot MyBatis 集成支付宝支付流程 核心流程 商户系统生成订单调用支付宝创建预支付订单用户跳转支付宝完成支付支付宝异步通知支付结果商户处理支付结果更新订单状态支付宝同步跳转回商户页面 代码实现示例&#xff08;电脑网站支付&#xff09; 1. 添加依赖 <!…...

海云安高敏捷信创白盒SCAP入选《中国网络安全细分领域产品名录》

近日&#xff0c;嘶吼安全产业研究院发布《中国网络安全细分领域产品名录》&#xff0c;海云安高敏捷信创白盒&#xff08;SCAP&#xff09;成功入选软件供应链安全领域产品名录。 在数字化转型加速的今天&#xff0c;网络安全已成为企业生存与发展的核心基石&#xff0c;为了解…...