def generate_random_wrong_path(env_id, start_idx, end_idx): env_config = load_env_config(env_id) current_path = load_path(env_id)[start_idx:end_idx] start_pos = current_path[0] landmark_locations = np.asarray( list(zip(env_config["xPos"], env_config["zPos"]))) distances = np.asarray( [np.linalg.norm(start_pos - p) for p in landmark_locations]) closest_lm_idx = np.argmin(distances) start_landmark = env_config["landmarkName"][closest_lm_idx] # For segment-level, we're never (if ever) gonna need more than 3 landmarks global NUM_LANDMARKS_VISISTED, DRONE_EDGE_CLEARANCE NUM_LANDMARKS_VISISTED = 3 DRONE_EDGE_CLEARANCE = 0 i = 0 while True: print(f"Attempt: {i}") i += 1 ret = try_make_curve(env_config, start_pos, start_landmark) if ret is not None: break pos_array, last_pos, last_landmark_visited = ret # Return a trajectory of the same length as the one which is being replaced return convert_path(pos_array[:(end_idx - start_idx)])
def faux_dataset_random_pt(eval_envs): print("Generating faux dataset") units = UnrealUnits(scale=1.0) dataset = [] for env_id in eval_envs: segment_dataset = [] config = load_env_config(env_id) template = load_template(env_id) start_pt = np.asarray(config["startPos"]) second_pt = np.asarray(config["startHeading"]) end_x = random.uniform(0, 1000) end_y = random.uniform(0, 1000) end_pt = np.asarray([end_x, end_y]) state1 = DroneState(None, start_pt) state2 = DroneState(None, second_pt) state3 = DroneState(None, end_pt) segment_dataset.append(bare_min_sample(state1, False, env_id)) segment_dataset.append(bare_min_sample(state2, False, env_id)) segment_dataset.append(bare_min_sample(state3, True, env_id)) dataset.append(segment_dataset) return dataset
def _load_drone_config(self, i, instance_id=None): conf_json = load_env_config(i) self._set_config(conf_json, "", "random_config", i=None, instance_id=instance_id)
def get_landmark_pos(self, env_id): template = load_template(env_id) config = load_env_config(env_id) landmark_idx = config["landmarkName"].index(template["landmark1"]) pos_x = config["xPos"][landmark_idx] pos_y = config["zPos"][landmark_idx] landmark_pos = np.asarray([pos_x, pos_y]) return landmark_pos
def make_curves_for_unique_config(config_id): start_pos, start_lm = None, None for env_id in range(config_id, config_id + NEW_CONFIG_EVERY_N, 1): config = load_env_config(env_id) curve_path = get_curve_path(env_id) plot_path = get_curve_plot_path(env_id) start_pos, start_lm = make_new_curve(config, curve_path, plot_path, start_pos, start_lm)
def faux_dataset_random_landmark(eval_envs): print("Generating faux dataset") units = UnrealUnits(scale=1.0) dataset = [] for env_id in eval_envs: segment_dataset = [] config = load_env_config(env_id) template = load_template(env_id) path = load_path(env_id) landmark_radii = config["radius"] start_pt = np.asarray(config["startPos"]) second_pt = np.asarray(config["startHeading"]) landmark_choice_ids = list(range(len(config["landmarkName"]))) choice_id = random.choice(landmark_choice_ids) target_x = config["xPos"][choice_id] target_y = config["zPos"][choice_id] target_lm_pos = np.asarray([target_x, target_y]) landmark_dir = target_lm_pos - start_pt method = template["side"] theta = math.atan2(landmark_dir[1], landmark_dir[0]) + math.pi if method == "infront": theta = theta elif method == "random": theta = random.random() * 2 * math.pi elif method == "behind": theta = theta + math.pi elif method == "left": theta = theta - math.pi / 2 elif method == "right": theta = theta + math.pi / 2 x, z = target_lm_pos[0], target_lm_pos[1] landmark_radius = landmark_radii[choice_id] sample_point = np.array([ x + math.cos(theta) * landmark_radius, z + math.sin(theta) * landmark_radius ]) state1 = DroneState(None, start_pt) state2 = DroneState(None, second_pt) state3 = DroneState(None, sample_point) segment_dataset.append(bare_min_sample(state1, False, env_id)) segment_dataset.append(bare_min_sample(state2, False, env_id)) segment_dataset.append(bare_min_sample(state3, True, env_id)) dataset.append(segment_dataset) return dataset
def gen_lm_aux_labels(self, env_id, instruction, affine): env_conf_json = load_env_config(env_id) landmark_names, landmark_indices, landmark_positions = get_landmark_locations_airsim( env_conf_json) landmark_pos_in_img = pos_m_to_px( np.asarray(landmark_positions)[:, 0:2], np.array([self.map_w, self.map_h])) landmark_pos_in_seg_img = apply_affine_on_pts(landmark_pos_in_img, affine) if False: plot_path_on_img(self.latest_img_dbg, landmark_pos_in_img) plot_path_on_img(self.latest_rot_img_dbg, landmark_pos_in_seg_img) cv2.imshow("img", self.latest_img_dbg) cv2.imshow("rot_img", self.latest_rot_img_dbg) cv2.waitKey(0) landmark_pos_t = torch.from_numpy(landmark_pos_in_seg_img).unsqueeze(0) landmark_indices_t = torch.LongTensor(landmark_indices).unsqueeze(0) mask1 = torch.gt(landmark_pos_t, 0) mask2 = torch.lt(landmark_pos_t, self.img_w) mask = mask1 * mask2 mask = mask[:, :, 0] * mask[:, :, 1] mask = mask landmark_pos_t = torch.masked_select( landmark_pos_t, mask.unsqueeze(2).expand_as(landmark_pos_t)).view([-1, 2]) landmark_indices_t = torch.masked_select(landmark_indices_t, mask).view([-1]) mentioned_names, mentioned_indices = get_mentioned_landmarks( self.thesaurus, instruction) mentioned_labels_t = empty_float_tensor(list( landmark_indices_t.size())).long() for i, landmark_idx_present in enumerate(landmark_indices_t): if landmark_idx_present in mentioned_indices: mentioned_labels_t[i] = 1 if len(landmark_indices_t) > 0: aux_label = { "lm_pos": landmark_pos_t, "lm_indices": landmark_indices_t, "lm_mentioned": mentioned_labels_t, "lm_visible": mask, } else: aux_label = { "lm_pos": [], "lm_indices": [], "lm_mentioned": [], "lm_visible": [] } return aux_label
def is_ambiguous(env_id): template = load_template(env_id) config = load_env_config(env_id) #TODO: Handle the case where instructions refer to multiple landmarks ref_landmark = template["landmark1"] occ_count = 0 for landmark_name in config["landmarkName"]: if has_ambiguous_noun_phrase(landmark_name, ref_landmark): occ_count += 1 if occ_count == 0: print("Error! Referred to landmark that's not found in env!") exit(-1) # More than one such landmark occurs in the test set if occ_count > 1: return True else: return False
def __getitem__(self, idx): if self.seg_level: env_id = self.seg_list[idx][0] set_idx = self.seg_list[idx][1] seg_idx = self.seg_list[idx][2] else: env_id = self.env_list[idx] print("top_down_dataset_sm __getitem__ load_env_config") env_conf_json = load_env_config(env_id) landmark_names, landmark_indices, landmark_positions = get_landmark_locations_airsim(env_conf_json) top_down_image = load_env_img(env_id) path = load_path(env_id) img_x = top_down_image.shape[0] img_y = top_down_image.shape[1] path_in_img_coords = self.cf_to_img(img_x, path) landmark_pos_in_img = self.as_to_img(img_x, np.asarray(landmark_positions)[:, 0:2]) self.pos_rand_image = self.pos_rand_range * img_x #self.plot_path_on_img(top_down_image, path_in_img_coords) #self.plot_path_on_img(top_down_image, landmark_pos_in_img) #cv2.imshow("top_down", top_down_image) #cv2.waitKey() input_images = [] input_instructions = [] label_images = [] aux_labels = [] # Somehow load the instruction with the start and end indices for each of the N segments if self.seg_level: instruction_segments = [self.all_instr[env_id][set_idx]["instructions"][seg_idx]] else: instruction_segments = self.all_instr[env_id][0]["instructions"] for seg_idx, seg in enumerate(instruction_segments): start_idx = seg["start_idx"] end_idx = seg["end_idx"] instruction = seg["instruction"] # TODO: Check for overflowz seg_path = path_in_img_coords[start_idx:end_idx] seg_img = top_down_image.copy() #test_plot = self.plot_path_on_img(seg_img, seg_path) # TODO: Validate the 0.5 choice, should it be 2? affine, cropsize = self.get_affine_matrix(seg_path, 0, [int(img_x / 2), int(img_y / 2)], 0.5) if affine is None: continue seg_img_rot = self.apply_affine(seg_img, affine, cropsize) seg_labels = np.zeros_like(seg_img[:, :, 0:1]).astype(float) seg_labels = self.plot_path_on_img(seg_labels, seg_path) seg_labels = gaussian_filter(seg_labels, 4) seg_labels_rot = self.apply_affine(seg_labels, affine, cropsize) #seg_labels_rot = gaussian_filter(seg_labels_rot, 4) seg_labels_rot = self.normalize_0_1(seg_labels_rot) # Change to true to visualize the paths / labels if False: cv2.imshow("rot_img", seg_img_rot) cv2.imshow("seg_labels", seg_labels_rot) rot_viz = seg_img_rot.astype(np.float64) / 512 rot_viz[:, :, 0] += seg_labels_rot.squeeze() cv2.imshow("rot_viz", rot_viz) cv2.waitKey(0) tok_instruction = tokenize_instruction(instruction, self.word2token) instruction_t = torch.LongTensor(tok_instruction).unsqueeze(0) # Get landmark classification labels landmark_pos_in_seg_img = self.apply_affine_on_pts(landmark_pos_in_img, affine) # Down-size images and labels if requested by the model if self.img_scale != 1.0: seg_img_rot = transform.resize( seg_img_rot, [seg_img_rot.shape[0] * self.img_scale, seg_img_rot.shape[1] * self.img_scale], mode="constant") seg_labels_rot = transform.resize( seg_labels_rot, [seg_labels_rot.shape[0] * self.img_scale, seg_labels_rot.shape[1] * self.img_scale], mode="constant") landmark_pos_in_seg_img = landmark_pos_in_seg_img * self.img_scale seg_img_rot = standardize_image(seg_img_rot) seg_labels_rot = standardize_image(seg_labels_rot) seg_img_t = torch.from_numpy(seg_img_rot).unsqueeze(0).float() seg_labels_t = torch.from_numpy(seg_labels_rot).unsqueeze(0).float() landmark_pos_t = torch.from_numpy(landmark_pos_in_seg_img).unsqueeze(0) landmark_indices_t = torch.LongTensor(landmark_indices).unsqueeze(0) mask1 = torch.gt(landmark_pos_t, 0) mask2 = torch.lt(landmark_pos_t, seg_img_t.size(2)) mask = mask1 * mask2 mask = mask[:, :, 0] * mask[:, :, 1] mask = mask landmark_pos_t = torch.masked_select(landmark_pos_t, mask.unsqueeze(2).expand_as(landmark_pos_t)).view([-1, 2]) landmark_indices_t = torch.masked_select(landmark_indices_t, mask).view([-1]) mentioned_names, mentioned_indices = get_mentioned_landmarks(self.thesaurus, instruction) mentioned_labels_t = empty_float_tensor(list(landmark_indices_t.size())).long() for i, landmark_idx_present in enumerate(landmark_indices_t): if landmark_idx_present in mentioned_indices: mentioned_labels_t[i] = 1 aux_label = { "landmark_pos": landmark_pos_t, "landmark_indices": landmark_indices_t, "landmark_mentioned": mentioned_labels_t, "visible_mask": mask, } if self.include_instr_negatives: # If we are to be using similar instructions according to the json file, then # initialize choices with similar instructions. Otherwise let choices be empty, and they will # be filled in the following lines. if self.instr_negatives_similar_only: choices = self.similar_instruction_map[str(env_id)][str(seg_idx)] else: choices = [] # If there are no similar instructions to this instruction, pick a completely random instruction if len(choices) == 0: while len(choices) == 0: env_options = list(self.similar_instruction_map.keys()) random_env = random.choice(env_options) seg_options = list(self.similar_instruction_map[random_env].keys()) if len(seg_options) == 0: continue random_seg = random.choice(seg_options) choices = self.similar_instruction_map[random_env][random_seg] pick = random.choice(choices) picked_env = pick["env_id"] picked_seg = pick["seg_idx"] picked_set = pick["set_idx"] picked_instruction = self.all_instr[picked_env][picked_set]["instructions"][picked_seg]["instruction"] tok_fake_instruction = tokenize_instruction(picked_instruction, self.word2token) aux_label["negative_instruction"] = torch.LongTensor(tok_fake_instruction).unsqueeze(0) input_images.append(seg_img_t) input_instructions.append(instruction_t) label_images.append(seg_labels_t) aux_labels.append(aux_label) return [input_images, input_instructions, label_images, aux_labels]
def generate_template_curve(config_id): config = load_env_config(config_id) # Make a curve that follows the given template make_template_curve(config, config_id)
def __call__(self, images, states, segment_data, mask): projector = PinholeProjector(img_x=images.size(3), img_y=images.size(2)) # presenter = Presenter() env_id = segment_data.metadata[0]["env_id"] conf_json = load_env_config(env_id) all_landmark_indices = get_landmark_name_to_index() landmark_names, landmark_indices, landmark_pos = get_landmark_locations_airsim(conf_json) path_array = load_path(env_id) goal_loc = self.__get_goal_location_airsim(path_array) # Traj length x 64 landmarks x 14 # 0-5: Present landmarks data # 0 - landmark present in img # 1-2 - landmark pix_x | pix_y # 3-5 - landmark world coords m_x | m_y # 6-7: Template data # 6 - landmark_mentioned index # 7 - mentioned_side index # 8 - landmark mentioned # 9-13: Goal data # 9-10 - goal_x_pix | goal_y_pix # 11-12 - goal_x | goal_y (world) # 13 - goal visible aux_labels = torch.zeros((images.size(0), len(all_landmark_indices), 14)) # Store goal location in airsim coordinates aux_labels[:, :, 11:13] = torch.from_numpy(goal_loc[0:2]).unsqueeze(0).unsqueeze(0).expand_as( aux_labels[:, :, 11:13]) for i, idx in enumerate(landmark_indices): aux_labels[:, idx, 3:6] = torch.from_numpy( landmark_pos[i]).unsqueeze(0).clone().repeat(aux_labels.size(0), 1, 1) for timestep in range(images.size(0)): # presenter.save_image(images[timestep], name="tmp.png", torch=True) if mask[timestep] == 0: continue cam_pos = states[timestep, 9:12] cam_rot = states[timestep, 12:16] goal_in_img, goal_in_cam, status = projector.world_point_to_image(cam_pos, cam_rot, goal_loc) if goal_in_img is not None: aux_labels[timestep, :, 9:11] = torch.from_numpy(goal_in_img[0:2]).unsqueeze(0).expand_as( aux_labels[timestep, :, 9:11]) aux_labels[timestep, :, 13] = 1.0 for i, landmark_world in enumerate(landmark_pos): landmark_idx = landmark_indices[i] landmark_in_img, landmark_in_cam, status = projector.world_point_to_image(cam_pos, cam_rot, landmark_world) # This is None if the landmark is behind the camera. if landmark_in_img is not None: # presenter.save_image(images[timestep], name="tmp.png", torch=True, draw_point=landmark_in_img) aux_labels[timestep, landmark_idx, 0] = 1.0 aux_labels[timestep, landmark_idx, 1:3] = torch.from_numpy(landmark_in_img[0:2]) # aux_labels[timestep, landmark_idx, 3:6] = torch.from_numpy(landmark_in_cam[0:3]) # aux_labels[timestep, landmark_idx, 8] = 1.0 if landmark_idx == mentioned_landmark_idx else 0 return aux_labels
def provider_lm_pos_lm_indices_fpv(segment_data, data): """ Data provider that gives the positions and indices of all landmarks visible in the FPV image. :param segment_data: segment dataset for which to provide data :return: ("lm_pos", lm_pos) - lm_pos is a list (over timesteps) of lists (over landmarks visible in image) of the landmark locations in image pixel coordinates ("lm_indices", lm_indices) - lm_indices is a list (over timesteps) of lists (over landmarks visible in image) of the landmark indices for every landmark included in lm_pos. These are the landmark classifier labels """ env_id = segment_data[0]["metadata"]["env_id"] domain = segment_data[0]["metadata"]["domain"] #if INSTRUCTIONS_FROM_FILE: # env_instr = load_instructions(env_id) conf_json = load_env_config(env_id) all_landmark_indices = get_landmark_name_to_index() landmark_names, landmark_indices, landmark_pos = get_landmark_locations_airsim( conf_json) params = P.get_current_parameters().get( "Model") or P.get_current_parameters().get("ModelPVN").get("Stage1") projector = PinholeCameraProjection(map_size_px=params["global_map_size"], world_size_px=params["world_size_px"], world_size_m=params["world_size_m"], img_x=params["img_w"], img_y=params["img_h"], cam_fov=params["cam_h_fov"], domain=domain, use_depth=False) traj_len = len(segment_data) lm_pos_fpv = [] lm_indices = [] lm_mentioned = [] lm_pos_map = [] for timestep in range(traj_len): t_lm_pos_fpv = [] t_lm_indices = [] t_lm_mentioned = [] t_lm_pos_map = [] if segment_data[timestep]["state"] is not None: cam_pos = segment_data[timestep]["state"].get_cam_pos_3d() cam_rot = segment_data[timestep]["state"].get_cam_rot() instruction_str = segment_data[timestep]["instruction"] mentioned_landmark_names, mentioned_landmark_indices = get_mentioned_landmarks_nl( instruction_str) for i, landmark_in_world in enumerate(landmark_pos): landmark_idx = landmark_indices[i] landmark_in_img, landmark_in_cam, status = projector.world_point_to_image( cam_pos, cam_rot, landmark_in_world) this_lm_mentioned = 1 if landmark_idx in mentioned_landmark_indices else 0 # This is None if the landmark is behind the camera. if landmark_in_img is not None: # presenter.save_image(images[timestep], name="tmp.png", torch=True, draw_point=landmark_in_img) t_lm_pos_fpv.append(landmark_in_img[0:2]) t_lm_pos_map.append(landmark_in_world[0:2]) t_lm_indices.append(landmark_idx) t_lm_mentioned.append(this_lm_mentioned) if len(t_lm_pos_fpv) > 0: t_lm_pos_fpv = torch.from_numpy(np.asarray(t_lm_pos_fpv)).float() t_lm_pos_map = torch.from_numpy(np.asarray(t_lm_pos_map)).float() t_lm_indices = torch.from_numpy(np.asarray(t_lm_indices)).long() t_lm_mentioned = torch.from_numpy( np.asarray(t_lm_mentioned)).long() else: t_lm_pos_fpv = None t_lm_pos_map = None t_lm_indices = None t_lm_mentioned = None lm_pos_fpv.append(t_lm_pos_fpv) lm_pos_map.append(t_lm_pos_map) lm_indices.append(t_lm_indices) lm_mentioned.append(t_lm_mentioned) return [("lm_pos_fpv", lm_pos_fpv), ("lm_indices", lm_indices), ("lm_mentioned", lm_mentioned), ("lm_pos_map", lm_pos_map)]
def ground_terms(word2id, clustered_corpus, landmark_names, train_instructions): # the clustered corpus is a dictionary of lists, where the keys are valid english words and the values are # lists of words found in the corpus that are assumed to be misspellings of the key valid words # We make the distinction that a word is any word in an instruction # Terms are words in the english vocabulary. Multiple words (misspellings) can map to a single term. num_terms = len(clustered_corpus) vocab_size = len(word2id) num_landmarks = len(landmark_names) # This is gonna be the new word2id, once we start using the thesaurus term2id = {} id2term = {} for i, term in enumerate(sorted(clustered_corpus.keys())): term2id[term] = i id2term[i] = term # Calculate the mutual information between each cluster and each landmark # Number of times each term appears in an instruction term_occurences = np.zeros(num_terms) # Number of times each landmark appears near a segment path landmark_occurences = np.zeros(num_landmarks) # The number of times each term and landmark combination appears in the instruction and near the path term_landmark_cooccurences = np.zeros((num_terms, num_landmarks)) # The number of total segments that were considered total_occurences = 0 landmark_indices = get_landmark_name_to_index() # Inverse the clusters so that we can efficiently map each word in each instruction to it's cluster core word2term = {} for real_word, misspellings in clustered_corpus.items(): for misspelling in misspellings: word2term[misspelling] = real_word # Count landmark and word occurences and co-occurences for env_id, instruction_sets in train_instructions.items(): path = load_path(env_id) env_config = load_env_config(env_id) for instruction_set in instruction_sets[0]["instructions"]: instruction_str = instruction_set["instruction"] start_idx = instruction_set["start_idx"] end_idx = instruction_set["end_idx"] present_landmarks = close_landmark_names(env_config, path, start_idx, end_idx) present_lm_indices = [ landmark_indices[lm] for lm in present_landmarks ] mentioned_words = split_instruction( clean_instruction(instruction_str)) mentioned_terms = words_to_terms(mentioned_words, word2term) for term in mentioned_terms: term_id = term2id[term] term_occurences[term_id] += 1 for lm_idx in present_lm_indices: landmark_occurences[lm_idx] += 1 for term in mentioned_terms: term_id = term2id[term] term_landmark_cooccurences[term_id][lm_idx] += 1 total_occurences += 1 term_prob = np.expand_dims(term_occurences / total_occurences, 1).repeat(num_landmarks, 1) landmark_prob = np.expand_dims(landmark_occurences / total_occurences, 0).repeat(num_terms, 0) term_and_landmark_prob = term_landmark_cooccurences / total_occurences # term_and_landmark_prob has dimensions 0: terms, 1: landmarks mutual_info_factor = term_and_landmark_prob / (landmark_prob * term_prob + 1e-27) #mutual_info_factor = term_and_landmark_prob / ((1 / num_landmarks) * term_prob + 1e-9) mutual_info = term_and_landmark_prob * np.log(mutual_info_factor + 1e-27) # The above line is the correct formula for mutual information. For our case, below formula might be better? # The mutual information is higher for common words than uncommon ones. We might prefer the opposite effect. # On the other hand, uncommon words are more likely to spuriously correlate with landmarks, which will cause a # less reliable corpus. #mutual_info = np.log(mutual_info_factor + 1e-27) # Ground each term and produce the thesaurus term_meanings = {} common_words = [] for i in range(num_terms): grounded_lm_indices = [ idx for idx in range(num_landmarks) if mutual_info[i][idx] > MUTUAL_INFO_THRESHOLD ] grounded_lm_names = [ landmark_names[idx] for idx in grounded_lm_indices ] mutual_infos = np.asarray( [mutual_info[i][idx] for idx in grounded_lm_indices]) args = list(np.argsort(mutual_infos)) grounded_lm_names = list( reversed([grounded_lm_names[idx] for idx in args])) mutual_infos = list(reversed([mutual_infos[idx] for idx in args])) # If the word is too common to be referring to a landmark, ignore ita this_term_prob = term_prob[i][0] if this_term_prob > MAX_TERM_PROB: common_words.append(id2term[i]) grounded_lm_names = [] mutual_infos = [] term_meanings[id2term[i]] = \ { "landmarks": grounded_lm_names, "mutual_info": mutual_infos, "term_prob": this_term_prob } for k in term_meanings.keys(): if len(term_meanings[k]["landmarks"]) > 0: print(k, term_meanings[k]) print("Ignored groundings for these common words: " + str(common_words)) return term_meanings, word2term
def show_landmark_locations(self, loop=True, states=None): # Show landmark locations in first-person images img_all = self.tensor_store.get("images") img_w_all = self.tensor_store.get("images_w") import rollout.run_metadata as md if md.IS_ROLLOUT: # TODO: Discard this and move this to PomdpInterface or something # (it's got nothing to do with the model) # load landmark positions from configs from data_io.env import load_env_config from learning.datasets.aux_data_providers import get_landmark_locations_airsim from learning.models.semantic_map.pinhole_camera_inv import PinholeCameraProjection projector = PinholeCameraProjection( map_size_px=self.params["global_map_size"], world_size_px=self.params["world_size_px"], world_size_m=self.params["world_size_m"], img_x=self.params["img_w"], img_y=self.params["img_h"], cam_fov=self.params["cam_h_fov"], #TODO: Handle correctly domain="sim", use_depth=False) conf_json = load_env_config(md.ENV_ID) landmark_names, landmark_indices, landmark_pos = get_landmark_locations_airsim( conf_json) cam_poses = self.cam_poses_from_states(states) cam_pos = cam_poses.position[0] cam_rot = cam_poses.orientation[0] lm_pos_map_all = [] lm_pos_img_all = [] for i, landmark_in_world in enumerate(landmark_pos): lm_pos_img, landmark_in_cam, status = projector.world_point_to_image( cam_pos, cam_rot, landmark_in_world) lm_pos_map = torch.from_numpy( transformations.pos_m_to_px( landmark_in_world[np.newaxis, :], self.params["global_map_size"], self.params["world_size_m"], self.params["world_size_px"])) lm_pos_map_all += [lm_pos_map[0]] if lm_pos_img is not None: lm_pos_img_all += [lm_pos_img] lm_pos_img_all = [lm_pos_img_all] lm_pos_map_all = [lm_pos_map_all] else: lm_pos_img_all = self.tensor_store.get("lm_pos_fpv_img") lm_pos_map_all = self.tensor_store.get("lm_pos_map") print("Plotting landmark points") for i in range(len(img_all)): p = Presenter() overlay_fpv = p.overlay_pts_on_image(img_all[i][0], lm_pos_img_all[i]) overlay_map = p.overlay_pts_on_image(img_w_all[i][0], lm_pos_map_all[i]) p.show_image(overlay_fpv, "landmarks_on_fpv_img", scale=8) p.show_image(overlay_map, "landmarks_on_map", scale=20) if not loop: break
def _get_config(self, name, i=None, instance_id=None): config = load_env_config(id) return config
def _get_config(self, name, i=None, instance_id=None): print("_get_config load_env_config") config = load_env_config(i) return config