住房和城乡建设网站 上海国内大型网站域名

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

住房和城乡建设网站 上海,国内大型网站域名,网站建设企业邮箱制作网站,做网站的用什么主机好激光雷达点云处理特征提取LIO-SAM 之FeatureExtraction实现细节 1. 特征提取实现过程总结1.0 特征提取过程小结1.1 类 FeatureExtraction 的整体结构与作用1.2 详细特征提取的过程1. 平滑度计算#xff08;calculateSmoothness()#xff09;2. 标记遮挡点#xff08;markOcc… 激光雷达点云处理特征提取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激光雷达点云数据的特征提取用于SLAMSimultaneous Localization and Mapping系统中的前端处理。特征提取的目的是从点云中识别出角点和平面点面点为后续的位姿估计和地图构建提供关键特征点。
1.0 特征提取过程小结 这段代码的主要目的是从LiDAR点云中提取出角点边缘和面点平面以便用于SLAM系统中。整个流程涉及 平滑度计算通过计算每个点的平滑度来区分平滑点和突变点。遮挡点标记通过深度差和像素间距来标记被遮挡的点和平行光束点。特征提取根据曲率值提取角点和面点分别用于位姿估计和地图构建。降采样和发布通过降采样减少数据冗余最终发布处理后的特征点云。 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 中以便后续进行排序。

1. 标记遮挡点markOccludedPoints() 该函数标记被遮挡的点以及光束平行的点以避免它们影响特征提取。主要逻辑如下 遮挡点 遍历每个点比较该点与相邻点的深度差即距离差。如果相邻两个点的列索引差小于 10表示在深度图像中的像素间距较小且深度差大于 0.3则认为是遮挡点并标记为已处理cloudNeighborPicked[i] 1即这些点将不会被选为特征点。 平行光束 如果前后点与当前点的距离差大于一定比例0.02 * cloudInfo.pointRange[i]则认为它们是平行光束这种情况下这些点也会被标记为已处理。
   
2. 特征提取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: 1004. 发布特征点云publishFeatureCloud() 在提取完角点和面点之后该函数将处理后的点云数据发布出去用于后续的地图优化和位姿估计。 2.0 特征提取数学推倒过程 数学推倒 3.0 FeatureExtraction Code #include utility.h #include lio_sam/cloud_info.hstruct 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::PointCloudPointType::Ptr extractedCloud;pcl::PointCloudPointType::Ptr cornerCloud;pcl::PointCloudPointType::Ptr surfaceCloud;pcl::VoxelGridPointType downSizeFilter;lio_sam::cloud_info cloudInfo;std_msgs::Header cloudHeader;std::vectorsmoothness_t cloudSmoothness;float *cloudCurvature;int *cloudNeighborPicked;int *cloudLabel;FeatureExtraction(){subLaserCloudInfo nh.subscribelio_sam::cloud_info(lio_sam/deskew/cloud_info, 1, FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());pubLaserCloudInfo nh.advertiselio_sam::cloud_info (lio_sam/feature/cloud_info, 1);pubCornerPoints nh.advertisesensor_msgs::PointCloud2(lio_sam/feature/cloud_corner, 1);pubSurfacePoints nh.advertisesensor_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::PointCloudPointType());cornerCloud.reset(new pcl::PointCloudPointType());surfaceCloud.reset(new pcl::PointCloudPointType());cloudCurvature new float[N_SCAN*Horizon_SCAN];cloudNeighborPicked new int[N_SCAN*Horizon_SCAN];cloudLabel new int[N_SCANHorizon_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[i1] cloudInfo.pointRange[i2] cloudInfo.pointRange[i3] cloudInfo.pointRange[i4] cloudInfo.pointRange[i5]; 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[i1];int columnDiff std::abs(int(cloudInfo.pointColInd[i1] - 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[i1] - 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::PointCloudPointType::Ptr surfaceCloudScan(new pcl::PointCloudPointType());pcl::PointCloudPointType::Ptr surfaceCloudScanDS(new pcl::PointCloudPointType());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; }