激光雷达点云数据内部空点补全

欢迎访问我的个人博客:zengzeyu.com

前言


点云数据区别于图像数据,不管是二维图像还是三维图像,图像数据都充满整个区域,二维图像中每个像素点都有值,灰度值、RGB值等;三维图像中有体数据(Voxel),根据光线投影算法等,可计算出每个体数据对应值,从而显示于显示器中。点云数据由于其扫描生成数据过程的特性,就决定了其在数据方面与图像数据不同,以机械式激光雷达为例,当出现以下情况时,该位置扫描生成的点云数据不存在(即为NAN点):

  • 激光发射器发射出去的激光未收到返回光束
  • 激光接收器接收到的返回激光强度超出阈值范围

另外,在数据处理阶段,可根据需要对部分数据进行滤波处理,赋值为NAN点,也能造成该出点云缺失情况。

PCL库中有自带判断点云数据是否含有NAN点的函数: pcl::PointCloud<pcl::PointXYZ>::is_dense(), 以及过滤NAN点函数:pcl::removeNaNFromPointCloud()

本文旨在补全内部空洞点,而不是去掉空洞点。

点云NAN点补全


本文补全原则基于有序点云(organised)进行处理,非有序点云无法进行处理(unorganised)。
以自动驾驶中使用的机械式激光雷达速腾聚创16线激光雷达RS-LiDAR-16为例,其生成的有序点云(organised)点云尺寸为 16 x 2016: 16为激光线数,2016为每一线激光绕中心一周旋转储存的点个数,因此有 16 x 2016 = 32256 个点,而实际得到的点数据基本不可能是32256,必有缺失。

补全规则

在每一线激光扫描得到一行点数据中,查找与NAN点最近的点进行补全,如果本行数据全部为NAN(虽然不可能发生),则此行可删除,调整点云尺寸。
该规则基于的原则:在同一线激光扫描得到的点中,由于水平方向数据分辨率很高,所以一行数据中每个点与其邻域内点相似。

算法设计

算法思路

为了简化叙述,本文将一线激光扫描得到数据缩小为 360 个点,即一帧点云尺寸变为 16x360。
以一线激光扫描数据为例,默认激光旋转方向为顺时针方向,采用线性差值方法进行补全,由于一线激光扫描一圈得到的数据在360°内任意位置都是对偶的,所以在空点附近查找两边非空点,用其值进行补全,具体参考下图示意。
图上半部分为一线激光雷达扫描得到数据鸟瞰图,其中黑色方块代表非空点,白色方块代表空点;下半部分为点距离图,根据线性插值方法可以补全非空点。非空点距离计算采用极坐标方式,首先得到线性插值得到的range,再使用当前转角转换到笛卡尔坐标系下,可得到其 x, y 坐标值,z 坐标值也采用同样的插值方法计算。

Screenshot from 2018-03-29 09:42:08.png

其中一个特殊情况:转角为 0° 与转角为 360° 是等效的,在查找过程中,当转角顺时针查找到 360 °时则置为 0°,当转角逆时针查找到 0° 时则置为 360°。

算法流程

根据以上思路,设计算法如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
def fix_nan_point(self, in_cloud):
#fix edeg nan point 1st
in_cloud = self.fix_left_edge_nan_point( in_cloud )
in_cloud = self.fix_right_edge_nan_point( in_cloud )
#fix centrol nan point
for i in range(in_cloud.shape[0]):
for j in range(1, in_cloud.shape[1]):
if in_cloud[i, j, -1] == -1:
nan_size = 1
left = j - 1
right = j + 1
while in_cloud[i, left, -1] == -1:
left -= 1
nan_size += 1
while in_cloud[i, right, -1] == -1:
right += 1
nan_size += 1
height_diff_cell = (in_cloud[i, right, 2] - in_cloud[i, left, 2]) / nan_size
range_diff_cell = (in_cloud[i, right, 3] - in_cloud[i, left, 3]) / nan_size
in_cloud[i, j, 2] = in_cloud[i, left, 2] + (j - left) * height_diff_cell
in_cloud[i, j, 3] = in_cloud[i, left, 3] + (j - left) * range_diff_cell
if abs(j - left) < abs(right-j):
in_cloud[i, j, -1] = in_cloud[i, left, -1]
else:
in_cloud[i, j, -1] = in_cloud[i, right, -1]
return in_cloud
def fix_left_edge_nan_point(self, in_cloud):
for i in range(in_cloud.shape[0]):
if in_cloud[i, 0, -1] == -1:
nan_size = 1
left = 359
right = 1
while in_cloud[i,left,-1] == -1:
left -= 1
nan_size += 1
while in_cloud[i,right,-1] == -1:
right += 1
nan_size +=1
height_diff_cell = (in_cloud[i, right, 2] - in_cloud[i, left, 2]) / nan_size
range_diff_cell = (in_cloud[i, right, 3] - in_cloud[i, left, 3]) / nan_size
in_cloud[i, 0, 2] = in_cloud[i, left, 2] + (360 - left) * height_diff_cell
in_cloud[i, 0, 3] = in_cloud[i, left, 3] + (360 - left) * range_diff_cell
if abs(360 - left) < right:
in_cloud[i, 0, -1] = in_cloud[i, left, -1]
else:
in_cloud[i, 0, -1] = in_cloud[i, right, -1]
return in_cloud
def fix_right_edge_nan_point(self, in_cloud):
for i in range(in_cloud.shape[0]):
if in_cloud[i, in_cloud.shape[1]-1, -1] == -1:
nan_size = 1
left = in_cloud.shape[1]-2
right = 0
while in_cloud[i,left,-1] == -1:
left -= 1
nan_size += 1
while in_cloud[i,right,-1] == -1:
right += 1
nan_size +=1
height_diff_cell = (in_cloud[i, right, 2] - in_cloud[i, left, 2]) / nan_size
range_diff_cell = (in_cloud[i, right, 3] - in_cloud[i, left, 3]) / nan_size
in_cloud[i, in_cloud.shape[1]-1, 2] = in_cloud[i, left, 2] + (in_cloud.shape[1]-1 - left) * height_diff_cell
in_cloud[i, in_cloud.shape[1]-1, 3] = in_cloud[i, left, 3] + (in_cloud.shape[1]-1 - left) * range_diff_cell
if abs(in_cloud.shape[1]-1 - left) < right + 1:
in_cloud[i, in_cloud.shape[1]-1, -1] = in_cloud[i, left, -1]
else:
in_cloud[i, in_cloud.shape[1]-1, -1] = in_cloud[i, right, -1]
return in_cloud

算法结果

下图结果为源数据先经过降采样之后,再进行补全NAN点操作。源数据中的 label 有三个值 [-1, 0, 1], 经过降采样然后补全操作只剩下 label 为[0, 1] 的点。

这里写图片描述

以上。


参考文献:

  1. PCL点云变换与移除NaN
  2. 速腾聚创自动驾驶激光雷达