LIO-SAM源码阅读笔记(三)---featureExtraction.cpp

代码：
https://github.com/TixiaoShan/LIO-SAM
特征提取这块的代码与之前的基本一样的思想，省去了LeGO-LOAM中scan-to-scan的内容，更加简洁。

1. 回调函数

    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();
      //标记出被遮挡的点 参考LOAM论文的介绍
        markOccludedPoints();
       //提取特征
        extractFeatures();
       //发布特征点云
        publishFeatureCloud();
    }

2. 平滑度计算

  void calculateSmoothness()
    {
   

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

        for (int i = 5; i < cloudSize - 5; i++)
        {
        
            //前五个点的距离属性（在imageProjection.cpp中被赋值）之和加后五个点的距离之和-10倍该点的距离 算出差值
            //其实就是确定一种连续点之间的变化关系 起伏趋势
            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;
            //先是否被选择以及标签值置0
            cloudNeighborPicked[i] = 0;
            cloudLabel[i] = 0;
            // cloudSmoothness for sorting
            //记录曲率以及索引
            cloudSmoothness[i].value = cloudCurvature[i];
            cloudSmoothness[i].ind = i;
        }
    }

3. 标记遮挡和平行点

 void markOccludedPoints()//主要是完成了该函数后cloudNeighborPicked中有了点是否选择为特征点的标记
    {