Lego-loam参考提取地面点论文
《Fast Segmentation of 3D Point Clouds for Ground Vehicles》
总的看 ：360度 分扇区，扇区内拟合直线，点云点到直线距离是否小于阈值，小于则为地面点。
首先，每个点一个标签；
把xy平面看成一个半径无限大的圆，在把这个圆划分为若干离散的扇区，然后定义一个角度Δɑ代表这个扇区的覆盖范围，从而扇区的个数M = 2PI / Δɑ;则进一步可判断一个点属于哪个扇区， segment(pi) = atan2(yi,xi) / Δɑ;
我们称在一个扇区的点为Ps .
但是注意，点是3D的点，不能 当成2D点来处理，所以下面还得继续做一些处理，
提出一个概率bin, 即一个扇区里，根据sqrt(xx + yy)来划分扇区中的每个点属于哪个bin, 如上图蓝色短线。bs j, s代表第几个扇区， j代表这个扇区中的第几个bin.
一般bin的范围是，， 来确定点属于哪个bin
显然，每个bin可能不只一个点，我们只要一个点为代表，直接选取z最小的点为这个bin的3D原型点，因为最可能在地面上。总结下， 这种每个bin中保留一个3D点的方法，合理的减少了点数量为后面的2D线拟合。拟合直线的复杂度则只依赖参数Δɑ和bin数量。
从原型点开始，进行直线拟合，
从每个扇区开始，用多个线程进行，每个线程负责几个扇区
我们开看一个扇区里， 扇区中有若干个bins_, 每个bin中最多有一个原型点即3D点，这个点是这个bin中的最小z的点云点。开始直线拟合，先把第一有值的bin的点左起点， 然后进行下面的操作：
1. 线的起点和要用于拟合的第二个点，之间的d即xy面的距离差不能超过阈值，意思是
拟合的起点和终点之间不能距离太远。
2. 线的起点和要用于拟合的第二个点，这两个点的高度z之差也不能太大，要小于阈值
满足这两个了，可以把第二个点加入到待拟合直线点集合中，现在有两个点了，具备拟合条件。当然，如果不满足上面的条件，从集合中删除第一个点，把第二个点加入集合作为新的起点。
（注意，bins_从第二个开始遍历，第一个会在在遍历之前加入待拟合直线线集合中）开始第三个bin的遍历了，现在集合中有两个点了， 进入到将要拟合代码，显然能进入这个了，代表已经有两个bin点了且不是长直线， ，现在开始看第三个bin了，所以拟合直线时，至少会会有三个点， 然后拟合。
因为是拟合的，所以计算下，刚才用于拟合的点集合中的点在现在拟合直线上的误差的平方。就是把x带入直线到y ，看这个y和这个点的y之间的差的平方，返回一个最大的误差即可，误差大于阈值了或者斜率大于阈值， 把新加入点集合的点弹出，然后
如果点集合没有至少三个点，则开始一条新的直线，集合中只保留最后一个点，其他剔除
，迭代器退一个。
如果有至少三个点的话，用剩下的点拟合一个直线，并直接认可这个直线，把集合中的头部和尾部点带入拟合直线得到两个z， 则可构成两个点了，把这个两个点看成直线返回，一条扇区里的直线完成。然后从点集合中把点删除到只剩下最后一个，从新开始一条直线了。
总结就是：就是不能往里加点拟合，直到新加入的点会导致误差超过阈值，或斜率超过阈值，就把这点之前的点认为合理，删除这个新加入的，拟合一个直线，肯定就满足要求了
另外，会将用于拟合直线的点集合的最后一个点带入其拟合的直线得到高度z，作为当前距离地面距离
直线拟合完了，开始分割****
根据点云点属于的扇区进行计算，计算点到其所在扇区拟合的直线的距离或者说误差
，方式就是把点(d,z)带入，带入d进去拟合的直线(当然要先判断这个点是否在这条直线的跨度内，即 用xy面内的距离d来衡量，这里直线定义是起终点的方式，所以看闯入的点(d,z)的d,是不是在起终点的d之间就行了)，得到一个expected_z，计算
std::fabs(z - expected_z)。这里说点在所属扇区的拟合直线内
但是，有可能啊，点不在所属扇区的直线，那就看其他相邻扇区，满足跨度要求，计算点到该拟合直线的误差，这个指小于一个阈值的话，就认为这个点是地面点，给个标签
，一个vector集合，其索引也对应着这个点云点在点云中的索引，必须索引对应一致，值赋值为1， 后面会根据这个集合来获取地面点呢，所以必须对应好。
