def visit_edge(i, edge):
self.state_table[i][edge] = []
for offset, segment in enumerate(utility.pairwise(self.graph.edge_geometry(edge).coords)):
if utility.intersect(sg.LineString(segment).bounds, bounds):
cost, constrained_state, projected_state = self.at(i, edge, offset)
projections.append((edge, offset, constrained_state, projected_state))
projection_costs.append(cost)
def __init__(self, graph: facility.SpatialGraph, transition, edge):
self.segments = []
self.length = 0.0
for coord in utility.pairwise(graph.edge_geometry(edge).coords):
#TODO: actually estimate link width and offset based on type and direction
segment = Segment(edge, coord, 0.0, 2.0, self.length, transition)
self.segments.append(segment)
self.length += segment.length
def solve(trajectory, graph, distance_cost_fcn, intersection_cost_fcn, greedy_factor):
logging.info("solving mapmatch for %s", trajectory['id'])
states = trajectory['state']
transition = trajectory['transition']
link_manager = LinkManager(graph, transition)
cumulative_distance = [0.0]
total_distance = 0.0
for s1, s2 in utility.pairwise(states):
distance = spatial.distance.euclidean(s1.x[0:2], s2.x[0:2])
total_distance += distance
cumulative_distance.append(total_distance)
projections = ProjectionManager(states, graph, link_manager)
def adjacent_nodes(key):
return key.adjacent_nodes(states, projections, graph, link_manager)
def state_cost(key):
return key.cost()
def transition_cost(current_state, next_state):
return current_state.cost_to(next_state, distance_cost_fcn, intersection_cost_fcn)
def handicap(state):
return state.handicap(distance_cost_fcn)
def heuristic(state):
return state.heuristic(states, distance_cost_fcn, cumulative_distance, greedy_factor)
def progress(state):
return state.progress()
def project(key):
return key.make_node(states, transition, projections, link_manager)
chain = markov.MarkovGraph(adjacent_nodes, state_cost, transition_cost,
handicap_fcn=handicap, state_projection=project)
start_time = time.time()
path, priority = chain.find_best(InitialNode(), FinalNode(), heuristic,
priority_threshold=50000.0, progress_fcn=progress, max_visited=len(states) * 20)
logging.info("elapsed_time: %.4f", time.time() - start_time)
if path is None:
logging.warning("trashing %s due to incomplete mapmatch", trajectory['id'])
return None
nodes = list([project(key) for key in path])
if priority > 0:
logging.warning("trashing %s due to bad mapmatch", trajectory['id'])
return None
return {'segment': list(format_path(nodes)),
'id': trajectory['id'],
'node': nodes,
'count': len(trajectory['state'])}
def extract_turn(trajectory, graph, predicate):
count = 0
for segment0, segment1 in utility.pairwise(trajectory['segment']):
if (segment0.edge is not None and segment0.end.attached
and segment1.edge is not None and segment1.begin.attached):
assert segment0.edge[1] == segment1.edge[0]
angle = graph.turn_angle(segment0.edge, segment1.edge)
if predicate(math.degrees(angle)):
count += 1
return count
def extract_elevation_stats(trajectory, graph, elevation):
dst_proj = pyproj.Proj(init='epsg:4326')
M2 = 0.0
M3 = 0.0
for n1, n2 in utility.pairwise(extract_nodes(trajectory)):
e1 = elevation.at(n1, dst_proj)
e2 = elevation.at(n2, dst_proj)
d = sg.Point(n1).distance(sg.Point(n2))
if d > 0.0:
slope = (e2 - e1) / d
M2 += (slope**2) * d
M3 += (slope**3) * d
return M2, M3
def best_path(weights, eigen_vectors, trajectory, graph: facility.SpatialGraph, intersection_collections, elevation, dst_proj):
def distance_cost(length, start, end, link):
start_elevation = elevation.at((start.x, start.y), dst_proj)
end_elevation = elevation.at((end.x, end.y), dst_proj)
cost = numpy.dot(numpy.dot(eigen_vectors[0:14,:].T, features.link_features(length, start_elevation, end_elevation, link, graph)), weights)
if link is None:
cost += 100.0 * length
return cost
def intersection_cost(a, b):
return numpy.dot(numpy.dot(eigen_vectors[14:21,:].T, features.intersection_features(a, b, graph, intersection_collections)), weights)
goal = BoundNode(trajectory['segment'][-1].geometry[-1], graph, final=True)
def adjacent_nodes(state):
return state.adjacent_nodes(graph, goal)
def state_cost(state):
return state.cost(graph, distance_cost)
def transition_cost(current_state, next_state):
return current_state.cost_to(next_state, graph, distance_cost, intersection_cost)
def heuristic(state):
return state.heuristic(graph, goal, 2.0)
chain = markov.MarkovGraph(adjacent_nodes, state_cost, transition_cost)
path = chain.find_best(BoundNode(trajectory['segment'][0].geometry[0], graph), goal, heuristic)
if path is None:
return None, None
path = list(path)
feature = numpy.zeros(21)
for node in path:
if isinstance(node, Node):
geometry = graph.edge_geometry(node.edge)
start = geometry.interpolate(node.begin)
end = geometry.interpolate(node.end)
start_elevation = elevation.at((start.x, start.y), dst_proj)
end_elevation = elevation.at((end.x, end.y), dst_proj)
feature[0:14] += features.link_features(node.length(),
start_elevation, end_elevation,
node.edge, graph)
for a, b in utility.pairwise(path):
if isinstance(a, Node) and isinstance(b, Node):
feature[14:21] += features.intersection_features(a.edge, b.edge, graph, intersection_collections)
return path, numpy.dot(eigen_vectors.T, feature)