自动驾驶系列（7）LeGO-LOAM源码解析（点云分割）

一、imageProjection —— 点云分割

1. cloudHandler()
2. copyPointCloud()
3. findStartEndAngle()
4. projectPointCloud()
5. groundRemoval()
6. cloudSegmentation()
7. labelComponents()
8. publishCloud()
9. resetParameters()

二、总体流程
订阅点云数据回调处理 -> 点云转换到pcl预处理 -> 截取一帧激光数据 -> 投影映射到图像 -> 地面移除 -> 点云分割 -> 发布点云数据 -> 重置参数;
2.1 findStartEndAngle()
findStartEndAngle 函数旨在从激光雷达的点云数据中找出开始点和结束点的航向角，并计算这两个航向角之间的差异。

• 计算开始点和结束点的航向角：

  使用 atan2 函数计算第一个点（即开始点）和最后一个点（即结束点）相对于原点（可能是激光雷达的中心）的航向角。atan2(y, x) 返回的是从x轴正方向到点(x, y)的矢量与x轴正方向之间的夹角，其值在 [-π, π] 范围内。由于这里使用负号，表示将角度视为顺时针旋转（这取决于您的具体应用场景和坐标系设置）。
  对于结束点，额外加上 2 * M_PI是为了处理一种特殊情况：即如果激光雷达扫描的最后一个点实际上是在扫描路径的开始部分（例如，激光雷达是连续旋转的），那么简单地计算两个点的 atan2 值可能会导致得到的角度差不是完整的扫描角度。通过加上 2 * M_PI，我们假设扫描已经完成了整整一圈。

• 计算角度差：

  最后，计算 endOrientation 和 startOrientation 之间的差值，即 orientationDiff，这个值代表了激光雷达扫描路径的完整角度范围。

    // 找到开始结束角度
    // 1.一圈数据的角度差，使用atan2计算;
    // 2.注意计算结果的范围合理性
    
    void findStartEndAngle(){
        // start and end orientation of this cloud
        // 1.开始点和结束点的航向角 (负号表示顺时针旋转)   
        segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);

        // 加 2 * M_PI 表示已经转转了一圈
        segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
                                                     laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;
        
        // 2.保证所有角度落在 [M_PI , 3M_PI] 上
        if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) {
            segMsg.endOrientation -= 2 * M_PI;
        } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
            segMsg.endOrientation += 2 * M_PI;
        segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
        
    }

2.2cloudSegmentation()
用于处理点云数据（特别是来自激光雷达的点云）的分割过程的函数。它首先通过遍历每个点云数据点（基于扫描的行和列），使用labelMat（一个存储每个点标签的矩阵）来决定哪些点需要被进一步处理（即非地面点）。接下来，它对这些点进行聚类分割，尽管聚类分割的具体实现（labelComponents函数）没有在这段代码中给出。之后，代码通过遍历所有点，根据标签和地面状态（存储在labelMat和groundMat中）来提取和存储用于不同目的的分割点云。

// 点云分割
// 首先对点云进行聚类标记，根据标签进行对应点云块存储;

void cloudSegmentation(){
    // segmentation process
    for (size_t i = 0; i < N_SCAN; ++i)     // 16线的 一个线束一个的遍历
        for (size_t j = 0; j < Horizon_SCAN; ++j)       // 水平 的 1800
            if (labelMat.at<int>(i,j) == 0)     // 对非地面点进行点云分割
                labelComponents(i, j);      //  调用这个函数对点云进行分割聚类

    int sizeOfSegCloud = 0;

    // extract segmented cloud for lidar odometry
    // 提取分割点云 用于激光雷达里程计
    for (size_t i = 0; i < N_SCAN; ++i) {

        segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;

        for (size_t j = 0; j < Horizon_SCAN; ++j) {
            if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
                // outliers that will not be used for optimization (always continue)
                // 勾勒出优化过程中不被使用的值

                // 1. 如果label为999999则跳过
                if (labelMat.at<int>(i,j) == 999999){
                    if (i > groundScanInd && j % 5 == 0){
                        outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                        continue;
                    }else{
                        continue;
                    }
                }

                // majority of ground points are skipped
                // 2. 如果为地，跳过index不是5的倍数的点
                if (groundMat.at<int8_t>(i,j) == 1){
                    if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
                        continue;
                }
                // mark ground points so they will not be considered as edge features later
                segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
                // mark the points' column index for marking occlusion later
                segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
                // save range info
                segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);
                // save seg cloud
                segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                // size of seg cloud
                ++sizeOfSegCloud;
            }
        }

        segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
    }
    
    // extract segmented cloud for visualization
    if (pubSegmentedCloudPure.getNumSubscribers() != 0){
        for (size_t i = 0; i < N_SCAN; ++i){
            for (size_t j = 0; j < Horizon_SCAN; ++j){
                if (labelMat.at<int>(i,j) > 0 && labelMat.at<int>(i,j) != 999999){
                    segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    segmentedCloudPure->points.back().intensity = labelMat.at<int>(i,j);
                }
            }
        }
    }
}

2.3 labelComponents()
目的是在点云数据中识别和标记出局部特征聚类，具体来说是平面点和边缘点。它通过遍历点云中的点，并根据点与其邻居之间的角度关系来判断这些点是否属于同一个聚类。

• 初始化队列和计数器：将传入的点(row, col)加入队列，并初始化相关计数器。
• 广度优先搜索：使用队列进行广度优先搜索，从队列中取出一个点，并标记其聚类标签。
• 遍历邻居点：对于每个被取出的点，遍历其所有邻居点（通过neighborIterator迭代）。
• 计算角度：根据当前点和邻居点的深度信息，计算它们之间的角度。这里使用了一个平面聚类的公式，通过角度的大小来判断两点是否趋向于在同一平面上。
• 判断并加入队列：如果计算出的角度大于某个阈值（segmentTheta），则认为当前邻居点属于同一个聚类，将其加入队列并标记聚类标签。
• 判断聚类是否有效：如果聚类中的点数超过某个阈值（30），则认为这是一个有效的聚类（可能是平面点）。
  如果点数不足但涉及的行数超过某个阈值（segmentValidLineNum），也认为这是一个有效的聚类（可能是边缘点）。
• 标记聚类：如果聚类有效，则增加labelCount，为下一个聚类准备新的标签。如果聚类无效，则将这些点标记为一个特殊的值（999999），表示它们不属于任何有效的聚类。

    // 标签组件
    // 局部特征检测聚类
    // 平面点与边缘点
    
    void labelComponents(int row, int col){
        // use std::queue std::vector std::deque will slow the program down greatly
        float d1, d2, alpha, angle;
        int fromIndX, fromIndY, thisIndX, thisIndY; 
        bool lineCountFlag[N_SCAN] = {false};

        // 传进来的两个参数，按照坐标不同分别给他们放到 X 与 Y 的数组中
        queueIndX[0] = row;
        queueIndY[0] = col;
        int queueSize = 1;      // 需要计算角度的点的数量
        int queueStartInd = 0;
        int queueEndInd = 1;

        allPushedIndX[0] = row;
        allPushedIndY[0] = col;
        int allPushedIndSize = 1;
        
        while(queueSize > 0){
            // Pop point
            fromIndX = queueIndX[queueStartInd];
            fromIndY = queueIndY[queueStartInd];
            --queueSize;
            ++queueStartInd;
            // Mark popped point
            labelMat.at<int>(fromIndX, fromIndY) = labelCount;
            // Loop through all the neighboring grids of popped grid

            // 遍历整个点云，遍历前后左右四个邻域点，求点之间的角度数值 
            for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
                // new index
                thisIndX = fromIndX + (*iter).first;
                thisIndY = fromIndY + (*iter).second;
                // index should be within the boundary
                if (thisIndX < 0 || thisIndX >= N_SCAN)
                    continue;
                // at range image margin (left or right side)
                if (thisIndY < 0)
                    thisIndY = Horizon_SCAN - 1;
                if (thisIndY >= Horizon_SCAN)
                    thisIndY = 0;
                // prevent infinite loop (caused by put already examined point back)
                if (labelMat.at<int>(thisIndX, thisIndY) != 0)
                    continue;

                 //比较得出较大深度与较小深度
                d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), 
                              rangeMat.at<float>(thisIndX, thisIndY));
                d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), 
                              rangeMat.at<float>(thisIndX, thisIndY));

                if ((*iter).first == 0)
                    alpha = segmentAlphaX;
                else
                    alpha = segmentAlphaY;

                angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));      //平面聚类公式，角度越大两点越趋向于平面
                //segmentTheta=60度
                if (angle > segmentTheta){

                    queueIndX[queueEndInd] = thisIndX;
                    queueIndY[queueEndInd] = thisIndY;
                    ++queueSize;
                    ++queueEndInd;

                    labelMat.at<int>(thisIndX, thisIndY) = labelCount;
                    lineCountFlag[thisIndX] = true;

                    allPushedIndX[allPushedIndSize] = thisIndX;
                    allPushedIndY[allPushedIndSize] = thisIndY;
                    ++allPushedIndSize;
                }
            }
        }

        // check if this segment is valid
        bool feasibleSegment = false;

        // 如果是 allPushedIndSize 累加的数量增加到了30 个 则判断这部分点云属于一个聚类
        if (allPushedIndSize >= 30)
            feasibleSegment = true;     //面点

        // 如果垂直 方向上点的数量大于 5个 默认是一个有效的聚类
        else if (allPushedIndSize >= segmentValidPointNum){
            int lineCount = 0;
            for (size_t i = 0; i < N_SCAN; ++i)
                if (lineCountFlag[i] == true)
                    ++lineCount;
            if (lineCount >= segmentValidLineNum)
                feasibleSegment = true;     //边点  
        }
        
        // segment is valid, mark these points
        if (feasibleSegment == true){
            ++labelCount;
        }else{ // segment is invalid, mark these points
            for (size_t i = 0; i < allPushedIndSize; ++i){
                labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
            }
        }
    }