本文档详细介绍了LIO-SAM中基于点云平滑度的特征点提取过程，包括计算曲率、标记无效点和提取角点与面片点。首先，计算每个点的曲率并排序，然后通过标记遮挡点和与激光束平行的点来筛选有效特征。接着，通过曲率阈值选择角点和面片点。最后，对非特征点进行下采样并发布提取的特征点。整个过程为LIO-SAM的定位与建图提供了关键的输入数据。
我们在前述的博客中介绍了LIO-SAM的一些准备工作、点云预处理等内容。之后，整理为cloud_info消息类型进行发布，以供后续特征点提取进行使用。
详细参考：
LIO-SAM代码逐行解读（1）-准备工作
LIO-SAM代码逐行解读（2）-点云预处理
接下来，我们进行后续的工作，根据点的平滑度来进行特征点提取，包括面片点与角点。
这一部分还是比较简单明了的，总体来说就是先计算平滑度，接着根据平滑度的值排序，选取其中的面片点与角点。
准备工作
- 引用头文件
// 引用自定义的函数
#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;
}
};
- FeatureExtraction类中变量定义
// 定义特征提取类，继承一个参数服务器
class FeatureExtraction : public ParamServer
{
public:
// 接听CloudInfo信息
ros::Subscriber subLaserCloudInfo;
// 发布LaserCloudInfo信息，角点与面片点
ros::Publisher pubLaserCloudInfo;
ros::Publisher pubCornerPoints;
ros::Publisher pubSurfacePoints;
// 提取的点云，角点与面片点 PointType = pcl::PointXYZI
pcl::PointCloud<PointType>::Ptr extractedCloud;
pcl::PointCloud<PointType>::Ptr cornerCloud;
pcl::PointCloud<PointType>::Ptr surfaceCloud;
// 下采样
pcl::VoxelGrid<PointType> downSizeFilter;
// cloudInfo变量
lio_sam::cloud_info cloudInfo;
std_msgs::Header cloudHeader;
// 平滑度变量
std::vector<smoothness_t> cloudSmoothness;
float *cloudCurvature;
int *cloudNeighborPicked; // 近邻点是否已经被标记为特征点
int *cloudLabel; // 点的类型
- 构造函数
FeatureExtraction()
{
// 接听cloud_info消息（去除畸变处理后）
subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
// 发布特征提取后的cloud_info消息 角点与平面点
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 主要的处理程序部分
* 1、订阅imageProjection节点传入的cloud_info信息，得到header与点云数据（转换成PCL格式）
* 2、
* @param msgIn
*/
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
cloudInfo = *msgIn; // new cloud info
cloudHeader = msgIn->header; // new cloud header
// 把提取出来的有效的点转成pcl的格式
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction
// 针对上一个节点提取出的有效点（extractedCloud），计算曲率，
calculateSmoothness();
// 标记被遮挡的点 与 与激光束平行的点
markOccludedPoints();
// 提取特征点
extractFeatures();
publishFeatureCloud();
}
- 计算每个激光点的曲率（平滑度）值
/**
* @brief 计算曲率
* 计算每个点的曲率并存放在cloudCurvature与cloudSmoothness变量中；
* 初始化近邻点是否被选中成为标记点、当前点类别标记等标志位变量
*/
void calculateSmoothness()
{
int cloudSize = extractedCloud->points.size();
for (int i = 5; i < cloudSize - 5; i++)
{
// 计算当前点和周围十个点的距离差 用的距离，而不是x,y,z
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;
}
}
- 标记两种类型的无效点（被遮挡的点、与激光束几乎平行的点）
判断方式
1）被遮挡的点：两个列号接近的点（列号相差10），深度距离差值比较大（大于0.3m），则认为距离远的点被遮挡。
2) 与激光束平行的点：如果一个点与其前后两个点之间的距离差值较大（大于0.02*当前点到激光雷达中心距离）则认为其余激光束平行。
/**
* @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];
// 计算两个有效点之间的列id差 （距离图像中的列号）
int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
// 只有比较靠近才有意义 （被遮挡）
if (columnDiff < 10){
// 10 pixel diff in range image
// 这样depth1容易被遮挡，因此其之前的5个点走设置为无效点
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;
}
}
- 提取角点与面片点
/**
* @brief 提取特征
* 1、遍历所有的扫描线，每一扫描线划分为6份
* 2、提取其中的角点与面片点，其中角点存放在cornerCloud变量中
* 3、除了角点之外的剩余点都放在surfaceCloudScan变量中，进行下采样，放在面片点变量surfaceCloud中
*/
void extractFeatures()
{
// 存储角点 面片点 两类特征点
cornerCloud->clear();
surfaceCloud->clear();
// 平面点 平面点下采样（非特征点，除了前面标记的角点点之外的所有点都放在了这个集合中）
pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>());
// 进行下采样，下采样后的点放在了surfaceCloud变量中
pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>());
// 循环所有的扫描线
for (int i = 0; i < N_SCAN; i++)
{
surfaceCloudScan->clear();
// 把每一根scan等分成6份，每份分别提取特征点
for (int j = 0; j < 6; j++)
{
// 根据之前得到的每个scan的起始和结束id来均分成6份
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--)
{
// 找到这个点对应的原先的索引（因为平滑度已经进行过排序，所以这里的k与ind的值不同）
int ind = cloudSmoothness[k].ind;
// 如果没有被认为是遮挡点或平行点 且曲率大于边缘点阈值（默认为1）
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
{
largestPickedNum++;
// 每段最多找20个角点
if (largestPickedNum <= 20){
// 标签置1表示是角点
cloudLabel[ind] = 1;
// 这个点收集进存储角点的点云中
cornerCloud->push_back(extractedCloud->points[ind]);
} else {
break;
}
// 将这个点周围的几个点设置成遮挡点，避免选取太集中（正方向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]));
// 列idx距离太远就算了，空间上也不会太集中
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
// 同理，避免选取太集中（负方向5个点）
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)
{
// -1表示是面点
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++)
{
// 注意这里是小于等于0,也就是说不是角点的都认为是面点了
if (cloudLabel[k] <= 0){
surfaceCloudScan->push_back(extractedCloud->points[k]);
}
}
}
surfaceCloudScanDS->clear();
// 因为面点太多了，所以做一个下采样
downSizeFilter.setInputCloud(surfaceCloudScan);
downSizeFilter.filter(*surfaceCloudScanDS);
// 下采样后的面片点存储到surfaceCloud变量中
*surfaceCloud += *surfaceCloudScanDS;
}
}
- 释放空间、发布提取的特征点
// 将一些不会用到的存储空间释放掉（下一个节点不会用到）
void freeCloudInfoMemory()
{
cloudInfo.startRingIndex.clear();
cloudInfo.endRingIndex.clear();
cloudInfo.pointColInd.clear();
cloudInfo.pointRange.clear();
}
/**
* @brief 发布相关消息
* 释放空间
* 发送角点 面片点 点云信息集合
*/
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);
}
Main函数
初始化一个FeatureExtraction 类，并执行特征提取。
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam");
/**
* @brief 接收经过去畸变预处理后的lio_sam/deskew/cloud_info信息
* 提取 角点与平面点 两类特征点
* 发布lio_sam/feature/cloud_corner与lio_sam/feature/cloud_surface两类消息
* 发布"lio_sam/feature/cloud_info"消息，存储两类特征点
*/
FeatureExtraction FE;
ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");
// 单线程处理
ros::spin();
return 0;
}
扫一扫
1918
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
