激光雷达点云处理特征提取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（激光雷达）点云数据的特征提取，用于SLAM（Simultaneous 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 info
cloudHeader = msgIn->header; // new cloud header
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction
calculateSmoothness();
markOccludedPoints();
extractFeatures();
publishFeatureCloud();
}
1. 平滑度计算（calculateSmoothness()）
这个函数计算每个点的平滑度，平滑度的定义是基于该点与其前后(5点)若干点之间的距离变化。具体步骤为：
for循环：- 遍历从第5个点到倒数第5个点，以避免处理边界的点。
- 计算该点前后5个点的距离差的平方和，并将该结果作为该点的曲率（即平滑度值
cloudCurvature[i]）。 - 初始化该点的
cloudNeighborPicked为 0（表示该点还没有被处理过）和cloudLabel为 0（标签，初始为未分类）。 - 将平滑度值和点的索引存储到
cloudSmoothness中，以便后续进行排序。
2. 标记遮挡点（markOccludedPoints()）
该函数标记被遮挡的点以及光束平行的点，以避免它们影响特征提取。主要逻辑如下：
-
遮挡点：
- 遍历每个点，比较该点与相邻点的深度差（即距离差）。
- 如果相邻两个点的列索引差小于 10（表示在深度图像中的像素间距较小），且深度差大于 0.3，则认为是遮挡点并标记为已处理（
cloudNeighborPicked[i] = 1），即这些点将不会被选为特征点。
-
平行光束：
- 如果前后点与当前点的距离差大于一定比例（
0.02 * cloudInfo.pointRange[i]），则认为它们是平行光束，这种情况下这些点也会被标记为已处理。
- 如果前后点与当前点的距离差大于一定比例（
3. 特征提取（extractFeatures()）
这个函数的主要任务是提取角点和面点，并根据曲率值将点云进行分类。主要逻辑如下：
- for 循环1-2：遍历激光雷达的扫描线
N_SCAN（通常是垂直方向上的扫描线数量），每条扫描线都被分为6个区域，逐个区域进行处理。-
for 循环3-4：处理每个区域的点，将该区域按平滑度（即曲率）从大到小排序，然后分成两个部分进行处理：
- 角点提取：
- 从平滑度最高的点开始，如果该点没有被遮挡且曲率值大于阈值
edgeThreshold，则将其标记为角点，并加入角点点云（cornerCloud）。 - 为了避免噪声点的影响，最多提取20个角点，并标记相邻的点为已处理，防止相邻的点被重复选取。
- 从平滑度最高的点开始，如果该点没有被遮挡且曲率值大于阈值
- 面点提取：
- 对于平滑度较低的点，如果曲率小于阈值
surfThreshold，则将其标记为面点，加入面点点云（surfaceCloud）。 - 同样，通过标记相邻点来避免重复选择。
- 对于平滑度较低的点，如果曲率小于阈值
- 角点提取：
-
for 循环5：对于那些没有被标记为角点且曲率较小的点，将它们视为面点。
-
- 降采样：通过
pcl::VoxelGrid对面点进行降采样，减少点云的冗余数据，提升后续处理效率。
# LOAM feature threshold
edgeThreshold: 1.0
surfThreshold: 0.1
edgeFeatureMinValidNum: 10
surfFeatureMinValidNum: 100
4. 发布特征点云（publishFeatureCloud()）
在提取完角点和面点之后，该函数将处理后的点云数据发布出去，用于后续的地图优化和位姿估计。
2.0 特征提取数学推倒过程
数学推倒
3.0 FeatureExtraction Code
#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::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;
std::vector<smoothness_t> cloudSmoothness;
float *cloudCurvature;
int *cloudNeighborPicked;
int *cloudLabel;
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);
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];
}
/**
* @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[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;
}
}
/**
* @brief 标记被遮挡的点
*
* 根据给定的点云信息，标记被遮挡的点和平行光束点。
*/
void markOccludedPoints()
{
int cloudSize = extractedCloud->points.size();
// mark occluded points and parallel beam points
for (int i = 5; i < cloudSize - 6; ++i)
{
// 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
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;
}
}
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();
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 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;
}
