LIO-SAM代码阅读——featureExtraction.cpp

小小肱二

已于 2022-09-24 20:11:05 修改

阅读量330

点赞数

分类专栏： SLAM LIO-SAM 文章标签： c++

于 2022-09-24 20:09:42 首次发布

本文链接：https://blog.csdn.net/qq_44305240/article/details/127025977

版权

SLAM 同时被 2 个专栏收录

3 篇文章 0 订阅

订阅专栏

LIO-SAM

3 篇文章 0 订阅

订阅专栏

本文详细介绍了如何使用C++处理激光雷达点云数据，提取角点和平面点特征。首先，计算曲率并标记遮挡和平行点。接着，通过对每根扫描线分段，按曲率排序，选取角点和面点。最后，对平面点进行降采样并发布特征点云数据。整个过程涉及曲率计算、点云筛选及特征提取等关键步骤。

摘要由CSDN通过智能技术生成

写在前面：

这个cpp的主要作用就是提取当前帧中的角点和平面点

首先我们看到构造函数中只有一个回调函数就是laserCloudInfoHandler，订阅来自imageProjection的cloud_Info结构体数据，上一篇文章中说到这个结构体中还没有填充角点和面点数据，这个cpp就会将其填充上。

OK我们进入回调函数中对每一个函数进行分析

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); // 把sensor_Msgs::PointCloud2类型的去畸变后的有效点转换成pcl形式

        calculateSmoothness();// 计算曲率

        markOccludedPoints();// 标记属于遮挡、平行两种情况的点，不做特征提取

        extractFeatures();// 提取特征点（角点、平面点）

        publishFeatureCloud();
    }

1.calculateSmoothness()：根据函数名字就知道这是个曲率计算函数，首先去除前五个和后五个点（点很多，丢掉的只是就牛一毛，不用担心），利用当前点云的前后五个点来计算每个点的距离插值，插值的平方就是曲率，并将两个标签（cloudNeighborPicked和cloudLable)初始化为0，然后将其曲率和索引保存在结构体cloudSmoothness中。

注意：这里的两个重要标签，cloudNeighborPicked表示该点云是否被处理过，如果被处理过就赋值1，没有被处理过就赋值0；cloudLable表示该点云是角点还是平面点，如果是角点就赋值1，平面点就赋值-1。

void calculateSmoothness()
    {
        int cloudSize = extractedCloud->points.size();
        // 遍历所有有效点（去除前五个点和后五个点）
        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;
        }
    }

2.markOccludedPoints()：标记属于 平行和遮挡 两种情况的点云，这两种点云后续不做特征提取，cloudNeighborPicked标签赋值为1

void markOccludedPoints()
    {
        int cloudSize = extractedCloud->points.size();
        // mark occluded points and parallel beam points
        for (int i = 5; i < cloudSize - 6; ++i)
        {
            // 1.遮挡点
            // 当前点和下一个点的深度值
            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
                if (depth1 - depth2 > 0.3){
                    cloudNeighborPicked[i - 5] = 1;//标记为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;
                }
            }
            // 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;// 标记为1
        }
    }

3.extractFeatures()：角点和平面点特征提取。首先将每根扫描线平均分为6段，将每段扫描线上的点通过by_value()函数将其曲率从小到大排列，然后遍历每一小段，从尾到头遍历，每小段取20个角点，然后将角点填充进cornerCloud队列中，将这些点的cloudLabel标签赋值为1，并且将当前点的前后五个点的cloudNeighborPicked赋值为1，不做处理（避免特征聚集）；角点取完以后，其他的点只要是没有被处理过并且曲率小于面点曲率阈值的点都标注为平面点（不限量），与角点处理类似，当前点的前后五个点的cloudNeighborPicked赋值为1，不做处理（避免特征聚集），平面点找完以后降采样，并填充进surfaceCloud队列中。

// 提取特征点（角点、平面点）
    /**
     * 点云角点、平面点特征提取
     * 1.遍历扫描线，每根扫描线扫描一周的点云划分为6段，针对每段提取20个角点、不限数量的平面点，加入角点集合、平面点集合
     * 2.认为非角点的点都是平面点，加入平面点云集合，最后降采样
     */
    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();

            for (int j = 0; j < 6; j++)
            {
                //把每根扫描线大概等分成6小段，每段起点为sp，终点为ep
                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;

                // 按照曲率将每小段中的点云从小到大排序
                //可以看出之前的by_value在这里被当成了判断函数来用
                std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());

                int largestPickedNum = 0;

                //按照曲率从大到小在每小段上遍历点云（这个for循环主要是保存角点）
                for (int k = ep; k >= sp; k--)
                {
                    int ind = cloudSmoothness[k].ind;
                    //没有被处理过并且曲率大于0.1的时角点
                    if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)//edgeThreshold = 0.1
                    {
                        largestPickedNum++;
                        // 每小段取20个角点
                        if (largestPickedNum <= 20){
                            cloudLabel[ind] = 1;// 角点Lable标记为1
                            cornerCloud->push_back(extractedCloud->points[ind]);
                        } else {
                            break;
                        }

                        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;
                    //没有被处理过并且曲率小于0.1的是平面点
                    if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
                    {
                        cloudLabel[ind] = -1;// 平面点Lable标记为-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++)
                {
                    // 上面Lable标记为1的是角点，因此非1（0和-1)都是平面点
                    if (cloudLabel[k] <= 0){
                        surfaceCloudScan->push_back(extractedCloud->points[k]);
                    }
                }
            }
            
            surfaceCloudScanDS->clear();
            // 平面点降采样
            downSizeFilter.setInputCloud(surfaceCloudScan);
            downSizeFilter.filter(*surfaceCloudScanDS);
            //平面点降采样结果填充进surfaceCloud
            *surfaceCloud += *surfaceCloudScanDS;
        }
    }

4.publishFeatureCloud()：填充cloud_Info.cloud_corner和cloud_surface，并在话题通道发布数据

pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 1);//发布填充了cloud_corner和cloud_surface后的CloudInfo
pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_corner", 1);//发布角点,rivz展示
pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_surface", 1);//发布平面点，rivz展示

/**
     * 发布角点、面点点云，发布带特征点云数据的当前激光帧点云信息
    */
    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);
    }