【SLAM】LIO-SAM解析——点云特征提取FeatureExtraction(3)
系列文章链接：
【SLAM】LIO-SAM解析——流程图(1)
【SLAM】LIO-SAM解析——数据预处理imageProjection(2)
【SLAM】LIO-SAM解析——特征提取featureTrack(3)
【SLAM】LIO-SAM解析——IMU预计分IMU-Preintegration(4)
【SLAM】LIO-SAM解析——后端优化MapOptimization(5)
【SLAM】LIO-SAM解析——里程计融合transformFusion(6)
知识点：
如何用一帧雷达/深度图数据每一个点/像素坐标的深度值确定当前点是角点还是平面点。
这一部分会接收来自imageProjection处理完的lidar帧数据，对应的点云是去畸变的，也就是在同一个坐标系下。这部分对此帧的点云进行面点，角点特征的分类。
这一章内容比前一章还要简单一些，作者在这里做了大幅度的点云降采样，有些奢侈了。工业中很少会使用密度如此高，精度如此高的传感器，数据处理往往是业界关注的问题。
我的思考：在实际的业务需求里，有必要对点云进行角点和平面点的区分吗？一定要单独写成一个独立的node/进程吗？这样的数据传输的成本是不是过高了？如果一定要按照这种方式区分角点和平面点，能不能在imageProjection里做了，省去数据传输的成本？
laserCloudInfoHandler():
输入：来自imageProjection的去畸变的一帧lidar点云；
输出：平面点和角点点云，由mapOptimization订阅。
3. 功能入口
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;
}核心内容就在创建FeatureExtraction对象时进行的，看一下他的构造函数：
FeatureExtraction()
{
// 订阅当前激光帧运动畸变校正后的点云信息
subLaserCloudInfo = nh.subscribe("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
// 发布当前激光帧提取特征之后的点云信息
pubLaserCloudInfo = nh.advertise ("lio_sam/feature/cloud_info", 1);
// 发布当前激光帧的角点点云
pubCornerPoints = nh.advertise("lio_sam/feature/cloud_corner", 1);
// 发布当前激光帧的面点点云
pubSurfacePoints = nh.advertise("lio_sam/feature/cloud_surface", 1);
// 初始化
initializationValue();
} 它的回调函数只有一个laserCloudInfoHandler()，接收来自imageProjection处理后的点云。
3.1 点云特征提取 laserCloudInfoHandler()
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
cloudInfo = *msgIn;
cloudHeader = msgIn->header;
// 当前激光帧运动畸变校正后的有效点云
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud);
// 计算当前激光帧点云中每个点的曲率
calculateSmoothness();
// 标记属于遮挡、平行两种情况的点，不做特征提取
markOccludedPoints();
// 点云角点、平面点特征提取
// 1、遍历扫描线，每根扫描线扫描一周的点云划分为6段，针对每段提取20个角点、不限数量的平面点，加入角点集合、平面点集合
// 2、认为非角点的点都是平面点，加入平面点云集合，最后降采样
extractFeatures();
// 发布角点、面点点云，发布带特征点云数据的当前激光帧点云信息
publishFeatureCloud();
}
3.1.1 曲率计算calculateSmoothness()
void calculateSmoothness()
{
// 遍历当前激光帧运动畸变校正后的有效点云
int cloudSize = extractedCloud->points.size();
for (int i = 5; i < cloudSize - 5; i++)
{
// 用当前激光点前后5个点计算当前点的曲率，平坦位置处曲率较小，角点处曲率较大；这个方法很简单但有效
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;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
// 存储该点曲率值、激光点一维索引
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
}
}pointRange是指该点到lidar坐标系原点的距离，所以说作者把曲率是用深度值差的平方来描述的。
3.1.2 选出(剔除)与激光方向平行或被遮挡的激光点markOccludedPoints()
void markOccludedPoints()
{
int cloudSize = extractedCloud->points.size();
// mark occluded points and parallel beam points
for (int i = 5; i < cloudSize - 6; ++i)
{
// 当前点和下一个点的深度值
float depth1 = cloudInfo.pointRange[i];
float depth2 = cloudInfo.pointRange[i+1];
// 用于判断这两个点是不是在同一个ring上
int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
//1. 判断是否存在遮挡
// 两个点在同一扫描线上
if (columnDiff < 10){
// 前者深度更大，说明后者把前者遮挡了，所以前者标记为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;
}
// 后者深度更大，说明前者把后者遮挡了，所以后者标记为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;
}
}
//2. 判断当前点是否在与激光帧平行的平面上
// 用前后相邻点判断当前点所在平面是否与激光束方向平行
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;
}
}
3.1.3 根据曲率把平面点和角点提取出来extractFeatures()
很长一大段，说白了就是根据曲率把点分为平面点和角点罢了。不过作者的传感器真NB啊，这么多点都丢了，我们开发用的传感器很low，不敢这么整。
void extractFeatures()
{
cornerCloud->clear();
surfaceCloud->clear();
pcl::PointCloud::Ptr surfaceCloudScan(new pcl::PointCloud());
pcl::PointCloud::Ptr surfaceCloudScanDS(new pcl::PointCloud());
// 遍历扫描线
for (int i = 0; i < N_SCAN; i++)
{
surfaceCloudScan->clear();
// 将一条扫描线扫描一周的点云数据，划分为6段，每段分开提取有限数量的特征，保证特征均匀分布
for (int j = 0; j < 6; j++)
{
// 每段点云的起始、结束索引；startRingIndex为扫描线起始第5个激光点在一维数组中的索引
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)
{
// 每段只取20个角点，如果单条扫描线扫描一周是1800个点，则划分6段，每段300个点，从中提取20个角点
largestPickedNum++;
if (largestPickedNum <= 20){
// 标记为角点
cloudLabel[ind] = 1;
// 加入角点点云
cornerCloud->push_back(extractedCloud->points[ind]);
} else {
break;
}
// 标记已被处理
cloudNeighborPicked[ind] = 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;
}
// 同一条扫描线上前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)
{
// 标记为平面点
cloudLabel[ind] = -1;
// 标记已被处理
cloudNeighborPicked[ind] = 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;
}
// 同一条扫描线上前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++)
{
if (cloudLabel[k] <= 0){
surfaceCloudScan->push_back(extractedCloud->points[k]);
}
}
}
// 平面点云降采样
surfaceCloudScanDS->clear();
downSizeFilter.setInputCloud(surfaceCloudScan);
downSizeFilter.filter(*surfaceCloudScanDS);
// 加入平面点云集合
*surfaceCloud += *surfaceCloudScanDS;
}
}
3.1.4 把提取的角点和平面点发布出去publishFeatureCloud()
void publishFeatureCloud()
{
// 清理
freeCloudInfoMemory();
// 发布角点、面点点云，用于rviz展示
cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);
cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
// 发布当前激光帧点云信息，加入了角点、面点点云数据，发布给mapOptimization
pubLaserCloudInfo.publish(cloudInfo);
}