本文阅读的代码为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之间的差值。附近点都是有效点的情况下，相邻点间的索引只差１
                            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