该博客详细介绍了点云图像特征提取的过程，包括曲率计算、遮挡点和平行点的判断、角点和面点的提取。通过ROS节点订阅去畸变点云，然后进行一系列处理，最终发布特征点云供其他模块使用。主要函数包括calculateSmoothness()、markOccludedPoints()和extractFeatures()，分别用于计算曲率、标记遮挡和平行点以及提取特征。
这个文件的功能就是提取点云图像的特征，主要是提取角点与面点，订阅经过imageProjection处理发布的去畸变点云，经过特征提取后发布一个特征提取完毕后的点云。
函数的整体流程图如下：
下面进入代码部分：
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;
}main函数还是一如既往的格式。
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();从main函数跳转来到构造函数部分，
可以注意到这个构造函数中，订阅了去畸变的点云地图，同时发布了点云消息以及角点面点，但实际在最终发布点云时，角点和面点还是会整合到点云消息中，然后发布给mapOptimization，角点和面点点云也会发布给rviz进行展示用途。
构造函数末尾有一个initializationValue的函数，这个函数初始化了一些值，不涉及具体的功能实现，在这里就不展开叙述了。
下面跳转到订阅话题中的laserCloudInfoHandler回调函数：
cloudInfo = *msgIn; // new cloud info
cloudHeader = msgIn->header; // new cloud header
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction
calculateSmoothness();
markOccludedPoints();
extractFeatures();
publishFeatureCloud();函数首先将去畸变后的点云进行了格式转换，将其转换为了pcl格式，然后调用了四个函数对这个数据进行处理，首先是calculateSmoothness()函数，这个函数主要用于计算点云中每个点的曲率，为下面的角点判断做一个铺垫；markOccludedPoints()函数则标记遮挡点和平行点，这个两种情况在特征提取时要被排除，防止生成错误的角点面点。extractFeatures()函数将角点面点提取，进行一些降采样操作。最后带特征点信息的点云地图消息经由publishFeatureCloud()函数发布出去。
先看一下calculateSmoothness()这个曲率计算函数：
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;这里作者在for中去掉了点云前后五个点，是因为在下面的计算中，是需要当前点前后五个点的数据来计算曲率，在计算曲率时，将当前点前后五个点，共十个点加起来减去十倍的当前点，利用这个距离差值来计算曲率值。
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
// cloudSmoothness for sorting
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;这四段代码前两句是给点做上标记，标记点是否被选择以及给点打上标签，这里因为还没进行到角点面点环节所以先给赋个0初值过后再议。
后两句将曲率及点的索引保存起来，在后面函数中使用。
曲率计算完毕后，我们来看一下判断遮挡平行的函数markOccludedPoints函数：
在正式判断点的流程中，首先通过一个if语句判断一下两个点是否是相邻的。之所以要这样进行判断的原因是，在雷达扫描的时候两个障碍物之间的空隙可能为无限远（超出雷达扫描范围），扫描的为NAN空值，这样在前面对点云处理时会去除这些无效点，这样一来两个原本中间有空隙的障碍物上的点云在一维索引中就表现为相邻的点，这一步就是将这种情况排除，保证比较的点是真正的相邻点。
特征点选取中，需要剔除掉被遮挡点和平行点的原因是，被遮挡点在雷达下瞬时运动过程中就可能因为遮挡原因消失，无法提供参考。而与雷达扫描线平面近乎平行的面上的点，雷达每次微小移动都会造成该点的大幅变化或者直接消失，因此也不适合用作参考。
对于遮挡点的处理，程序是将被遮挡点的相邻五个点去掉，如图：
B为遮挡点，A为被遮挡点，在图中情况下，雷达左右移动时，向左运动时AB两点都不会有所变化，但雷达向右图示运动时，A点会被B点遮挡从而消失，同时A点左侧i-5至i个点也有可能会被遮挡，因此在程序中去掉了容易被遮挡的五个点。同样的，如果将AB障碍物左右对调，前遮挡物B位于左侧，便会去掉被遮挡点A及右侧五个点（i至i+5）。
而平行点：
图示可以看出与雷达扫描线近乎平行的的面上扫描的点并不稳定，并且图示情况雷达向左运动时点还会发生丢失情况。
在进行完曲率计算与去除不稳定的点这两步工作后便可以开始进行特征提取工作了，这项工作在函数extractFeatures函数中进行。
cornerCloud->clear();
surfaceCloud->clear();首先清除了角点和面点的点云。
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;
}
下面是一层套一层的for循环，具体的实现形式这里就不展开了，按顺序说一下这个有些复杂的循环函数的功能吧。
首先是最外层的for循环（for (int i = 0; i < N_SCAN; i++)），这是一个遍历scan的循环，也就是说内部的这些函数要对所有的scan线都执行一遍。下面进入这个for循环内部。
在内部遇到的第一个for循环（for (int j = 0; j < 6; j++)）是把每根scan分为六分，然后用for函数将曲率进行排序，开始提取角点（for (int k = ep; k >= sp; k--)）判断一下该点是不是遮挡点或者平行点，而且曲率要满足要求，将其标记为1（角点），每段角点数不大于20，然后储存进角点点云。
然后将角点的周围点处理一下，设置为遮挡点，防止密密麻麻一片角点。
然后提取面点（for (int k = sp; k <= ep; k++)），同样判断是不是遮挡点以及平行点，然后判断曲率小于阈值。标记为-1（面点），同样防止密集将点周围设为遮挡点。最后储存进面点点云for (int k = sp; k <= ep; k++)。这里的if判断的方式是采用了<=0，也就是说吧非角点的点全部认为是面点了。
在最外层for函数的最后，面点还是太多，做了一个面点的降采样。特征提取部分到此结束。
最后的publishFeatureCloud函数先是清空了部分参数，然后将完成角点面点特征提取的点云进行了发布。这样整个的特征提取功能完成。
扫一扫
964
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
