title: 激光雷達點雲數據內部空點補全
date: 2018-03-28 22:55:54
categories: ROS
tags:
- ROS
- PCL
歡迎訪問我的個人博客：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 座標值也採用同樣的插值方法計算。
其中一個特殊情況：轉角爲 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] 的點。
以上。
參考文獻：