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点云指针,提取、角点、面
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`bLaserCloudInfo = 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.2
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];
}
//将订阅的点云信息传到cloudInfo
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
cloudInfo = *msgIn; // new cloud info
cloudHeader = msgIn->header; // new cloud header
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction,转成ros信息点云
//计算曲率
calculateSmoothness();
//标记遮挡和平行点
markOccludedPoints();
//提取角点和面特帧
extractFeatures();
//发布特征点云
publishFeatureCloud();
}
//计算曲率 10个点
void calculateSmoothness()
{
int cloudSize = extractedCloud->points.size();
//序号从第五个开始,前后五个,共10个
for (int i = 5; i < cloudSize - 5; i++)
{ //但前点与附近10点的距离差和
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 sorting
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
}
}
//标记不提取的点云特征,遮挡平行点
void markOccludedPoints()
{
int cloudSize = extractedCloud->points.size();
// mark occluded points and parallel beam points
for (int i = 5; i < cloudSize - 6; ++i)//与后一个点插值,所以减去6
{
// occluded points
float 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 image范围图像中的像素差异
//对相邻两帧进行测距,附近前后五帧的也受当前评测影响
//在一定范围内 ,设为1,不再考虑成为特征点
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]));
//与前后点的距离大于深度的百分之2,视为远离群点、斜坡点、凹凸面点和空旷区域点,设置为筛选过、弃用
//因为噪声点会影响与该障碍物平面的匹配
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();
//六次循环,每次都要找2个角点(平滑差)和四个面(平滑好)
for (int j = 0; j < 6; j++)
{
//startRingIndex从4到20 共16次
//假如起始0,终点是360度,将一帧云平均分乘6份,每一份60度
//ep是end_index sp 是start_index 如:startRingIndex[i] =1 endRingIndex[i]=60
//sp1 : 1 sp2 : 11 sp3 : 21 sp6 : 51
//ep1 : 10 ep2 : 20 ep3 : 30........ep6 : 60
//最后的点的曲率最大,如果满足条件就是角点 (6-j)/6 j/6
int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
/// (5 - j)/6 (j + 1)/6 -1
int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
if (sp >= ep)
continue;
//cloudSmoothness.包括曲率和序号,将曲率从小到达排序
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;
// edgeThreshold: 1.0
// surfThreshold: 0.1
// 可提取特帧 曲率
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)//边缘
{
largestPickedNum++;
//每一段最多选取20个点
if (largestPickedNum <= 20){
cloudLabel[ind] = 1;//因为是角点所以不是面 ,标记为1指的角点,-1指的是面
cornerCloud->push_back(extractedCloud->points[ind]);
} else {
break;
}
//已经标记了角点,就不再重复标记
cloudNeighborPicked[ind] = 1;
//同时避免了特征点聚集,所以对前后的五个角点不再提取特征
for (int l = 1; l <= 5; l++)//前5
{ //一帧云与光心距离
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--)//后5
{
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]);
}
}
}
//对面进行pcl库降采样,临时保存在surfaceCloudScanDS
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 memory
freeCloudInfoMemory();
// save newly extracted features
cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);
cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
// publish to mapOptimization
pubLaserCloudInfo.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;
}
参考博文:
https://bbs.csdn.net/topics/398260808
https://zhuanlan.zhihu.com/p/182408931
https://zhuanlan.zhihu.com/p/57351961
https://zhuanlan.zhihu.com/p/111388877