def draw(rs, distance, remaining, is_explored=False): start_edge_idx = rs.distance_to_edge(distance, return_idx=True) for edge_idx in xrange(start_edge_idx, len(rs.edges)): edge = rs.edges[edge_idx] edge_distance = distance - rs.edge_distances[edge.id] start_edge_pos = graph.EdgePos(edge, edge_distance) start = start_edge_pos.point() if edge_distance + remaining < edge.segment().length(): end_edge_pos = graph.EdgePos(edge, edge_distance + remaining) end = end_edge_pos.point() else: end = edge.dst.point if not is_explored: for p in geom.draw_line(start.sub(origin), end.sub(origin), geom.Point(window_size, window_size)): p_small = p.scale(128.0 / window_size) tile[p_small.x, p_small.y] = 1.0 remaining -= edge.segment().length() - edge_distance distance = rs.edge_distances[edge.id] + edge.segment().length() + 0.001 if remaining <= 0: return for next_rs in rs.out_rs(path.gc.edge_to_rs): if rs == next_rs or next_rs.is_opposite(rs): continue draw(next_rs, 0, remaining, is_explored=path.is_explored(next_rs.edges[0]))
def vector_to_action(extension_vertex, angle_outputs, threshold):
# mask out buckets that are similar to existing edges
blacklisted_buckets = set()
for edge in extension_vertex.out_edges:
angle = geom.Point(1, 0).signed_angle(edge.segment().vector())
bucket = int((angle + math.pi) * 64.0 / math.pi / 2)
for offset in range(6):
clockwise_bucket = (bucket + offset) % 64
counterclockwise_bucket = (bucket + 64 - offset) % 64
blacklisted_buckets.add(clockwise_bucket)
blacklisted_buckets.add(counterclockwise_bucket)
seen_vertices = set()
search_queue = []
nearby_points = {}
seen_vertices.add(extension_vertex)
for edge in extension_vertex.out_edges:
search_queue.append((graph.EdgePos(edge, 0), 0))
while len(search_queue) > 0:
edge_pos, distance = search_queue[0]
search_queue = search_queue[1:]
if distance > 0:
nearby_points[edge_pos.point()] = distance
if distance >= 4 * SEGMENT_LENGTH:
continue
edge = edge_pos.edge
l = edge.segment().length()
if edge_pos.distance + SEGMENT_LENGTH < l:
search_queue.append(
(graph.EdgePos(edge, edge_pos.distance + SEGMENT_LENGTH),
distance + SEGMENT_LENGTH))
elif edge.dst not in seen_vertices:
seen_vertices.add(edge.dst)
for edge in edge.dst.out_edges:
search_queue.append(
(graph.EdgePos(edge, 0), distance + l - edge_pos.distance))
# any leftover targets above threshold?
best_bucket = None
best_value = None
for bucket in range(64):
if bucket in blacklisted_buckets:
continue
next_point = model_utils.get_next_point(extension_vertex.point, bucket,
SEGMENT_LENGTH)
bad = False
for nearby_point, distance in nearby_points.items():
if nearby_point.distance(next_point) < 0.5 * (SEGMENT_LENGTH +
distance):
bad = True
break
if bad:
continue
value = angle_outputs[bucket]
if value > threshold and (best_bucket is None or value > best_value):
best_bucket = bucket
best_value = value
x = numpy.zeros((64, ), dtype='float32')
if best_bucket is not None:
x[best_bucket] = best_value
return x
def get_example(traintest='train'):
while True:
if traintest == 'train':
tile = random.choice(train_tiles)
elif traintest == 'test':
tile = random.choice(val_tiles)
edge_ids, edge_probs = get_tile_edgeprobs(tile)
if len(edge_ids) > 80 or len(edge_ids) > 0:
break
# determine rotation factor
rotation = None
if ENABLE_ROTATION:
rotation = random.random() * 2 * math.pi
rect = get_tile_rect(tile)
small_rect = rect.add_tol(-WINDOW_SIZE * FETCH_FACTOR / 2)
# get random edge position
edge_id = numpy.random.choice(edge_ids, p=edge_probs)
gc = tiles.get_gc(tile.region)
edge = gc.graph.edges[edge_id]
distance = random.random() * edge.segment().length()
# convert to point and add noise
point = graph.EdgePos(edge, distance).point()
if random.random() < PROB_FROM_ROAD:
if random.random() < 0.2:
noise_amount = 10 * SEGMENT_LENGTH
else:
noise_amount = ROAD_WIDTH / 1.5
noise = geom.Point(random.random() * 2 * noise_amount - noise_amount,
random.random() * 2 * noise_amount - noise_amount)
point = point.add(noise)
point = small_rect.clip(point)
else:
point = geom.Point(random.randint(0,
small_rect.lengths().x - 1),
random.randint(0,
small_rect.lengths().y - 1))
point = point.add(small_rect.start)
point = small_rect.clip(point)
# match point to edge if possible
threshold = ROAD_WIDTH
closest_edge = None
closest_distance = None
for edge in gc.edge_index.search(point.bounds().add_tol(threshold)):
d = edge.segment().distance(point)
if d < threshold and (closest_edge is None or d < closest_distance):
closest_edge = edge
closest_distance = d
closest_pos = None
if closest_edge is not None:
closest_pos = closest_edge.closest_pos(point)
# generate input
origin = point.sub(geom.Point(WINDOW_SIZE / 2, WINDOW_SIZE / 2))
tile_origin = origin.sub(rect.start)
fetch_rect = geom.Rectangle(
tile_origin,
tile_origin.add(geom.Point(WINDOW_SIZE, WINDOW_SIZE))).add_tol(
WINDOW_SIZE * (FETCH_FACTOR - 1) / 2)
big_ims = tiles.cache.get_window(tile.region, rect, fetch_rect)
input = big_ims['input'].astype('float32') / 255.0
if rotation:
input = scipy.ndimage.interpolation.rotate(input,
rotation * 180 / math.pi,
reshape=False,
order=0)
input = input[WINDOW_SIZE / 2:3 * WINDOW_SIZE / 2,
WINDOW_SIZE / 2:3 * WINDOW_SIZE / 2, :]
# compute targets
if closest_edge is not None:
angle_targets = compute_targets(gc, point, closest_pos)
if rotation:
shift = int(rotation * 32 / math.pi)
new_targets = numpy.zeros((64, ), 'float32')
for i in xrange(64):
new_targets[(i + shift) % 64] = angle_targets[i]
angle_targets = new_targets
else:
angle_targets = numpy.zeros((64, ), 'float32')
detect_targets = numpy.zeros((64 * FETCH_FACTOR, 64 * FETCH_FACTOR, 1),
dtype='float32')
if not NO_DETECT:
fetch_rect = geom.Rectangle(
origin, origin.add(geom.Point(WINDOW_SIZE, WINDOW_SIZE))).add_tol(
WINDOW_SIZE * (FETCH_FACTOR - 1) / 2)
for edge in gc.edge_index.search(fetch_rect.add_tol(32)):
start = edge.src.point.sub(fetch_rect.start).scale(
float(64) / WINDOW_SIZE)
end = edge.dst.point.sub(fetch_rect.start).scale(
float(64) / WINDOW_SIZE)
for p in geom.draw_line(
start, end, geom.Point(64 * FETCH_FACTOR,
64 * FETCH_FACTOR)):
detect_targets[p.x, p.y, 0] = 1
if rotation:
detect_targets = scipy.ndimage.interpolation.rotate(detect_targets,
rotation *
180 / math.pi,
reshape=False,
order=0)
detect_targets = detect_targets[32:96, 32:96, :]
info = {
'region': tile.region,
'point': point,
'origin': origin,
'closest_pos': closest_pos,
'rotation': rotation,
}
return info, input, angle_targets, detect_targets
def compute_targets_by_best(path, extension_vertex, segment_length):
angle_targets = numpy.zeros((64,), 'float32')
allow_reconnect = False
def best_angle_to_pos(pos):
angle_points = [get_next_point(extension_vertex.point, angle_bucket, segment_length) for angle_bucket in xrange(64)]
distances = [angle_point.distance(pos.point()) for angle_point in angle_points]
point_angle = numpy.argmin(distances) * math.pi * 2 / 64.0 - math.pi
edge_angle = geom.Point(1, 0).signed_angle(pos.edge.segment().vector())
avg_vector = vector_from_angle(point_angle).add(vector_from_angle(edge_angle))
avg_angle = geom.Point(1, 0).signed_angle(avg_vector)
return int((avg_angle + math.pi) * 64.0 / math.pi / 2)
def set_angle_bucket_soft(target_bucket):
for offset in xrange(31):
clockwise_bucket = (target_bucket + offset) % 64
counterclockwise_bucket = (target_bucket + 64 - offset) % 64
for bucket in [clockwise_bucket, counterclockwise_bucket]:
angle_targets[bucket] = max(angle_targets[bucket], pow(0.75, offset))
def set_by_positions(positions):
# get existing angle buckets, don't use any that are within 3 buckets
bad_buckets = set()
for edge in extension_vertex.out_edges:
edge_angle = geom.Point(1, 0).signed_angle(edge.segment().vector())
edge_bucket = int((edge_angle + math.pi) * 64.0 / math.pi / 2)
for offset in xrange(3):
clockwise_bucket = (edge_bucket + offset) % 64
counterclockwise_bucket = (edge_bucket + 64 - offset) % 64
bad_buckets.add(clockwise_bucket)
bad_buckets.add(counterclockwise_bucket)
for pos in positions:
best_angle_bucket = best_angle_to_pos(pos)
if best_angle_bucket in bad_buckets:
continue
set_angle_bucket_soft(best_angle_bucket)
if extension_vertex.edge_pos is not None:
cur_edge = extension_vertex.edge_pos.edge
cur_rs = path.gc.edge_to_rs[cur_edge.id]
prev_rs = None
if len(extension_vertex.in_edges) >= 1:
prev_vertex = extension_vertex.in_edges[0].src
if prev_vertex.edge_pos is not None:
prev_edge = prev_vertex.edge_pos.edge
prev_rs = path.gc.edge_to_rs[prev_edge.id]
def get_potential_rs(segment_length, allow_backwards):
potential_rs = []
if cur_rs.edge_distances[cur_edge.id] + extension_vertex.edge_pos.distance + segment_length < cur_rs.length():
potential_rs.append(cur_rs)
else:
for rs in cur_rs.out_rs(path.gc.edge_to_rs):
if rs == cur_rs or rs.is_opposite(cur_rs):
continue
potential_rs.append(rs)
if allow_backwards and cur_rs.edge_distances[cur_edge.id] + extension_vertex.edge_pos.distance < segment_length / 2 and prev_rs is not None:
for rs in cur_rs.in_rs(path.gc.edge_to_rs):
if rs == cur_rs or rs.is_opposite(cur_rs) or rs == prev_rs or rs.is_opposite(prev_rs):
continue
# add the opposite of this rs so that we are going away from extension_vertex
opposite_rs = path.gc.edge_to_rs[rs.edges[0].get_opposite_edge().id]
potential_rs.append(opposite_rs)
# at very beginning of path, we can go in either direction
if len(path.graph.edges) == 0:
# TODO: fix get_opposite_rs for loops
# currently, if there is a loop, then the rs corresponding to the loop may start at
# any point along the loop, and get_opposite_rs will fail
# I think it may be okay if the loop isn't completely isolated (circle with no
# intersections), but definitely it fails for isolated loops
#potential_rs.append(cur_rs.get_opposite_rs(path.gc.edge_to_rs))
opposite_rs1 = cur_rs.get_opposite_rs(path.gc.edge_to_rs)
opposite_rs2 = path.gc.edge_to_rs[cur_rs.edges[-1].get_opposite_edge().id]
potential_rs.append(opposite_rs2)
if opposite_rs1 != opposite_rs2:
if opposite_rs1 is None:
print 'warning: using opposite_rs2 for rs {}'.format(opposite_rs2.id)
else:
raise Exception('opposite_rs1 ({}) != opposite_rs2 ({})'.format(opposite_rs1.id, opposite_rs2.id))
return potential_rs
potential_rs = get_potential_rs(segment_length, True)
potential_far_rs = get_potential_rs(3 * segment_length, False)
# reconnect if there is a nearby path vertex such that:
# (1) extension_vertex and the nearby vertex are far in the path graph
# (2) but close in the ground truth graph
# (3) and rs is either same as current one, or outgoing from a ground truth vertex that we are entering
if len(extension_vertex.in_edges) <= 1:
# first, get outgoing road segments from potential_far_rs
reconnectable_rs = set([rs for rs in potential_far_rs if path.is_explored(graph.EdgePos(rs.edges[0], 0)) and rs != cur_rs and not cur_rs.is_opposite(rs)])
reconnectable_rs.add(cur_rs)
reconnectable_rs.add(path.gc.edge_to_rs[cur_rs.edges[-1].get_opposite_edge().id])
# now, satisfy 1 and 2, while using set above to satisfy 3
nearby_path_vertices = set(graph.get_nearby_vertices(extension_vertex, 6))
gt_distances = graph.shortest_distances_from_source(cur_edge.dst, max_distance=3*segment_length)
r = extension_vertex.point.bounds().add_tol(3*segment_length)
for nearby_edge_id in path.edge_rtree.intersection((r.start.x, r.start.y, r.end.x, r.end.y)):
nearby_edge = path.graph.edges[nearby_edge_id]
for nearby_vertex in [nearby_edge.src, nearby_edge.dst]:
if nearby_vertex.edge_pos is None:
continue
elif nearby_vertex.point.distance(extension_vertex.point) > 3*segment_length:
continue
elif nearby_vertex in nearby_path_vertices:
continue
elif path.gc.edge_to_rs[nearby_vertex.edge_pos.edge.id] not in reconnectable_rs:
continue
gt_distance = min(
gt_distances.get(nearby_vertex.edge_pos.edge.src.id, 99999) + nearby_vertex.edge_pos.distance,
gt_distances.get(nearby_vertex.edge_pos.edge.dst.id, 99999) + nearby_vertex.edge_pos.reverse().distance
)
if gt_distance < 3*segment_length:
allow_reconnect = True
if len(potential_rs) + 1 > len(extension_vertex.out_edges):
if DEBUG: print '... compute_targets_by_best: potential_rs={}'.format([rs.id for rs in potential_rs])
expected_positions = []
for rs in potential_rs:
pos = rs.closest_pos(extension_vertex.point)
if path.is_explored(pos):
continue
rs_follow_positions = graph.follow_graph(pos, segment_length, explored_node_pairs=path.explored_pairs)
if DEBUG: print '... compute_targets_by_best: rs {}: closest pos to extension point {} is on edge {}@{} at {}'.format(rs.id, extension_vertex.point, pos.edge.id, pos.distance, pos.point())
for rs_follow_pos in rs_follow_positions:
if DEBUG: print '... compute_targets_by_best: rs {}: ... {}@{} at {}'.format(rs.id, rs_follow_pos.edge.id, rs_follow_pos.distance, rs_follow_pos.point())
expected_positions.extend(rs_follow_positions)
set_by_positions(expected_positions)
else:
if DEBUG: print '... compute_targets_by_best: found {} potential rs but already have {} outgoing edges'.format(len(potential_rs), len(extension_vertex.out_edges))
else:
if DEBUG: print '... compute_targets_by_best: edge_pos is None'
return angle_targets, allow_reconnect
