LeGO-LOAM 几个特有函数的分析（2）

接上回LeGO-LOAM 几个特有函数的分析（1）

二、广度优先遍历

广度优先遍历（Breadth-First Search, BFS）是一种用于遍历或搜索树或图的算法。这种算法从树的根（或图的某一指定节点）开始，然后探索邻近的节点，之后对每一个邻近的节点，它再去探索它们各自相邻的节点，这个过程持续进行直到访问所有可达的节点。

广度优先遍历的主要特点是它按照距离起始点的“层次”来遍历。首先访问距离起点最近的节点，然后是它们的邻居，如此类推。

2.1 广度优先遍历的步骤：

1. 初始化：首先将起始节点放入队列中。

2. 遍历：

   • 从队列中弹出一个节点。
   • 检查该节点是否为目标节点。如果是，则完成搜索。
   • 将该节点的所有未访问过的邻居节点加入队列。

3. 重复：重复步骤2，直到队列为空或找到目标节点。

4. 结束：当队列为空且目标未找到，或已找到目标节点时，算法结束。

2.2基于 BFS 的点云聚类和外点剔除

2.2.1原理

 

 2.2.2源码注释

    void labelComponents(int row, int col){
        // use std::queue std::vector std::deque will slow the program down greatly
        // 声明所需的变量，输入的ROW和col是单帧点云第几行第几列的点
        // 用于存储距离和角度计算的临时变量
        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;
        //很巧妙，有有效邻点就加一，每次循环减1，实现bfs广度优先遍历关键
        while(queueSize > 0){
            // Pop point
            // 取出当前点x,y坐标
            fromIndX = queueIndX[queueStartInd];
            fromIndY = queueIndY[queueStartInd];
            //队列大小减一
            --queueSize;
            //索引加一
            ++queueStartInd;
            // Mark popped point
            // 标记该点为一类，聚类就是给点加标签，标签一致的就是一类
            labelMat.at(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
                // 如果raw为0或者15，上或者下没有邻点，跳过
                if (thisIndX < 0 || thisIndX >= N_SCAN)
                    continue;
                // at range image margin (left or right side)
                //设置矩阵最两边的点也为邻点，因为VLP16是360度
                //在cow为0时左边的邻点，在1799
                if (thisIndY < 0)
                    thisIndY = Horizon_SCAN - 1;
                //在cow为1799时左边的邻点，在0
                if (thisIndY >= Horizon_SCAN)
                    thisIndY = 0;
                // prevent infinite loop (caused by put already examined point back)
                // 如果该点已被标记，则跳过
                if (labelMat.at(thisIndX, thisIndY) != 0)
                    continue;
                // 计算角度差以决定是否将邻点加入到当前区域
                // 距离雷达远的是D1，近的是D2
                d1 = std::max(rangeMat.at(fromIndX, fromIndY),
                              rangeMat.at(thisIndX, thisIndY));
                d2 = std::min(rangeMat.at(fromIndX, fromIndY), 
                              rangeMat.at(thisIndX, thisIndY));
                //（0，-1），（0，1），意味着是一条线上的点，角度是360/1800*3.14/180=0.0035
                if ((*iter).first == 0)
                    alpha = segmentAlphaX;
                else
                //（1，0），（-1，0），意味着是上下两条线上的点，角度是30/（16-1）*3.14/180=0.035
                    alpha = segmentAlphaY;
                //计算图中angle角度
                angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
                //如果角度大于60度
                if (angle > segmentTheta){
                    //把此邻点放入队列
                    queueIndX[queueEndInd] = thisIndX;
                    queueIndY[queueEndInd] = thisIndY;
                    //增加size
                    ++queueSize;
                    //末尾索引右移
                    ++queueEndInd;
                    //把此邻点赋上和之前取出来的点一样的标签
                    labelMat.at(thisIndX, thisIndY) = labelCount;
                    //这行有点被标记过
                    lineCountFlag[thisIndX] = true;
                    //保存聚类结果
                    allPushedIndX[allPushedIndSize] = thisIndX;
                    allPushedIndY[allPushedIndSize] = thisIndY;
                    ++allPushedIndSize;
                }
            }
        }

        // check if this segment is valid
        bool feasibleSegment = false;
        //如果聚类大于30则认为是一个好的聚类
        if (allPushedIndSize >= 30)
            feasibleSegment = true;
        //如果大于5，而且都是竖着的超过3个，也认为是一个好聚类，可能是树，电线杆
        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){
                //不成功，则标记为999999，代表依托答辩
                labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999;
            }
        }
    }

 需要注意的点：

一是 邻点的定义，就是代表取当前点上下左右四个点

std::pair neighbor;
neighbor.first = -1; neighbor.second =  0; neighborIterator.push_back(neighbor);
neighbor.first =  0; neighbor.second =  1; neighborIterator.push_back(neighbor);
neighbor.first =  0; neighbor.second = -1; neighborIterator.push_back(neighbor);
neighbor.first =  1; neighbor.second =  0; neighborIterator.push_back(neighbor);

 二是 巧妙的通过queueSize 实现广度优先遍历算法的核心

开始是int queueSize =1，让其进入循环

while(queueSize > 0){
    //队列大小减一
    --queueSize;
    for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
        //如果角度大于60度
        if (angle > segmentTheta){
            //增加size
            ++queueSize;
        }
    }
}

三是 聚类时候，大于30个点，或者大于5个点，但是有三个竖着的聚为一类

我觉得原因是考虑到竖着的点距离远的因素

四是 通过计算角度来判断是否是邻点

想象一下，是不是D1越长，angle越小

2.3函数的调用

用此种方式实现了一帧雷达所有点的聚类

        for (size_t i = 0; i < N_SCAN; ++i)
            for (size_t j = 0; j < Horizon_SCAN; ++j)
                //上一个函数说过地面点label被置为1 
                //如果这个点既不是地面点也没有聚类过，开始聚类
                if (labelMat.at(i,j) == 0)
                    labelComponents(i, j);