无人驾驶中的激光雷达（一）基于深度图的点云聚类方法

基于深度图的点云聚类方法

本篇博文主要讲述一种基于深度图的点云聚类方法。
基于深度图的点云聚类方法最早且被应用最多的文章是 Bogoslavskyi等人提出的"Fast range image-based segmentation of sparse 3D laser scans for online operation" 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

这里主要是基于这篇文章采用的点云聚类思想，对一些细节进行了更改并添加了一些其他东西来改善聚类效果，先放代码链接和效果：

https://github.com/wangx1996/LIDAR-Segmentation-Based-on-Range-Image

1.扫描线补偿

激光雷达的基本原理就是利用传感器发射的激光束反射的时间差来实现对周围物体的精准测距，但激光有可能打在某些反射率较低的物体时，激光束无法返回传感器，就造成该扫描线在那一时刻变成无效值，从整体点云看就是有该物体有部分点云缺失，比较典型的情况就是激光打在玻璃上或是打在黑色车辆上。

如图所示，测试车辆左侧的黑色车辆就造成了点云缺失：

这种情况会造成一个物体在聚类时会被分成多个小物体，因此为了改善这种情况需要对这些缺失的点进行补偿。

这里参考文章 Fast Multi-Pass 3D Point Segmentation Based on a Structured Mesh Graph for Ground Vehicles," 2018 IEEE Intelligent Vehicles Symposium (IV)中的做法：

遍历每一次激光器发射的点云数据，也就是一次发射的数据，当两个有数据的点之间有一定数量的缺失点并且这两个点的距离满足一定的阈值，就可以假设这两个点之间的点近似为线性关系，通过线性插值的方式补全点云。

补全后的效果蓝色的点为补全后的点，可以看见车辆上原本有大量的缺失点都被补全了：

2.地面点移除

参考这篇文章：Fast segmentation of 3D point clouds: A paradigm on LiDAR data for autonomous vehicle applications," 2017 IEEE International Conference on Robotics and Automation (ICRA)

具体有https://adamshan.blog.csdn.net/article/details/84569000讲过，可以看一下

效果如图：

3.深度图生成

深度图指的是将点云的数据转为平面数据形式进行显示，每个像素可以存储该点的距离、intensity等值，这种形式与kdtree或者octree不同的是，深度图有利于快速查找得到某一点的相邻点，并且这些点是具有一定顺序关系的，而不是依靠距离度量来查找相邻点。

这里参考Curved-Voxel Clustering for Accurate Segmentation of 3D LiDAR Point Clouds with Real-Time Performance," 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)

采用构建hash表的形式来代替一般的cv::Mat深度图形式, 哈希的键值则选择将图的像素按顺序拆解的值，比如一个图的高为$h$，宽为$w$，某一个点的像素坐标为$（row，col）$，那么按顺序拆解该图后，像素点的索引可以表示为：

$index=row\ast w+col$

最终构建的深度图如下所示：

4.邻域搜索与阈值设计

原有的论文里采用的是BFS的搜索方式来聚类某一个cluster，BFS在进行搜索的时候就需要额外的队列存储相邻元素，一定程度上运行时消耗较多内存，因此这里改做遍历图的形式，不需要再产生队列存储，对于每一个像素也是同样进行四邻域搜索进行阈值判断，但是这样做就无法一次性聚类完一个物体，因此会出现一个物体再聚类过程中出现多个cluster，但这些cluster在聚类的过程中会相互满足连通要求，这就需要对这些个cluster进行融合，因此需要类别冲突处理。

这里参考：A clustering method for efficient segmentation of 3D laser data," 2008 IEEE International Conference on Robotics and Automation, Pasadena, CA, 2008
具体可以看这篇论文是如何处理的。

对于阈值的设计，原论文中提到的一种启发式的阈值，对于垂直的墙或者一些两点间夹角较小的的物体无法很好地进行聚类，因此这里采用论文：
Line Extraction in 2D Range Images for Mobile Robotics. Journal of Intelligent and Robotic Systems 40, 267–297 (2004)
的方法。

该方法本用于2D激光雷达的断点判断，这里则用作判断3D雷达相邻两点是否为断点。

该阈值的示意图如下：
其中：$P_{n-1}$为当前的中心点，判断周边的点是否与该点形成断点， $\lambda$为设定的阈值内容，$\Delta \Phi$为激光雷达的角度分辨率，$r$为该点的量测距离，$D_{max}$为最终阈值大小，具体的计算方式可以自己推一下，比较基本的几何知识。
最终计算公式：



这里${\delta }_r$也是人工设定阈值

5.最终效果

6.代码链接

已经实现放在github上了：
https://github.com/wangx1996/LIDAR-Segmentation-Based-on-Range-Image