【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::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();
    }

它的回调函数只有一个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();
&nb