def reset_metric(self, episode):
self._step_count = 0
self._metric = None
self._top_down_map = self.get_original_map()
agent_position = self._sim.get_agent_state().position
a_x, a_y = maps.to_grid(
agent_position[0],
agent_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
self._previous_xy_location = (a_y, a_x)
if self._config.DRAW_SHORTEST_PATH:
# draw shortest path
self._shortest_path_points = self._sim.get_straight_shortest_path_points(
agent_position, episode.goals[0].position)
self._shortest_path_points = [
maps.to_grid(
p[0],
p[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)[::-1] for p in self._shortest_path_points
]
maps.draw_path(
self._top_down_map,
self._shortest_path_points,
maps.MAP_SHORTEST_PATH_COLOR,
self.line_thickness,
)
# draw source and target points last to avoid overlap
if self._config.DRAW_SOURCE_AND_TARGET:
self.draw_source_and_target(episode)
def draw_source_and_target(self, episode):
# mark source point
s_x, s_y = maps.to_grid(
episode.start_position[0],
episode.start_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
point_padding = 2 * int(
np.ceil(self._map_resolution[0] / MAP_THICKNESS_SCALAR))
self._top_down_map[s_x - point_padding:s_x + point_padding + 1,
s_y - point_padding:s_y + point_padding +
1, ] = maps.MAP_SOURCE_POINT_INDICATOR
# mark target point
t_x, t_y = maps.to_grid(
episode.goals[0].position[0],
episode.goals[0].position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
self._top_down_map[t_x - point_padding:t_x + point_padding + 1,
t_y - point_padding:t_y + point_padding +
1, ] = maps.MAP_TARGET_POINT_INDICATOR
def get_map(env, map_res):
# top down map
top_down_map = maps.get_topdown_map(env.sim,
num_samples=1000000,
map_resolution=(map_res, map_res))
top_down_map, rmin, rmax, cmin, cmax = crop_map(top_down_map)
# target/goal
target_position = env.current_episode.goals[0].position
target_position_grid = maps.to_grid(target_position[0], target_position[2],
maps.COORDINATE_MIN,
maps.COORDINATE_MAX,
(map_res, map_res))
# agent's current position # to_grid converts real world (x,y) to pixel (x,y)
agent_position = env.sim.get_agent_state().position
agent_position_grid = maps.to_grid(agent_position[0], agent_position[2],
maps.COORDINATE_MIN,
maps.COORDINATE_MAX, (map_res, map_res))
# grid offset for map
agent_position_grid = (agent_position_grid[0] - rmin,
agent_position_grid[1] - cmin)
target_position_grid = (target_position_grid[0] - rmin,
target_position_grid[1] - cmin)
tdm = top_down_map.copy()
# agent pos
m1 = 4
tdm[agent_position_grid[0] - m1:agent_position_grid[0] + m1,
agent_position_grid[1] - m1:agent_position_grid[1] + m1] = 0
# target pos
m2 = 7
tdm[target_position_grid[0] - m1:target_position_grid[0] + m1,
target_position_grid[1] - m2:target_position_grid[1] + m2] = 0
tdm[target_position_grid[0] - m2:target_position_grid[0] + m2,
target_position_grid[1] - m1:target_position_grid[1] + m1] = 0
assert agent_position_grid[0] > 0 and agent_position_grid[
0] < top_down_map.shape[0], print('assert1 ', agent_position_grid,
top_down_map.shape)
assert target_position_grid[0] > 0 and target_position_grid[
0] < top_down_map.shape[0], print('assert2 ', target_position_grid,
top_down_map.shape)
assert agent_position_grid[1] > 0 and agent_position_grid[
1] < top_down_map.shape[1], print('assert3 ', agent_position_grid,
top_down_map.shape)
assert target_position_grid[1] > 0 and target_position_grid[
1] < top_down_map.shape[1], print('assert4 ', target_position_grid,
top_down_map.shape)
return tdm, agent_position_grid, target_position_grid
def update_map(self, agent_position):
a_x, a_y = maps.to_grid(
agent_position[0],
agent_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
# Don't draw over the source point
if self._top_down_map[a_x, a_y] != maps.MAP_SOURCE_POINT_INDICATOR:
color = 10 + min(
self._step_count * 245 // self._config.MAX_EPISODE_STEPS, 245)
thickness = int(
np.round(self._map_resolution[0] * 2 / MAP_THICKNESS_SCALAR))
cv2.line(
self._top_down_map,
self._previous_xy_location,
(a_y, a_x),
color,
thickness=thickness,
)
self._previous_xy_location = (a_y, a_x)
return self._top_down_map, a_x, a_y
def reset_metric(self, episode, *args: Any, **kwargs: Any):
self._step_count = 0
self._metric = None
self._top_down_map = self.get_original_map()
agent_position = self._sim.get_agent_state().position
a_x, a_y = maps.to_grid(
agent_position[2],
agent_position[0],
self._top_down_map.shape[0:2],
sim=self._sim,
)
self._previous_xy_location = (a_y, a_x)
self.update_fog_of_war_mask(np.array([a_x, a_y]))
# draw source and target parts last to avoid overlap
self._draw_goals_view_points(episode)
self._draw_goals_aabb(episode)
self._draw_goals_positions(episode)
self._draw_shortest_path(episode, agent_position)
if self._config.DRAW_SOURCE:
self._draw_point(episode.start_position,
maps.MAP_SOURCE_POINT_INDICATOR)
def update_map(self, agent_position):
a_x, a_y = maps.to_grid(
agent_position[2],
agent_position[0],
self._top_down_map.shape[0:2],
sim=self._sim,
)
# Don't draw over the source point
if self._top_down_map[a_x, a_y] != maps.MAP_SOURCE_POINT_INDICATOR:
color = 10 + min(
self._step_count * 245 // self._config.MAX_EPISODE_STEPS, 245)
thickness = self.line_thickness
cv2.line(
self._top_down_map,
self._previous_xy_location,
(a_y, a_x),
color,
thickness=thickness,
)
self.update_fog_of_war_mask(np.array([a_x, a_y]))
self._previous_xy_location = (a_y, a_x)
return self._top_down_map, a_x, a_y
def get_original_map(self, episode):
top_down_map = maps.get_topdown_map(
self._sim,
self._map_resolution,
self._num_samples,
self._config.DRAW_BORDER,
)
range_x = np.where(np.any(top_down_map, axis=1))[0]
range_y = np.where(np.any(top_down_map, axis=0))[0]
self._ind_x_min = range_x[0]
self._ind_x_max = range_x[-1]
self._ind_y_min = range_y[0]
self._ind_y_max = range_y[-1]
if self._config.DRAW_SOURCE_AND_TARGET:
# mark source point
s_x, s_y = maps.to_grid(
episode.start_position[0],
episode.start_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
point_padding = 2 * int(
np.ceil(self._map_resolution[0] / MAP_THICKNESS_SCALAR))
top_down_map[s_x - point_padding:s_x + point_padding + 1,
s_y - point_padding:s_y + point_padding +
1, ] = maps.MAP_SOURCE_POINT_INDICATOR
# mark target point
t_x, t_y = maps.to_grid(
episode.goals[0].position[0],
episode.goals[0].position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
top_down_map[t_x - point_padding:t_x + point_padding + 1,
t_y - point_padding:t_y + point_padding +
1, ] = maps.MAP_TARGET_POINT_INDICATOR
return top_down_map
def _draw_point(self, position, point_type):
t_x, t_y = maps.to_grid(
position[2],
position[0],
self._top_down_map.shape[0:2],
sim=self._sim,
)
self._top_down_map[t_x - self.point_padding:t_x + self.point_padding +
1, t_y - self.point_padding:t_y +
self.point_padding + 1, ] = point_type
def reset_metric(self, *args: Any, episode, **kwargs: Any):
self._step_count = 0
self._metric = None
self._top_down_map = self.get_original_map()
self._valid_map = self._top_down_map != MAP_INVALID_POINT
self._explored_map = np.zeros_like(self._valid_map)
agent_position = self._sim.get_agent_state().position
a_x, a_y = maps.to_grid(
agent_position[0],
agent_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
goal_idx = getattr(episode, "goal_idx", 0)
self._previous_xy_location = (a_y, a_x)
if self._config.DRAW_SHORTEST_PATH:
# draw shortest path
self._shortest_path_points = self._sim.get_straight_shortest_path_points(
agent_position, episode.goals[goal_idx].position)
self._shortest_path_points = [
maps.to_grid(
p[0],
p[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)[::-1] for p in self._shortest_path_points
]
maps.draw_path(
self._top_down_map,
self._shortest_path_points,
maps.MAP_SHORTEST_PATH_COLOR,
self.line_thickness,
)
self.update_fog_of_war_mask(np.array([a_x, a_y]))
# draw source and target points last to avoid overlap
if self._config.DRAW_SOURCE_AND_TARGET:
self.draw_source_and_target(episode)
def draw_source_and_target(self, episode):
# mark source point
s_x, s_y = maps.to_grid(
episode.start_position[0],
episode.start_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
point_padding = 2 * int(
np.ceil(self._map_resolution[0] / MAP_THICKNESS_SCALAR))
self._top_down_map[s_x - point_padding:s_x + point_padding + 1,
s_y - point_padding:s_y + point_padding +
1, ] = maps.MAP_SOURCE_POINT_INDICATOR
if not self._config.DRAW_ALL_GOALS:
goal_idx = getattr(episode, "goal_idx", 0)
# mark target point
t_x, t_y = maps.to_grid(
episode.goals[goal_idx].position[0],
episode.goals[goal_idx].position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
self._top_down_map[t_x - point_padding:t_x + point_padding + 1,
t_y - point_padding:t_y + point_padding +
1, ] = maps.MAP_TARGET_POINT_INDICATOR
else:
for goal in episode.goals:
# mark target point
t_x, t_y = maps.to_grid(
goal.position[0],
goal.position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
self._top_down_map[t_x - point_padding:t_x + point_padding + 1,
t_y - point_padding:t_y + point_padding +
1, ] = maps.MAP_TARGET_POINT_INDICATOR
def _draw_point(self, position, point_type):
t_x, t_y = maps.to_grid(
position[0],
position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
self._top_down_map[t_x - self.point_padding:t_x + self.point_padding +
1, t_y - self.point_padding:t_y +
self.point_padding + 1, ] = point_type
def reset_metric(self, episode):
self._step_count = 0
self._metric = None
self._top_down_map = self.get_original_map(episode)
agent_position = self._sim.get_agent_state().position
a_x, a_y = maps.to_grid(
agent_position[0],
agent_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
self._previous_xy_location = (a_y, a_x)
def _draw_goals_aabb(self, episode):
if self._config.DRAW_GOAL_AABBS:
for goal in episode.goals:
try:
sem_scene = self._sim.semantic_annotations()
object_id = goal.object_id
assert int(
sem_scene.objects[object_id].id.split("_")[-1]
) == int(
goal.object_id
), f"Object_id doesn't correspond to id in semantic scene objects dictionary for episode: {episode}"
center = sem_scene.objects[object_id].aabb.center
x_len, _, z_len = (
sem_scene.objects[object_id].aabb.sizes / 2.0
)
# Nodes to draw rectangle
corners = [
center + np.array([x, 0, z])
for x, z in [
(-x_len, -z_len),
(-x_len, z_len),
(x_len, z_len),
(x_len, -z_len),
(-x_len, -z_len),
]
]
map_corners = [
maps.to_grid(
p[0],
p[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
for p in corners
]
maps.draw_path(
self._top_down_map,
map_corners,
maps.MAP_TARGET_BOUNDING_BOX,
self.line_thickness,
)
except AttributeError:
pass
def _draw_shortest_path(self, episode: Episode,
agent_position: AgentState):
if self._config.DRAW_SHORTEST_PATH:
self._shortest_path_points = self._sim.get_straight_shortest_path_points(
agent_position, episode.goals[0].position)
self._shortest_path_points = [
maps.to_grid(p[2],
p[0],
self._top_down_map.shape[0:2],
sim=self._sim) for p in self._shortest_path_points
]
maps.draw_path(
self._top_down_map,
self._shortest_path_points,
maps.MAP_SHORTEST_PATH_COLOR,
self.line_thickness,
)
def reset_metric(self, *args: Any, episode, **kwargs: Any):
self._step_count = 0
self._metric = None
self._top_down_map = self.get_original_map()
agent_position = self._sim.get_agent_state().position
a_x, a_y = maps.to_grid(
agent_position[0],
agent_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
self._previous_xy_location = (a_y, a_x)
self.update_fog_of_war_mask(np.array([a_x, a_y]))
# draw source last to avoid overlap
if self._config.DRAW_SOURCE:
self._draw_point(episode.start_position, maps.MAP_SOURCE_POINT_INDICATOR)
scene_bb = sim.get_active_scene_graph().get_root_node().cumulative_bb
height = scene_bb.y().min
if display:
top_down_map = maps.get_topdown_map(
sim.pathfinder, height, meters_per_pixel=meters_per_pixel
)
recolor_map = np.array(
[[255, 255, 255], [128, 128, 128], [0, 0, 0]], dtype=np.uint8
)
top_down_map = recolor_map[top_down_map]
grid_dimensions = (top_down_map.shape[0], top_down_map.shape[1])
# convert world trajectory points to maps module grid points
trajectory = [
maps.to_grid(
path_point[2],
path_point[0],
grid_dimensions,
pathfinder=sim.pathfinder,
)
for path_point in path_points
]
grid_tangent = mn.Vector2(
trajectory[1][1] - trajectory[0][1], trajectory[1][0] - trajectory[0][0]
)
path_initial_tangent = grid_tangent / grid_tangent.length()
initial_angle = math.atan2(path_initial_tangent[0], path_initial_tangent[1])
# draw the agent and trajectory on the map
maps.draw_path(top_down_map, trajectory)
maps.draw_agent(
top_down_map, trajectory[0], initial_angle, agent_radius_px=8
)
print("\nDisplay the map with agent and path overlay:")
def get_map_with_path(env, map_res, agent_path):
# top down map
top_down_map = maps.get_topdown_map(env.sim,
num_samples=1000000,
map_resolution=(map_res, map_res))
top_down_map, rmin, rmax, cmin, cmax = crop_map(top_down_map)
tdm = top_down_map.copy()
# target/goal
target_position = env.current_episode.goals[0].position
target_position_grid = maps.to_grid(target_position[0], target_position[2],
maps.COORDINATE_MIN,
maps.COORDINATE_MAX,
(map_res, map_res))
for k in range(len(agent_path)):
pos = agent_path[k]
# agent's position # to_grid converts real world (x,y) to pixel (x,y)
agent_position = pos #env.sim.get_agent_state().position
agent_position_grid = maps.to_grid(agent_position[0],
agent_position[2],
maps.COORDINATE_MIN,
maps.COORDINATE_MAX,
(map_res, map_res))
# grid offset for map
agent_position_grid = (agent_position_grid[0] - rmin,
agent_position_grid[1] - cmin)
# agent pos
m1 = 1
color = [255, 0, 0]
if (k == 0):
m1 = 3
color = [0, 255, 0]
tdm[agent_position_grid[0] - m1:agent_position_grid[0] + m1,
agent_position_grid[1] - m1:agent_position_grid[1] + m1, :] = color
assert agent_position_grid[0] > 0 and agent_position_grid[
0] < top_down_map.shape[0], print('assert1 ', agent_position_grid,
top_down_map.shape)
assert agent_position_grid[1] > 0 and agent_position_grid[
1] < top_down_map.shape[1], print('assert3 ', agent_position_grid,
top_down_map.shape)
target_position_grid = (target_position_grid[0] - rmin,
target_position_grid[1] - cmin)
# target pos
m2 = 3
color = [0, 0, 255]
tdm[target_position_grid[0] - m2:target_position_grid[0] + m2,
target_position_grid[1] - m2:target_position_grid[1] + m2, :] = color
#tdm[target_position_grid[0]-m1:target_position_grid[0]+m1,target_position_grid[1]-m2:target_position_grid[1]+m2,:] = color
#tdm[target_position_grid[0]-m2:target_position_grid[0]+m2,target_position_grid[1]-m1:target_position_grid[1]+m1,:] = color
assert target_position_grid[0] > 0 and target_position_grid[
0] < top_down_map.shape[0], print('assert2 ', target_position_grid,
top_down_map.shape)
assert target_position_grid[1] > 0 and target_position_grid[
1] < top_down_map.shape[1], print('assert4 ', target_position_grid,
top_down_map.shape)
return tdm
def to_grid(pos, map_resolution):
return maps.to_grid(pos[0], pos[2], maps.COORDINATE_MIN,
maps.COORDINATE_MAX, [map_resolution, map_resolution])
def _get_mesh_occupancy(self, episode, agent_state):
episode_id = (episode.episode_id, episode.scene_id)
if self.current_episode_id != episode_id:
self.current_episode_id = episode_id
# Transpose to make x rightward and y downward
self._top_down_map = self.get_original_map().T
agent_position = agent_state.position
agent_rotation = agent_state.rotation
a_x, a_y = maps.to_grid(
agent_position[0],
agent_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
# Crop region centered around the agent
mrange = int(self.map_size * 1.5)
ego_map = self._top_down_map[
(a_y - mrange): (a_y + mrange), (a_x - mrange): (a_x + mrange)
]
if ego_map.shape[0] == 0 or ego_map.shape[1] == 0:
ego_map = np.zeros((2 * mrange + 1, 2 * mrange + 1), dtype=np.uint8)
# Rotate to get egocentric map
# Negative since the value returned is clockwise rotation about Y,
# but we need anti-clockwise rotation
agent_heading = -compute_heading_from_quaternion(agent_rotation)
agent_heading = math.degrees(agent_heading)
half_size = ego_map.shape[0] // 2
center = (half_size, half_size)
M = cv2.getRotationMatrix2D(center, agent_heading, scale=1.0)
ego_map = (
cv2.warpAffine(
ego_map * 255,
M,
(ego_map.shape[1], ego_map.shape[0]),
flags=cv2.INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT,
borderValue=(1,),
).astype(np.float32)
/ 255.0
)
mrange = int(self.map_size)
ego_map = ego_map[
(half_size - mrange): (half_size + mrange),
(half_size - mrange): (half_size + mrange),
]
ego_map[ego_map > 0.5] = 1.0
ego_map[ego_map <= 0.5] = 0.0
# This map is currently 0 if occupied and 1 if unoccupied. Flip it.
ego_map = 1.0 - ego_map
# Flip the x axis because to_grid() flips the conventions
ego_map = np.flip(ego_map, axis=1)
# Get forward region infront of the agent
half_size = ego_map.shape[0] // 2
quarter_size = ego_map.shape[0] // 4
center = (half_size, half_size)
ego_map = ego_map[0:half_size, quarter_size: (quarter_size + half_size)]
# Append explored status in the 2nd channel
ego_map = np.stack([ego_map, np.ones_like(ego_map)], axis=2)
return ego_map
# head_cat = 'S'
# for i, loc in df[df['head_cat'] == head_cat].iterrows():
ctr = 0
for i, loc in df.iterrows():
# if ctr > 50000:
# break
# else:
# ctr += 1
max_weight_i = np.argmax(loc[aux_tasks])
color = colors[max_weight_i]
p_x, p_y = maps.to_grid(
loc['pos_x'],
loc['pos_y'],
COORDINATE_MIN,
COORDINATE_MAX,
map_resolution,
)
a_x = p_x - CLIPPED_X
a_y = p_y - CLIPPED_Y
box = (np.ones((*box_size, 3)) * 255 * color).astype(np.uint8)
utils.paste_overlapping_image(
background=top_down_map,
foreground=box,
location=(a_x, a_y)
)
a, b = 4, 4
n = 9
r = 3
first_list = read_file_list(first_file)
second_list = read_file_list(second_file)
matches = first_list.keys()
first_xyz = [[float(value) for value in first_list[a][0:3]]
for a in matches]
second_xyz = [[float(value) * float(1.0) for value in second_list[b][0:3]]
for b in matches]
first_rot = [[float(value) for value in first_list[a][3:]]
for a in matches]
grid_dimensions = (hablab_topdown_map.shape[0],
hablab_topdown_map.shape[1])
trajectory = [
maps.to_grid(
-(path_point[2]) + 19.25,
(path_point[0] - 2.04),
grid_dimensions,
pathfinder=sim.pathfinder,
) for path_point in second_xyz
]
ground_truth = [
maps.to_grid(
-(path_point[2]) + 19.25,
(path_point[0] - 2.04),
grid_dimensions,
pathfinder=sim.pathfinder,
) for path_point in first_xyz
]
colored_map = maps.colorize_topdown_map(hablab_topdown_map)
trajectory = np.array(trajectory)
def _get_mesh_occupancy(self):
agent_position = self.current_episode.start_position
agent_rotation = quaternion_xyzw_to_wxyz(self.current_episode.start_rotation)
a_x, a_y = maps.to_grid(
agent_position[0],
agent_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
# The map size here represents size around the agent, not infront.
mrange = int(self.map_size * 1.5 / 2.0)
# Add extra padding if map range is coordinates go out of bounds
y_start = a_y - mrange
y_end = a_y + mrange
x_start = a_x - mrange
x_end = a_x + mrange
x_l_pad, y_l_pad, x_r_pad, y_r_pad = 0, 0, 0, 0
H, W = self._top_down_map.shape
if x_start < 0:
x_l_pad = int(-x_start)
x_start += x_l_pad
x_end += x_l_pad
if x_end >= W:
x_r_pad = int(x_end - W + 1)
if y_start < 0:
y_l_pad = int(-y_start)
y_start += y_l_pad
y_end += y_l_pad
if y_end >= H:
y_r_pad = int(y_end - H + 1)
ego_map = np.pad(self._top_down_map, ((y_l_pad, y_r_pad), (x_l_pad, x_r_pad)))
ego_map = ego_map[y_start : (y_end + 1), x_start : (x_end + 1)]
if ego_map.shape[0] == 0 or ego_map.shape[1] == 0:
ego_map = np.zeros((2 * mrange + 1, 2 * mrange + 1), dtype=np.uint8)
# Rotate to get egocentric map
# Negative since the value returned is clockwise rotation about Y,
# but we need anti-clockwise rotation
agent_heading = -compute_heading_from_quaternion(agent_rotation)
agent_heading = math.degrees(agent_heading)
half_size = ego_map.shape[0] // 2
center = (half_size, half_size)
M = cv2.getRotationMatrix2D(center, agent_heading, scale=1.0)
ego_map = cv2.warpAffine(
ego_map,
M,
(ego_map.shape[1], ego_map.shape[0]),
flags=cv2.INTER_NEAREST,
borderMode=cv2.BORDER_CONSTANT,
borderValue=(1,),
)
ego_map = ego_map.astype(np.float32)
mrange = int(self.map_size / 2.0)
start_coor = half_size - mrange
end_coor = int(start_coor + self.map_size - 1)
ego_map = ego_map[start_coor : (end_coor + 1), start_coor : (end_coor + 1)]
# This map is currently 0 if occupied and 1 if unoccupied. Flip it.
ego_map = 1.0 - ego_map
# Flip the x axis because to_grid() flips the conventions
ego_map = np.flip(ego_map, axis=1)
# Append explored status in the 2nd channel
ego_map = np.stack([ego_map, np.ones_like(ego_map)], axis=2)
return ego_map
