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

本文主要是介绍【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,用于接收点云数据。
      • 发布器 pubLaserCloudInfopubCornerPointspubSurfacePoints,分别用于发布处理后的点云信息、角点特征和面点特征。
      • 点云指针 extractedCloudcornerCloudsurfaceCloud,用于保存原始提取点云和特征点云。
      • cloudCurvaturecloudNeighborPickedcloudLabel,这些数组用于存储每个点的曲率、是否被选中、点的分类标签。
  • 构造函数 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;
}

这篇关于【LVI-SAM】激光雷达点云处理特征提取LIO-SAM 之FeatureExtraction实现细节的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



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

相关文章

无人叉车3d激光slam多房间建图定位异常处理方案-墙体画线地图切分方案

墙体画线地图切分方案 针对问题:墙体两侧特征混淆误匹配,导致建图和定位偏差,表现为过门跳变、外月台走歪等 ·解决思路:预期的根治方案IGICP需要较长时间完成上线,先使用切分地图的工程化方案,即墙体两侧切分为不同地图,在某一侧只使用该侧地图进行定位 方案思路 切分原理:切分地图基于关键帧位置,而非点云。 理论基础:光照是直线的,一帧点云必定只能照射到墙的一侧,无法同时照到两侧实践考虑:关

hdu1043(八数码问题,广搜 + hash(实现状态压缩) )

利用康拓展开将一个排列映射成一个自然数,然后就变成了普通的广搜题。 #include<iostream>#include<algorithm>#include<string>#include<stack>#include<queue>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#inclu

【C++】_list常用方法解析及模拟实现

相信自己的力量,只要对自己始终保持信心,尽自己最大努力去完成任何事,就算事情最终结果是失败了,努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一:什么是list? •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二:list常用接口 •🌰1.默认成员函数 🔥构造函数(⭐) 🔥析构函数 •🌰2.list对象

【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

✨✨ 欢迎大家来到景天科技苑✨✨ 🎈🎈 养成好习惯,先赞后看哦~🎈🎈 🏆 作者简介:景天科技苑 🏆《头衔》:大厂架构师,华为云开发者社区专家博主,阿里云开发者社区专家博主,CSDN全栈领域优质创作者,掘金优秀博主,51CTO博客专家等。 🏆《博客》:Python全栈,前后端开发,小程序开发,人工智能,js逆向,App逆向,网络系统安全,数据分析,Django,fastapi

让树莓派智能语音助手实现定时提醒功能

最初的时候是想直接在rasa 的chatbot上实现,因为rasa本身是带有remindschedule模块的。不过经过一番折腾后,忽然发现,chatbot上实现的定时,语音助手不一定会有响应。因为,我目前语音助手的代码设置了长时间无应答会结束对话,这样一来,chatbot定时提醒的触发就不会被语音助手获悉。那怎么让语音助手也具有定时提醒功能呢? 我最后选择的方法是用threading.Time

Android实现任意版本设置默认的锁屏壁纸和桌面壁纸(两张壁纸可不一致)

客户有些需求需要设置默认壁纸和锁屏壁纸  在默认情况下 这两个壁纸是相同的  如果需要默认的锁屏壁纸和桌面壁纸不一样 需要额外修改 Android13实现 替换默认桌面壁纸: 将图片文件替换frameworks/base/core/res/res/drawable-nodpi/default_wallpaper.*  (注意不能是bmp格式) 替换默认锁屏壁纸: 将图片资源放入vendo

C#实战|大乐透选号器[6]:实现实时显示已选择的红蓝球数量

哈喽,你好啊,我是雷工。 关于大乐透选号器在前面已经记录了5篇笔记,这是第6篇; 接下来实现实时显示当前选中红球数量,蓝球数量; 以下为练习笔记。 01 效果演示 当选择和取消选择红球或蓝球时,在对应的位置显示实时已选择的红球、蓝球的数量; 02 标签名称 分别设置Label标签名称为:lblRedCount、lblBlueCount

Kubernetes PodSecurityPolicy:PSP能实现的5种主要安全策略

Kubernetes PodSecurityPolicy:PSP能实现的5种主要安全策略 1. 特权模式限制2. 宿主机资源隔离3. 用户和组管理4. 权限提升控制5. SELinux配置 💖The Begin💖点点关注,收藏不迷路💖 Kubernetes的PodSecurityPolicy(PSP)是一个关键的安全特性,它在Pod创建之前实施安全策略,确保P

【生成模型系列(初级)】嵌入(Embedding)方程——自然语言处理的数学灵魂【通俗理解】

【通俗理解】嵌入(Embedding)方程——自然语言处理的数学灵魂 关键词提炼 #嵌入方程 #自然语言处理 #词向量 #机器学习 #神经网络 #向量空间模型 #Siri #Google翻译 #AlexNet 第一节:嵌入方程的类比与核心概念【尽可能通俗】 嵌入方程可以被看作是自然语言处理中的“翻译机”,它将文本中的单词或短语转换成计算机能够理解的数学形式,即向量。 正如翻译机将一种语言

2、PF-Net点云补全

2、PF-Net 点云补全 PF-Net论文链接:PF-Net PF-Net (Point Fractal Network for 3D Point Cloud Completion)是一种专门为三维点云补全设计的深度学习模型。点云补全实际上和图片补全是一个逻辑,都是采用GAN模型的思想来进行补全,在图片补全中,将部分像素点删除并且标记,然后卷积特征提取预测、判别器判别,来训练模型,生成的像