def rng_target_dist_field(batch_size, gtG, rng, max_dist, max_dist_to_compute,
nodes=None, compute_path=False):
# Sample a single node, compute distance to all nodes less than max_dist,
# sample nodes which are a particular distance away.
dists = []; pred_maps = []; paths = []; start_node_ids = []
end_node_ids = rng.choice(gtG.num_vertices(), size=(batch_size,),
replace=False).tolist()
for i in range(batch_size):
dist, pred_map = gt.topology.shortest_distance(
gt.GraphView(gtG, reversed=True), source=gtG.vertex(end_node_ids[i]),
target=None, max_dist=max_dist_to_compute, pred_map=True)
dist = np.array(dist.get_array())
pred_map = np.array(pred_map.get_array())
dists.append(dist)
pred_maps.append(pred_map)
# Randomly sample nodes which are withing max_dist
near_ids = np.where(dist <= max_dist)[0]
start_node_id = rng.choice(near_ids, size=(1,), replace=False)[0]
start_node_ids.append(start_node_id)
path = None
if compute_path:
path = get_path_ids(start_node_ids[i], end_node_ids[i], pred_map)
paths.append(path)
return start_node_ids, end_node_ids, dists, pred_maps, paths
def rng_next_goal(start_node_ids, batch_size, gtG, rng, max_dist,
max_dist_to_compute, node_room_ids, nodes=None,
compute_path=False, dists_from_start_node=None):
# Compute the distance field from the starting location, and then pick a
# destination in another room if possible otherwise anywhere outside this
# room.
dists = []; pred_maps = []; paths = []; end_node_ids = [];
for i in range(batch_size):
room_id = node_room_ids[start_node_ids[i]]
# Compute distances.
if dists_from_start_node == None:
dist, pred_map = gt.topology.shortest_distance(
gt.GraphView(gtG, reversed=False), source=gtG.vertex(start_node_ids[i]),
target=None, max_dist=max_dist_to_compute, pred_map=True)
dist = np.array(dist.get_array())
else:
dist = dists_from_start_node[i]
# Randomly sample nodes which are within max_dist.
near_ids = dist <= max_dist
near_ids = near_ids[:, np.newaxis]
# Check to see if there is a non-negative node which is close enough.
non_same_room_ids = node_room_ids != room_id
non_hallway_ids = node_room_ids != -1
good1_ids = np.logical_and(near_ids, np.logical_and(non_same_room_ids, non_hallway_ids))
good2_ids = np.logical_and(near_ids, non_hallway_ids)
good3_ids = near_ids
if np.any(good1_ids):
end_node_id = rng.choice(np.where(good1_ids)[0])
elif np.any(good2_ids):
end_node_id = rng.choice(np.where(good2_ids)[0])
elif np.any(good3_ids):
end_node_id = rng.choice(np.where(good3_ids)[0])
else:
logging.error('Did not find any good nodes.')
# Compute distance to this new goal for doing distance queries.
dist, pred_map = gt.topology.shortest_distance(
gt.GraphView(gtG, reversed=True), source=gtG.vertex(end_node_id),
target=None, max_dist=max_dist_to_compute, pred_map=True)
dist = np.array(dist.get_array())
pred_map = np.array(pred_map.get_array())
dists.append(dist)
pred_maps.append(pred_map)
end_node_ids.append(end_node_id)
path = None
if compute_path:
path = get_path_ids(start_node_ids[i], end_node_ids[i], pred_map)
paths.append(path)
return start_node_ids, end_node_ids, dists, pred_maps, paths
def rng_room_to_room(batch_size, gtG, rng, max_dist, max_dist_to_compute,
node_room_ids, nodes=None, compute_path=False):
# Sample one of the rooms, compute the distance field. Pick a destination in
# another room if possible otherwise anywhere outside this room.
dists = []; pred_maps = []; paths = []; start_node_ids = []; end_node_ids = [];
room_ids = np.unique(node_room_ids[node_room_ids[:,0] >= 0, 0])
for i in range(batch_size):
room_id = rng.choice(room_ids)
end_node_id = rng.choice(np.where(node_room_ids[:,0] == room_id)[0])
end_node_ids.append(end_node_id)
# Compute distances.
dist, pred_map = gt.topology.shortest_distance(
gt.GraphView(gtG, reversed=True), source=gtG.vertex(end_node_id),
target=None, max_dist=max_dist_to_compute, pred_map=True)
dist = np.array(dist.get_array())
pred_map = np.array(pred_map.get_array())
dists.append(dist)
pred_maps.append(pred_map)
# Randomly sample nodes which are within max_dist.
near_ids = dist <= max_dist
near_ids = near_ids[:, np.newaxis]
# Check to see if there is a non-negative node which is close enough.
non_same_room_ids = node_room_ids != room_id
non_hallway_ids = node_room_ids != -1
good1_ids = np.logical_and(near_ids, np.logical_and(non_same_room_ids, non_hallway_ids))
good2_ids = np.logical_and(near_ids, non_hallway_ids)
good3_ids = near_ids
if np.any(good1_ids):
start_node_id = rng.choice(np.where(good1_ids)[0])
elif np.any(good2_ids):
start_node_id = rng.choice(np.where(good2_ids)[0])
elif np.any(good3_ids):
start_node_id = rng.choice(np.where(good3_ids)[0])
else:
logging.error('Did not find any good nodes.')
start_node_ids.append(start_node_id)
path = None
if compute_path:
path = get_path_ids(start_node_ids[i], end_node_ids[i], pred_map)
paths.append(path)
return start_node_ids, end_node_ids, dists, pred_maps, paths