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