LIO-sam源码分析：featureExtraction.cpp

这一块代码和LeGO的思想是一致的，而且还略去了scan-to-scan的内容，整体更加简介。

构造函数–》回调函数FeatureExtraction()：

订阅：
lio_sam/deskew/cloud_info
发布：
lio_sam/feature/cloud_info
lio_sam/feature/cloud_corner
lio_sam/feature/cloud_surface

函数首先会进入回调函数：laserCloudInfoHandler

void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
    cloudInfo = *msgIn; // new cloud info，订阅的去畸变点云
    cloudHeader = msgIn->header; // new cloud header
    //转到ros中的extractedCloud
    pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction
    //平滑度（曲率）计算  为了区分出边缘点和平面点 
    calculateSmoothness();
    //标记出被遮挡的点，边界过远一侧去除
    markOccludedPoints();
    //提取特征
    extractFeatures();
    //发布特征点云
    publishFeatureCloud();
}

calculateSmoothness

计算曲率

void calculateSmoothness()
{
    int cloudSize = extractedCloud->points.size();
    //这里跟loam源码基本很像 从第五个点开始到结束前5个 计算某个点的平滑度
    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;
    }
}

markOccludedPoints

标记遮挡和平行点

void markOccludedPoints()
{
    int cloudSize = extractedCloud->points.size();
    // mark occluded points and parallel beam points
    //标记遮挡点和平行光束点 看到这个就知道完全与LOAM的思想一致 理论部分建议看LOAM论文
    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
            //根据深度差异 进行区分 并设定标志变量为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]));
        
        if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
            cloudNeighborPicked[i] = 1;
    }
}

extractFeatures

特征提取：将每一层点云分成6份，每一份中，对点的曲率进行排序，sp和ep分别是这段点云的起始点与终止点。从而判断出角点与平面点。

void extractFeatures()
{
    //角点/边缘点 平面点 点云清空下面准备存进来对应点云了
    cornerCloud->clear();
    surfaceCloud->clear();
    
    //这里是借助雷达扫描原理 以下方的7根线的扫描作为平面点的提取 参考LEGO-LOAM 
    //LIOSAM里只分了边缘点与平面点，没有对地面点进行特定的区分
    //这里是分成6块处理的操作
    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++)//16线
    {
        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){ //边缘点选取20个
                        cloudLabel[ind] = 1;
                        cornerCloud->push_back(extractedCloud->points[ind]);
                    } else {
                        break;
                    }
                    
                    //防止特征点聚集  
                    // 从ind+l开始后面5个点，每个点index之间的差值，
                    // 确保columnDiff<=10,然后标记为我们需要的点
                    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;
    }
}

publishFeatureCloud

发布函数，最后发布cloudInfo点云。

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);
}