def adjacent_nodes(self, graph: facility.SpatialGraph, goal: BoundNode):
if goal.on(self.edge):
yield goal
return
for next_edge in graph.adjacent(self.edge[1]):
if goal.on(next_edge):
yield Node(next_edge, 0.0, goal.edges[next_edge][0])
else:
yield Node(next_edge, 0.0, graph.edge_geometry(next_edge).length)
def __init__(self, coord, graph: facility.SpatialGraph, final: bool=False):
self.final = final
self.coord = sg.Point(coord)
self.edges = {}
for x in graph.search_edge_nearest(utility.bb_buffer(self.coord, 5.0), 10):
edge = x.object
u, v, k = edge
link = graph.edge_geometry(edge)
projection = link.project(self.coord)
self.edges[u, v, k] = (projection, link.length)
self.edges[v, u, k] = (link.length - projection, link.length)
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 adjacent_nodes(self, states, projections: ProjectionManager, graph: facility.SpatialGraph, geometry: LinkManager):
for offset in projections.search_edge(self.anchor.idx + 1, self.edge, 0):
yield LinkedNode.Key(self.edge, offset, self.anchor.idx + 1)
distance = self.distance + self.link.length
for next_edge in graph.adjacent(self.edge[1]):
u, v, k = next_edge
if (v, u, k) != self.edge:
yield ForwardingNode.Key(self.anchor, distance, next_edge, self.projected_state)
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)
def adjacent_nodes(self, states, projections: ProjectionManager, graph: facility.SpatialGraph, geometry: LinkManager):
if self.idx + 1 == len(states):
yield FinalNode()
return
for offset in projections.search_edge(self.idx + 1, self.edge, self.offset):
yield LinkedNode.Key(self.edge, offset, self.idx + 1)
yield JumpingNode.Key(self)
distance = self.link.length - self.segment.distance
for next_edge in graph.adjacent(self.edge[1]):
u, v, k = next_edge
if (v, u) != self.edge:
yield ForwardingNode.Key(self, distance, next_edge, self.next_projected_state)
def cost_to(self, other, graph: facility.SpatialGraph, link_cost_fcn, intersection_cost_fcn):
assert isinstance(other, Node)
geometry = graph.edge_geometry(other.edge)
projection = geometry.project(self.coord)
return link_cost_fcn(geometry.distance(self.coordinates()), self.coord, geometry.interpolate(projection), None)
def heuristic(self, graph: facility.SpatialGraph, goal: BoundNode, greedy_factor: float):
return graph.node_geometry(self.edge[1]).distance(goal.coordinates()) * greedy_factor
def cost(self, graph: facility.SpatialGraph, link_cost_fcn):
geometry = graph.edge_geometry(self.edge)
return link_cost_fcn(self.length(), geometry.interpolate(self.begin), geometry.interpolate(self.end), self.edge)