def get_observation(self, *args: Any, observations, episode,
**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 == 3:
rel_start_pos = np.array([-agent_position[2], agent_position[0]],
dtype=np.float32)
else:
rel_start_pos = agent_position.astype(np.float32)
rotation_world_agent = agent_state.rotation
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
relative_start_heading = np.array([
self._quat_to_xy_heading(rotation_world_agent.inverse() *
rotation_world_start)
])
return np.concatenate([rel_start_pos, relative_start_heading])
def get_observation(self, observations, episode):
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):
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 get_rel_heading(self, posA, rotA, posB):
direction_vector = np.array([0, 0, -1])
quat = quaternion_from_coeff(rotA).inverse()
# The heading vector and heading angle are in arctan2 format
heading_vector = quaternion_rotate_vector(quat, direction_vector)
heading = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
heading = self.normalize_angle(heading)
adjusted_heading = np.pi / 2 - heading
camera_horizon_vec = [
np.cos(adjusted_heading),
np.sin(adjusted_heading), 0
]
# This vectors are in habitat format we need to rotate them.
rotated_posB = [posB[0], -posB[2], posB[1]]
rotated_posA = [posA[0], -posA[2], posA[1]]
target_vector = np.array(rotated_posB) - np.array(rotated_posA)
y = target_vector[0] * camera_horizon_vec[1] - \
target_vector[1] * camera_horizon_vec[0]
x = target_vector[0] * camera_horizon_vec[0] + \
target_vector[1] * camera_horizon_vec[1]
return np.arctan2(y, x)
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_rel_elevation(self, posA, rotA, cameraA, posB):
direction_vector = np.array([0, 0, -1])
quat = quaternion_from_coeff(rotA)
rot_vector = quaternion_rotate_vector(quat.inverse(), direction_vector)
camera_quat = quaternion_from_coeff(cameraA)
camera_vector = quaternion_rotate_vector(camera_quat.inverse(),
direction_vector)
elevation_angle = dir_angle_between_quaternions(quat, camera_quat)
rotated_posB = [posB[0], -posB[2], posB[1]]
rotated_posA = [posA[0], -posA[2], posA[1]]
target_vector = np.array(rotated_posB) - np.array(rotated_posA)
target_z = target_vector[2]
target_length = np.linalg.norm([target_vector[0], target_vector[1]])
rel_elevation = np.arctan2(target_z, target_length)
return rel_elevation - elevation_angle
def get_observation(self, *args: Any, observations, episode: Episode,
**kwargs: Any):
goal_idx = getattr(episode, "goal_idx", 0)
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[goal_idx].position,
dtype=np.float32)
return self._compute_pointgoal(source_position, rotation_world_start,
goal_position)
def get_observation(self, observations, episode):
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)
with open(log_path, 'r') as f:
data = json.load(f)
locs = []
for ep_num, ep in enumerate(data):
info = ep['info']
episode_info, start_rot, actions, weights, gps, heading = info.values()
print(episode_info.keys())
_, _, start_pos, start_rot, more_ep_info, goals, start_room, shortest_path = episode_info.values()
# if not in_goal_box(goals[0]["position"]):
# continue
for i, reading in enumerate(gps):
reading_pos = [reading[1], start_pos[1], -reading[0]]
# reading_pos = [reading[0], start_pos[1], reading[1]]
head_phi = heading[i] # this is already global
rotation_world_start = quaternion_from_coeff(start_rot)
global_offset = quaternion_rotate_vector(
rotation_world_start, reading_pos
)
global_pos = global_offset + start_pos
cur_x, _, cur_y = global_pos
loc_info = [cur_x, cur_y, head_phi, actions[i]]
loc_info.extend(weights[i])
locs.append(loc_info)
df = pd.DataFrame(locs, columns=cols, index=[i for i in range(len(locs))])
for task in aux_tasks:
df[task] = pd.to_numeric(df[task])
df['head_cat'] = pd.cut(df['heading'], [-1 * np.pi, -1 * np.pi / 2, 0, np.pi/2, np.pi],# bins=dist_bins,
labels=['N', 'W', 'S', 'E'])
df['head_cat'].describe()
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
