def get_starting_locations(gcs, segment_length, region=None):
all_starting_locations = {}
with open(startlocs_path, 'r') as f:
# top-level is dict from tile to starting location lists
for tile, locs in json.load(f).items():
tile_region = tile.split('_')[0]
if region is not None and tile_region != region:
continue
elif tile_region not in gcs:
continue
starting_locations = []
# each loc is a dict with keys 'x', 'y', 'edge_id'
for loc in locs:
point = geom.Point(int(loc['x']), int(loc['y']))
edge_pos = gcs[tile_region].graph.edges[int(
loc['edge_id'])].closest_pos(point)
next_positions = graph.follow_graph(edge_pos, segment_length)
if not next_positions:
continue
starting_locations.append([{
'point': point,
'edge_pos': edge_pos,
}, {
'point': next_positions[0].point(),
'edge_pos': next_positions[0],
}])
all_starting_locations[tile] = starting_locations
return all_starting_locations
def get_starting_locations(gcs, segment_length, region=None):
all_starting_locations = {}
with open(startlocs_path, 'r') as f:
# top-level is dict from tile to starting location lists
for tile, locs in json.load(f).items():
tile_region = tile.split('_')[0]
if region is not None and tile_region != region:
continue
elif tile_region not in gcs:
continue
starting_locations = []
# each loc is a dict with keys 'x', 'y', 'edge_id'
for loc in locs:
point = geom.Point(int(loc['x']), int(loc['y']))
edge_pos = gcs[tile_region].graph.edges[int(loc['edge_id'])].closest_pos(point)
next_positions = graph.follow_graph(edge_pos, segment_length)
if not next_positions:
continue
starting_locations.append([{
'point': point,
'edge_pos': edge_pos,
}, {
'point': next_positions[0].point(),
'edge_pos': next_positions[0],
}])
all_starting_locations[tile] = starting_locations
return all_starting_locations
def helper_compute_viz_points(path, extension_vertex, segment_length):
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]
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 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)
mm_point = extension_vertex.edge_pos.point()
nx_points = []
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()))
nx_points.extend([pos.point() for pos in rs_follow_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)))
return {
'mm': mm_point,
'nx': nx_points,
}
else:
if DEBUG: print('... compute_targets_by_best: edge_pos is None')
return None
def helper_compute_viz_points(path, extension_vertex, segment_length):
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]
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 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)
mm_point = extension_vertex.edge_pos.point()
nx_points = []
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())
nx_points.extend([pos.point() for pos in rs_follow_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))
return {
'mm': mm_point,
'nx': nx_points,
}
else:
if DEBUG: print '... compute_targets_by_best: edge_pos is None'
return None
best_distance = None
for candidate in tile_data['gc'].edge_index.search(p.bounds().add_tol(32)):
pos = candidate.closest_pos(p)
distance = pos.point().distance(p)
if best_pos is None or distance < best_distance:
best_pos = pos
best_distance = distance
return best_pos
pos1_point = MANUAL_POINT1.add(MANUAL_RELATIVE)
pos1_pos = match_point(pos1_point)
if MANUAL_POINT2:
pos2_point = MANUAL_POINT2.add(MANUAL_RELATIVE)
pos2_pos = match_point(pos2_point)
else:
next_positions = graph.follow_graph(pos1_pos, SEGMENT_LENGTH)
pos2_point = next_positions[0].point()
pos2_pos = next_positions[0]
start_loc = [{
'point': pos1_point,
'edge_pos': pos1_pos,
}, {
'point': pos2_point,
'edge_pos': pos2_pos,
}]
path = model_utils.Path(tile_data['gc'], tile_data, start_loc=start_loc)
else:
g = graph.read_graph(EXISTING_GRAPH_FNAME)
r = g.bounds()
def compute_targets_by_best(path, extension_vertex, segment_length):
angle_targets = numpy.zeros((64, ), 'float32')
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)
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
def compute_targets(gc, point, edge_pos):
angle_targets = numpy.zeros((64, ), 'float32')
def best_angle_to_pos(pos):
angle_points = [
model_utils.get_next_point(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 = geom.vector_from_angle(point_angle, 50).add(
geom.vector_from_angle(edge_angle, 50))
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):
for pos in positions:
best_angle_bucket = best_angle_to_pos(pos)
set_angle_bucket_soft(best_angle_bucket)
cur_edge = edge_pos.edge
cur_rs = gc.edge_to_rs[cur_edge.id]
potential_rs = []
if cur_rs.edge_distances[
cur_edge.id] + edge_pos.distance + SEGMENT_LENGTH < cur_rs.length(
):
potential_rs.append(cur_rs)
else:
for rs in cur_rs.out_rs(gc.edge_to_rs):
if rs == cur_rs or rs.is_opposite(cur_rs):
continue
potential_rs.append(rs)
opposite_rs = gc.edge_to_rs[cur_rs.edges[-1].get_opposite_edge().id]
if cur_rs.edge_distances[
cur_edge.id] + edge_pos.distance - SEGMENT_LENGTH > 0:
potential_rs.append(opposite_rs)
else:
for rs in opposite_rs.out_rs(gc.edge_to_rs):
if rs == opposite_rs or rs.is_opposite(opposite_rs):
continue
potential_rs.append(rs)
expected_positions = []
for rs in potential_rs:
pos = rs.closest_pos(point)
rs_follow_positions = graph.follow_graph(pos, SEGMENT_LENGTH)
expected_positions.extend(rs_follow_positions)
set_by_positions(expected_positions)
return angle_targets
def compute_targets_by_best(path, extension_vertex, segment_length):
angle_targets = numpy.zeros((64,), 'float32')
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)
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
def compute_targets_by_cgan(path, extension_vertex, segment_length, angle_outputs):
rotation_target = 64
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)
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]
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 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)
if len(potential_rs) + 1 > len(extension_vertex.out_edges):
potential_rs = [rs for rs in potential_rs if not path.is_explored(rs.edges[0])]
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)
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)
angle_buckets = [best_angle_to_pos(pos) for pos in expected_positions]
if angle_buckets:
rotation_target = random.choice(angle_buckets)
rotation_amount = rotation_target - numpy.argmax(angle_outputs)
angle_targets = numpy.zeros((65,), dtype='float32')
for i in xrange(65):
angle_targets[i] = angle_outputs[(i - rotation_amount + 65) % 65]
angle_targets_copy = numpy.zeros((65,), dtype='float32')
angle_targets_copy[:] = angle_targets[:]
angle_targets_copy[rotation_target] = 0
if numpy.max(angle_targets) - numpy.max(angle_targets_copy) < 0.5:
if numpy.max(angle_targets) >= 0.5:
angle_targets = angle_targets * 0.5
angle_targets[rotation_target] += random.random() * 0.1 + 0.4
return angle_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
