激光雷达点云数据内部空点补全算法
欢迎访问我的个人博客：zengzeyu.com
前言
点云数据区别于图像数据，不管是二维图像还是三维图像，图像数据都充满整个区域，二维图像中每个像素点都有值，灰度值、RGB值等；三维图像中有体数据（Voxel），根据光线投影算法等，可计算出每个体数据对应值，从而显示于显示器中。点云数据由于其扫描生成数据过程的特性，就决定了其在数据方面与图像数据不同，以机械式激光雷达为例，当出现以下情况时，该位置扫描生成的点云数据不存在（即为NAN点）：
- 激光发射器发射出去的激光未收到返回光束
- 激光接收器接收到的返回激光强度超出阈值范围
另外，在数据处理阶段，可根据需要对部分数据进行滤波处理，赋值为NAN点，也能造成该出点云缺失情况。
PCL库中有自带判断点云数据是否含有NAN点的函数： pcl::PointCloud， 以及过滤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 坐标值也采用同样的插值方法计算。
其中一个特殊情况：转角为 0° 与转角为 360° 是等效的，在查找过程中，当转角顺时针查找到 360 °时则置为 0°，当转角逆时针查找到 0° 时则置为 360°。
算法流程
根据以上思路，设计算法如下:
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. 速腾聚创自动驾驶激光雷达