用BFS（广度优先搜索）对16线激光雷达点云进行聚类分析

    广度优先搜索（BFS）是一种可以用于连通性分析的算法，也可以用于迷宫问题的求解。最近看点云处理的相关算法，发现BFS也可以用于点云的聚类，于是用16线激光雷达点云做了下实验。
    不过首先需要对激光雷达点云进行有序排列，将点云数据根据行列存入一个点云变量中。16线激光雷达的扫描范围为360度，一共16条扫描线。横向角度分辨率为0.2度，纵向为2度。于是一条扫描线上的激光点数目最多为：
$$360/0.2=1800$$
于是可以把一帧点云数据有序地存放到一个16x1800的3维矩阵中。
路面检测

    如图所示，底部黑色粗实线代表路面，蓝色粗实线代表障碍物。P1和P2时同一列上相邻的两个激光点。此时$\angle a>0$。当不存在障碍物时，P2落在地面上的P3位置，此时$\angle a=0$。根据这个性质就可以把路面点给标记出来。$\angle a$的计算比较简单就不放公式了。
聚类
然后就可以不考虑路面点，而对剩下的点进行BFS聚类，效果如下。

其中白色点为标记出来的道路平面上的点，其余是根据相邻点的空间距离得到的聚类结果。

代码写的有点粗糙

#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <ope