def generate_random_wrong_path(env_id, start_idx, end_idx):
env_config = load_env_config(env_id)
current_path = load_path(env_id)[start_idx:end_idx]
start_pos = current_path[0]
landmark_locations = np.asarray(
list(zip(env_config["xPos"], env_config["zPos"])))
distances = np.asarray(
[np.linalg.norm(start_pos - p) for p in landmark_locations])
closest_lm_idx = np.argmin(distances)
start_landmark = env_config["landmarkName"][closest_lm_idx]
# For segment-level, we're never (if ever) gonna need more than 3 landmarks
global NUM_LANDMARKS_VISISTED, DRONE_EDGE_CLEARANCE
NUM_LANDMARKS_VISISTED = 3
DRONE_EDGE_CLEARANCE = 0
i = 0
while True:
print(f"Attempt: {i}")
i += 1
ret = try_make_curve(env_config, start_pos, start_landmark)
if ret is not None:
break
pos_array, last_pos, last_landmark_visited = ret
# Return a trajectory of the same length as the one which is being replaced
return convert_path(pos_array[:(end_idx - start_idx)])
def faux_dataset_random_pt(eval_envs):
print("Generating faux dataset")
units = UnrealUnits(scale=1.0)
dataset = []
for env_id in eval_envs:
segment_dataset = []
config = load_env_config(env_id)
template = load_template(env_id)
start_pt = np.asarray(config["startPos"])
second_pt = np.asarray(config["startHeading"])
end_x = random.uniform(0, 1000)
end_y = random.uniform(0, 1000)
end_pt = np.asarray([end_x, end_y])
state1 = DroneState(None, start_pt)
state2 = DroneState(None, second_pt)
state3 = DroneState(None, end_pt)
segment_dataset.append(bare_min_sample(state1, False, env_id))
segment_dataset.append(bare_min_sample(state2, False, env_id))
segment_dataset.append(bare_min_sample(state3, True, env_id))
dataset.append(segment_dataset)
return dataset
def _load_drone_config(self, i, instance_id=None):
conf_json = load_env_config(i)
self._set_config(conf_json,
"",
"random_config",
i=None,
instance_id=instance_id)
def get_landmark_pos(self, env_id):
template = load_template(env_id)
config = load_env_config(env_id)
landmark_idx = config["landmarkName"].index(template["landmark1"])
pos_x = config["xPos"][landmark_idx]
pos_y = config["zPos"][landmark_idx]
landmark_pos = np.asarray([pos_x, pos_y])
return landmark_pos
def make_curves_for_unique_config(config_id):
start_pos, start_lm = None, None
for env_id in range(config_id, config_id + NEW_CONFIG_EVERY_N, 1):
config = load_env_config(env_id)
curve_path = get_curve_path(env_id)
plot_path = get_curve_plot_path(env_id)
start_pos, start_lm = make_new_curve(config, curve_path, plot_path,
start_pos, start_lm)
def faux_dataset_random_landmark(eval_envs):
print("Generating faux dataset")
units = UnrealUnits(scale=1.0)
dataset = []
for env_id in eval_envs:
segment_dataset = []
config = load_env_config(env_id)
template = load_template(env_id)
path = load_path(env_id)
landmark_radii = config["radius"]
start_pt = np.asarray(config["startPos"])
second_pt = np.asarray(config["startHeading"])
landmark_choice_ids = list(range(len(config["landmarkName"])))
choice_id = random.choice(landmark_choice_ids)
target_x = config["xPos"][choice_id]
target_y = config["zPos"][choice_id]
target_lm_pos = np.asarray([target_x, target_y])
landmark_dir = target_lm_pos - start_pt
method = template["side"]
theta = math.atan2(landmark_dir[1], landmark_dir[0]) + math.pi
if method == "infront":
theta = theta
elif method == "random":
theta = random.random() * 2 * math.pi
elif method == "behind":
theta = theta + math.pi
elif method == "left":
theta = theta - math.pi / 2
elif method == "right":
theta = theta + math.pi / 2
x, z = target_lm_pos[0], target_lm_pos[1]
landmark_radius = landmark_radii[choice_id]
sample_point = np.array([
x + math.cos(theta) * landmark_radius,
z + math.sin(theta) * landmark_radius
])
state1 = DroneState(None, start_pt)
state2 = DroneState(None, second_pt)
state3 = DroneState(None, sample_point)
segment_dataset.append(bare_min_sample(state1, False, env_id))
segment_dataset.append(bare_min_sample(state2, False, env_id))
segment_dataset.append(bare_min_sample(state3, True, env_id))
dataset.append(segment_dataset)
return dataset
def gen_lm_aux_labels(self, env_id, instruction, affine):
env_conf_json = load_env_config(env_id)
landmark_names, landmark_indices, landmark_positions = get_landmark_locations_airsim(
env_conf_json)
landmark_pos_in_img = pos_m_to_px(
np.asarray(landmark_positions)[:, 0:2],
np.array([self.map_w, self.map_h]))
landmark_pos_in_seg_img = apply_affine_on_pts(landmark_pos_in_img,
affine)
if False:
plot_path_on_img(self.latest_img_dbg, landmark_pos_in_img)
plot_path_on_img(self.latest_rot_img_dbg, landmark_pos_in_seg_img)
cv2.imshow("img", self.latest_img_dbg)
cv2.imshow("rot_img", self.latest_rot_img_dbg)
cv2.waitKey(0)
landmark_pos_t = torch.from_numpy(landmark_pos_in_seg_img).unsqueeze(0)
landmark_indices_t = torch.LongTensor(landmark_indices).unsqueeze(0)
mask1 = torch.gt(landmark_pos_t, 0)
mask2 = torch.lt(landmark_pos_t, self.img_w)
mask = mask1 * mask2
mask = mask[:, :, 0] * mask[:, :, 1]
mask = mask
landmark_pos_t = torch.masked_select(
landmark_pos_t,
mask.unsqueeze(2).expand_as(landmark_pos_t)).view([-1, 2])
landmark_indices_t = torch.masked_select(landmark_indices_t,
mask).view([-1])
mentioned_names, mentioned_indices = get_mentioned_landmarks(
self.thesaurus, instruction)
mentioned_labels_t = empty_float_tensor(list(
landmark_indices_t.size())).long()
for i, landmark_idx_present in enumerate(landmark_indices_t):
if landmark_idx_present in mentioned_indices:
mentioned_labels_t[i] = 1
if len(landmark_indices_t) > 0:
aux_label = {
"lm_pos": landmark_pos_t,
"lm_indices": landmark_indices_t,
"lm_mentioned": mentioned_labels_t,
"lm_visible": mask,
}
else:
aux_label = {
"lm_pos": [],
"lm_indices": [],
"lm_mentioned": [],
"lm_visible": []
}
return aux_label
def is_ambiguous(env_id):
template = load_template(env_id)
config = load_env_config(env_id)
#TODO: Handle the case where instructions refer to multiple landmarks
ref_landmark = template["landmark1"]
occ_count = 0
for landmark_name in config["landmarkName"]:
if has_ambiguous_noun_phrase(landmark_name, ref_landmark):
occ_count += 1
if occ_count == 0:
print("Error! Referred to landmark that's not found in env!")
exit(-1)
# More than one such landmark occurs in the test set
if occ_count > 1:
return True
else:
return False
def __getitem__(self, idx):
if self.seg_level:
env_id = self.seg_list[idx][0]
set_idx = self.seg_list[idx][1]
seg_idx = self.seg_list[idx][2]
else:
env_id = self.env_list[idx]
print("top_down_dataset_sm __getitem__ load_env_config")
env_conf_json = load_env_config(env_id)
landmark_names, landmark_indices, landmark_positions = get_landmark_locations_airsim(env_conf_json)
top_down_image = load_env_img(env_id)
path = load_path(env_id)
img_x = top_down_image.shape[0]
img_y = top_down_image.shape[1]
path_in_img_coords = self.cf_to_img(img_x, path)
landmark_pos_in_img = self.as_to_img(img_x, np.asarray(landmark_positions)[:, 0:2])
self.pos_rand_image = self.pos_rand_range * img_x
#self.plot_path_on_img(top_down_image, path_in_img_coords)
#self.plot_path_on_img(top_down_image, landmark_pos_in_img)
#cv2.imshow("top_down", top_down_image)
#cv2.waitKey()
input_images = []
input_instructions = []
label_images = []
aux_labels = []
# Somehow load the instruction with the start and end indices for each of the N segments
if self.seg_level:
instruction_segments = [self.all_instr[env_id][set_idx]["instructions"][seg_idx]]
else:
instruction_segments = self.all_instr[env_id][0]["instructions"]
for seg_idx, seg in enumerate(instruction_segments):
start_idx = seg["start_idx"]
end_idx = seg["end_idx"]
instruction = seg["instruction"]
# TODO: Check for overflowz
seg_path = path_in_img_coords[start_idx:end_idx]
seg_img = top_down_image.copy()
#test_plot = self.plot_path_on_img(seg_img, seg_path)
# TODO: Validate the 0.5 choice, should it be 2?
affine, cropsize = self.get_affine_matrix(seg_path, 0, [int(img_x / 2), int(img_y / 2)], 0.5)
if affine is None:
continue
seg_img_rot = self.apply_affine(seg_img, affine, cropsize)
seg_labels = np.zeros_like(seg_img[:, :, 0:1]).astype(float)
seg_labels = self.plot_path_on_img(seg_labels, seg_path)
seg_labels = gaussian_filter(seg_labels, 4)
seg_labels_rot = self.apply_affine(seg_labels, affine, cropsize)
#seg_labels_rot = gaussian_filter(seg_labels_rot, 4)
seg_labels_rot = self.normalize_0_1(seg_labels_rot)
# Change to true to visualize the paths / labels
if False:
cv2.imshow("rot_img", seg_img_rot)
cv2.imshow("seg_labels", seg_labels_rot)
rot_viz = seg_img_rot.astype(np.float64) / 512
rot_viz[:, :, 0] += seg_labels_rot.squeeze()
cv2.imshow("rot_viz", rot_viz)
cv2.waitKey(0)
tok_instruction = tokenize_instruction(instruction, self.word2token)
instruction_t = torch.LongTensor(tok_instruction).unsqueeze(0)
# Get landmark classification labels
landmark_pos_in_seg_img = self.apply_affine_on_pts(landmark_pos_in_img, affine)
# Down-size images and labels if requested by the model
if self.img_scale != 1.0:
seg_img_rot = transform.resize(
seg_img_rot,
[seg_img_rot.shape[0] * self.img_scale,
seg_img_rot.shape[1] * self.img_scale], mode="constant")
seg_labels_rot = transform.resize(
seg_labels_rot,
[seg_labels_rot.shape[0] * self.img_scale,
seg_labels_rot.shape[1] * self.img_scale], mode="constant")
landmark_pos_in_seg_img = landmark_pos_in_seg_img * self.img_scale
seg_img_rot = standardize_image(seg_img_rot)
seg_labels_rot = standardize_image(seg_labels_rot)
seg_img_t = torch.from_numpy(seg_img_rot).unsqueeze(0).float()
seg_labels_t = torch.from_numpy(seg_labels_rot).unsqueeze(0).float()
landmark_pos_t = torch.from_numpy(landmark_pos_in_seg_img).unsqueeze(0)
landmark_indices_t = torch.LongTensor(landmark_indices).unsqueeze(0)
mask1 = torch.gt(landmark_pos_t, 0)
mask2 = torch.lt(landmark_pos_t, seg_img_t.size(2))
mask = mask1 * mask2
mask = mask[:, :, 0] * mask[:, :, 1]
mask = mask
landmark_pos_t = torch.masked_select(landmark_pos_t, mask.unsqueeze(2).expand_as(landmark_pos_t)).view([-1, 2])
landmark_indices_t = torch.masked_select(landmark_indices_t, mask).view([-1])
mentioned_names, mentioned_indices = get_mentioned_landmarks(self.thesaurus, instruction)
mentioned_labels_t = empty_float_tensor(list(landmark_indices_t.size())).long()
for i, landmark_idx_present in enumerate(landmark_indices_t):
if landmark_idx_present in mentioned_indices:
mentioned_labels_t[i] = 1
aux_label = {
"landmark_pos": landmark_pos_t,
"landmark_indices": landmark_indices_t,
"landmark_mentioned": mentioned_labels_t,
"visible_mask": mask,
}
if self.include_instr_negatives:
# If we are to be using similar instructions according to the json file, then
# initialize choices with similar instructions. Otherwise let choices be empty, and they will
# be filled in the following lines.
if self.instr_negatives_similar_only:
choices = self.similar_instruction_map[str(env_id)][str(seg_idx)]
else:
choices = []
# If there are no similar instructions to this instruction, pick a completely random instruction
if len(choices) == 0:
while len(choices) == 0:
env_options = list(self.similar_instruction_map.keys())
random_env = random.choice(env_options)
seg_options = list(self.similar_instruction_map[random_env].keys())
if len(seg_options) == 0:
continue
random_seg = random.choice(seg_options)
choices = self.similar_instruction_map[random_env][random_seg]
pick = random.choice(choices)
picked_env = pick["env_id"]
picked_seg = pick["seg_idx"]
picked_set = pick["set_idx"]
picked_instruction = self.all_instr[picked_env][picked_set]["instructions"][picked_seg]["instruction"]
tok_fake_instruction = tokenize_instruction(picked_instruction, self.word2token)
aux_label["negative_instruction"] = torch.LongTensor(tok_fake_instruction).unsqueeze(0)
input_images.append(seg_img_t)
input_instructions.append(instruction_t)
label_images.append(seg_labels_t)
aux_labels.append(aux_label)
return [input_images, input_instructions, label_images, aux_labels]
def generate_template_curve(config_id):
config = load_env_config(config_id)
# Make a curve that follows the given template
make_template_curve(config, config_id)
def __call__(self, images, states, segment_data, mask):
projector = PinholeProjector(img_x=images.size(3), img_y=images.size(2))
# presenter = Presenter()
env_id = segment_data.metadata[0]["env_id"]
conf_json = load_env_config(env_id)
all_landmark_indices = get_landmark_name_to_index()
landmark_names, landmark_indices, landmark_pos = get_landmark_locations_airsim(conf_json)
path_array = load_path(env_id)
goal_loc = self.__get_goal_location_airsim(path_array)
# Traj length x 64 landmarks x 14
# 0-5: Present landmarks data
# 0 - landmark present in img
# 1-2 - landmark pix_x | pix_y
# 3-5 - landmark world coords m_x | m_y
# 6-7: Template data
# 6 - landmark_mentioned index
# 7 - mentioned_side index
# 8 - landmark mentioned
# 9-13: Goal data
# 9-10 - goal_x_pix | goal_y_pix
# 11-12 - goal_x | goal_y (world)
# 13 - goal visible
aux_labels = torch.zeros((images.size(0), len(all_landmark_indices), 14))
# Store goal location in airsim coordinates
aux_labels[:, :, 11:13] = torch.from_numpy(goal_loc[0:2]).unsqueeze(0).unsqueeze(0).expand_as(
aux_labels[:, :, 11:13])
for i, idx in enumerate(landmark_indices):
aux_labels[:, idx, 3:6] = torch.from_numpy(
landmark_pos[i]).unsqueeze(0).clone().repeat(aux_labels.size(0), 1, 1)
for timestep in range(images.size(0)):
# presenter.save_image(images[timestep], name="tmp.png", torch=True)
if mask[timestep] == 0:
continue
cam_pos = states[timestep, 9:12]
cam_rot = states[timestep, 12:16]
goal_in_img, goal_in_cam, status = projector.world_point_to_image(cam_pos, cam_rot, goal_loc)
if goal_in_img is not None:
aux_labels[timestep, :, 9:11] = torch.from_numpy(goal_in_img[0:2]).unsqueeze(0).expand_as(
aux_labels[timestep, :, 9:11])
aux_labels[timestep, :, 13] = 1.0
for i, landmark_world in enumerate(landmark_pos):
landmark_idx = landmark_indices[i]
landmark_in_img, landmark_in_cam, status = projector.world_point_to_image(cam_pos, cam_rot,
landmark_world)
# This is None if the landmark is behind the camera.
if landmark_in_img is not None:
# presenter.save_image(images[timestep], name="tmp.png", torch=True, draw_point=landmark_in_img)
aux_labels[timestep, landmark_idx, 0] = 1.0
aux_labels[timestep, landmark_idx, 1:3] = torch.from_numpy(landmark_in_img[0:2])
# aux_labels[timestep, landmark_idx, 3:6] = torch.from_numpy(landmark_in_cam[0:3])
# aux_labels[timestep, landmark_idx, 8] = 1.0 if landmark_idx == mentioned_landmark_idx else 0
return aux_labels
def provider_lm_pos_lm_indices_fpv(segment_data, data):
"""
Data provider that gives the positions and indices of all landmarks visible in the FPV image.
:param segment_data: segment dataset for which to provide data
:return: ("lm_pos", lm_pos) - lm_pos is a list (over timesteps) of lists (over landmarks visible in image) of the
landmark locations in image pixel coordinates
("lm_indices", lm_indices) - lm_indices is a list (over timesteps) of lists (over landmarks visible in image)
of the landmark indices for every landmark included in lm_pos. These are the landmark classifier labels
"""
env_id = segment_data[0]["metadata"]["env_id"]
domain = segment_data[0]["metadata"]["domain"]
#if INSTRUCTIONS_FROM_FILE:
# env_instr = load_instructions(env_id)
conf_json = load_env_config(env_id)
all_landmark_indices = get_landmark_name_to_index()
landmark_names, landmark_indices, landmark_pos = get_landmark_locations_airsim(
conf_json)
params = P.get_current_parameters().get(
"Model") or P.get_current_parameters().get("ModelPVN").get("Stage1")
projector = PinholeCameraProjection(map_size_px=params["global_map_size"],
world_size_px=params["world_size_px"],
world_size_m=params["world_size_m"],
img_x=params["img_w"],
img_y=params["img_h"],
cam_fov=params["cam_h_fov"],
domain=domain,
use_depth=False)
traj_len = len(segment_data)
lm_pos_fpv = []
lm_indices = []
lm_mentioned = []
lm_pos_map = []
for timestep in range(traj_len):
t_lm_pos_fpv = []
t_lm_indices = []
t_lm_mentioned = []
t_lm_pos_map = []
if segment_data[timestep]["state"] is not None:
cam_pos = segment_data[timestep]["state"].get_cam_pos_3d()
cam_rot = segment_data[timestep]["state"].get_cam_rot()
instruction_str = segment_data[timestep]["instruction"]
mentioned_landmark_names, mentioned_landmark_indices = get_mentioned_landmarks_nl(
instruction_str)
for i, landmark_in_world in enumerate(landmark_pos):
landmark_idx = landmark_indices[i]
landmark_in_img, landmark_in_cam, status = projector.world_point_to_image(
cam_pos, cam_rot, landmark_in_world)
this_lm_mentioned = 1 if landmark_idx in mentioned_landmark_indices else 0
# This is None if the landmark is behind the camera.
if landmark_in_img is not None:
# presenter.save_image(images[timestep], name="tmp.png", torch=True, draw_point=landmark_in_img)
t_lm_pos_fpv.append(landmark_in_img[0:2])
t_lm_pos_map.append(landmark_in_world[0:2])
t_lm_indices.append(landmark_idx)
t_lm_mentioned.append(this_lm_mentioned)
if len(t_lm_pos_fpv) > 0:
t_lm_pos_fpv = torch.from_numpy(np.asarray(t_lm_pos_fpv)).float()
t_lm_pos_map = torch.from_numpy(np.asarray(t_lm_pos_map)).float()
t_lm_indices = torch.from_numpy(np.asarray(t_lm_indices)).long()
t_lm_mentioned = torch.from_numpy(
np.asarray(t_lm_mentioned)).long()
else:
t_lm_pos_fpv = None
t_lm_pos_map = None
t_lm_indices = None
t_lm_mentioned = None
lm_pos_fpv.append(t_lm_pos_fpv)
lm_pos_map.append(t_lm_pos_map)
lm_indices.append(t_lm_indices)
lm_mentioned.append(t_lm_mentioned)
return [("lm_pos_fpv", lm_pos_fpv), ("lm_indices", lm_indices),
("lm_mentioned", lm_mentioned), ("lm_pos_map", lm_pos_map)]
def ground_terms(word2id, clustered_corpus, landmark_names,
train_instructions):
# the clustered corpus is a dictionary of lists, where the keys are valid english words and the values are
# lists of words found in the corpus that are assumed to be misspellings of the key valid words
# We make the distinction that a word is any word in an instruction
# Terms are words in the english vocabulary. Multiple words (misspellings) can map to a single term.
num_terms = len(clustered_corpus)
vocab_size = len(word2id)
num_landmarks = len(landmark_names)
# This is gonna be the new word2id, once we start using the thesaurus
term2id = {}
id2term = {}
for i, term in enumerate(sorted(clustered_corpus.keys())):
term2id[term] = i
id2term[i] = term
# Calculate the mutual information between each cluster and each landmark
# Number of times each term appears in an instruction
term_occurences = np.zeros(num_terms)
# Number of times each landmark appears near a segment path
landmark_occurences = np.zeros(num_landmarks)
# The number of times each term and landmark combination appears in the instruction and near the path
term_landmark_cooccurences = np.zeros((num_terms, num_landmarks))
# The number of total segments that were considered
total_occurences = 0
landmark_indices = get_landmark_name_to_index()
# Inverse the clusters so that we can efficiently map each word in each instruction to it's cluster core
word2term = {}
for real_word, misspellings in clustered_corpus.items():
for misspelling in misspellings:
word2term[misspelling] = real_word
# Count landmark and word occurences and co-occurences
for env_id, instruction_sets in train_instructions.items():
path = load_path(env_id)
env_config = load_env_config(env_id)
for instruction_set in instruction_sets[0]["instructions"]:
instruction_str = instruction_set["instruction"]
start_idx = instruction_set["start_idx"]
end_idx = instruction_set["end_idx"]
present_landmarks = close_landmark_names(env_config, path,
start_idx, end_idx)
present_lm_indices = [
landmark_indices[lm] for lm in present_landmarks
]
mentioned_words = split_instruction(
clean_instruction(instruction_str))
mentioned_terms = words_to_terms(mentioned_words, word2term)
for term in mentioned_terms:
term_id = term2id[term]
term_occurences[term_id] += 1
for lm_idx in present_lm_indices:
landmark_occurences[lm_idx] += 1
for term in mentioned_terms:
term_id = term2id[term]
term_landmark_cooccurences[term_id][lm_idx] += 1
total_occurences += 1
term_prob = np.expand_dims(term_occurences / total_occurences,
1).repeat(num_landmarks, 1)
landmark_prob = np.expand_dims(landmark_occurences / total_occurences,
0).repeat(num_terms, 0)
term_and_landmark_prob = term_landmark_cooccurences / total_occurences
# term_and_landmark_prob has dimensions 0: terms, 1: landmarks
mutual_info_factor = term_and_landmark_prob / (landmark_prob * term_prob +
1e-27)
#mutual_info_factor = term_and_landmark_prob / ((1 / num_landmarks) * term_prob + 1e-9)
mutual_info = term_and_landmark_prob * np.log(mutual_info_factor + 1e-27)
# The above line is the correct formula for mutual information. For our case, below formula might be better?
# The mutual information is higher for common words than uncommon ones. We might prefer the opposite effect.
# On the other hand, uncommon words are more likely to spuriously correlate with landmarks, which will cause a
# less reliable corpus.
#mutual_info = np.log(mutual_info_factor + 1e-27)
# Ground each term and produce the thesaurus
term_meanings = {}
common_words = []
for i in range(num_terms):
grounded_lm_indices = [
idx for idx in range(num_landmarks)
if mutual_info[i][idx] > MUTUAL_INFO_THRESHOLD
]
grounded_lm_names = [
landmark_names[idx] for idx in grounded_lm_indices
]
mutual_infos = np.asarray(
[mutual_info[i][idx] for idx in grounded_lm_indices])
args = list(np.argsort(mutual_infos))
grounded_lm_names = list(
reversed([grounded_lm_names[idx] for idx in args]))
mutual_infos = list(reversed([mutual_infos[idx] for idx in args]))
# If the word is too common to be referring to a landmark, ignore ita
this_term_prob = term_prob[i][0]
if this_term_prob > MAX_TERM_PROB:
common_words.append(id2term[i])
grounded_lm_names = []
mutual_infos = []
term_meanings[id2term[i]] = \
{
"landmarks": grounded_lm_names,
"mutual_info": mutual_infos,
"term_prob": this_term_prob
}
for k in term_meanings.keys():
if len(term_meanings[k]["landmarks"]) > 0:
print(k, term_meanings[k])
print("Ignored groundings for these common words: " + str(common_words))
return term_meanings, word2term
def show_landmark_locations(self, loop=True, states=None):
# Show landmark locations in first-person images
img_all = self.tensor_store.get("images")
img_w_all = self.tensor_store.get("images_w")
import rollout.run_metadata as md
if md.IS_ROLLOUT:
# TODO: Discard this and move this to PomdpInterface or something
# (it's got nothing to do with the model)
# load landmark positions from configs
from data_io.env import load_env_config
from learning.datasets.aux_data_providers import get_landmark_locations_airsim
from learning.models.semantic_map.pinhole_camera_inv import PinholeCameraProjection
projector = PinholeCameraProjection(
map_size_px=self.params["global_map_size"],
world_size_px=self.params["world_size_px"],
world_size_m=self.params["world_size_m"],
img_x=self.params["img_w"],
img_y=self.params["img_h"],
cam_fov=self.params["cam_h_fov"],
#TODO: Handle correctly
domain="sim",
use_depth=False)
conf_json = load_env_config(md.ENV_ID)
landmark_names, landmark_indices, landmark_pos = get_landmark_locations_airsim(
conf_json)
cam_poses = self.cam_poses_from_states(states)
cam_pos = cam_poses.position[0]
cam_rot = cam_poses.orientation[0]
lm_pos_map_all = []
lm_pos_img_all = []
for i, landmark_in_world in enumerate(landmark_pos):
lm_pos_img, landmark_in_cam, status = projector.world_point_to_image(
cam_pos, cam_rot, landmark_in_world)
lm_pos_map = torch.from_numpy(
transformations.pos_m_to_px(
landmark_in_world[np.newaxis, :],
self.params["global_map_size"],
self.params["world_size_m"],
self.params["world_size_px"]))
lm_pos_map_all += [lm_pos_map[0]]
if lm_pos_img is not None:
lm_pos_img_all += [lm_pos_img]
lm_pos_img_all = [lm_pos_img_all]
lm_pos_map_all = [lm_pos_map_all]
else:
lm_pos_img_all = self.tensor_store.get("lm_pos_fpv_img")
lm_pos_map_all = self.tensor_store.get("lm_pos_map")
print("Plotting landmark points")
for i in range(len(img_all)):
p = Presenter()
overlay_fpv = p.overlay_pts_on_image(img_all[i][0],
lm_pos_img_all[i])
overlay_map = p.overlay_pts_on_image(img_w_all[i][0],
lm_pos_map_all[i])
p.show_image(overlay_fpv, "landmarks_on_fpv_img", scale=8)
p.show_image(overlay_map, "landmarks_on_map", scale=20)
if not loop:
break
def _get_config(self, name, i=None, instance_id=None):
config = load_env_config(id)
return config
def _get_config(self, name, i=None, instance_id=None):
print("_get_config load_env_config")
config = load_env_config(i)
return config
