Permalink
Name already in use
A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?
MultiHopRideSharing/pathgenerator.py
Go to fileThis commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
executable file
373 lines (320 sloc)
13.1 KB
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import numpy as np | |
import networkx as nx | |
import geohelper as gh | |
from networkx.exception import NetworkXNoPath | |
class PathGenerator(object): | |
""" | |
Path_Generator generates a simulated real trajectory | |
""" | |
def __init__(self, G, cycle=60): | |
self.cycle = cycle | |
self.G = G | |
N = len(self.G.nodes()) | |
self.node_lats = np.zeros(N, 'float32') | |
self.node_lons = np.zeros(N, 'float32') | |
self.node_ids = np.zeros(N) | |
for i, (node_id, data) in enumerate(self.G.nodes(data=True)): | |
self.node_lats[i] = data['lat'] | |
self.node_lons[i] = data['lon'] | |
self.node_ids[i] = node_id | |
def get_node_locs(self): | |
return zip(self.node_lats, self.node_lons) | |
def shortest_path(self, source, target, weight='length', distance=True): | |
## A* search for shortest path | |
path = nx.astar_path(self.G, source, target, self.__grand_circle, weight=weight) | |
if distance: | |
distance = sum(self.G[u][v].get(weight, 1) for u, v in zip(path[:-1], path[1:])) | |
return path, distance | |
else: | |
return path | |
def map_matching_shortest_path(self, origin, destination, weight='length', noise=1e-2, maxtry=20): | |
ptry = 0 | |
while 1: | |
mmtry = 0 | |
lat, lon = origin | |
while 1: | |
try: | |
su, sv, sd = self.map_match((lat, lon)) | |
break | |
except ValueError: | |
#print ("MM ERROR: ", origin) | |
if mmtry > maxtry: | |
raise | |
mmtry += 1 | |
lat += np.random.uniform(-noise, noise) | |
lon += np.random.uniform(-noise, noise) | |
mmtry = 0 | |
lat, lon = destination | |
while 1: | |
try: | |
tu, tv, td = self.map_match((lat, lon)) | |
break | |
except ValueError: | |
#print ("MM ERROR: ", destination) | |
if mmtry > maxtry: | |
raise | |
mmtry += 1 | |
lat += np.random.uniform(-noise, noise) | |
lon += np.random.uniform(-noise, noise) | |
try: | |
path, distance = self.shortest_path(su, tu, weight=weight) | |
break | |
except NetworkXNoPath: | |
#print ("NetworkXNoPath: %d, %d" % (su, tu)) | |
if ptry > maxtry: | |
raise | |
ptry += 1 | |
if len(nx.single_source_shortest_path_length(self.G, su)) < 1000: | |
self.G.remove_node(su) | |
#print ("REMOVE NODE: %d" % su) | |
if len(nx.single_source_shortest_path_length(self.G, tu)) < 1000: | |
self.G.remove_node(tu) | |
#print ("REMOVE NODE: %d" % tu) | |
source = su, sv, sd | |
target = tu, tv, td | |
return path, distance, source, target | |
def generate_path(self, origin, destination, step, path, source, target): | |
"""determine the shortest path from source to target and return locations on the path | |
""" | |
if len(path) < 3: | |
return [destination] | |
su, sv, sd = source | |
tu, tv, td = target | |
trajectory = [] | |
ds = step | |
# origin~ | |
if path[1] == su or path[1] == sv: | |
start_node = path.pop(0) | |
elif path[0] == su: | |
start_node = sv | |
else: | |
start_node = sv | |
end_node = path.pop(0) | |
lats, lons, bearings, lengths = self.get_segments_in_order(start_node, end_node) | |
if start_node < end_node: | |
d = sd | |
else: | |
d = sum(lengths) - sd | |
for lat, lon, b, l in zip(lats[:-1], lons[:-1], bearings, lengths): | |
if d > l: | |
d -= l | |
continue | |
if d > 0: | |
lat, lon = gh.end_lat_lon(lat, lon, d, b) | |
trajectory.append((lat, lon)) | |
l -= d | |
d = 0 | |
locs, ds = self.create_trajectory(lat, lon, b, l, step, ds) | |
trajectory += locs | |
start_node = end_node | |
# intermediate | |
for end_node in path[:-1]: | |
lats, lons, bearings, lengths = self.get_segments_in_order(start_node, end_node) | |
for lat, lon, b, l in zip(lats[:-1], lons[:-1], bearings, lengths): | |
locs, ds = self.create_trajectory(lat, lon, b, l, step, ds) | |
trajectory += locs | |
start_node = end_node | |
# ~destination | |
end_node = path[-1] | |
if not (start_node == tu or start_node == tv): | |
lats, lons, bearings, lengths = self.get_segments_in_order(start_node, end_node) | |
for lat, lon, b, l in zip(lats[:-1], lons[:-1], bearings, lengths): | |
locs, ds = self.create_trajectory(lat, lon, b, l, step, ds) | |
trajectory += locs | |
start_node = end_node | |
if start_node == tu: | |
end_node = tv | |
else: | |
end_node = tu | |
lats, lons, bearings, lengths = self.get_segments_in_order(start_node, end_node) | |
if start_node < end_node: | |
d = td | |
else: | |
d = sum(lengths) - td | |
for lat, lon, b, l in zip(lats[:-1], lons[:-1], bearings, lengths): | |
if d < l: | |
locs, ds = self.create_trajectory(lat, lon, b, d, step, ds) | |
trajectory += locs | |
trajectory.append(gh.end_lat_lon(lat, lon, d, b)) | |
break | |
locs, ds = self.create_trajectory(lat, lon, b, l, step, ds) | |
trajectory += locs | |
d -= l | |
return trajectory | |
def create_trajectory(self, lat, lon, bearing, distance, step, init_step): | |
lats = [] | |
lons = [] | |
ds = init_step | |
while distance > ds: | |
lat, lon = gh.end_lat_lon(lat, lon, ds, bearing) | |
lats.append(lat) | |
lons.append(lon) | |
distance -= ds | |
ds = step | |
ds -= distance | |
return zip(lats, lons), ds | |
def get_segments_in_order(self, start_node, end_node): | |
edge = self.G.get_edge_data(start_node, end_node) | |
if not edge or 'lat' not in edge: | |
edge = self.G.get_edge_data(end_node, start_node) | |
d = edge['seg_length'] + [edge['length']] | |
lengths = [d2 - d1 for d1, d2 in zip(d[:-1], d[1:])] | |
bearings = edge['bearing'] | |
lats = edge['lat'] | |
lons = edge['lon'] | |
if start_node > end_node: | |
bearings = [b + np.pi for b in bearings[::-1]] | |
lengths = lengths[::-1] | |
lats = lats[::-1] | |
lons = lons[::-1] | |
return lats, lons, bearings, lengths | |
def map_match(self, loc, geo_range=0.0018): | |
"""Search the most probable path on which the GPS signals are observed in a given graph | |
Parameters | |
---------- | |
geo_range: float; buffer value of boundary box in degree | |
""" | |
lat, lon = loc | |
G = self.__get_subgraph(lat, lon, geo_range) | |
roads = G.edges() | |
N = len(roads) | |
if N == 0: | |
raise ValueError("""No nodes within search area around input location.""") | |
road_lengths = np.zeros(N, 'float16') | |
road_ids = np.zeros(N) | |
road_distance = np.ones((N), 'float16') * float('inf') | |
node_distance = np.zeros((N), 'float16') | |
for i, road in enumerate(roads): | |
data = self.G.get_edge_data(*road) | |
if 'lat' in data: | |
road_lengths[i] = data['length'] | |
road_ids[i] = data['id'] | |
(_, road_distance[i], node_distance[i]) = self.__get_nearest_segment(lat, lon, data) | |
nearest = road_distance.argmin() | |
#print (type(nearest),type(roads)) | |
test_var = list(roads) | |
#print (roads) | |
#print (road_distance,road_distance.shape) | |
#print (N,type(roads)) | |
#print (road_distance.argmin()) | |
u = int(test_var[nearest][0]) | |
v = int(test_var[nearest][1]) | |
d = node_distance[nearest] | |
if u > v: | |
u, v = v, u | |
return u, v, d | |
def mm_convert(self, loc, georange=0.0018): | |
u, v, d = self.map_match(loc, georange) | |
lats, lons, bearings, lengths = self.get_segments_in_order(u, v) | |
for lat, lon, b, l in zip(lats[:-1], lons[:-1], bearings, lengths): | |
if d > l: | |
d -= l | |
elif d > 0: | |
return gh.end_lat_lon(lat, lon, d, b) | |
else: | |
return lat, lon | |
return lats[-1], lons[-1] | |
def __get_nearest_segment(self, lat, lon, data): | |
"""Compute geometric properties between candidate roads and observation points | |
Parameters | |
---------- | |
lat: float; | |
lon: float; | |
data: dictionary; road data | |
Returns | |
------- | |
nearest_seg: index of road segments closest to observation | |
road_distnace: distance between observation and the closest road | |
node_distance: distance from the node with higher ID to matched point | |
""" | |
road_lats = np.array(data['lat']) | |
road_lons = np.array(data['lon']) | |
bearings = np.array(data['bearing']) | |
seg_lengths = np.array(data['seg_length']+[data['length']]) | |
h = gh.distance_in_meters(road_lats, road_lons, lat, lon) | |
h1 = h[:-1] | |
h2 = h[1:] | |
theta = gh.bearing_in_radians(road_lats, road_lons, lat, lon) | |
cos1 = np.cos(theta[:-1] - bearings) | |
cos2 = -np.cos(theta[1:] - bearings) | |
d = h1 * np.sqrt(1 - cos1 ** 2) * (np.sign(cos1) == np.sign(cos2)) \ | |
+ h1 * (np.sign(cos1) < np.sign(cos2)) + h2 * (np.sign(cos1) > np.sign(cos2)) | |
nearest_seg = d.argmin() #size: T | |
cos1 = cos1[nearest_seg] | |
cos2 = cos2[nearest_seg] | |
h1 = h1[nearest_seg] | |
road_distance = d[nearest_seg] | |
node_distance = (h1 * cos1) * (np.sign(cos1) == np.sign(cos2)) \ | |
+ seg_lengths[nearest_seg] * ~(np.sign(cos1) > np.sign(cos2)) \ | |
+ seg_lengths[nearest_seg+1] * (np.sign(cos1) > np.sign(cos2)) | |
return (nearest_seg, road_distance, node_distance) | |
def __grand_circle(self, source_id, target_id): | |
source = self.G.node[source_id] | |
target = self.G.node[target_id] | |
return gh.distance_in_meters(source['lat'], source['lon'], target['lat'], target['lon']) | |
def __get_subgraph(self, lat, lon, geo_range): | |
""" Draw a square bounding box containing all of the observation points | |
Extract nodes within this bounding box and return a subgraph containing the nodes | |
Parameters | |
---------- | |
G: networkx Graph object; graph representation of the road network | |
geo_range: float; buffer value of boundary box | |
""" | |
lat_min, lat_max = lat - geo_range, lat + geo_range | |
lon_min, lon_max = lon - geo_range, lon + geo_range | |
# sub_id = gh.nodes_within_square(self.G, [lat_max, lon_max], [lat_min, lon_min]) | |
lats = self.node_lats | |
lons = self.node_lons | |
sub_ids = self.node_ids[(lats < lat_max) & (lats > lat_min) & (lons < lon_max) & (lons > lon_min)] | |
return self.G.subgraph(sub_ids) | |
if __name__ == '__main__': | |
import _pickle as pickle | |
import pandas as pd | |
import json | |
import time | |
print ("Loading NYC road network graph...") | |
graph_path = 'data/nyc_network_graph.pkl' | |
with open(graph_path, 'r') as f: | |
G = pickle.load(f) | |
path_generator = PathGenerator(G) | |
print ("Loading sample ride requests...") | |
requests_path = 'data/requests_sample.csv' | |
df = pd.read_csv(requests_path, nrows=150) | |
trajectories = {} | |
ride_requests = zip(df.request_id.values, df.trip_time.values, df[['pickup_latitude', 'pickup_longitude']].values, | |
df[['dropoff_latitude', 'dropoff_longitude']].values) | |
print ("Start generating paths:") | |
print ("# of ride requests: %d" % len(ride_requests)) | |
start = time.time() | |
n = 0 | |
for rid, trip_time, origin, destination in ride_requests: | |
try: | |
path = path_generator.get_path(origin, destination, trip_time) | |
trajectories[rid] = path | |
n += 1 | |
if n % 100 == 0: | |
print ("%d elapsed time: %.2f" % (n, time.time() - start)) | |
except: | |
continue | |
print ("%d elapsed time: %.2f" % (n, time.time() - start)) | |
with open('data/trajectories.json', 'wb') as f: | |
json.dump(trajectories, f) | |
print ("Complete!") | |
# class FastPathGenerator(object): | |
# def generate_path(self, origin, destination, timestep): | |
# lat, lon = origin | |
# tlat, tlon = destination | |
# distance = gh.distance_in_meters(lat, lon, tlat, tlon) | |
# bearing = gh.bearing_in_radians(lat, lon, tlat, tlon) | |
# lats = [lat] | |
# lons = [lon] | |
# ds = distance / timestep | |
# while distance > ds: | |
# lat, lon = gh.end_lat_lon(lat, lon, ds, bearing) | |
# lats.append(lat) | |
# lons.append(lon) | |
# distance -= ds | |
# ds = step | |
# | |
# ds -= distance | |
# return zip(lats, lons), ds |