【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/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());

        // 发布当前激光帧提取特征之后的点云信息
        pubLaserCloudInfo = nh.advertise ("lio_sam/feature/cloud_info", 1);
        // 发布当前激光帧的角点点云
        pubCornerPoints = nh.advertise("lio_sam/feature/cloud_corner", 1);
        // 发布当前激光帧的面点点云
        pubSurfacePoints = nh.advertise("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();
        
        // 发布角点、面点点云,发布带特征点云数据的当前激光帧点云信息
        publishFeatureCloud();
    }


3.1.1 曲率计算calculateSmoothness()

    void calculateSmoothness()
    {
        // 遍历当前激光帧运动畸变校正后的有效点云
        int cloudSize = extractedCloud->points.size();
        for (int i = 5; i < cloudSize - 5; i++)
        {
            // 用当前激光点前后5个点计算当前点的曲率,平坦位置处曲率较小,角点处曲率较大;这个方法很简单但有效
            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;

            cloudNeighborPicked[i] = 0;
            cloudLabel[i] = 0;
            
            // 存储该点曲率值、激光点一维索引
            cloudSmoothness[i].value = cloudCurvature[i];
            cloudSmoothness[i].ind = i;
        }
    }

pointRange是指该点到lidar坐标系原点的距离,所以说作者把曲率是用深度值差的平方来描述的。

3.1.2 选出(剔除)与激光方向平行或被遮挡的激光点markOccludedPoints()

    void markOccludedPoints()
    {
        int cloudSize = extractedCloud->points.size();
        // mark occluded points and parallel beam points

        for (int i = 5; i < cloudSize - 6; ++i)
        {
            // 当前点和下一个点的深度值
            float depth1 = cloudInfo.pointRange[i];
            float depth2 = cloudInfo.pointRange[i+1];

            // 用于判断这两个点是不是在同一个ring上
            int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));

            //1. 判断是否存在遮挡
            // 两个点在同一扫描线上
            if (columnDiff < 10){
                // 前者深度更大,说明后者把前者遮挡了,所以前者标记为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;
                }
                // 后者深度更大,说明前者把后者遮挡了,所以后者标记为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;
        }
    }
    


3.1.3 根据曲率把平面点和角点提取出来extractFeatures()


很长一大段,说白了就是根据曲率把点分为平面点和角点罢了。不过作者的传感器真NB啊,这么多点都丢了,我们开发用的传感器很low,不敢这么整。

    void extractFeatures()
    {
        cornerCloud->clear();
        surfaceCloud->clear();

        pcl::PointCloud::Ptr surfaceCloudScan(new pcl::PointCloud());
        pcl::PointCloud::Ptr surfaceCloudScanDS(new pcl::PointCloud());

        // 遍历扫描线
        for (int i = 0; i < N_SCAN; i++)
        {
            surfaceCloudScan->clear();

            // 将一条扫描线扫描一周的点云数据,划分为6段,每段分开提取有限数量的特征,保证特征均匀分布
            for (int j = 0; j < 6; j++)
            {
                // 每段点云的起始、结束索引;startRingIndex为扫描线起始第5个激光点在一维数组中的索引
                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)
                    {
                        // 每段只取20个角点,如果单条扫描线扫描一周是1800个点,则划分6段,每段300个点,从中提取20个角点
                        largestPickedNum++;
                        if (largestPickedNum <= 20){
                            // 标记为角点
                            cloudLabel[ind] = 1;
                            // 加入角点点云
                            cornerCloud->push_back(extractedCloud->points[ind]);
                        } else {
                            break;
                        }

                        // 标记已被处理
                        cloudNeighborPicked[ind] = 1;
                        // 同一条扫描线上后5个点标记一下,不再处理,避免特征聚集
                        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;
                        }
                        // 同一条扫描线上前5个点标记一下,不再处理,避免特征聚集
                        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;

                        // 同一条扫描线上后5个点标记一下,不再处理,避免特征聚集
                        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;
                        }
                        // 同一条扫描线上前5个点标记一下,不再处理,避免特征聚集
                        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;
        }
    }


  3.1.4 把提取的角点和平面点发布出去publishFeatureCloud()

    void publishFeatureCloud()
    {
        // 清理
        freeCloudInfoMemory();
        // 发布角点、面点点云,用于rviz展示
        cloudInfo.cloud_corner  = publishCloud(&pubCornerPoints,  cornerCloud,  cloudHeader.stamp, lidarFrame);
        cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
        // 发布当前激光帧点云信息,加入了角点、面点点云数据,发布给mapOptimization
        pubLaserCloudInfo.publish(cloudInfo);
    }
    


 

你可能感兴趣的:(SLAM,自动驾驶,人工智能,机器学习)