将点云生成BEV图像，之后对bev进行图像处理，找到直线，然后生成label.
python 代码实现，用于记录
import pickle, os, math
import numpy as nppython
from infer_lane import read_pcd
import cv2
from skimage.morphology import remove_small_objects,remove_small_holes
from skimage.measure import label
from skimage.color import label2rgb
from skimage.transform import resize
from PIL import Image
from matplotlib import pyplot as plt
from skimage.morphology import dilation, erosion
pcd_file = '/workspace/data/01/pcd/000201.pcd'
label_file = '/workspace/data/01/label/000201.label'
class Points2BEV():
def __init__(self) -> None:
data = []
with open(pcd_file, 'r') as f:
lines = f.readlines()[10:]
for line in lines:
point = []
line = list(line.strip('\n').split(' '))
x = float(line[0])
y = float(line[1])
z = float(line[2])
i = float(line[3])
r = math.sqrt(x**2 + y**2 + z**2) * i
point.append(x)
point.append(y)
point.append(z)
point.append(i)
point.append(r)
data.append(point)
# points = list(map(lambda line: list(map(lambda x: float(x), line.split(' '))), lines))
points = np.array(data) # N * 5
with open(label_file, 'r') as f:
lines = f.readlines()
label = list(map(lambda line: list(map(lambda x: float(x), line.split(' '))), lines))
label = np.array(label) # n x 1
points = np.array(np.hstack([points, label])) # N * 6
# lane points label == 4
lane_idxs = np.where(points[:,5] == 4)[0] # np.argwhere(points[:,5] == 4).flatten()
self.lane_points = points[lane_idxs] # lane points
rgb = np.ones([points.shape[0], 3])
rgb[lane_idxs, 1:] = 0
xyzrgb = np.array(np.hstack([points[:,:3], rgb]), dtype=np.float32)
# from baseline.utils.rviz import Rviz
# rviz = Rviz(xyzrgb)
# rviz.publish('xyzrgb')
bev_h = 1152
bev_w = 576
# self.bev_img = np.zeros((bev_h, bev_w))
self.bev_img = self.toBEV()
print('bev_img:', self.bev_img.shape)
def toBEV(self):
bev_h = 1152
bev_w = 576
list_grid_xy = [0.06, 0.04]
line_num = 6
line_color_list = [(0, 0, 255),(0, 50, 255),(0, 255, 255),(255, 255, 0),(255, 0, 255),(255, 0, 100)]
list_filter_roi = [0.02, 69.14, -11.52, 11.52, -2.0, 1.5]
bev_img = np.zeros((bev_h, bev_w))
x_min, x_max, y_min, y_max, z_min, z_max = list_filter_roi
x_grid, y_grid = list_grid_xy
list_pc_values = self.lane_points.tolist()
lane_points_roi = np.array(list(filter(lambda point: \
(point[0] > x_min) and (point[0] < x_max) and \
(point[1] > y_min) and (point[1] < y_max) and \
(point[2] > z_min) and (point[2] < z_max), list_pc_values)))
# for i, p in enumerate(lane_points_roi):
# x_img = int(-p[1] / bev_y_grid) # x axis is -y in lidar
# y_img = int(-p[0] / bev_x_grid) # y axis is -x in lidar
# x_img = x_img - int(np.floor(y_min / bev_y_grid))
# replace for loop
## 下面的两行操作直接将图像和lidar的坐标系对齐，即对x,y进行了翻转
x_img = (-lane_points_roi[:,1] / y_grid).astype(np.int32) # x axis is -y in lidar
y_img = (-lane_points_roi[:,0] / x_grid).astype(np.int32) # y axis is -x in lidar
x_img -= int(np.floor(y_min / y_grid))
y_img += int(np.floor(x_max / x_grid))
w_img = int((y_max - y_min) / y_grid)
h_img = int((x_max - x_min) / x_grid)
# print(h_img, w_img)
img = np.zeros([h_img, w_img], dtype=np.uint8)
img[y_img, x_img] = 255
## 下面的x, y没有翻转，图像是反的， np.flip(np.flip(bev_img, 0), 1) 进行x,y方向的偏转之后就可以正常了，看个人需求
x_img = ((lane_points_roi[:,0] - x_min) // x_grid)
y_img = ((lane_points_roi[:,1] - y_min) // y_grid)
# x_img = ((lane_points_roi[:,0] - x_min) / x_grid).astype(np.int32)
# y_img = ((lane_points_roi[:,1] - y_min) / y_grid).astype(np.int32)
# x_img = x_img.reshape(-1, 1)
# y_img = y_img.reshape(-1, 1)
img[x_img, y_img] = 255
# cv2.imshow('img_gray', img)
# cv2.waitKey(0)
# img_pil = Image.fromarray(img)
# img_pil.show()
# np.save('bev_img.npy', img_pil)
self.bev_img = img
return img
def hough_line(self):
print(cv2.__version__)
img_gray = cv2.cvtColor(np.asarray(self.bev_img, dtype=np.uint8), cv2.COLOR_BGR2BGRA)
canny = cv2.Canny(img_gray, 50, 100, )
# cv2.imshow('img_gray', img_gray)
# cv2.waitKey(0)
linesP = cv2.HoughLinesP(canny, 10, np.pi/180, 60, minLineLength=1, maxLineGap=500)
print(len(linesP))
line_num = 0
xy_list = []
xy1 = []
for line_points in linesP:
xy_list.append(line_points[0])
x1,y1,x2,y2 = line_points[0]
xy1.append([x1,y1])
if abs(x1 - x2) > 5:
line_num += 1
# cv2.line(canny, (x1, y1), (x2, y2), (255,0,0),1)
count = 0
i = 1
list = []
k = 0
lane_dict = {}
lane_list = []
while 1:
if len(xy_list) == 0:
break
if len(list) > 0:
list.insert(0, 0)
temp = xy_list
xy_list = [xy_list[i] for i in range(0, len(xy_list), 1) if i not in list]
for i,v in enumerate(list):
lane_list.append(temp[v])
lane_dict.update({f'{k}':lane_list})
lane_list = []
list = []
i = 1
while 1:
if len(xy_list) == 0:
break
if i == len(xy_list):
break
if i == len(xy_list):
val = abs(xy_list[0][0] - xy_list[-1][0])
else:
val = abs(xy_list[0][0] - xy_list[i][0])
if val <= 25:
list.append(i)
# print('loop2: ', i, len(xy_list), 'val: ', val , xy_list[0][0] ," xy_list[i][0]: ", xy_list[i][0])
i += 1
k += 1
if len(xy_list) <= 0:
break
if len(list) == 0:
print(list)
break
v_list = []
v_dict = {}
for i,v in enumerate(lane_dict.values()):
v_list.append(v[0][0])
v_dict.update({f'{i+1}':v[0][0]})
v_list.sort()
sort_lane = {}
for i in range(1, len(v_list)+1):
key = [k for k, v in v_dict.items() if v==v_list[i-1]]
sort_lane.update({f'lane_{i}':lane_dict[key[0]]})
for k,v in sort_lane.items():
x1_list = []
y1_list = []
x2_list = []
y2_list = []
for i in v:
x1,y1,x2,y2 = i
x1_list.append(x1)
y1_list.append(y1)
x2_list.append(x2)
y2_list.append(y2)
x1_mean = sum(x1_list)/len(x1_list)
y1_mean = sum(y1_list)/len(y1_list)
x2_mean = sum(x2_list)/len(x2_list)
y2_mean = sum(y2_list)/len(y2_list)
slope = (y2_mean-y1_mean)/(x2_mean-x1_mean)
b = y1_mean - slope*x1_mean
ymax = 1150
ymin = 0
x1 = int((ymin -b)/slope)
x2 = int((ymax -b)/slope)
print(x1, ymin, x2, ymax)
cv2.line(canny, (x1, ymin), (x2, ymax), (255,0,0),2)
# bev to label
point_label_dilated = remove_small_objects(canny.astype(bool), 500)
point_label_dilated_remove = remove_small_holes(point_label_dilated.astype(bool))
label_img = label(point_label_dilated_remove.astype(np.uint8), background=0,connectivity=2)
# pil_img = Image.fromarray(cv2.cvtColor(label_img, cv2.COLOR_GRAY2RGB))
# pil_img.show()
label_img = label_img.astype(np.uint8)
label_img -= 1
print('label_img: ', label_img.shape, np.unique(label_img))
print(label_img)
colored_label = label2rgb(label_img, bg_label=255)
resize(colored_label, (144,144))
plt.imshow(colored_label)
pil_img = Image.fromarray(cv2.cvtColor(canny, cv2.COLOR_GRAY2RGB))
pil_img.show()
if __name__=="__main__":
tobev = Points2BEV()
# tobev.toBEV()
tobev.hough_line()
参考链接：http://ronny.rest/tutorials/module/pointclouds_01/point_cloud_birdseye/
## 对上面toBEV函数的优化，用numpy数组的操作，代替list的遍历，性能会有提升
def point_projection(points, list_roi_xyz = [0.02, 46.08, -11.52, 11.52, -3.0, 1.1],
list_grid_xy = [0.04, 0.02],
list_img_size_xy=[1152, 1152],
list_value_idx = [2, 3, 4],
list_list_range = [[-3.0,1], [0,255], [0,32768]],
is_flip=False):
x_min, x_max, y_min, y_max, z_min, z_max = list_roi_xyz
idx = np.where((points[:, 0] > x_min) & (points[:, 0] < x_max) &
(points[:, 1] > y_min) & (points[:, 1] < y_max) &
(points[:, 2] > z_min) & (points[:, 2] < z_max))
points_roi = points[idx[0]]
x_min, _, y_min, _, _, _ = list_roi_xyz
x_grid, y_grid = list_grid_xy
data = dict()
data['points'] = points_roi
list_xy_values = points_roi[:,:2].tolist()
x_img = ((points_roi[:,0] - x_min)/ x_grid).astype(np.int32)
y_img = ((points_roi[:,1] - y_min) / y_grid).astype(np.int32)
# x_img -= int(np.floor(x_min / x_grid))
# y_img -= int(np.floor(y_min / y_grid))
x_img = x_img.reshape(-1, 1)
y_img = y_img.reshape(-1, 1)
arr_xy_values = np.concatenate((x_img, y_img), axis=1)
data.update({'img_idx': arr_xy_values})
# data['img_idx'] = arr_xy_values
n_channels = len(list_value_idx)
temp_img = np.full((list_img_size_xy[0], list_img_size_xy[1], n_channels), 0, dtype=float)
list_list_values = [] # z, intensity, reflectivity
for channel_idx, value_idx in enumerate(list_value_idx):
temp_arr = points_roi[:,value_idx].copy()
# Normalize
v_min, v_max = list_list_range[channel_idx]
temp_arr[np.where(temp_arr<v_min)] = v_min
temp_arr[np.where(temp_arr>v_max)] = v_max
temp_arr = (temp_arr-v_min)/(v_max-v_min)
# list_list_values.append(temp_arr)
for idx, xy in enumerate(arr_xy_values):
temp_img[xy[0], xy[1], channel_idx] = temp_arr[idx]
if is_flip:
temp_img = np.flip(np.flip(temp_img, 0), 1).copy()
# data.update({'input_img': temp_img})
data['input_img'] = temp_img
return data
import os
import json
import numpy as np
import cv2
def read_json(file):
# 打开JSON文件
with open(file, 'r') as f:
# # 将文件内容读取到字符串变量中
# data = f.read()
# # 解析JSON字符串为Python对象
# obj = json.loads(data)
data = json.load(f)
# 输出Python对象
objs = data[0].get('object')
lane_dict = {}
for i in objs:
lane_dict[i['obj_id']] = i['pointsArr']
return lane_dict
def points_to_bev_label(points_dict, list_roi_xyz = [0.0, 69.12, -11.52, 11.52, -2.5, 0.5],
list_voxel_xy = [0.48, 0.16],
list_grid_xy = [144, 144],):
x_min, x_max, y_min, y_max, z_min, z_max = list_roi_xyz
x_grid, y_grid = list_grid_xy
x_voxel, y_voxel = list_voxel_xy
label_img = np.full(list_grid_xy, 255, dtype=np.uint8)
for k, v in points_dict.items():
points = np.array(v)
# You would have noticed that the x and y axes were swapped,
# and direction reversed so that we can now start dealing with image coordinates.
# Shifting to New Origin, The x and y data is still not quite ready to be mapped to an image.
# We may still have negative x and y values. So we need to shift the data to make (0,0) the smallest value.
# x_img = ((-points[:, 1] - y_min) // y_voxel).astype(int) # x axis is -y in lidar
# y_img = ((-points[:, 0] + x_max) // x_voxel).astype(int) # y axis is -x in lidar
# for i, x in enumerate(x_img):
# label_img[y_img[i], x_img[i]] = k
# for i in range(len(x_img)-1):
# cv2.line(label_img, (x_img[i], y_img[i]), (x_img[i+1], y_img[i+1]), k, 1)
x_img = ((points[:, 0] - x_min) // x_voxel).astype(int)
y_img = ((points[:, 1] - y_min) // y_voxel).astype(int)
for i, x in enumerate(x_img):
label_img[x_img[i], y_img[i]] = k
for i in range(len(x_img)-1):
cv2.line(label_img, (y_img[i], x_img[i]), (y_img[i+1], x_img[i+1]), k, 1)
temp_img = np.flip(np.flip(label_img, 0), 1).copy()
cv2.imwrite('test.png', label_img)
cv2.imwrite('test1.png', temp_img)
# # 显示图像
# cv2.imshow('image', label_img)
# cv2.waitKey(0)
# cv2.destroyAllWindows()
if __name__=="__main__":
path = './json'
for i in os.listdir(path):
file = os.path.join(path, i)
# print(file)
lane_dict = read_json(file)
points_to_bev_label(lane_dict)