LIO-SAM 详读代码笔记 -- 4.featureExtraction

这个模块的主要任务就是从点云中分类出均匀分布的边角点和平面点。就是订阅刚刚在imageProjection.cpp总结中发布的lio_sam/deskew/cloud_info消息，然后提取边缘点（角点），平面点，然后填充cloud_info的cloud_corner和cloud_surface字段，重新以"lio_sam/feature/cloud_info"的形式发布出去，让MapOptimization订阅。

图2.ros topic 发布与订阅关系图关系图（还是叫图2，因为是同一个图）

图1.代码架构图(数据流图) ## 成员变量列表

变量名	类型	注释	备注
subLaserCloudInfo	ros::Subscriber	订阅从 imageProjection 发出来CloudInfo结构的topic， "lio_sam/deskew/cloud_info"	-
pubLaserCloudInfo	ros::Publisher	经过处理后的点云及相关信息封装在Cloudinfo,发送给MapOptimization "lio_sam/feature/cloud_info"	-
pubCornerPoints	ros::Publisher	lio_sam/feature/cloud_corner", 特征提取的边角点点云 ，发出去 在该系统中是没有人订阅的	-
pubSurfacePoints	ros::Publisher	"lio_sam/feature/cloud_surface" 特征提取的平面点点云 ，发出去 在该系统中是没有人订阅的	-
extractedCloud	pcl::PointCloud::Ptr	存放从imageProjection 发送过来的点云，供特征提取使用	-
cornerCloud	pcl::PointCloud::Ptr	提取出来的角点点云	-
surfaceCloud	pcl::PointCloud::Ptr	提取出来的平面点点云	-
downSizeFilter	pcl::VoxelGrid	平面点下采样过滤器	-
cloudInfo	lio_sam::cloud_info	封装点云及相关处理信息，发送给MapOptimization	-
cloudHeader	std_msgs::Header	ros 头消息	-
cloudSmoothness	std::vector	曲率结构体数组，这一帧点云中每个点对应的曲率保存在这个数组中	-
*cloudCurvature	float	曲率，对应大小[N_SCAN*Horizon_SCAN]	-
*cloudNeighborPicked	int	是否被处理的标志 对应大小 [N_SCAN*Horizon_SCAN] 标志着在遍历的点云选角点和平面点时是否处理这个点	-
*cloudLabel	int	分类标签 0 表示未处理 1 表示边角点 -1 表示平面点 对应大小 [N_SCAN*Horizon_SCAN] 其实这个变量的在这里的作用最终就是标志完角点后，<=0 的都可能是平面点，这个地方个人认为是有问题的，那些cloudLabel[k] = 0 的点可能是角点的相邻点，被跳过了，但是不代表就是平面点，而且很可能是边角点，这样一起添加进平面点的集合，可能会引起匹配误差	-

1. 按一维数组的形式遍历点云计算每一个点的曲率。
2. 跳过 容易被遮挡的点，和与激光线平行的平面点。
3. 提起边角点和平面点，这里没有像LOAM 、lego_LOAM 那样，提取曲率特别大的两点和曲率特别小的4点作为scan-scan的特征点，因为在这个系统中已经抛弃了scan-scan的操作，用IMU 的积分数据作为MapOptimization中scan-map优化的初始值，替换掉scan-scan得出的位姿。

//  保存曲率和特征点索引的结构体
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;
    }
};

/**
     * @brief 点云是按一维的形式存储，从第五个点开始，遍历到倒数第5个点，
     * 就是丢掉点云的最前面5个点，和后面5个点，计算所有点的曲率
     * 每个点曲率的计算方法是 前后各5个点的距离中心的距离与当前点距离中心的距离作差 之和
     * 初始化cloudNeighborPicked ，里面元素均为0 表示所有的点都是待分类的点
     * 初始化cloudLabel 里面元素均为0 表示所有的点还未分类
     */
    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;
        }
    }
    /**
     * @brief 这个函数就是跳过那些 可能是容易被遮挡的点和被认为是所在平面平行于激光线的点
     * 
     */
    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]));
            // 如果列索引相差太大，说明这两点原本就不相邻 跳过，不筛平行点和遮挡点
            if (columnDiff < 10){
                // 10 pixel diff in range image
                // 如果 两点距离中心的距离的差大于 0.3m 认为是容易被遮挡的点
                // 远处的点及相邻的5点 均不处理
                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;
        }
    }

    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为扫描线起始第5个激光点在一维数组中的索引
                //注意：所有的点云在这里都是以"一维数组"的形式保存
                //startRingIndex和 endRingIndex 在imageProjection.cpp中的 cloudExtraction函数里被填入
                //假设 当前ring在一维数组中起始点是m，结尾点为n（不包括n），那么6段的起始点分别为：
                // m + [(n-m)/6]*j   j从0～5
                // 化简为 [（6-j)*m + nj ]/6
                // 6段的终止点分别为：
                // m + (n-m)/6 + [(n-m)/6]*j -1  j从0～5,-1是因为最后一个,减去1
                // 化简为 [（5-j)*m + (j+1)*n ]/6 -1
                //这块不必细究边缘值到底是不是划分的准（例如考虑前五个点是不是都不要，还是说只不要前四个点），
                //只是尽可能的分开成六段，首位相接的地方不要。因为庞大的点云中，一两个点其实无关紧要。
                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; // 临时变量，只为控制在区间内边角点的数量(最大20个)
                // 从大到小遍历
                for (int k = ep; k >= sp; k--)
                {
                    int ind = cloudSmoothness[k].ind;
                    // 只有cloudNeighborPicked[ind] == 0 也就是没有被处理过的点才有机会被选中
                    if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
                    {
                        largestPickedNum++;
                        if (largestPickedNum <= 20){
                            cloudLabel[ind] = 1; // 1 为边角点
                            cornerCloud->push_back(extractedCloud->points[ind]);
                        } else {
                            break;
                        }
                        // 前后相连的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]));
                            // 当然 索引相差大于10 说明这两点不是相邻点
                            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)
                    {

                        cloudLabel[ind] = -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++)
                {
                    if (cloudLabel[k] <= 0){
                        // 这个地方个人认为是有问题的，那些cloudLabel[k] = 0 的点可能是角点的相邻点，被跳过了，
                        //但是不代表就是平面点，而且很可能是边角点，这样一起添加进平面点的集合，可能会引起匹配误差
                        surfaceCloudScan->push_back(extractedCloud->points[k]);
                    }
                }
            }

            //对平面点进行下采样
            surfaceCloudScanDS->clear();
            downSizeFilter.setInputCloud(surfaceCloudScan);
            downSizeFilter.filter(*surfaceCloudScanDS);

            *surfaceCloud += *surfaceCloudScanDS;
        }
    }