本文阅读的代码为2020年11月1日下载的github的最新master。
如果代码后续更新了请以github为准。

1 main()

int main(int argc, char** argv)
{ros::init(argc, argv, "lio_sam");FeatureExtraction FE;// \033[1;32m，\033[0m 终端显示成绿色ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");ros::spin();    // 一个线程return 0;
}

2 FeatureExtraction类

2.1 成员变量

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;               // 发布topic时的时间戳std::vector<smoothness_t> cloudSmoothness;  // 存储每个点的曲率与索引float *cloudCurvature;          // 存储每个点的曲率int *cloudNeighborPicked;       // 不进行特征提取的点的索引int *cloudLabel;                // 标记面点的索引
};

2.2 构造函数

    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);// 降采样的参数 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 = new float[N_SCAN*Horizon_SCAN];cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];cloudLabel = new int[N_SCAN*Horizon_SCAN];}

2.3 消息的回调

    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 extraction// 计算每个点的曲率calculateSmoothness();// 标记遮挡和平行点markOccludedPoints();// 提取surface和corner特征extractFeatures();// 发布特征点云publishFeatureCloud();}

2.4 calculateSmoothness()

    // 11个点距离的偏离程度作为曲率void calculateSmoothness(){int cloudSize = extractedCloud->points.size();for (int i = 5; i < cloudSize - 5; i++){// 如果是球面，则当前点周围的10个点的距离之和　减去　当前点距离的10倍　应该等于0float 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;}}

2.5 markOccludedPoints()

    // 标记遮挡点与平行点，不进行特征提取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]));// 相邻两点如果列索引太小，则这个点周围的点不进行特征提取// 平行线和遮挡的判断参考LOAMif (columnDiff < 10){// 10 pixel diff in range image// 如果相邻两点距离大于0.3，选出6个点if (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 beam// 平行线的情况,根据左右两点与该点的深度差,确定该点是否会被选择为特征点float 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;}}

2.6 extractFeatures()

    // 进行角点与面点的提取，角点直接保存，面点要经过降采样之后再保存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();// 每根线分成6部分for (int j = 0; j < 6; j++){// 从startRingIndex到endRingIndex,分成6分// 第一份的索引就是 startRingIndex* (6-j)/6 + endRingInde * j/6int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;// ep 就是sp的下一个循环的值的前一个索引，ep[j] = sp[j+1] - 1int 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--){// 最后的点 的曲率最大，如果满足条件，就是角点// edgeThreshold为0.1，正圆的曲率为0int ind = cloudSmoothness[k].ind;if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold){// 每一段最多只取20个角点largestPickedNum++;if (largestPickedNum <= 20){cloudLabel[ind] = 1; // 都是角点了肯定不是面点cornerCloud->push_back(extractedCloud->points[ind]);} else {break;}// 防止特征点聚集，将ind及其前后各5个点标记，不做特征点提取cloudNeighborPicked[ind] = 1;for (int l = 1; l <= 5; l++){// 每个点index之间的差值。附近点都是有效点的情况下，相邻点间的索引只差１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){// 标记面点的索引的值为-1cloudLabel[ind] = -1;// 这个点及前后各5个点不再进行提取特征，防止平面点聚集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;}}}// 面点临时保存在surfaceCloudScanfor (int k = sp; k <= ep; k++){if (cloudLabel[k] <= 0){surfaceCloudScan->push_back(extractedCloud->points[k]);}}} // for// 对面点进行降采样，结果临时保存在 surfaceCloudScanDSsurfaceCloudScanDS->clear();downSizeFilter.setInputCloud(surfaceCloudScan);downSizeFilter.filter(*surfaceCloudScanDS);// 将临时变量中的值放入surfaceCloud*surfaceCloud += *surfaceCloudScanDS;} // for }

2.7 publishFeatureCloud()

    // 发布 lio_sam/feature/cloud_infovoid 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);}

2.8 freeCloudInfoMemory()

    // free cloud info memoryvoid freeCloudInfoMemory(){cloudInfo.startRingIndex.clear();cloudInfo.endRingIndex.clear();cloudInfo.pointColInd.clear();cloudInfo.pointRange.clear();}

总结

这个类只是接收ImageProjection类发出的点云数据，进行特征点提取，提取出了角点与平面点，并且对平面点进行了降采样之后再保存。

提取特征点的策略：

首先计算每个点的曲率，曲率是通过这个点与其前后各5个点距离值的偏离程度算出来的，正圆情况下曲率为0。
之后，进行一些遮挡点与平面点的标记
之后，对每条线分成6个部分，对每个部分的曲率值进行排序，曲率大的满足要求的为角点，曲率小的满足要求的是平面点，角点直接保存，面点进行降采样之后再保存。

REFERENCES

https://github.com/JokerJohn/opensource_slam_noted/blob/master/LIO-SAM-noted/src/featureExtraction.cpp#L74
开源SLAM系统：LIO-SAM源码解析：http://xchu.net/2020/08/19/51liosam/
LIO_SAM实测运行，论文学习及代码注释[附对应google driver数据]：https://blog.csdn.net/unlimitedai/article/details/107378759#t1

LIO-SAM探秘第三章之代码解析（二） --- featureExtraction.cpp相关推荐

python爬虫实战之旅（第三章：数据解析（xpath法））
上接:第三章:数据解析(bs4法) 下接:第四章:验证码识别 1.xpath解析简介最常用且最便捷高效的一种解析方式.通用性很好 xpath解析原理实例化一个etree的对象,且需要将被解析的页面 ...
python爬虫实战之旅（第三章：数据解析（bs4法））
上接:第三章:数据解析(正则法) 下接:第三章:数据解析(xpath法) 1.数据解析步骤标签定位提取标签,标签属性中存储的数据值 2.bs4数据解析的原理实例化一个BeautifulSoup对 ...
计算机组成原理译码器选择,计算机组成原理第三章习题参考解析.doc
计算机组成原理第三章习题参考解析第3章习题参考答案 1.设有一个具有20位地址和32位字长的存储器,问 (1) 该存储器能存储多少字节的信息? (2) 如果存储器由512K×8位SRAM芯片组成,需 ...
Kali Linux 网络扫描秘籍第三章端口扫描（二）
第三章端口扫描(二) 作者:Justin Hutchens 译者:飞龙协议:CC BY-NC-SA 4.0 3.6 Scapy 隐秘扫描执行 TCP 端口扫描的一种方式就是执行一部分.目标端口上 ...
第三章：数据解析---聚焦爬虫
文章目录第三章:数据解析---聚焦爬虫注:本页示例所用的test.html文档一.编码流程二.数据解析分类三.数据解析原理概述四.bs4要点 1.bs4数据解析原理: 2.相关属性: 五. ...
GRBL三：gcode代码解析
GRBL三:gcode代码解析 1.G00X_Y_Z_ :快速定位指令,_代表具体数值可以同时针对X轴Y轴Z轴移动,只快速定位,不切削加 ...
CodeCombat代码全记录（Python学习利器）--SARVEN沙漠（第三章）代码10
毒气攻击 # 计算所有食人魔的总生命值.def sumHealth(enemies):# 创建一个变量,将它设为0后开始运算totalHealth = 0# 初始化循环索引为0enemyIndex = ...
（数据库系统概论|王珊）第三章关系数据库标准语言SQL-第二、三节：数据定义
文章目录零:有关说明 (1)安装数据库与建表 (2)一些语法说明一:模式的定义和删除(SCHEMA) (1)定义模式 (2)删除模式二:基本表的定义.删除和修改(TABLE) (1)定义基本表 ...
c#面向对象与程序设计第三版第三章例题代码_C#程序设计教程 | 教与学（教学大纲）...
<C#程序设计教程>课程教学大纲执笔人:xxx,xxx,xxx 编写日期:年月一.课程基本信息 1．课程名称:C#程序设计教程 2．课程编号: 3．课程体系/类别: 4．课程性质: ...
UNIX-LINUX编程实践教程-第三章-实例代码注解-ls2
一问题对ls1的功能进行扩展,以达到类似ll命令的功能. 二分析在ls1中,我们利用readdir()函数和dirent结构体来获得目标文件夹内的文件名(dirent->d_name). ...

LIO-SAM探秘第三章之代码解析（二） --- featureExtraction.cpp