def get_landmark_locations_airsim(config_json):
landmark_names = []
landmark_positions = []
units = UnrealUnits()
for i, landmarkName in enumerate(config_json["landmarkName"]):
x_pos = config_json["xPos"][i]
y_pos = config_json["zPos"][i]
pt = np.asarray([x_pos, y_pos])
pt_as = np.zeros(3)
pt_as[0:2] = units.pos2d_to_as(pt)
# TODO: Grab this from the parameter server
pt_as[
2] = 0.0 if LANDMARKS_ON_FLOOR else -1.0 # Landmarks assumed to be floating 1m above ground.
landmark_names.append(landmarkName)
landmark_positions.append(pt_as)
if ADD_NULL_LANDMARK:
null_pos = draw_null_landmark_pos(landmark_positions)
landmark_names.append(get_null_landmark_name())
landmark_positions.append(null_pos)
name2idx = get_landmark_name_to_index(add_empty=ADD_NULL_LANDMARK)
landmark_indices = [name2idx[name] for name in landmark_names]
return landmark_names, landmark_indices, landmark_positions
def __get_goal_location_airsim(self, path_array):
units = UnrealUnits(1.0)
goal_x = path_array[-1][0]
goal_y = path_array[-1][1]
pt = np.asarray([goal_x, goal_y])
pt_as = np.zeros(3)
pt_as[0:2] = units.pos2d_to_as(pt)
return pt_as
def __get_goal_location_airsim(goal):
units = UnrealUnits()
goal_x = goal[0]
goal_y = goal[1]
pt = np.asarray([goal_x, goal_y])
pt_as = np.zeros(2)
pt_as[0:2] = units.pos2d_to_as(pt)
return pt_as
def load_and_convert_env_config(env_id):
config = load_env_config(env_id)
units = UnrealUnits()
config["x_pos_as"] = []
config["y_pos_as"] = []
for i,landmark in enumerate(config["landmarkName"]):
x = config["xPos"][i]
y = config["zPos"][i]
conv = units.pos2d_to_as(np.asarray([x,y]))
config["x_pos_as"].append(conv[0])
config["y_pos_as"].append(conv[1])
return config
def get_landmark_locations_airsim(config_json=None, add_empty=False, env_id=None):
assert config_json is not None or env_id is not None, "At least one of config_json or env_id must be provided"
if config_json is None:
config_json = load_env_config(env_id)
landmark_names = []
landmark_positions = []
units = UnrealUnits()
for i, landmarkName in enumerate(config_json["landmarkName"]):
x_pos = config_json["xPos"][i]
y_pos = config_json["zPos"][i]
pt = np.asarray([x_pos, y_pos])
pt_as = np.zeros(3)
pt_as[0:2] = units.pos2d_to_as(pt)
landmark_names.append(landmarkName)
landmark_positions.append(pt_as)
landmark_indices = get_landmark_name_to_index(add_empty=add_empty)
landmark_indices = [landmark_indices[name] for name in landmark_names]
return landmark_names, landmark_indices, landmark_positions
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
def convert_path(raw_path):
units = UnrealUnits()
conv_path = np.asarray([units.pos2d_to_as(p) for p in raw_path])
return conv_path
def convert_yaw_from_config(yaw):
units = UnrealUnits()
yaw_conv = units.yaw_to_as(yaw)
return yaw_conv
def convert_pos_from_config(pos):
units = UnrealUnits()
pos_conv = units.pos2d_to_as(pos)
return pos_conv