【LVI-SAM】激光雷达点云处理特征提取LIO-SAM 之FeatureExtraction实现细节

激光雷达点云处理特征提取LIO-SAM 之FeatureExtraction实现细节

    • 1. 特征提取实现过程总结
      • 1.0 特征提取过程小结
      • 1.1 类 `FeatureExtraction` 的整体结构与作用
      • 1.2 详细特征提取的过程
        • 1. 平滑度计算（`calculateSmoothness()`）
        • 2. 标记遮挡点（`markOccludedPoints()`）
        • 3. 特征提取（`extractFeatures()`）
        • 4. 发布特征点云（`publishFeatureCloud()`）
    • 2.0 特征提取数学推倒过程
    • 3.0 FeatureExtraction Code

1. 特征提取实现过程总结

这段代码实现了基于LiDAR（激光雷达）点云数据的特征提取，用于SLAM（Simultaneous Localization and Mapping）系统中的前端处理。特征提取的目的是从点云中识别出角点和平面点（面点），为后续的位姿估计和地图构建提供关键特征点。
在这里插入图片描述

1.0 特征提取过程小结

这段代码的主要目的是从LiDAR点云中提取出角点（边缘）和面点（平面），以便用于SLAM系统中。整个流程涉及：

  1. 平滑度计算：通过计算每个点的平滑度来区分平滑点和突变点。
  2. 遮挡点标记：通过深度差和像素间距来标记被遮挡的点和平行光束点。
  3. 特征提取：根据曲率值提取角点和面点，分别用于位姿估计和地图构建。
  4. 降采样和发布：通过降采样减少数据冗余，最终发布处理后的特征点云。

1.1 类 FeatureExtraction 的整体结构与作用

  • 类成员：

    • 该类通过 ROS 订阅与发布机制接收来自雷达的点云信息，并在处理后发布提取的特征。
    • 重要的类成员包括：
      • 订阅器 subLaserCloudInfo，用于接收点云数据。
      • 发布器 pubLaserCloudInfo、pubCornerPoints 和 pubSurfacePoints，分别用于发布处理后的点云信息、角点特征和面点特征。
      • 点云指针 extractedCloud、cornerCloud 和 surfaceCloud，用于保存原始提取点云和特征点云。
      • cloudCurvature、cloudNeighborPicked 和 cloudLabel，这些数组用于存储每个点的曲率、是否被选中、点的分类标签。

  • 构造函数 FeatureExtraction：

    • 初始化了订阅与发布机制。
    • 调用了 initializationValue() 函数来初始化一些数据结构和参数。

  • 回调函数 laserCloudInfoHandler：

    • 处理订阅到的点云信息，调用以下核心功能：calculateSmoothness()（计算每个点的平滑度）、markOccludedPoints()（标记被遮挡的点）和 extractFeatures()（特征提取），最后发布特征点云。

1.2 详细特征提取的过程

   void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn){cloudInfo = *msgIn; // new cloud infocloudHeader = msgIn->header; // new cloud headerpcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extractioncalculateSmoothness();markOccludedPoints();extractFeatures();publishFeatureCloud();}
1. 平滑度计算（calculateSmoothness()）

这个函数计算每个点的平滑度，平滑度的定义是基于该点与其前后(5点)若干点之间的距离变化。具体步骤为：

  • for 循环：
    • 遍历从第5个点到倒数第5个点，以避免处理边界的点。
    • 计算该点前后5个点的距离差的平方和，并将该结果作为该点的曲率（即平滑度值 cloudCurvature[i]）。
    • 初始化该点的 cloudNeighborPicked 为 0（表示该点还没有被处理过）和 cloudLabel 为 0（标签，初始为未分类）。
    • 将平滑度值和点的索引存储到 cloudSmoothness 中，以便后续进行排序。
2. 标记遮挡点（markOccludedPoints()）

该函数标记被遮挡的点以及光束平行的点，以避免它们影响特征提取。主要逻辑如下：

  • 遮挡点：

    • 遍历每个点，比较该点与相邻点的深度差（即距离差）。
    • 如果相邻两个点的列索引差小于 10（表示在深度图像中的像素间距较小），且深度差大于 0.3，则认为是遮挡点并标记为已处理（cloudNeighborPicked[i] = 1），即这些点将不会被选为特征点。

  • 平行光束：

    • 如果前后点与当前点的距离差大于一定比例（0.02 * cloudInfo.pointRange[i]），则认为它们是平行光束，这种情况下这些点也会被标记为已处理。
3. 特征提取（extractFeatures()）

这个函数的主要任务是提取角点和面点，并根据曲率值将点云进行分类。主要逻辑如下：

  • for 循环1-2：遍历激光雷达的扫描线 N_SCAN（通常是垂直方向上的扫描线数量），每条扫描线都被分为6个区域，逐个区域进行处理。

    • for 循环3-4：处理每个区域的点，将该区域按平滑度（即曲率）从大到小排序，然后分成两个部分进行处理：

      • 角点提取：
        • 从平滑度最高的点开始，如果该点没有被遮挡且曲率值大于阈值 edgeThreshold，则将其标记为角点，并加入角点点云（cornerCloud）。
        • 为了避免噪声点的影响，最多提取20个角点，并标记相邻的点为已处理，防止相邻的点被重复选取。
      • 面点提取：
        • 对于平滑度较低的点，如果曲率小于阈值 surfThreshold，则将其标记为面点，加入面点点云（surfaceCloud）。
        • 同样，通过标记相邻点来避免重复选择。

    • for 循环5：对于那些没有被标记为角点且曲率较小的点，将它们视为面点。

  • 降采样：通过 pcl::VoxelGrid 对面点进行降采样，减少点云的冗余数据，提升后续处理效率。
  # LOAM feature thresholdedgeThreshold: 1.0surfThreshold: 0.1edgeFeatureMinValidNum: 10surfFeatureMinValidNum: 100
4. 发布特征点云（publishFeatureCloud()）

在提取完角点和面点之后，该函数将处理后的点云数据发布出去，用于后续的地图优化和位姿估计。

2.0 特征提取数学推倒过程

数学推倒

3.0 FeatureExtraction Code

#include "utility.h"
#include "lio_sam/cloud_info.h"struct smoothness_t{ float value;size_t ind;
};struct by_value{ bool operator()(smoothness_t const &left, smoothness_t const &right) { return left.value < right.value;}
};class FeatureExtraction : public ParamServer
{public:ros::Subscriber subLaserCloudInfo;ros::Publisher pubLaserCloudInfo;ros::Publisher pubCornerPoints;ros::Publisher pubSurfacePoints;pcl::PointCloud<PointType>::Ptr extractedCloud;pcl::PointCloud<PointType>::Ptr cornerCloud;pcl::PointCloud<PointType>::Ptr surfaceCloud;pcl::VoxelGrid<PointType> downSizeFilter;lio_sam::cloud_info cloudInfo;std_msgs::Header cloudHeader;std::vector<smoothness_t> cloudSmoothness;float *cloudCurvature;int *cloudNeighborPicked;int *cloudLabel;FeatureExtraction(){subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 1);pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_corner", 1);pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_surface", 1);initializationValue();}void initializationValue(){cloudSmoothness.resize(N_SCAN*Horizon_SCAN);downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize);extractedCloud.reset(new pcl::PointCloud<PointType>());cornerCloud.reset(new pcl::PointCloud<PointType>());surfaceCloud.reset(new pcl::PointCloud<PointType>());cloudCurvature = new float[N_SCAN*Horizon_SCAN];cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];cloudLabel = new int[N_SCAN*Horizon_SCAN];}/*** @brief 计算平滑度** 遍历提取的点云数据，计算每个点的平滑度，并保存到对应数组中。** @note 对于点云中的每个点，计算其与前五个和后五个点的距离差的平方和作为平滑度。*       同时初始化相邻点被选中的状态为0，以及点的标签为0。*       将平滑度值以及对应的索引保存到cloudSmoothness数组中，以便后续排序。*/void calculateSmoothness(){int cloudSize = extractedCloud->points.size();for (int i = 5; i < cloudSize - 5; i++){float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]+ cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]+ cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10+ cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]+ cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]+ cloudInfo.pointRange[i+5];            cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;cloudNeighborPicked[i] = 0;cloudLabel[i] = 0;// cloudSmoothness for sortingcloudSmoothness[i].value = cloudCurvature[i];cloudSmoothness[i].ind = i;}}/*** @brief 标记被遮挡的点** 根据给定的点云信息，标记被遮挡的点和平行光束点。*/void markOccludedPoints(){int cloudSize = extractedCloud->points.size();// mark occluded points and parallel beam pointsfor (int i = 5; i < cloudSize - 6; ++i){// occluded pointsfloat depth1 = cloudInfo.pointRange[i];float depth2 = cloudInfo.pointRange[i+1];int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));if (columnDiff < 10){// 10 pixel diff in range imageif (depth1 - depth2 > 0.3){cloudNeighborPicked[i - 5] = 1;cloudNeighborPicked[i - 4] = 1;cloudNeighborPicked[i - 3] = 1;cloudNeighborPicked[i - 2] = 1;cloudNeighborPicked[i - 1] = 1;cloudNeighborPicked[i] = 1;}else if (depth2 - depth1 > 0.3){cloudNeighborPicked[i + 1] = 1;cloudNeighborPicked[i + 2] = 1;cloudNeighborPicked[i + 3] = 1;cloudNeighborPicked[i + 4] = 1;cloudNeighborPicked[i + 5] = 1;cloudNeighborPicked[i + 6] = 1;}}// parallel beamfloat diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])cloudNeighborPicked[i] = 1;}}void extractFeatures(){cornerCloud->clear();surfaceCloud->clear();pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>());for (int i = 0; i < N_SCAN; i++){surfaceCloudScan->clear();for (int j = 0; j < 6; j++){int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;if (sp >= ep)continue;std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());int largestPickedNum = 0;for (int k = ep; k >= sp; k--){int ind = cloudSmoothness[k].ind;if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold){largestPickedNum++;if (largestPickedNum <= 20){cloudLabel[ind] = 1;cornerCloud->push_back(extractedCloud->points[ind]);} else {break;}cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++){int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--){int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}for (int k = sp; k <= ep; k++){int ind = cloudSmoothness[k].ind;if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold){cloudLabel[ind] = -1;cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++) {int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}for (int k = sp; k <= ep; k++){if (cloudLabel[k] <= 0){surfaceCloudScan->push_back(extractedCloud->points[k]);}}}surfaceCloudScanDS->clear();downSizeFilter.setInputCloud(surfaceCloudScan);downSizeFilter.filter(*surfaceCloudScanDS);*surfaceCloud += *surfaceCloudScanDS;}}void freeCloudInfoMemory(){cloudInfo.startRingIndex.clear();cloudInfo.endRingIndex.clear();cloudInfo.pointColInd.clear();cloudInfo.pointRange.clear();}void publishFeatureCloud(){// free cloud info memoryfreeCloudInfoMemory();// save newly extracted featurescloudInfo.cloud_corner  = publishCloud(pubCornerPoints,  cornerCloud,  cloudHeader.stamp, lidarFrame);cloudInfo.cloud_surface = publishCloud(pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);// publish to mapOptimizationpubLaserCloudInfo.publish(cloudInfo);}
};int main(int argc, char** argv)
{ros::init(argc, argv, "lio_sam");FeatureExtraction FE;ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");ros::spin();return 0;
}

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

如若内容造成侵权/违法违规/事实不符，请联系一条长河网进行投诉反馈，一经查实，立即删除！

相关文章

nvm及nodejs安装相关

nvm及nodejs安装相关

安装 1.清空文件夹&#xff0c;卸载nvm及nodejs 2.下载安装包 https://github.com/coreybutler/nvm-windows/releases &#xff08;也下载有&#xff09; 3.安装nvm 地址写D:/nvm和D:/nodejs 4.安装nodejs nvm ls available //查询版本 nvm install 16.20.2 //安装对应版…
阅读更多...
【H2O2|全栈】关于HTML（1）认识HTML

【H2O2|全栈】关于HTML（1）认识HTML

HTML相关知识 目录 前言 准备工作 WEB前端是什么&#xff1f; HTML是什么&#xff1f; 如何运行HTML文件&#xff1f; 标签 概念 分类 双标签和单标签 行内标签和块标签 HTML文档结构 预告和回顾 UI设计相关 Markdown | Md文档相关 项目合作管理相关 后话 前…
阅读更多...
idea快捷键_idea 2024 复制光标所在行 或 复制选择内容,并把复制内容插入光标位置下面 (必备)

idea快捷键_idea 2024 复制光标所在行 或 复制选择内容,并把复制内容插入光标位置下面 (必备)

Ctrl G 在当前文件跳转到指定行处 Ctrl J 插入自定义动态代码模板 &#xff08;必备&#xff09; Ctrl P 方法参数提示显示 &#xff08;必备&#xff09; Ctrl Q 光标所在的变量 / 类名 / 方法名等上面&#xff08;也可以在提示补充的时候按&#xff09;&#xff0c;显示文…
阅读更多...
基于SpringBoot的校园跑腿系统+LW参考示例

基于SpringBoot的校园跑腿系统+LW参考示例

系列文章目录 1.基于SSM的洗衣房管理系统原生微信小程序LW参考示例 2.基于SpringBoot的宠物摄影网站管理系统LW参考示例 3.基于SpringBootVue的企业人事管理系统LW参考示例 4.基于SSM的高校实验室管理系统LW参考示例 5.基于SpringBoot的二手数码回收系统原生微信小程序LW参考示…
阅读更多...
tabBar设置底部菜单以及选项iconfont图标

tabBar设置底部菜单以及选项iconfont图标

1.tabBar设置底部菜单 在官网里面可以了解到tabBar组件的一些知识和注意点&#xff1a; 需要给页面设置一个底部导航的话可以在pages.json里面设置一个tabBar标签&#xff0c;在其里面设置pagePath和text属性。 可以看的pagePath是跳转的地址&#xff0c;text是下面导航的文字…
阅读更多...
DataLoader使用

DataLoader使用

文章目录 一、认识dataloader二、DataLoader整合数据集三、使用DataLoader展示图片方法四、去除结尾不满足batch_size设值图片的展示 一、认识dataloader DataLoader 用于封装数据集&#xff0c;并提供批量加载数据的迭代器。它支持自动打乱数据、多线程数据加载等功能。datas…
阅读更多...
SpringDataJPA系列(7)Jackson注解在实体中应用

SpringDataJPA系列(7)Jackson注解在实体中应用

SpringDataJPA系列(7)Jackson注解在实体中应用 常用的Jackson注解 Springboot中默认集成的是Jackson&#xff0c;我们可以在jackson依赖包下看到Jackson有多个注解 一般常用的有下面这些&#xff1a; 一个实体的示例 测试方法如下&#xff1a; 按照上述图片中的序号做个简…
阅读更多...
汽车网络安全的未来：将车辆视为端点

汽车网络安全的未来：将车辆视为端点

汽车行业面临着许多与其他行业的成功企业相同的网络安全风险和威胁&#xff0c;但它也在应对一些独特的风险和威胁。 Nuspire 的首席威胁分析师 Josh Smith&#xff08;一家在汽车领域有着深厚根基并保护通用汽车和斯巴鲁等客户的托管安全服务提供商&#xff09;谈到了当前的风…
阅读更多...
多个路由器级联实现子网的方式

多个路由器级联实现子网的方式

好久没写博客啦&#xff0c;最近搬家&#xff0c;换了网络环境&#xff0c;简单记录一下网络配置。 拓扑图就不画了&#xff0c;光猫 - > 华为TC7102路由 -> 华为AX2 Pro路由 -> 各种设备&#xff0c;简单表示就是这样。 原因是第一个路由是房东的&#xff0c;我希望自…
阅读更多...
Lombok jar包引入和用法

Lombok jar包引入和用法

大家好&#xff0c;今天分享一个在编写代码时的快捷方法。 当我们在封装实体类时&#xff0c;会使用set、get等一些方法。如下图&#xff0c;不但费事还影响代码的美观。 那么如何才能减少代码的冗余呢&#xff0c;首先lib中导入lombok的jar包并添加库。 此处我已导入&#xf…
阅读更多...
【软件工程】软件开发模型

【软件工程】软件开发模型

三、瀑布模型 四、几种软件开发模型的主要特点 题目 判断题 选择题 小结
阅读更多...
linux中PATH变量-详细介绍(1)

linux中PATH变量-详细介绍(1)

配置完后可以通过echo $PATH查看配置结果。 生效方法&#xff1a;立即生效 有效期限&#xff1a;临时改变&#xff0c;只能在当前的终端窗口中有效&#xff0c;当前窗口关闭后就会恢复原有的path配置 用户局限&#xff1a;仅对当前用户 2.对用户生效 因为写入到 .bash_prof…
阅读更多...
1014 Waiting in Line

1014 Waiting in Line

链接&#xff1a; 1014 Waiting in Line - PAT (Advanced Level) Practice (pintia.cn) 大致题意&#xff1a; 有 n 个窗口&#xff0c;每个窗口最多能容纳 m 人同时排队。一共有 k 个顾客&#xff0c;他们每个人有一个服务时长 t[i]。顾客们从早上 8 点开始服务。如果一个顾…
阅读更多...
【Python 千题 —— 算法篇】无重复字符最长子段

【Python 千题 —— 算法篇】无重复字符最长子段

Python 千题持续更新中 …… 脑图地址 &#x1f449;&#xff1a;⭐https://twilight-fanyi.gitee.io/mind-map/Python千题.html⭐ 题目背景 在编程过程中&#xff0c;处理字符串的任务时常遇到&#xff0c;其中一个经典问题是查找无重复字符的最长子串。这在很多应用场景中…
阅读更多...
KRTSt内嵌Lua脚本

KRTSt内嵌Lua脚本

KRTSt内嵌Lua脚本 Lua 简介 Lua是一门强大、高效、轻量、可嵌入的脚本语言。它支持多种编程架构&#xff1a;过程编程、面向对象编程&#xff08;OOP&#xff09;、函数式编程、数据驱动编程及数据描述。 Lua结合了简洁的过程语法和强大的数据描述结构&#xff08;基于关联数…
阅读更多...
《Web性能权威指南》-网络技术概览-读书笔记

《Web性能权威指南》-网络技术概览-读书笔记

注&#xff1a;TCP/IP等知识牵涉面太广&#xff0c;且不说本文&#xff0c;哪怕是原书&#xff0c;限于篇幅&#xff0c;很多知识点都是大致介绍下。如果想深入理解&#xff0c;需要更一步Google相关页面资料。 延迟与带宽 WPO&#xff0c;Web Performance Optimization&…
阅读更多...
算法day09 二叉树

算法day09 二叉树

class Node<V>{V value;Node left;Node right; } 一、用递归和非递归分别实现二叉树的前序&#xff0c;中序&#xff0c;后序遍历 非递归方式&#xff1a; 前序遍历 根左右 0&#xff09;利用stack后进先出的特点 要输出根左右的顺序&#xff0c;将元素右边先放入栈中…
阅读更多...
【CanMV K230】圆形检测

【CanMV K230】圆形检测

【CanMV K230】圆形检测 什么是圆形检测圆形检测应用领域1.工业自动化2.机器人视觉3.医学图像分析4.目标识别5.质量检测6.研究和开发 K230应用相关函数官方例程HDMI屏幕使用圆形检测 本篇内容&#xff1a; 什么是圆形检测圆形检测应用领域K230应用&#xff08;包含相应函数及例…
阅读更多...
SAP学习笔记 - 开发03 - CDSView开发环境搭建，Eclipse中连接SAP，CDSView创建

SAP学习笔记 - 开发03 - CDSView开发环境搭建，Eclipse中连接SAP，CDSView创建

上一章讲了BTP的账号创建&#xff0c;环境搭建等内容。 SAP学习笔记 - 开发02 - BTP实操流程&#xff08;账号注册&#xff0c;BTP控制台&#xff0c;BTP集成开发环境搭建&#xff09;-CSDN博客 本章继续讲SAP开发。 - CDSView 的开发环境&#xff08;Eclipse&#xff09;搭建…
阅读更多...
C++初阶：STL详解（一）——string类

C++初阶：STL详解（一）——string类

✨✨小新课堂开课了&#xff0c;欢迎欢迎~✨✨ &#x1f388;&#x1f388;养成好习惯&#xff0c;先赞后看哦~&#x1f388;&#x1f388; 所属专栏&#xff1a;C&#xff1a;由浅入深篇 小新的主页&#xff1a;编程版小新-CSDN博客 1.为什么会有string类 C 语言中&#xff0c…
阅读更多...
最新文章

Copyright @ 2022~2023