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
