calib = Calibration(
img_path.replace(".png", ".txt").replace("image_2", "calib"))
bev_map = (bev_map.transpose(1, 2, 0) * 255).astype(np.uint8)
bev_map = cv2.resize(bev_map, (cnf.BEV_HEIGHT, cnf.BEV_WIDTH))
for box_idx, (cls_id, x, y, z, h, w, l, yaw) in enumerate(labels):
# Draw rotated box
yaw = -yaw
y1 = int((x - cnf.boundary['minX']) / cnf.DISCRETIZATION)
x1 = int((y - cnf.boundary['minY']) / cnf.DISCRETIZATION)
w1 = int(w / cnf.DISCRETIZATION)
l1 = int(l / cnf.DISCRETIZATION)
drawRotatedBox(bev_map, x1, y1, w1, l1, yaw,
cnf.colors[int(cls_id)])
# Rotate the bev_map
bev_map = cv2.rotate(bev_map, cv2.ROTATE_180)
labels[:, 1:] = lidar_to_camera_box(labels[:, 1:], calib.V2C, calib.R0,
calib.P2)
img_rgb = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR)
img_rgb = show_rgb_image_with_boxes(img_rgb, labels, calib)
out_img = merge_rgb_to_bev(img_rgb,
bev_map,
output_width=configs.output_width)
cv2.imshow('bev_map', out_img)
if cv2.waitKey(0) & 0xff == 27:
break
detections, bev_map, fps = do_detect(configs, model, bev_map, is_front=True)
# Draw prediction in the image
bev_map = (bev_map.permute(1, 2, 0).numpy() * 255).astype(np.uint8)
bev_map = cv2.resize(bev_map, (cnf.BEV_WIDTH, cnf.BEV_HEIGHT))
bev_map = draw_predictions(bev_map, detections, configs.num_classes)
# Rotate the bev_map
bev_map = cv2.rotate(bev_map, cv2.ROTATE_180)
img_path = metadatas['img_path'][0]
img_bgr = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2BGR)
calib = Calibration(configs.calib_path)
kitti_dets = convert_det_to_real_values(detections)
if len(kitti_dets) > 0:
kitti_dets[:, 1:] = lidar_to_camera_box(kitti_dets[:, 1:], calib.V2C, calib.R0, calib.P2)
img_bgr = show_rgb_image_with_boxes(img_bgr, kitti_dets, calib)
out_img = merge_rgb_to_bev(img_bgr, bev_map, output_width=configs.output_width)
write_credit(out_img, (80, 210), text_author='Cre: github.com/maudzung', org_fps=(80, 250), fps=fps)
if out_cap is None:
out_cap_h, out_cap_w = out_img.shape[:2]
fourcc = cv2.VideoWriter_fourcc(*'MJPG')
out_path = os.path.join(configs.results_dir, '{}_front.avi'.format(configs.foldername))
print('Create video writer at {}'.format(out_path))
out_cap = cv2.VideoWriter(out_path, fourcc, 30, (out_cap_w, out_cap_h))
out_cap.write(out_img)
if out_cap:
out_cap.release()
def evaluate_mAP(val_loader, model, configs, logger):
batch_time = AverageMeter('Time', ':6.3f')
data_time = AverageMeter('Data', ':6.3f')
progress = ProgressMeter(len(val_loader), [batch_time, data_time],
prefix="Evaluation phase...")
labels = []
sample_metrics = [] # List of tuples (TP, confs, pred)
# switch to evaluate mode
model.eval()
class_id = {0:'Car', 1:'Pedestrian', 2:'Cyclist'}
with torch.no_grad():
start_time = time.time()
for batch_idx, batch_data in enumerate(tqdm(val_loader)):
metadatas, targets= batch_data
batch_size = len(metadatas['img_path'])
voxelinput = metadatas['voxels']
coorinput = metadatas['coors']
numinput = metadatas['num_points']
dtype = torch.float32
voxelinputr = torch.tensor(
voxelinput, dtype=torch.float32, device=configs.device).to(dtype)
coorinputr = torch.tensor(
coorinput, dtype=torch.int32, device=configs.device)
numinputr = torch.tensor(
numinput, dtype=torch.int32, device=configs.device)
t1 = time_synchronized()
outputs = model(voxelinputr, coorinputr, numinputr)
outputs = outputs._asdict()
outputs['hm_cen'] = _sigmoid(outputs['hm_cen'])
outputs['cen_offset'] = _sigmoid(outputs['cen_offset'])
# detections size (batch_size, K, 10)
img_path = metadatas['img_path'][0]
#print(img_path)
calib = Calibration(img_path.replace(".png", ".txt").replace("image_2", "calib"))
detections = decode(outputs['hm_cen'], outputs['cen_offset'], outputs['direction'], outputs['z_coor'],
outputs['dim'], K=configs.K)
detections = detections.cpu().numpy().astype(np.float32)
detections = post_processing(detections, configs.num_classes, configs.down_ratio, configs.peak_thresh)
for i in range(configs.batch_size):
detections[i] = convert_det_to_real_values(detections[i])
img_path = metadatas['img_path'][i]
#rint(img_path)
datap = str.split(img_path,'/')
filename = str.split(datap[7],'.')
file_write_obj = open('../result/' + filename[0] + '.txt', 'w')
lidar_path = '/' + datap[1] + '/' + datap[2] + '/' + datap[3] + '/' + \
datap[4] + '/' + datap[5] + '/' + 'velodyne' + '/' + filename[0] + '.bin'
#print(lidar_path)
#show3dlidar(lidar_path, detections[i], calib.V2C, calib.R0, calib.P2)
dets = detections[i]
if len(dets) >0 :
dets[:, 1:] = lidar_to_camera_box(dets[:, 1:], calib.V2C, calib.R0, calib.P2)
for box_idx, label in enumerate(dets):
location, dim, ry = label[1:4], label[4:7], label[7]
if ry < -np.pi:
ry = 2*np.pi + ry
if ry > np.pi:
ry = -2*np.pi + ry
corners_3d = compute_box_3d(dim, location, ry)
corners_2d = project_to_image(corners_3d, calib.P2)
minxy = np.min(corners_2d, axis=0)
maxxy = np.max(corners_2d, axis=0)
bbox = np.concatenate([minxy, maxxy], axis=0)
if bbox[0] < 0 or bbox[2]<0:
continue
if bbox[1] > 1272 or bbox[3] > 375:
continue
oblist = ['Car',' ','0.0', ' ', '0', ' ', '-10', ' ','%.2f'%bbox[0], ' ', \
'%.2f' %bbox[1], ' ','%.2f'%bbox[2], ' ','%.2f'%bbox[3], ' ','%.2f'%dim[0], ' ','%.2f'%dim[1], ' ','%.2f'%dim[2], ' ', \
'%.2f' %location[0], ' ','%.2f'%location[1], ' ','%.2f'%location[2], ' ', '%.2f'%ry, '\n']
file_write_obj.writelines(oblist)
file_write_obj.close()
'''for sample_i in range(len(detections)):
def predictions_to_kitti_format(img_detections,
calib,
img_shape_2d,
img_size,
RGB_Map=None):
predictions = []
for detections in img_detections:
if detections is None:
continue
# Rescale boxes to original image
for x, y, z, h, w, l, im, re, *_, cls_pred in detections:
predictions.append([
cls_pred, x / img_size, y / img_size, z, h / img_size,
w / img_size, l / img_size, im, re
])
predictions = kitti_bev_utils.inverse_yolo_target(np.array(predictions),
cnf.boundary)
if predictions.shape[0]:
predictions[:, 1:] = transformation.lidar_to_camera_box(
predictions[:, 1:], calib.V2C, calib.R0, calib.P)
objects_new = []
corners3d = []
for index, l in enumerate(predictions):
if l[0] == 0:
str = "Car"
elif l[0] == 1:
str = "Pedestrian"
elif l[0] == 2:
str = "Cyclist"
else:
str = "Ignore"
line = '%s -1 -1 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0' % str
obj = kitti_data_utils.Object3d(line)
obj.t = l[1:4]
obj.h, obj.w, obj.l = l[4:7]
obj.ry = np.arctan2(math.sin(l[7]), math.cos(l[7]))
_, corners_3d = kitti_data_utils.compute_box_3d(obj, calib.P)
corners3d.append(corners_3d)
objects_new.append(obj)
if len(corners3d) > 0:
corners3d = np.array(corners3d)
img_boxes, _ = calib.corners3d_to_img_boxes(corners3d)
img_boxes[:, 0] = np.clip(img_boxes[:, 0], 0, img_shape_2d[1] - 1)
img_boxes[:, 1] = np.clip(img_boxes[:, 1], 0, img_shape_2d[0] - 1)
img_boxes[:, 2] = np.clip(img_boxes[:, 2], 0, img_shape_2d[1] - 1)
img_boxes[:, 3] = np.clip(img_boxes[:, 3], 0, img_shape_2d[0] - 1)
img_boxes_w = img_boxes[:, 2] - img_boxes[:, 0]
img_boxes_h = img_boxes[:, 3] - img_boxes[:, 1]
box_valid_mask = np.logical_and(img_boxes_w < img_shape_2d[1] * 0.8,
img_boxes_h < img_shape_2d[0] * 0.8)
for i, obj in enumerate(objects_new):
x, z, ry = obj.t[0], obj.t[2], obj.ry
beta = np.arctan2(z, x)
alpha = -np.sign(beta) * np.pi / 2 + beta + ry
obj.alpha = alpha
obj.box2d = img_boxes[i, :]
if RGB_Map is not None:
labels, noObjectLabels = kitti_bev_utils.read_labels_for_bevbox(
objects_new)
if not noObjectLabels:
labels[:, 1:] = transformation.camera_to_lidar_box(
labels[:, 1:], calib.V2C, calib.R0,
calib.P) # convert rect cam to velo cord
target = kitti_bev_utils.build_yolo_target(labels)
kitti_bev_utils.draw_box_in_bev(RGB_Map, target)
return objects_new
def show3dlidar(pointpaht, detections,V2C, R0, P2):
pointcloud = np.fromfile(pointpaht, dtype=np.float32).reshape(-1, 4)
x = pointcloud[:, 0] # x position of point
xmin = np.amin(x, axis=0)
xmax = np.amax(x, axis=0 )
y = pointcloud[:, 1] # y position of point
ymin = np.amin(y, axis=0)
ymax = np.amax(y, axis=0)
z = pointcloud[:, 2] # z position of point
zmin = np.amin(z, axis=0)
zmax = np.amax(z, axis=0)
d = np.sqrt(x ** 2 + y ** 2) # Map Distance from sensor
vals = 'height'
if vals == "height":
col = z
else:
col = d
fig = mayavi.mlab.figure(bgcolor=(0, 0, 0), size=(640, 500))
mayavi.mlab.points3d(x, y, z,
col, # Values used for Color
mode="point",
# 灰度图的伪彩映射
colormap='Blues', # 'bone', 'copper', 'gnuplot'
# color=(0, 1, 0), # Used a fixed (r,g,b) instead
figure=fig,
)
# 绘制原点
mayavi.mlab.points3d(0, 0, 0, color=(1, 1, 1), mode="sphere",scale_factor=0.2)
print(detections.shape)
detections[:, 1:8] = lidar_to_camera_box(detections[:, 1:8], V2C, R0, P2)
for i in range(detections.shape[0]):
h = float(detections[i][4])
w = float(detections[i][5])
l = float(detections[i][6])
x = float(detections[i][1])
y = float(detections[i][2])
z = float(detections[i][3])
x_corners = [l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2] ;
y_corners = [0, 0, 0, 0, -h, -h, -h, -h] ;
z_corners = [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2];
#print(x_corners)
#print(detections[i])
R = roty(float(detections[i][7]))
corners_3d = np.dot(R, np.vstack([x_corners, y_corners, z_corners]))
# print corners_3d.shape
#corners_3d = np.zeros((3,8))
corners_3d[0, :] = corners_3d[0, :] + x;
corners_3d[1, :] = corners_3d[1, :] + y;
corners_3d[2, :] = corners_3d[2, :] + z;
corners_3d = np.transpose(corners_3d)
box3d_pts_3d_velo = project_rect_to_velo(corners_3d)
#x1, y1, z1 = box3d_pts_3d_velo[0, :]
#x2, y2, z2 = box3d_pts_3d_velo[1, :]
if detections[i][0] == 1.0:
draw_gt_boxes3d([box3d_pts_3d_velo],1,color=(1,0,0), fig=fig)
else:
draw_gt_boxes3d([box3d_pts_3d_velo], 1, color=(0, 1, 0), fig=fig)
# 绘制坐标
'''axes = np.array(
[[20.0, 0.0, 0.0, 0.0], [0.0, 20.0, 0.0, 0.0], [0.0, 0.0, 20.0, 0.0]],
dtype=np.float64,
)
#x轴
mayavi.mlab.plot3d(
[0, axes[0, 0]],
[0, axes[0, 1]],
[0, axes[0, 2]],
color=(1, 0, 0),
tube_radius=None,
figure=fig,
)
#y轴
mayavi.mlab.plot3d(
[0, axes[1, 0]],
[0, axes[1, 1]],
[0, axes[1, 2]],
color=(0, 1, 0),
tube_radius=None,
figure=fig,
)
#z轴
mayavi.mlab.plot3d(
[0, axes[2, 0]],
[0, axes[2, 1]],
[0, axes[2, 2]],
color=(0, 0, 1),
tube_radius=None,
figure=fig,
)'''
mayavi.mlab.show()
