基于角度方法的地面检测

基于角度方法的地面检测

（感谢前辈）转自：https://zhuanlan.zhihu.com/p/75659410

方法来源是：Effificient Online Segmentation for Sparse 3D Laser Scans

分割地面的三个假设：

1 激光雷达和地面是平行的；

2 地面曲率较小；

3 最底下的一根射线的某些部分一定观察到了地面；

分割地面的原理

该方法是基于深度图，需要用深度图计算出上下两行之间的角度图；

以最底下一行的第一个非0点作为地面检测起点，如果小于30（或者45）度就是地面，再然后，如果相邻二个线条之间的角度变换小于5（或者7）度，就是一个类别，不断的往外扩散标注所有的角度图；

角度的计算方法：

分割的步骤：

1 压缩投影 _depth_image

2 修复_depth_image （这一步可以去掉，效果不佳）

3 计算角度图 angle_image

4 SG平滑

5 ZeroOutGroundBFS 过滤地面

分割效果：

之前阈值用的作者原来的角度阈值，效果不太好，后来更新为论文写的阈值后，效果变得比较好了；

速度：

python 运行比较慢，需要600ms, 作者自己提到的c++ 仅需要6-10ms;

分割代码：

def angle_map_obtain(depth_map):"""获取每行的反射点的高度角度差值注意：  总的行数会少了一行:param depth_map::return:"""yradians_image = depth_map[:, :, 5]  # 64*870*5d = depth_map[:, :, 3]sin_angles = np.sin(yradians_image)cos_angles = np.cos(yradians_image)zmat = d * sin_anglesxmat = d * cos_anglesdelta_z = abs(np.diff(zmat, axis=0))delta_x = abs(np.diff(xmat, axis=0))angle_map = np.arctan2(delta_z, delta_x)  # (63, 870)return angle_mapdef zero_out_ground_BFS(angle_image, depth_map,# start_threshold=np.radians(30),start_threshold=np.radians(45),# ground_remove_angle=np.radians(7),ground_remove_angle=np.radians(5),dilate=False):"""分割地面的主要代码部分"""depth = depth_map[:, :, 3]depth_shape = depth.shape# 初始 label , label matrixlabel = 1label_matrix = np.zeros(depth_shape)# 循环每列for j in range(depth_shape[1]):i = depth_shape[0] - 2while i > 0 and depth[i, j] < 0.001:i -= 1  # 它是从最下面往上面找的，也就是从最底下开始找起，if angle_image[i, j] > start_threshold:continue  # 初始行的角度超过了阈值，肯定不是地面if label_matrix[i, j] == 0:ground_BFS(i, j, label, label_matrix,depth, angle_image,ground_remove_angle, )if dilate:kernel = uniform_kernel(9)label_matrix = cv2.dilate(label_matrix, kernel, iterations=1)return label_matrixdef ground_BFS(i, j, label, label_matrix,depth, angle_image,ground_remove_angle):# BFS 地面打标签部分# TODO 用set 更合适，因为会添加进去很多重复的点，可以用set的自动去重；Q = [(i, j)]while len(Q) > 0:r, c = Q[0]Q.pop(0)  # 默认是丢弃最后一个元素， 要放在这里，不然会死循环if label_matrix[r, c] > 0:continue  # 已经标注# label 赋值label_matrix[r, c] = label# 深度值为0，不纳入计算if depth[r, c] < 0.0001:continuefor rn, cn in neighbourhood(r, c):# TODO 这里的尺寸还存在着问题if rn >= 63 or rn < 0 or cn < 0 or cn >= 869:continue  # 超出范围的点，不考虑if label_matrix[rn, cn] > 0:continue  # 已经标注if abs(angle_image[r, c] - angle_image[rn, cn]) < ground_remove_angle:Q.append((rn, cn))

还有一部分把点云投影到深度图的部分，以及修复深度图的部分没有贴上来了，投影的之前写过，修复的部分效果不大；