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