def get_top_down_ground_truth_static_ego(env_id, start_idx, img_w, img_h, map_w, map_h): """ Returns the ground-truth label oriented in the global map frame :param env_id: :param start_idx: :param img_w: :param img_h: :param map_w: :param map_h: :return: """ path = load_path(env_id) #instruction_segments = [self.all_instr[env_id][set_idx]["instructions"][seg_idx]] start_pt, dir_yaw = tdd.get_start_pt_and_yaw(path, start_idx, map_w, map_h, 0) affine = tdd.get_affine_matrix(start_pt, dir_yaw, img_w, img_h) seg_labels = np.zeros([img_w, img_h, 2]).astype(float) path_in_img = cf_to_img(path, np.array([map_w, map_h])) #gauss_sigma = map_w / 96 gauss_sigma = map_w / 32 seg_labels[:, :, 0] = tdd.plot_path_on_img(seg_labels[:, :, 0], path_in_img) if len(path_in_img) > 1: seg_labels[:, :, 1] = tdd.plot_dot_on_img(seg_labels[:, :, 1], path_in_img[-1], gauss_sigma) seg_labels_rot = tdd.apply_affine(seg_labels, affine, img_w, img_h) seg_labels_rot[:, :, 0] = gaussian_filter(seg_labels_rot[:, :, 0], gauss_sigma) seg_labels_rot[:, :, 1] = gaussian_filter(seg_labels_rot[:, :, 1], gauss_sigma) DEBUG = True if DEBUG: cv2.imshow("l_traj", seg_labels_rot[:, :, 0]) cv2.imshow("l_endpt", seg_labels_rot[:, :, 1]) cv2.waitKey(0) # Standardize both channels separately (each has mean zero, unit variance) seg_labels_path = standardize_2d_prob_dist(seg_labels_rot[:, :, 0:1]) seg_labels_endpt = standardize_2d_prob_dist(seg_labels_rot[:, :, 1:2]) seg_labels_rot = np.concatenate((seg_labels_path, seg_labels_endpt), axis=0) seg_labels_t = torch.from_numpy(seg_labels_rot).unsqueeze(0).float() return seg_labels_t
def get_top_down_ground_truth_dynamic_global(env_id, start_idx, end_idx, drone_pos_as, img_w, img_h, map_w, map_h): """ Returns the ground-truth label oriented in the global map frame :param env_id: :param start_idx: :param img_w: :param img_h: :param map_w: :param map_h: :return: """ PROFILE = False prof = SimpleProfiler(False, PROFILE) path = load_path(env_id, anno=True) #print(len(path), start_idx, end_idx) path = path[start_idx:end_idx] #instruction_segments = [self.all_instr[env_id][set_idx]["instructions"][seg_idx]] prof.tick("load_path") units = UnrealUnits(1.0) drone_pos_cf = units.pos3d_from_as(drone_pos_as) #print("Dynamic ground truth for ", env_id, start_idx, end_idx) gt_dynamic = get_dynamic_ground_truth_v2(path, drone_pos_cf[:2]) #Presenter().plot_path(env_id, [path[start_idx:end_idx], gt_dynamic]) prof.tick("gen_gt_path") seg_labels = np.zeros([img_w, img_h, 2]).astype(float) path_in_img = cf_to_img(gt_dynamic, np.array([map_w, map_h])) gauss_sigma = map_w / 96 seg_labels[:, :, 0] = tdd.plot_path_on_img(seg_labels[:, :, 0], path_in_img) if len(path_in_img) > 1: seg_labels[:, :, 1] = tdd.plot_dot_on_img(seg_labels[:, :, 1], path_in_img[-1], gauss_sigma) prof.tick("plot_path") seg_labels[:, :, 0] = gaussian_filter(seg_labels[:, :, 0], gauss_sigma) seg_labels[:, :, 1] = gaussian_filter(seg_labels[:, :, 1], gauss_sigma) # Standardize both channels separately (each has mean zero, unit variance) seg_labels_path = standardize_2d_prob_dist(seg_labels[:, :, 0:1]) seg_labels_endpt = standardize_2d_prob_dist(seg_labels[:, :, 1:2]) prof.tick("process_img") DEBUG = False if DEBUG: gt_path_in_img = cf_to_img(path, np.asarray([map_w, map_h])) dbg_labels_gt = np.zeros([img_w, img_h, 1]) dbg_labels_gt[:, :, 0] = tdd.plot_path_on_img(dbg_labels_gt[:, :, 0], gt_path_in_img) Presenter().show_image(dbg_labels_gt, "dbg", torch=False, waitkey=10, scale=4) Presenter().show_image(torch.from_numpy(seg_labels_path), "l_path", torch=True, waitkey=10, scale=4) Presenter().show_image(torch.from_numpy(seg_labels_endpt), "l_endp", torch=True, waitkey=100, scale=4) seg_labels = np.concatenate((seg_labels_path, seg_labels_endpt), axis=0) seg_labels_t = torch.from_numpy(seg_labels).unsqueeze(0).float() prof.tick("prep_out") prof.print_stats() return seg_labels_t