LIOSLAM代码详细注释二featureExtraction.cpp

featureExtraction.cpp

#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点云指针&＃xff0c;提取、角点、面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; //发布topic的时间戳std::vector<smoothness_t> cloudSmoothness; //存储每个点的曲率和索引号float *cloudCurvature; //存储每个点的曲率int *cloudNeighborPicked; //不进行特征提取的索引int *cloudLabel; //标记面的索引FeatureExtraction(){su&＃96;bLaserCloudInfo &＃61; nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1,&FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());pubLaserCloudInfo &＃61; nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 1);pubCornerPoints &＃61; nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_corner", 1);pubSurfacePoints &＃61; nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_surface", 1);initializationValue();}void initializationValue(){cloudSmoothness.resize(N_SCAN*Horizon_SCAN);//降采样参数0.2downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize);extractedCloud.reset(new pcl::PointCloud<PointType>());cornerCloud.reset(new pcl::PointCloud<PointType>());surfaceCloud.reset(new pcl::PointCloud<PointType>());cloudCurvature &＃61; new float[N_SCAN*Horizon_SCAN];cloudNeighborPicked &＃61; new int[N_SCAN*Horizon_SCAN];cloudLabel &＃61; new int[N_SCAN*Horizon_SCAN];}
//将订阅的点云信息传到cloudInfo void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn){cloudInfo &＃61; *msgIn; // new cloud infocloudHeader &＃61; msgIn->header; // new cloud headerpcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction&＃xff0c;转成ros信息点云//计算曲率calculateSmoothness();//标记遮挡和平行点markOccludedPoints();//提取角点和面特帧extractFeatures();//发布特征点云publishFeatureCloud();}
//计算曲率 10个点void calculateSmoothness(){int cloudSize &＃61; extractedCloud->points.size();//序号从第五个开始&＃xff0c;前后五个&＃xff0c;共10个for (int i &＃61; 5; i < cloudSize - 5; i&＃43;&＃43;){ //但前点与附近10点的距离差和float diffRange &＃61; cloudInfo.pointRange[i-5] &＃43; cloudInfo.pointRange[i-4]&＃43; cloudInfo.pointRange[i-3] &＃43; cloudInfo.pointRange[i-2]&＃43; cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10&＃43; cloudInfo.pointRange[i&＃43;1] &＃43; cloudInfo.pointRange[i&＃43;2]&＃43; cloudInfo.pointRange[i&＃43;3] &＃43; cloudInfo.pointRange[i&＃43;4]&＃43; cloudInfo.pointRange[i&＃43;5]; cloudCurvature[i] &＃61; diffRange*diffRange;//diffX * diffX &＃43; diffY * diffY &＃43; diffZ * diffZ;cloudNeighborPicked[i] &＃61; 0;cloudLabel[i] &＃61; 0;// cloudSmoothness for sortingcloudSmoothness[i].value &＃61; cloudCurvature[i];cloudSmoothness[i].ind &＃61; i;}}
//标记不提取的点云特征&＃xff0c;遮挡平行点void markOccludedPoints(){int cloudSize &＃61; extractedCloud->points.size();// mark occluded points and parallel beam pointsfor (int i &＃61; 5; i < cloudSize - 6; &＃43;&＃43;i)//与后一个点插值&＃xff0c;所以减去6{// occluded pointsfloat depth1 &＃61; cloudInfo.pointRange[i];float depth2 &＃61; cloudInfo.pointRange[i&＃43;1];//列索引间的距离int columnDiff &＃61; std::abs(int(cloudInfo.pointColInd[i&＃43;1] - cloudInfo.pointColInd[i]));if (columnDiff < 10){// 10 pixel diff in range image范围图像中的像素差异 //对相邻两帧进行测距&＃xff0c;附近前后五帧的也受当前评测影响//在一定范围内 &＃xff0c;设为1,不再考虑成为特征点if (depth1 - depth2 > 0.3){cloudNeighborPicked[i - 5] &＃61; 1;cloudNeighborPicked[i - 4] &＃61; 1;cloudNeighborPicked[i - 3] &＃61; 1;cloudNeighborPicked[i - 2] &＃61; 1;cloudNeighborPicked[i - 1] &＃61; 1;cloudNeighborPicked[i] &＃61; 1;}else if (depth2 - depth1 > 0.3){cloudNeighborPicked[i &＃43; 1] &＃61; 1;cloudNeighborPicked[i &＃43; 2] &＃61; 1;cloudNeighborPicked[i &＃43; 3] &＃61; 1;cloudNeighborPicked[i &＃43; 4] &＃61; 1;cloudNeighborPicked[i &＃43; 5] &＃61; 1;cloudNeighborPicked[i &＃43; 6] &＃61; 1;}}// parallel beam平行线的 情况&＃xff0c;左右两点与该点的深度差&＃xff0c;确定该点是否会被选择成为特征点float diff1 &＃61; std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));float diff2 &＃61; std::abs(float(cloudInfo.pointRange[i&＃43;1] - cloudInfo.pointRange[i]));//与前后点的距离大于深度的百分之2,视为远离群点、斜坡点、凹凸面点和空旷区域点&＃xff0c;设置为筛选过、弃用//因为噪声点会影响与该障碍物平面的匹配if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])cloudNeighborPicked[i] &＃61; 1;}}
//进行角点与面点的提取&＃xff0c;角点直接保存&＃xff0c;面需要经过降采样之后保存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 &＃61; 0; i < N_SCAN; i&＃43;&＃43;){surfaceCloudScan->clear();//六次循环&＃xff0c;每次都要找2个角点&＃xff08;平滑差&＃xff09;和四个面&＃xff08;平滑好&＃xff09;for (int j &＃61; 0; j < 6; j&＃43;&＃43;){//startRingIndex从4到20 共16次//假如起始0,终点是360度&＃xff0c;将一帧云平均分乘6份&＃xff0c;每一份60度//ep是end_index sp 是start_index 如&＃xff1a;startRingIndex[i] &＃61;1 endRingIndex[i]&＃61;60//sp1 : 1 sp2 : 11 sp3 : 21 sp6 : 51//ep1 : 10 ep2 : 20 ep3 : 30........ep6 : 60//最后的点的曲率最大&＃xff0c;如果满足条件就是角点 &＃xff08;6-j&＃xff09;/6 j/6int sp &＃61; (cloudInfo.startRingIndex[i] * (6 - j) &＃43; cloudInfo.endRingIndex[i] * j) / 6;/// (5 - j)/6 &＃xff08;j &＃43; 1)/6 -1int ep &＃61; (cloudInfo.startRingIndex[i] * (5 - j) &＃43; cloudInfo.endRingIndex[i] * (j &＃43; 1)) / 6 - 1;if (sp >&＃61; ep)continue;//cloudSmoothness.包括曲率和序号&＃xff0c;将曲率从小到达排序std::sort(cloudSmoothness.begin()&＃43;sp, cloudSmoothness.begin()&＃43;ep, by_value());int largestPickedNum &＃61; 0;//从后往前读取&＃xff0c;越前面曲率越低越不满足边缘要求//进行角点提取for (int k &＃61; ep; k >&＃61; sp; k--){int ind &＃61; cloudSmoothness[k].ind; // edgeThreshold: 1.0// surfThreshold: 0.1// 可提取特帧 曲率 if (cloudNeighborPicked[ind] &＃61;&＃61; 0 && cloudCurvature[ind] > edgeThreshold)//边缘{largestPickedNum&＃43;&＃43;;//每一段最多选取20个点if (largestPickedNum <&＃61; 20){cloudLabel[ind] &＃61; 1;//因为是角点所以不是面 &＃xff0c;标记为1指的角点&＃xff0c;-1指的是面cornerCloud->push_back(extractedCloud->points[ind]);} else {break;}//已经标记了角点&＃xff0c;就不再重复标记cloudNeighborPicked[ind] &＃61; 1;//同时避免了特征点聚集&＃xff0c;所以对前后的五个角点不再提取特征for (int l &＃61; 1; l <&＃61; 5; l&＃43;&＃43;)//前5{ //一帧云与光心距离int columnDiff &＃61; std::abs(int(cloudInfo.pointColInd[ind &＃43; l] - cloudInfo.pointColInd[ind &＃43; l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind &＃43; l] &＃61; 1;}for (int l &＃61; -1; l >&＃61; -5; l--)//后5{int columnDiff &＃61; std::abs(int(cloudInfo.pointColInd[ind &＃43; l] - cloudInfo.pointColInd[ind &＃43; l &＃43; 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind &＃43; l] &＃61; 1;}}}//进行面提取for (int k &＃61; sp; k <&＃61; ep; k&＃43;&＃43;){int ind &＃61; cloudSmoothness[k].ind;if (cloudNeighborPicked[ind] &＃61;&＃61; 0 && cloudCurvature[ind] < surfThreshold){cloudLabel[ind] &＃61; -1; //标记面特征cloudNeighborPicked[ind] &＃61; 1; //不再重复提取特征for (int l &＃61; 1; l <&＃61; 5; l&＃43;&＃43;) {int columnDiff &＃61; std::abs(int(cloudInfo.pointColInd[ind &＃43; l] - cloudInfo.pointColInd[ind &＃43; l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind &＃43; l] &＃61; 1;}for (int l &＃61; -1; l >&＃61; -5; l--) {int columnDiff &＃61; std::abs(int(cloudInfo.pointColInd[ind &＃43; l] - cloudInfo.pointColInd[ind &＃43; l &＃43; 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind &＃43; l] &＃61; 1;}}}//对面进行保存for (int k &＃61; sp; k <&＃61; ep; k&＃43;&＃43;){if (cloudLabel[k] <&＃61; 0){surfaceCloudScan->push_back(extractedCloud->points[k]);}}}//对面进行pcl库降采样&＃xff0c;临时保存在surfaceCloudScanDSsurfaceCloudScanDS->clear();downSizeFilter.setInputCloud(surfaceCloudScan);downSizeFilter.filter(*surfaceCloudScanDS);//保存临时值*surfaceCloud &＃43;&＃61; *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 &＃61; publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);cloudInfo.cloud_surface &＃61; 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;
}


参考博文&＃xff1a;
https://bbs.csdn.net/topics/398260808
https://zhuanlan.zhihu.com/p/182408931
https://zhuanlan.zhihu.com/p/57351961
https://zhuanlan.zhihu.com/p/111388877