def test_intersection(self): FloatTensor = torch.FloatTensor nB = 5 nT = 5 nA = 5 nH = 44 nW = 50 nGT = 3 priors = torch.zeros(1, 1, nA, nH, nW, 4) grid_x = torch.arange(0, float(nH)).mul(self._fov_height / nH).repeat(nW, 1).view([1, 1, 1, nH, nW]).type( FloatTensor) grid_y = torch.arange(-nW / 2, nW / 2).mul(self._fov_width / nW).repeat(nH, 1).view([1, 1, 1, nH, nW]).type( FloatTensor) priors[..., 0] = grid_x.expand(-1, -1, nA, -1, -1).contiguous() priors[..., 1] = grid_y.expand(-1, -1, nA, -1, -1).contiguous() anchor_w = torch.arange(1, float(nA + 1)).view(1, 1, nA, 1, 1) anchor_l = torch.arange(1, float(nA + 1)).view(1, 1, nA, 1, 1).mul(2) priors[..., 2] = anchor_l priors[..., 3] = anchor_w point_form_priors = point_form_fafe(priors) ground_truth = torch.rand(nGT, 4).mul(40) point_form_gt = point_form_fafe(ground_truth) gt = point_form_gt.unsqueeze(0).unsqueeze(0).unsqueeze(0).expand(nA, nH, nW, -1, -1) prior = point_form_priors.squeeze(0).squeeze(0).unsqueeze(3).expand(-1, -1, -1, nGT, -1) # lollie = our_intersect(gt, prior) # overlap = our_jaccard(gt, prior) match = our_match(truths=gt, priors=prior)
def test_point_form_conversion(self): FloatTensor = torch.FloatTensor nB = 5 nT = 5 nA = 5 nH = 44 nW = 50 priors = torch.zeros(1, 1, nA, nH, nW, 4) grid_x = torch.arange(0, float(nH)).mul(self._fov_height / nH).repeat(nW, 1).view([1, 1, 1, nH, nW]).type( FloatTensor) grid_y = torch.arange(-nW / 2, nW / 2).mul(self._fov_width / nW).repeat(nH, 1).view([1, 1, 1, nH, nW]).type( FloatTensor) priors[..., 0] = grid_x.expand(-1, -1, nA, -1, -1).contiguous() priors[..., 1] = grid_y.expand(-1, -1, nA, -1, -1).contiguous() anchor_w = torch.arange(1, float(nA + 1)).view(1, 1, nA, 1, 1) anchor_l = torch.arange(1, float(nA + 1)).view(1, 1, nA, 1, 1).mul(2) priors[..., 2] = anchor_l priors[..., 3] = anchor_w new_priors = point_form_fafe(priors) print(priors.shape) print(new_priors.shape) print(priors[0][0][:][0][0]) print(new_priors[0][0][:][0][0])
def test_our_match(self): FloatTensor = torch.FloatTensor nB = 5 nT = 5 nA = 3 nH = 44 nW = 50 # Could be [nB=1, nT=1, nA=5, nH=44, nW=50, (x1, y1, x2, y2)] priors = torch.zeros(1, 1, nA, nH, nW, 4) grid_x = torch.arange(0, float(nH)).mul(self._fov_height / nH).repeat(nW, 1).view([1, 1, 1, nH, nW]).type( FloatTensor) grid_y = torch.arange(-nW / 2, nW / 2).mul(self._fov_width / nW).repeat(nH, 1).view([1, 1, 1, nH, nW]).type( FloatTensor) priors[..., 0] = grid_x.expand(-1, -1, nA, -1, -1).contiguous() priors[..., 1] = grid_y.expand(-1, -1, nA, -1, -1).contiguous() anchor_w = torch.arange(1, float(nA + 1)).view(1, 1, nA, 1, 1).mul(2) anchor_l = torch.arange(1, float(nA + 1)).view(1, 1, nA, 1, 1) priors[..., 2] = anchor_w priors[..., 3] = anchor_l new_priors = point_form_fafe(priors) print(new_priors.shape) print(priors[0][0][:][0][0]) print(new_priors[0][0][:][0][0]) # ground_truth: (#batches x #conseq_frames x # max targets forever x # ground truth dimensions) non_zero_gt = torch.Tensor([[14.9328, 5.4076, 4.4339, 1.8233, -2.1514, 1.0000], [5.9154, -1.9502, 1.7852, 0.8246, -1.7485, 1.0000], [39.5655, 8.8195, 5.5303, 1.8953, 1.9189, 1.0000]]) point_from_gt = point_form_fafe(non_zero_gt[..., 0:4]) print(point_from_gt.shape) print(point_from_gt) gt = point_from_gt.unsqueeze(0).unsqueeze(0).unsqueeze(0).expand(nA, nH, nW, -1, -1) prior = new_priors[0][0][...].unsqueeze(3).expand_as(gt) mask, gt_mask = our_match(truths_pf=gt, priors=prior, threshold=0.5) print("mask shape: {}".format(mask.shape)) print("gt mask shape: {}".format(gt_mask.shape)) print("gt mask: {}".format(gt_mask))
def test_point_form_ground_truth(self): ground_truth = torch.rand((2, 5, 50, 6)) # x, y, l, w, rot, class point_from_gt = point_form_fafe(ground_truth[..., 0:4]) print(point_from_gt.shape)
def eval_with_GT(model_path, data_path, config_path, v2=False): timestr = strftime("%Y-%m-%d_%H:%M") showroom_path = get_showroom_path(model_path, full_path_bool=True) print('Images will be saved to:\n\t{}'.format(showroom_path)) velo_path = os.path.join(data_path, 'training', 'velodyne') config = load_config(config_path) start_time = time() input_config = InputConfig(config['INPUT_CONFIG']) eval_config = EvalConfig(config['EVAL_CONFIG']) if v2: model = HaveFafe4Eval(input_config) else: model = FafeNet(input_config) if eval_config.use_cuda: if eval_config.cuda_device == 0: device = torch.device("cuda:0") print('Using CUDA:{}\n'.format(0)) elif eval_config.cuda_device == 1: device = torch.device("cuda:1") print('Using CUDA:{}\n'.format(1)) else: print('Functionality for CUDA device cuda:{} not yet implemented.'. format(eval_config.cuda_device)) print('Using cuda:0 instead...\n') device = torch.device("cuda:0") model = model.to(device) else: device = torch.device("cpu") print('Using CPU\n') imu_path = os.path.join(data_path, 'training', 'oxts') model.load_state_dict( torch.load( model_path, map_location=lambda storage, loc: storage)["model_state_dict"]) model.eval() print("Model loaded, loading time: {}".format(round( time() - start_time, 2))) timestr = strftime("%Y-%m-%d_%H:%M") eval_head = FafePredict(input_config, eval_config) eval_head.eval() print("Evalutation Model Loaded!") testing_datasets = [ TemporalBEVsDataset(input_config, root_dir=data_path, split='training', sequence=seq) for seq in eval_config.validation_seqs ] testing_dataset = ConcatDataset(testing_datasets) testing_loader = DataLoader(dataset=testing_dataset, batch_size=eval_config.batch_size, num_workers=eval_config.num_workers, shuffle=False) print("Starting to iterate over batches") if eval_config.use_gospa: gospa_scores_dict = {} for batch_idx, batch in enumerate(testing_loader): input, target, info = batch if eval_config.use_cuda: input = input.to(device) target = target.to(device) timeit = time() with torch.no_grad(): out_det, out_reg = model(input) inference = eval_head(out_det, out_reg) print("\nTime to propagate through network: {}".format( round(time() - timeit, 2))) nB = target.shape[0] nT = input_config.num_conseq_frames tic = time() for batch_index in range(nB): current_sequence = info["sequence"][batch_index] imud = load_imu(imu_path, current_sequence) print("Batch: {} \t| Seq: {}".format( batch_index, info["sequence"][batch_index]), end='') if eval_config.save_figs: frame = info["GT_indices"][0] file = os.path.join(velo_path, info["sequence"][batch_index], str(frame.item()).zfill(6) + '.bin') pc = np.fromfile(file, dtype=np.float32).reshape((-1, 4)) fig = draw_lidar( pc, off_screen_rendering=eval_config.off_screen_rendering) if eval_config.use_gospa: current_frame = info["Current_index"][batch_index].item() gospa_scores_dict[int(current_sequence), current_frame] = np.zeros(nT) for time_index in range(nT): # Get ground truths non_zero_gt_mask = target[batch_index][time_index].sum( dim=1) != 0 non_zero_gt = target[batch_index][time_index][non_zero_gt_mask] # Transform gt to 2D bbox x1y1x2y2 form point_form_gt = point_form_fafe(non_zero_gt) # Transform gt to 3D bbox form point_form_gt_3d = point_form_3d(point_form_gt, input_config.z_center, input_config.z_height, device=device) # Rotate gt 3D bboxes gt_center = center_size_3d(non_zero_gt, input_config.z_center, device) point_form_gt_3d_rot = rotate_3d_bbx(point_form_gt_3d, non_zero_gt[:, 4], gt_center, device) # Get inference inference_reg = inference[batch_index][time_index] # Calculate GOSPA if ordered to do so if eval_config.use_gospa: if inference_reg.is_cuda: gospa_gt = non_zero_gt[:, 0:2].cpu() gospa_inf = inference_reg[:, 0:2].cpu() else: gospa_gt = non_zero_gt[:, 0:2] gospa_inf = inference_reg[:, 0:2] gospa_score = GOSPA(gospa_gt, gospa_inf) gospa_scores_dict[int(current_sequence), current_frame][time_index] = gospa_score if eval_config.save_figs and eval_config.draw_gospa: fig = draw_gospa(gospa_score, fig, time_index) # Get probabilities of objects inference_probabilities = inference_reg[ ..., -1] if eval_config.show_confidence else None # Transform inference to 2D bbox x1y1x2y2 inf_points_2d = point_form_fafe(inference_reg) # Transform inference to 3D bbox inference_points = point_form_3d(inf_points_2d, input_config.z_center, input_config.z_height, device) # Rotate inference 3D bboxes inference_center = center_size_3d(inference_reg, input_config.z_center, device) inference_points_rot = rotate_3d_bbx(inference_points, inference_reg[:, 4], inference_center, device) # Translate center points according to time_index translated_gt_c = translate_center(gt_center, imu_data=imud[frame.item()], timestep=time_index, dt=input_config.dt, device=device) translated_infer_c = translate_center( inference_center, imu_data=imud[frame.item()], timestep=time_index, dt=input_config.dt, device=device) # TODO: Move these if-statements further up to reduce computation if eval_config.save_figs: if time_index == 0: fig = draw_gt_boxes3d(point_form_gt_3d_rot, fig, color=eval_config.gt_color) fig = draw_gt_boxes3d( inference_points_rot, fig, color=eval_config.infer_color, probabilities=inference_probabilities) else: fig = draw_points_3d(translated_gt_c, fig, color=eval_config.gt_color) fig = draw_points_3d(translated_infer_c, fig, color=eval_config.infer_color) if eval_config.save_figs: filename = "_".join( ("Infer", "seq", info["sequence"][batch_index], "f", str(frame.item()), timestr + ".png")) filepath = os.path.join(showroom_path, 'oracle_view', filename) filename_top = "_".join( ("Top_Infer", "seq", info["sequence"][batch_index], "f", str(frame.item()), timestr + ".png")) filepath_top = os.path.join(showroom_path, 'top_view', filename_top) mayavi.mlab.view(azimuth=90, elevation=0, focalpoint=[24., 0, 0], distance=120.0, figure=fig) mayavi.mlab.savefig(filepath_top, figure=fig) mayavi.mlab.view( azimuth=180, elevation=70, focalpoint=[12.0909996, -1.04700089, -2.03249991], distance=62.0, figure=fig) mayavi.mlab.savefig(filepath, figure=fig) print('\t | time: {}'.format(round(time() - tic, 2))) print(gospa_scores_dict)
def eval_with_GT(fafe_model_path, pp_model_path, data_path, config_path): timestr = strftime("%Y-%m-%d_%H:%M") showroom_path = get_showroom_path(fafe_model_path, full_path_bool=True) print('Images will be saved to:\n\t{}'.format(showroom_path)) velo_path = os.path.join(data_path, 'training', 'velodyne') config = load_config(config_path) start_time = time() input_config = InputConfig(config['INPUT_CONFIG']) train_config = TrainConfig(config['TRAIN_CONFIG']) eval_config = EvalConfig(config['EVAL_CONFIG']) model_config = ModelConfig(config['MODEL_CONFIG']) post_config = PostConfig(config['POST_CONFIG']) pillar = PillarOfFafe(input_config, train_config.batch_size, train_config.verbose) if model_config.model == 'little_fafe': fafe = LittleFafe(input_config) else: fafe = FafeNet(input_config) if eval_config.use_cuda: if eval_config.cuda_device == 0: device = torch.device("cuda:0") print('Using CUDA:{}\n'.format(0)) elif eval_config.cuda_device == 1: device = torch.device("cuda:1") print('Using CUDA:{}\n'.format(1)) else: print('Functionality for CUDA device cuda:{} not yet implemented.'.format(eval_config.cuda_device)) print('Using cuda:0 instead...\n') device = torch.device("cuda:0") pillar = pillar.to(device) fafe = fafe.to(device) else: device = torch.device("cpu") print('Using CPU\n') imu_path = os.path.join(data_path, 'training', 'oxts') pillar.load_state_dict(torch.load(pp_model_path, map_location=lambda storage, loc: storage)["model_state_dict"]) fafe.load_state_dict(torch.load(fafe_model_path, map_location=lambda storage, loc: storage)["model_state_dict"]) pillar.eval() fafe.eval() print("Model loaded, loading time: {}".format(round(time() - start_time, 2))) timestr = strftime("%Y-%m-%d_%H:%M") eval_head = FafePredict(input_config, eval_config) eval_head.eval() print("Evalutation Model Loaded!") testing_datasets = [VoxelDataset(input_config, root_dir=data_path, split='training', sequence=seq) for seq in eval_config.validation_seqs] testing_dataset = ConcatDataset(testing_datasets) testing_loader = DataLoader(dataset=testing_dataset, batch_size=eval_config.batch_size, num_workers=eval_config.num_workers, shuffle=False) print("Starting to iterate over batches") if eval_config.use_gospa: gospa_scores_dict = {} for batch_idx, batch in enumerate(testing_loader): # Create Pillar Pseudo Img voxel_stack, coord_stack, num_points_stack, num_nonempty_voxels, target, info = batch # Move all input data to the correct device if not using CPU if train_config.use_cuda: voxel_stack = voxel_stack.to(device) coord_stack = coord_stack.to(device) num_points_stack = num_points_stack.to(device) num_nonempty_voxels = num_nonempty_voxels.to(device) target = target.to(device) timeit = time() with torch.no_grad(): pseudo_stack = [] for time_iter in range(input_config.num_conseq_frames): pseudo_image = pillar(voxel_stack[:, time_iter], num_points_stack[:, time_iter], coord_stack[:, time_iter], num_nonempty_voxels[:, time_iter]) if input_config.pp_verbose: print("Pseudo_img: {}".format(pseudo_image.unsqueeze(1).shape)) pseudo_stack.append(pseudo_image.unsqueeze(1)) pseudo_torch = torch.cat(pseudo_stack, dim=1) if train_config.use_cuda: pseudo_torch = pseudo_torch.to(device) target = target.to(device) # Forward propagation. Reshape by squishing together Time and Channel dimensions. # Reshaping basically as we do with BEV, stacking them on top of eachother. out_detection, out_regression = fafe.forward( pseudo_torch.reshape(-1, pseudo_torch.shape[1] * pseudo_torch.shape[2], pseudo_torch.shape[-2], pseudo_torch.shape[-1])) inference = eval_head(out_detection, out_regression) print("\nTime to propagate through network: {}".format(round(time() - timeit, 2))) nB = target.shape[0] nT = input_config.num_conseq_frames tic = time() for batch_index in range(nB): current_sequence = info["sequence"][batch_index] frame = info["GT_indices"][0] if torch.is_tensor(current_sequence): current_sequence = current_sequence.item() imud = load_imu(imu_path, current_sequence) print("Batch: {} \t| Seq: {} Frame: {}".format(batch_index, current_sequence, frame.item()), end='') if eval_config.save_raw: if not os.path.exists("showroom/detections_" +str(current_sequence).zfill(4)): os.mkdir("showroom/detections_" +str(current_sequence).zfill(4)) filename = os.path.join("showroom", "detections_{}".format(str(current_sequence).zfill(4)), str(int(frame)).zfill(4) + ".pt") torch.save(inference, filename) if eval_config.save_figs: file = os.path.join(velo_path, str(current_sequence).zfill(4), str(frame.item()).zfill(6) + '.bin') pc = np.fromfile(file, dtype=np.float32).reshape((-1, 4)) fig = draw_lidar(pc, off_screen_rendering=eval_config.off_screen_rendering) if eval_config.use_gospa: current_frame = info["Current_index"][batch_index].item() gospa_scores_dict[int(current_sequence), current_frame] = np.zeros(nT) for time_index in range(nT): # Get ground truths non_zero_gt_mask = target[batch_index][time_index].sum(dim=1) != 0 non_zero_gt = target[batch_index][time_index][non_zero_gt_mask] # Transform gt to 2D bbox x1y1x2y2 form point_form_gt = point_form_fafe(non_zero_gt) # Transform gt to 3D bbox form point_form_gt_3d = point_form_3d(point_form_gt, input_config.z_center, input_config.z_height, device=device) # Rotate gt 3D bboxes gt_center = center_size_3d(non_zero_gt, input_config.z_center, device) point_form_gt_3d_rot = rotate_3d_bbx(point_form_gt_3d, non_zero_gt[:, 4], gt_center, device) # Get inference inference_reg = inference[batch_index][time_index] # Filter out if outside FOV filtered_inference_reg = [] for inf_reg in inference_reg: np_inf_reg = inf_reg.cpu().data.numpy() if within_fov(np_inf_reg[0:2], min_angle=0.78, max_angle=2.45, max_radius=100): filtered_inference_reg.append(np_inf_reg) # Filter out if too closely located filtered_inference_reg = too_close(filtered_inference_reg, eval_config.distance_threshold) if eval_config.save_detections_as_measurements and time_index == 0: for fir in filtered_inference_reg: file1 = open(showroom_path + "/detections_" + str(current_sequence).zfill(4) + ".txt", "a") # Save it on the format PMBM wants it... p = [x, y, rot] with x defined right wards and y forward file1.write(str(frame.item()) + ' Car:' + str(fir[5]) + ' ' + str(- fir[1]) + ' ' + str( fir[0]) + ' ' + str(fir[4]) + "\n") file1.close() # Make filtered_inference_reg numpy because GOSPA needs it to be filtered_inference_reg = np.array(filtered_inference_reg) # Calculate GOSPA if ordered to do so if eval_config.use_gospa: if inference_reg.is_cuda: gospa_gt = non_zero_gt[:, 0:2].cpu() # gospa_inf = inference_reg[:, 0:2].cpu() gospa_inf = filtered_inference_reg[:, 0:2] if len(filtered_inference_reg) > 0 else np.array([]) else: gospa_gt = non_zero_gt[:, 0:2] # gospa_inf = inference_reg[:, 0:2] gospa_inf = filtered_inference_reg[:, 0:2] gospa_score = GOSPA(gospa_gt, gospa_inf) gospa_scores_dict[int(current_sequence), current_frame][time_index] = gospa_score if eval_config.save_figs and eval_config.draw_gospa: fig = draw_gospa(gospa_score, fig, time_index) # Make filtered_inference_reg torch tensor because point forms need it to be filtered_inference_reg = torch.from_numpy(filtered_inference_reg).to(device) # If no outputs -> plot ev. gt and continue if len(filtered_inference_reg) == 0: if time_index == 0: fig = draw_gt_boxes3d(point_form_gt_3d_rot, fig, color=eval_config.gt_color) continue else: # Translate center points according to time_index translated_gt_c = translate_center(gt_center, imu_data=imud[frame.item()], timestep=time_index, dt=input_config.dt, device=device) fig = draw_points_3d(translated_gt_c, fig, color=eval_config.gt_color) continue # Get probabilities of objects inference_probabilities = filtered_inference_reg[..., -1] if eval_config.show_confidence else None # Transform inference to 2D bbox x1y1x2y2 inf_points_2d = point_form_fafe(filtered_inference_reg) # Transform inference to 3D bbox inference_points = point_form_3d(inf_points_2d, input_config.z_center, input_config.z_height, device) # Rotate inference 3D bboxes inference_center = center_size_3d(filtered_inference_reg, input_config.z_center, device) inference_points_rot = rotate_3d_bbx(inference_points, filtered_inference_reg[:, 4], inference_center, device) # Translate center points according to time_index translated_gt_c = translate_center(gt_center, imu_data=imud[frame.item()], timestep=time_index, dt=input_config.dt, device=device) translated_infer_c = translate_center(inference_center, imu_data=imud[frame.item()], timestep=time_index, dt=input_config.dt, device=device) # TODO: Move these if-statements further up to reduce computation if eval_config.save_figs: if time_index == 0: fig = draw_gt_boxes3d(point_form_gt_3d_rot, fig, color=eval_config.gt_color) fig = draw_gt_boxes3d(inference_points_rot, fig, color=eval_config.infer_color, probabilities=inference_probabilities) else: fig = draw_points_3d(translated_gt_c, fig, color=eval_config.gt_color) fig = draw_points_3d(translated_infer_c, fig, color=eval_config.infer_color) if eval_config.save_figs: filename = "_".join( ("Infer", "seq", str(current_sequence), "f", str(frame.item()), timestr + ".png")) filepath = os.path.join(showroom_path, 'oracle_view', filename) filename_top = "_".join( ("Top_Infer", "seq", str(current_sequence), "f", str(frame.item()), timestr + ".png")) filepath_top = os.path.join(showroom_path, 'top_view', filename_top) mayavi.mlab.view(azimuth=90, elevation=0, focalpoint=[24., 0, 0], distance=120.0, figure=fig) mayavi.mlab.savefig(filepath_top, figure=fig) mayavi.mlab.view(azimuth=180, elevation=70, focalpoint=[12.0909996, -1.04700089, -2.03249991], distance=62.0, figure=fig) mayavi.mlab.savefig(filepath, figure=fig) if eval_config.use_gospa: # Save gospa scores in txt file file2 = open(showroom_path + "/gospa_scores_" + str(current_sequence).zfill(4) + ".txt", "a") row_str = str(frame.item()) for time_index in range(nT): row_str += (' ' + str(gospa_scores_dict[int(current_sequence), current_frame][time_index])) file2.write(row_str + "\n") file2.close() print('\t | time: {}'.format(round(time() - tic, 2)))