本文阅读的代码为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.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];
}
2.3 消息的回调
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();
// 提取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倍 应该等于0
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;
}
}
2.5 markOccludedPoints()
// 标记遮挡点与平行点,不进行特征提取
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]));
// 相邻两点如果列索引太小,则这个点周围的点不进行特征提取
// 平行线和遮挡的判断参考LOAM
if (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/6
int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
// ep 就是sp的下一个循环的值的前一个索引,ep[j] = sp[j+1] - 1
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--)
{
// 最后的点 的曲率最大,如果满足条件,就是角点
// edgeThreshold为0.1,正圆的曲率为0
int 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之间的差值。附近点都是有效点的情况下,相邻点间的索引只差1
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)
{
// 标记面点的索引的值为-1
cloudLabel[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;
}
}
}
// 面点临时保存在surfaceCloudScan
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0){
surfaceCloudScan->push_back(extractedCloud->points[k]);
}
}
} // for
// 对面点进行降采样,结果临时保存在 surfaceCloudScanDS
surfaceCloudScanDS->clear();
downSizeFilter.setInputCloud(surfaceCloudScan);
downSizeFilter.filter(*surfaceCloudScanDS);
// 将临时变量中的值放入surfaceCloud
*surfaceCloud += *surfaceCloudScanDS;
} // for
}
2.7 publishFeatureCloud()
// 发布 lio_sam/feature/cloud_info
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);
}
2.8 freeCloudInfoMemory()
// free cloud info memory
void 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