前言

广度优先遍历（BFS）算计介绍

广度优先遍历（BFS）和深度优先遍历（DFS）同属于两种经典的图遍历算法

广度优先遍历算法：首先从某个节点出发，一层一层的遍历，下一层必须等到上一层节点全部遍历完之后才会开始遍历
基本思想：尽最大程度辐射能够覆盖的节点，并对其进行访问。

用一道BFS的题来进行举例

• 以M节点为顶点的话
  则遍历顺序为：MNQP或MNPQ或MQNP或MQPN或MPQN或MPNQ

• 以N节点为顶点的话
  遍历顺序为：NMOQ或NMQO或NOQM或NOMQ或NQOM或NQMO

• 以Q节点为顶点的话
  遍历顺序为：QMNPR

进行BFS算法实现的时候都是以一个队列的形式进行
下面举一个节点多些的例子，以队列的形式更好理解算法是如何进行的

这是一个无向图，即用线连的两个节点均个互相到达对方，例如A可以到B，B也可以到A

以A为顶点，以队列的形式，走下BFS算法的流程如下：

• 第一个节点是A，先将A 入队列。当前队列为：A
• 从队列中取出A，通过A找到两个节点分是 B 和 C，将它俩如队列。当前队列为：BC
• 从队列中取出B，通过B可以找到A和D，A访问过，忽略，将D入队列。当前队列为:CD
• 从队列中取出C，通过C可以找到A、D、F，A和D访问过，忽略，将F入队列。当前队列为：DF
• 从队列中取出D，通过D可以找到BCEF，BCF已访问，忽略，将E入队列。当前队列为：FE
• 从队列中取出F，通过F可以找到CD，CD已访问，忽略，没有可以加入队列的。当前队列为：E
• 从队列中取出E，通过E可以找到DG，D已访问，忽略，G加入队列。当前队列为：G
• 从队列中取出G，通过G可以找到E，E已访问，忽略，没有可以加入队列的。当前队列为：空
• 判断队列为空，广度优先遍历结束。

所以上面图，访问的顺序为：ABCDFEG

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

点云聚类就是把空间上距离相近的点，作为一个集合，打上要给标签
集合中点数过少的集合，认为是杂点，需要剔除，在特征提取的时候不考虑这些点。

点云的聚类的方式就是基于广度优先遍历算法（BFS）

BFS算法适用于图数据结构，为了把单帧激光雷达点云运用上BFS算法，首先需要将其建模成一个图模型。

一帧点云建立图模型方法：
将一帧点云投影到一个平面地图上，比如velodyne-16的激光雷达（有16根线，每个线有1800个点），将其投影到16*1800大小的图上。

一个格子就代表一个点，格子里的值就是物体到雷达中心的距离

现在图有了，就该定义顶点和边了。

顶点：激光雷达一帧里面的每个点都作为一个顶点

边：一个点的上下左右，的点的连线，作为边，该点的上下左右也就是其邻接点

由于机械室激光雷达水平方向上是360°的，所以左右边界和另一个也构成邻接点。但是垂直方向上不连续的，所以不能把上下边界作为邻接点。

根据BFS算法，遍历可以从任意一个有效的点开始，至到所有的点都被遍历到

基于BFS的点云聚类方法：

如果没有被处理说明这是一个新的聚类，然后执行BFS的步骤

将队列里的首个元素弹出，然后将该元素近邻塞入队列末尾，在代码中为了执行效率，没有使用std::queue，使用的是普通数组，用双指针实现的一个队列的加入和弹出。

元素近邻就是上面建立的图模型的上下左右

聚类判别原理—通过一个角度判断
没有通过距离判断的原因，就是应为水平角度分辨率和垂直角度分辨率差别很大。所以水平方向和垂直方向的近邻点的距离会小很多。

要判断的角度如下图所示，该角度越大，说明离得越近，角度越小，说明离得越远


图中的，di和di+1与角度塞塔，是已知的，可以直接求得要判断的角度