def provider_past_trajectory(segment_data, data):
traj_len = len(segment_data)
canvas = np.zeros((64, 64))
canvases_t = []
last_pos = None
for timestep in range(traj_len):
if segment_data[timestep]["state"] is None:
break
pos_as = segment_data.state[timestep].state[9:12]
pos_map = pos_m_to_px(pos_as[np.newaxis, :], img_size_px=32)[0]
if last_pos != None:
coords = [last_pos, pos_map]
last_pos = pos_map
tdd.plot_path_on_img(canvas, coords)
cv2.imshow("past_traje", canvas)
canvas_t = torch.from_numpy(canvas.copy())
canvases_t.append(canvas_t)
canvases_t = torch.stack(canvases_t, dim=0)
return [("past_trajectory_map", canvases_t)]
def get_top_down_ground_truth_static_ego(env_id, start_idx, img_w, img_h,
map_w, map_h):
"""
Returns the ground-truth label oriented in the global map frame
:param env_id:
:param start_idx:
:param img_w:
:param img_h:
:param map_w:
:param map_h:
:return:
"""
path = load_path(env_id)
#instruction_segments = [self.all_instr[env_id][set_idx]["instructions"][seg_idx]]
start_pt, dir_yaw = tdd.get_start_pt_and_yaw(path, start_idx, map_w, map_h,
0)
affine = tdd.get_affine_matrix(start_pt, dir_yaw, img_w, img_h)
seg_labels = np.zeros([img_w, img_h, 2]).astype(float)
path_in_img = cf_to_img(path, np.array([map_w, map_h]))
#gauss_sigma = map_w / 96
gauss_sigma = map_w / 32
seg_labels[:, :, 0] = tdd.plot_path_on_img(seg_labels[:, :, 0],
path_in_img)
if len(path_in_img) > 1:
seg_labels[:, :, 1] = tdd.plot_dot_on_img(seg_labels[:, :, 1],
path_in_img[-1], gauss_sigma)
seg_labels_rot = tdd.apply_affine(seg_labels, affine, img_w, img_h)
seg_labels_rot[:, :, 0] = gaussian_filter(seg_labels_rot[:, :, 0],
gauss_sigma)
seg_labels_rot[:, :, 1] = gaussian_filter(seg_labels_rot[:, :, 1],
gauss_sigma)
DEBUG = True
if DEBUG:
cv2.imshow("l_traj", seg_labels_rot[:, :, 0])
cv2.imshow("l_endpt", seg_labels_rot[:, :, 1])
cv2.waitKey(0)
# Standardize both channels separately (each has mean zero, unit variance)
seg_labels_path = standardize_2d_prob_dist(seg_labels_rot[:, :, 0:1])
seg_labels_endpt = standardize_2d_prob_dist(seg_labels_rot[:, :, 1:2])
seg_labels_rot = np.concatenate((seg_labels_path, seg_labels_endpt),
axis=0)
seg_labels_t = torch.from_numpy(seg_labels_rot).unsqueeze(0).float()
return seg_labels_t
def get_reward(self, v_dist_w, cam_pos, action, done, first):
# Prepare things
pos_in_map_m = cam_pos[0:1, 0:2] # * self.world_size_px / self.
pos_in_map_px = torch.from_numpy(
transformations.pos_m_to_px(pos_in_map_m.detach().cpu().numpy(),
self.world_size_px, self.world_size_m,
self.world_size_px))[0]
if self.last_pos is None:
self.last_pos = pos_in_map_px
self.visited_dist = tdd.plot_path_on_img(
self.visited_dist, [self.last_pos, pos_in_map_px])
#Presenter().show_image(self.visited_dist, "visited_dist", scale=4, waitkey=1)
visit_dist = v_dist_w.inner_distribution[0, 0, :, :]
visit_dist = visit_dist.detach().cpu().numpy()
goal_unobserved_prob = v_dist_w.outer_prob_mass[0, 1].item()
goal_observed_prob = 1 - goal_unobserved_prob
# -----------------------------------------------------------------------
# Calculate exploration reward, using probability that goal is observed as a potential function
# Don't ever reduce the potential - only increase it
goal_unobserved_potential = -goal_unobserved_prob
if self.prev_exploration_potential is None:
self.prev_exploration_potential = goal_unobserved_potential
goal_unobserved_potential = max(goal_unobserved_potential,
self.prev_exploration_potential)
exploration_reward = (
goal_unobserved_potential -
self.prev_exploration_potential) * self.exploration_alpha
self.prev_exploration_potential = goal_unobserved_potential
# -----------------------------------------------------------------------
# Calculate visitation reward (potential shaped by visitation probability)
# Give reward for visiting the high-probability states at next timestep
visit_potential = -self.wasserstein_distance(self.visited_dist,
visit_dist)
if self.prev_visit_potential is None:
self.prev_visit_potential = visit_potential
visit_reward = (visit_potential -
self.prev_visit_potential) * self.visit_alpha
self.prev_visit_potential = visit_potential
# -----------------------------------------------------------------------
# Calculate stop reward consisting of EMD(stop,goal), P(stop=goal), and -P(stop_oob)
if action[3] > 0.5 or done:
partial_stop_dist = v_dist_w.inner_distribution[
0, 1, :, :].detach().cpu().numpy()
stopped_dist = tdd.plot_path_on_img(self.empty_distribution(),
[pos_in_map_px, pos_in_map_px])
stop_wd = self.wasserstein_distance(partial_stop_dist,
stopped_dist)
stop_reward = -stop_wd * self.stop_alpha
# Calculate reward proportional to P(p_g = p_stop)
pos_in_map_m = cam_pos[0:1, 0:2] # * self.world_size_px / self.
pos_in_map_px = torch.from_numpy(
transformations.pos_m_to_px(
pos_in_map_m.detach().cpu().numpy(), self.world_size_px,
self.world_size_m, self.world_size_px))
pos_x = int(pos_in_map_px[0, 0].item() + 0.5)
pos_y = int(pos_in_map_px[0, 1].item() + 0.5)
pos_x = min(max(pos_x, 0), partial_stop_dist.shape[0] - 1)
pos_y = min(max(pos_y, 0), partial_stop_dist.shape[1] - 1)
stop_prob_at_pos = partial_stop_dist[pos_x, pos_y].item()
stop_prob_prop = stop_prob_at_pos / (partial_stop_dist.max() +
1e-10)
stop_p_reward = stop_prob_prop * self.stop_p_alpha
# Add negative reward for stopping when P(goal oob) is high
stop_oob_reward = -self.stop_oob_alpha * goal_unobserved_prob
else:
stop_reward = 0.0
stop_p_reward = 0.0
stop_oob_reward = 0.0
# -----------------------------------------------------------------------
self.last_pos = pos_in_map_px
return visit_reward, stop_reward, exploration_reward, stop_oob_reward, stop_p_reward
def get_top_down_ground_truth_dynamic_global(env_id, start_idx, end_idx,
drone_pos_as, img_w, img_h, map_w,
map_h):
"""
Returns the ground-truth label oriented in the global map frame
:param env_id:
:param start_idx:
:param img_w:
:param img_h:
:param map_w:
:param map_h:
:return:
"""
PROFILE = False
prof = SimpleProfiler(False, PROFILE)
path = load_path(env_id, anno=True)
#print(len(path), start_idx, end_idx)
path = path[start_idx:end_idx]
#instruction_segments = [self.all_instr[env_id][set_idx]["instructions"][seg_idx]]
prof.tick("load_path")
units = UnrealUnits(1.0)
drone_pos_cf = units.pos3d_from_as(drone_pos_as)
#print("Dynamic ground truth for ", env_id, start_idx, end_idx)
gt_dynamic = get_dynamic_ground_truth_v2(path, drone_pos_cf[:2])
#Presenter().plot_path(env_id, [path[start_idx:end_idx], gt_dynamic])
prof.tick("gen_gt_path")
seg_labels = np.zeros([img_w, img_h, 2]).astype(float)
path_in_img = cf_to_img(gt_dynamic, np.array([map_w, map_h]))
gauss_sigma = map_w / 96
seg_labels[:, :, 0] = tdd.plot_path_on_img(seg_labels[:, :, 0],
path_in_img)
if len(path_in_img) > 1:
seg_labels[:, :, 1] = tdd.plot_dot_on_img(seg_labels[:, :, 1],
path_in_img[-1], gauss_sigma)
prof.tick("plot_path")
seg_labels[:, :, 0] = gaussian_filter(seg_labels[:, :, 0], gauss_sigma)
seg_labels[:, :, 1] = gaussian_filter(seg_labels[:, :, 1], gauss_sigma)
# Standardize both channels separately (each has mean zero, unit variance)
seg_labels_path = standardize_2d_prob_dist(seg_labels[:, :, 0:1])
seg_labels_endpt = standardize_2d_prob_dist(seg_labels[:, :, 1:2])
prof.tick("process_img")
DEBUG = False
if DEBUG:
gt_path_in_img = cf_to_img(path, np.asarray([map_w, map_h]))
dbg_labels_gt = np.zeros([img_w, img_h, 1])
dbg_labels_gt[:, :, 0] = tdd.plot_path_on_img(dbg_labels_gt[:, :, 0],
gt_path_in_img)
Presenter().show_image(dbg_labels_gt,
"dbg",
torch=False,
waitkey=10,
scale=4)
Presenter().show_image(torch.from_numpy(seg_labels_path),
"l_path",
torch=True,
waitkey=10,
scale=4)
Presenter().show_image(torch.from_numpy(seg_labels_endpt),
"l_endp",
torch=True,
waitkey=100,
scale=4)
seg_labels = np.concatenate((seg_labels_path, seg_labels_endpt), axis=0)
seg_labels_t = torch.from_numpy(seg_labels).unsqueeze(0).float()
prof.tick("prep_out")
prof.print_stats()
return seg_labels_t
