代码实战 | 用LeGO-LOAM实现BFS点云聚类和噪点剔除

// 1. 按行遍历所有点进行分割聚类，更新labelMat
for (size_t i = 0; i < N_SCAN; ++i) {
    for (size_t j = 0; j < Horizon_SCAN; ++j) {
        // labelMat 存储了每个点的聚类标记
        if (labelMat.at<int>(i,j) == 0) {
            // 对未被标记的点执行BFS, 同时进行聚类
            labelComponents(i, j); 
        }
    }
}

// 2. 对噪声/地面点/分隔点进一步处理
int sizeOfSegCloud = 0;
// extract segmented cloud for lidar odometry
for (size_t i = 0; i < N_SCAN; ++i) {
    // 记录每一条scan中分割出的有效点起始index和终止index
    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) 不用于优化的异常值
            // 2.1. 过滤掉所有的噪声点, 另作存储;
            if (labelMat.at<int>(i, j) == 999999) { // 异常点
                // 是噪点，噪点横轴坐标是5的倍数就进行存储
                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.2. 压缩地面点, 只取五分之一的地面点进行存储;
            if (groundMat.at<int8_t>(i, j) == 1) {
                if (j%5 !=0 && j>5 && j<Horizon_SCAN-5)
                    continue;
            }

            // 2.3. 保留所有的有效分割点;
            // 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;
}

if (labelMat.at<int>(i, j) == 999999) { // 异常点
    // 是噪点，噪点横轴坐标是5的倍数就进行存储
    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.2. 压缩地面点, 只取五分之一的地面点进行存储;
if (groundMat.at<int8_t>(i, j) == 1) {
    if (j%5 !=0 && j>5 && j<Horizon_SCAN-5)
        continue;
}

// 2.3. 保留所有的有效分割点;
// 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;

// 3. 可视化分割后的点云(同类点云分配相同的强度值)，不包括地面点云
// extract segmented cloud for visualization 
if (pubSegmentedCloudPure.getNumSubscribers() != 0) {
    // 遍历range map 中的点云                                                               
    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);
            }
        }
    }
}

// 1. 使用C++ 容器会导致计算速度变慢, 作者使用数组模拟了一个队列
float d1, d2, alpha, angle;
int fromIndX, fromIndY, thisIndX, thisIndY; 
bool lineCountFlag[N_SCAN] = {false};

// 使用数组模拟队列
queueIndX[0] = row;
queueIndY[0] = col;
int queueSize = 1;
int queueStartInd = 0; // pop 数据的哨兵
int queueEndInd = 1; // push 数据的哨兵

// 存储分割的点云
allPushedIndX[0] = row;
allPushedIndY[0] = col;
int allPushedIndSize = 1; // 记录分割点云的点数量

// 2. 进行BFS点云分割
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){
        // 2.1. 提供有效的邻居点(正确的坐标和未被标记)
        // 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;

        // 2.2. 计算d1(长边)和d2(短边)
        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));

        // 2.3. 根据所取点的位置设置alpha值
        if ((*iter).first == 0) // 邻居点与中心点同一条扫描线上
            alpha = segmentAlphaX;
        else // 邻居点与中心点在相连的扫描线上
            alpha = segmentAlphaY;
        
        // 2.4. 计算角度, 并根据阈值分类
        angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
        if (angle > segmentTheta){ // 此处segmentTheta = 60.0
            // 把此有效点push入队列中
            queueIndX[queueEndInd] = thisIndX;
            queueIndY[queueEndInd] = thisIndY;
            ++queueSize;
            ++queueEndInd;

            // 标记此点与中心点同样的label
            labelMat.at<int>(thisIndX, thisIndY) = labelCount;
            lineCountFlag[thisIndX] = true;

            //追踪分割的点云
            allPushedIndX[allPushedIndSize] = thisIndX;
            allPushedIndY[allPushedIndSize] = thisIndY;
            ++allPushedIndSize;
        }
    }
}

// Pop point 提取一个数据
fromIndX = queueIndX[queueStartInd];
fromIndY = queueIndY[queueStartInd];
--queueSize;
++queueStartInd;
// Mark popped point
labelMat.at<int>(fromIndX, fromIndY) = labelCount;

// 2.1. 提供有效的邻居点(正确的坐标和未被标记)
// 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;

// 2.2. 计算d1(长边)和d2(短边)
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));

// 2.3. 根据所取点的位置设置alpha值
if ((*iter).first == 0) // 邻居点与中心点同一条扫描线上
    alpha = segmentAlphaX;
else // 邻居点与中心点在相连的扫描线上
    alpha = segmentAlphaY;

// 2.4. 计算角度, 并根据阈值分类
angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
if (angle > segmentTheta){ // 此处segmentTheta = 60.0
    // 把此有效点push入队列中
    queueIndX[queueEndInd] = thisIndX;
    queueIndY[queueEndInd] = thisIndY;
    ++queueSize;
    ++queueEndInd;

    // 标记此点与中心点同样的label
    labelMat.at<int>(thisIndX, thisIndY) = labelCount;
    lineCountFlag[thisIndX] = true;

    //追踪分割的点云
    allPushedIndX[allPushedIndSize] = thisIndX;
    allPushedIndY[allPushedIndSize] = thisIndY;
    ++allPushedIndSize;
}

// 3. 判断分隔是否有效
// check if this segment is valid 
bool feasibleSegment = false;
// 如果分割出的点云个数大于30个则认为是此分割是有效的
if (allPushedIndSize >= 30) 
    feasibleSegment = true;
else if (allPushedIndSize >= segmentValidPointNum){ // 如果分割的点云数量不满足30且大于最小点数要求，则进一步分析
    int lineCount = 0;
    // 统计行数
    for (size_t i = 0; i < N_SCAN; ++i)
        if (lineCountFlag[i] == true)
            ++lineCount;
    // 如果行数大于3，则认为也是有效的点云分割
    if (lineCount >= segmentValidLineNum)
        feasibleSegment = true;            
}

// 4. 如果分隔有效, 则更新labelCount; 如果无效, 则标记为噪声
// segment is valid, mark these points
if (feasibleSegment == true){ // 如果是有效分割, 则更新labelCount
    ++labelCount;
}else{ // segment is invalid, mark these points, 如果分割是无效的, 则标记为无效标签值(很大的标签值, 即位噪点)
    for (size_t i = 0; i < allPushedIndSize; ++i){
        labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999; // 
    }
}