def get_observation(self, observations, episode, *args: Any, **kwargs: Any): episode_uniq_id = f"{episode.scene_id} {episode.episode_id}" if episode_uniq_id != self._current_episode_id: self._episode_time = 0.0 self._current_episode_id = episode_uniq_id agent_state = self._sim.get_agent_state() origin = np.array(episode.start_position, dtype=np.float32) rotation_world_start = quaternion_from_coeff(episode.start_rotation) agent_position_xyz = agent_state.position rotation_world_agent = agent_state.rotation agent_position_xyz = quaternion_rotate_vector( rotation_world_start.inverse(), agent_position_xyz - origin) agent_heading = self._quat_to_xy_heading( rotation_world_agent.inverse() * rotation_world_start) ep_time = self._episode_time self._episode_time += 1.0 return np.array([ -agent_position_xyz[2], agent_position_xyz[0], agent_heading, ep_time ], dtype=np.float32)
def get_observation(self, observations, episode, *args: Any, **kwargs: Any): agent_state = self._sim.get_agent_state() rotation_world_agent = agent_state.rotation rotation_world_start = quaternion_from_coeff(episode.start_rotation) return self._quat_to_xy_heading(rotation_world_agent.inverse() * rotation_world_start)
def get_observation(self, observations, episode: Episode, *args: Any, **kwargs: Any): source_position = np.array(episode.start_position, dtype=np.float32) rotation_world_start = quaternion_from_coeff(episode.start_rotation) goal_position = np.array(episode.goals[0].position, dtype=np.float32) return self._compute_pointgoal(source_position, rotation_world_start, goal_position)
def check_state(agent_state, position, rotation): assert (angle_between_quaternions(agent_state.rotation, quaternion_from_coeff(rotation)) < 1e-5), "Agent's rotation diverges from the shortest path." assert np.allclose( agent_state.position, position ), "Agent's position position diverges from the shortest path's one."
def get_observation(self, *args: Any, observations, episode, **kwargs: Any): agent_state = self._sim.get_agent_state() agent_position = agent_state.position rotation_world_start = quaternion_from_coeff(episode.start_rotation) goal_position = np.array(episode.goals[0].position, dtype=np.float32) return self._compute_pointgoal( agent_position, rotation_world_start, goal_position )
def get_observation(self, observations, episode, *args: Any, **kwargs: Any): agent_state = self._sim.get_agent_state() origin = np.array(episode.start_position, dtype=np.float32) rotation_world_start = quaternion_from_coeff(episode.start_rotation) agent_position = agent_state.position agent_position = quaternion_rotate_vector( rotation_world_start.inverse(), agent_position - origin) if self._dimensionality == 2: return np.array([-agent_position[2], agent_position[0]], dtype=np.float32) else: return agent_position.astype(np.float32)
def main(args): config = get_config() mapper_config = config.RL.ANS.MAPPER mapper_config.defrost() mapper_config.map_size = 130 mapper_config.map_scale = 0.02 mapper_config.freeze() mapper = Mapper(mapper_config, None) M = args.global_map_size skip_scene = args.skip_scene config_path = args.config_path save_dir = args.save_dir safe_mkdir(save_dir) seen_map_save_root = os.path.join(save_dir, "seen_area_maps") wall_map_save_root = os.path.join(save_dir, "wall_maps") semantic_map_save_root = os.path.join(save_dir, "semantic_maps") json_save_path = os.path.join(save_dir, "all_maps_info.json") config = habitat_extensions.get_extended_config(config_path) scenes_list = glob.glob(f"") dataset_path = config.DATASET.DATA_PATH.replace("{split}", config.DATASET.SPLIT) with gzip.open(dataset_path, "rt") as fp: dataset = json.load(fp) num_episodes = len(dataset["episodes"]) print("===============> Loading data per scene") scene_to_data = {} if num_episodes == 0: content_path = os.path.join( dataset_path[: -len(f"{config.DATASET.SPLIT}.json.gz")], "content" ) scene_paths = glob.glob(f"{content_path}/*") print(f"Number of scenes found: {len(scene_paths)}") for scene_data_path in scene_paths: with gzip.open(scene_data_path, "rt") as fp: scene_data = json.load(fp) num_episodes += len(scene_data["episodes"]) scene_id = scene_data["episodes"][0]["scene_id"].split("/")[-1] scene_to_data[scene_id] = scene_data["episodes"] else: for ep in dataset["episodes"]: scene_id = ep["scene_id"].split("/")[-1] if scene_id not in scene_to_data: scene_to_data[scene_id] = [] scene_to_data[scene_id].append(ep) print("===============> Computing heights for different floors in each scene") scenes_to_floor_heights = {} for scene_id, scene_data in scene_to_data.items(): # Identify the number of unique floors in this scene floor_heights = [] for ep in scene_data: height = ep["start_position"][1] if len(floor_heights) == 0: floor_heights.append(height) # Measure height difference from all existing floors d2floors = map(lambda x: abs(x - height), floor_heights) d2floors = np.array(list(d2floors)) if not np.any(d2floors < 0.5): floor_heights.append(height) # Store this in the dict scenes_to_floor_heights[scene_id] = floor_heights env = DummyRLEnv(config=config) env.seed(1234) _ = env.reset() device = torch.device("cuda:0") safe_mkdir(seen_map_save_root) safe_mkdir(wall_map_save_root) safe_mkdir(semantic_map_save_root) # Data format for saving top-down maps per scene: # For each split, create a json file that contains the following dictionary: # key - scene_id # value - [{'floor_height': ..., # 'seen_map_path': ..., # 'wall_map_path': ..., # 'world_position': ..., # 'world_heading': ...}, # ., # ., # ., # ] # The floor_height specifies a single height value on that floor. # All other heights within 0.5m of this height will correspond to this floor. # The *_map_path specifies the path to a .npy file that contains the # corresponding map. This map is in the world coordinate system, not episode # centric start-view coordinate system. # The world_position is the (X, Y, Z) position of the agent w.r.t. which this # map was computed. The world_heading is the clockwise rotation (-Z to X) # of the agent in the world coordinates. # The .npy files will be stored in seen_map_save_root and wall_map_save_root. # Create top-down maps per scene, per floor per_scene_per_floor_maps = {} print("===============> generate meta information for gt map") for target_scene in tqdm.tqdm(scene_to_data.keys()): per_scene_per_floor_maps[target_scene] = {} for episode in scene_to_data[target_scene]: scene_id = target_scene start_position = episode['start_position'] start_rotation = episode['start_rotation'] start_height = start_position[1] floor_heights = scenes_to_floor_heights[scene_id] d2floors = map(lambda x: abs(x - start_height), floor_heights) d2floors = np.array(list(d2floors)) floor_idx = np.where(d2floors < 0.5)[0][0].item() if floor_idx in per_scene_per_floor_maps[scene_id]: continue start_heading = compute_heading_from_quaternion(quaternion_from_coeff(start_rotation)) seen_map_save_path = f"{seen_map_save_root}/{scene_id}_{floor_idx}.npy" wall_map_save_path = f"{wall_map_save_root}/{scene_id}_{floor_idx}.npy" semantic_map_save_path = f"{semantic_map_save_root}/{scene_id}_{floor_idx}.npy" save_dict = { "seen_map_path": seen_map_save_path, "wall_map_path": wall_map_save_path, "semantic_map_path": semantic_map_save_path, "floor_height": start_height, "start_rotation": start_rotation, "world_position": start_position, "world_heading": start_heading, "scene_id": episode['scene_id'] } per_scene_per_floor_maps[scene_id][floor_idx] = save_dict if len(per_scene_per_floor_maps[scene_id]) == len(scenes_to_floor_heights[scene_id]): break print("===============> save meta information for gt map") save_json = {} for scene in per_scene_per_floor_maps.keys(): scene_save_data = [] for floor_idx, floor_data in per_scene_per_floor_maps[scene].items(): scene_save_data.append(floor_data) save_json[scene] = scene_save_data json.dump(save_json, open(json_save_path, "w")) print("===============> start to draw semantic map") scene_ids = sorted(list(per_scene_per_floor_maps.keys())) print(scene_ids) start_scene = scene_ids[skip_scene] print(f"===============> start with scene {start_scene}") for target_scene in tqdm.tqdm(scene_ids[skip_scene:],desc='scenes', position=0): for floor_idx in per_scene_per_floor_maps[target_scene]: scene_meta_info = per_scene_per_floor_maps[target_scene][floor_idx] # don't regenerate maps if os.path.isfile(scene_meta_info['semantic_map_path']): continue print(scene_meta_info) env.habitat_env.current_episode.start_position = scene_meta_info['world_position'] env.habitat_env.current_episode.start_rotation = scene_meta_info['start_rotation'] env.habitat_env.current_episode.scene_id = scene_meta_info['scene_id'] env.habitat_env.reconfigure(env.habitat_env._config) _ = env.habitat_env.task.reset(env.habitat_env.current_episode) scene_id = target_scene agent_state = env.habitat_env.sim.get_agent_state() start_position = np.array(agent_state.position) global_seen_map, global_wall_map = get_episode_map( env, mapper, M, config, device ) #generate semantic layers global_semantic_map = generate_semantic_layers(env,mapper, M,config,global_seen_map) seen_map_save_path = f"{seen_map_save_root}/{scene_id}_{floor_idx}.npy" wall_map_save_path = f"{wall_map_save_root}/{scene_id}_{floor_idx}.npy" semantic_map_save_path = f"{semantic_map_save_root}/{scene_id}_{floor_idx}.npy" np.save(seen_map_save_path, (global_seen_map > 0)) np.save(wall_map_save_path, (global_wall_map > 0)) np.save(semantic_map_save_path, (global_semantic_map > 0)) # clean the memory to avoid overflow global_seen_map = None global_wall_map = None global_semantic_map = None gc.collect()
def test_mp3d_eqa_sim_correspondence(): eqa_config = get_config(CFG_TEST) if not mp3d_dataset.Matterport3dDatasetV1.check_config_paths_exist( eqa_config.DATASET): pytest.skip("Please download Matterport3D EQA dataset to data folder.") dataset = make_dataset(id_dataset=eqa_config.DATASET.TYPE, config=eqa_config.DATASET) with habitat.Env(config=eqa_config, dataset=dataset) as env: env.episodes = [ episode for episode in dataset.episodes if int(episode.episode_id) in TEST_EPISODE_SET[:EPISODES_LIMIT] ] ep_i = 0 cycles_n = 2 while cycles_n > 0: env.reset() episode = env.current_episode assert (len( episode.goals) == 1), "Episode has no goals or more than one." assert (len(episode.shortest_paths) == 1 ), "Episode has no shortest paths or more than one." start_state = env.sim.get_agent_state() assert np.allclose( start_state.position, episode.start_position ), "Agent's start position diverges from the shortest path's one." rgb_mean = 0 logger.info("{id} {question}\n{answer}".format( id=episode.episode_id, question=episode.question.question_text, answer=episode.question.answer_text, )) for step_id, point in enumerate(episode.shortest_paths[0]): cur_state = env.sim.get_agent_state() logger.info( "diff position: {} diff rotation: {} " "cur_state.position: {} shortest_path.position: {} " "cur_state.rotation: {} shortest_path.rotation: {} action: {}" "".format( cur_state.position - point.position, angle_between_quaternions( cur_state.rotation, quaternion_from_coeff(point.rotation), ), cur_state.position, point.position, cur_state.rotation, point.rotation, point.action, )) assert np.allclose( [cur_state.position[0], cur_state.position[2]], [point.position[0], point.position[2]], atol=CLOSE_STEP_THRESHOLD * (step_id + 1), ), "Agent's path diverges from the shortest path." if point.action != OLD_STOP_ACTION_ID: obs = env.step(action=point.action) if not env.episode_over: rgb_mean += obs["rgb"][:, :, :3].mean() if ep_i < len(RGB_EPISODE_MEANS): # Slightly bigger atol for basis meshes rgb_mean = rgb_mean / len(episode.shortest_paths[0]) assert np.isclose( RGB_EPISODE_MEANS[int(episode.episode_id)], rgb_mean, atol=0.5, ), "RGB output doesn't match the ground truth." ep_i = (ep_i + 1) % EPISODES_LIMIT if ep_i == 0: cycles_n -= 1