def test_finding_paths(): folder = tempfile.mkdtemp() s3 = S3('driverpete-storage') filename = s3.download("_testing/testing_merged_1", folder) data = filter_gps_data(read_compressed_trajectory(filename)) endpoints = find_endpoints(data) assert(len(endpoints) == 2) print(trajectory_point_to_str(data, _get_point_index(data, endpoints[0]))) print(trajectory_point_to_str(data, _get_point_index(data, endpoints[1]))) assert(_get_point_index(data, endpoints[0]) == 479) assert(_get_point_index(data, endpoints[1]) == 670) AtoB_paths, BtoA_paths = find_routes(data, endpoints, verbose=False) AtoB_paths_indices = _extract_indices(data, AtoB_paths) BtoA_paths_indices = _extract_indices(data, BtoA_paths) print(AtoB_paths_indices) print(BtoA_paths_indices) assert(AtoB_paths_indices == [[488, 656], [947, 1117], [1364, 1549], [2216, 2401], [2630, 2898], [4400, 4526]]) assert(BtoA_paths_indices == [[134, 455], [683, 887], [1141, 1316], [1582, 1783], [2429, 2597], [3975, 4170]])
def process_gps_data(filename): #stationary_distance_threshold=120 # clean up data data = read_compressed_trajectory(filename) print("Length of data: %d" % len(data)) data = filter_gps_data(data) print("Length of filtered data: %d" % len(data)) endpoints = find_endpoints(data) print("Found %d endpoints:" % len(endpoints)) for u in endpoints: print(trajectory_point_to_str([u], 0)) assert(len(endpoints) == 2) AtoB_paths_data, BtoA_paths_data = find_routes(data, endpoints, verbose=False) def _extract_indices(data, paths): result = [] for p in paths: indices = [(data[:, 0] == p[0, 0]).nonzero()[0][0], (data[:, 0] == p[p.shape[0]-1, 0]).nonzero()[0][0]] result.append(indices) return result AtoB_paths_indices = _extract_indices(data, AtoB_paths_data) BtoA_paths_indices = _extract_indices(data, BtoA_paths_data) print(AtoB_paths_indices) print(BtoA_paths_indices) AtoB_paths = [Path(d) for d in AtoB_paths_data] BtoA_paths = [Path(d) for d in BtoA_paths_data] AtoB_paths = sorted(AtoB_paths, key=lambda x: x.get_duration()) BtoA_paths = sorted(BtoA_paths, key=lambda x: x.get_duration()) print("ATOB") for p in AtoB_paths: print(p.get_duration()/60, str(p.start_time())) p.show(str(p.get_duration()/60)) print("BTOA") for p in BtoA_paths: print(p.get_duration()/60, str(p.start_time())) p.show(str(p.get_duration()/60))
def process(self, point): if self._from_endpoint_index is None: index = self._get_closest_endpoint_index(point) if index is not None: self._start_route(point, index) return else: dt = delta_float_time(self._current_route[-1][0], point[0]) if dt > self._continuity_threshold: if self._verbose: dist = distance(point, self._endpoints[self._from_endpoint_index]) print('Continuity ruined from %s\n to %s\n dt=%smin, dist=%s' % (trajectory_point_to_str([self._current_route[-1]], 0), trajectory_point_to_str([point], 0), dt/60., dist)) index = self._get_closest_endpoint_index(point) if index == self._from_endpoint_index: if self._verbose: print(' !BUT Didnt move too far from beginning though') self._stop_route() self._start_route(point, index) else: self._stop_route() return self._current_route.append(point) index = self._get_closest_endpoint_index(point) if index == self._from_endpoint_index: if self._verbose: print('made a loop or didnt move') self._stop_route() self._start_route(point, index) elif index == self._to_endpoint_index(): if self._from_endpoint_index == 0: self._AtoB_routes.append(np.array(self._current_route)) if self._verbose: print('A to B found: %s TO %s' % (trajectory_point_to_str(self._current_route, 0), trajectory_point_to_str(self._current_route, len(self._current_route)-1))) self._stop_route() else: self._BtoA_routes.append(np.array(self._current_route)) if self._verbose: print('B to A found: %s TO %s' % (trajectory_point_to_str(self._current_route, 0), trajectory_point_to_str(self._current_route, len(self._current_route)-1))) self._stop_route()
def _start_route(self, point, endpoint_index): assert(endpoint_index is not None) self._from_endpoint_index = endpoint_index self._current_route.append(point) if self._verbose: print('Starting route from %s' % trajectory_point_to_str([point], 0))
def test_finding_paths_with_state_2(): folder = tempfile.mkdtemp() s3 = S3('driverpete-storage') pieces = [] pieces_keys = [ "_testing/testing_sequence0/data/14-09-2015_09-15-01_PDT", "_testing/testing_sequence0/data/14-09-2015_11-03-24_PDT", "_testing/testing_sequence0/data/14-09-2015_13-49-55_PDT", "_testing/testing_sequence0/data/14-09-2015_18-20-13_PDT", "_testing/testing_sequence0/data/14-09-2015_19-59-23_PDT", "_testing/testing_sequence0/data/15-09-2015_09-32-15_PDT", "_testing/testing_sequence0/data/15-09-2015_22-31-21_PDT" ] for k in pieces_keys: filename = s3.download(k, folder) pieces.append(read_compressed_trajectory(filename)) filtered_pieces = [] endpoints = [] findendpoints_state = None filter_state = None for pi, piece in enumerate(pieces): vel_filter = VelocityOutliersFilter() if filter_state is not None: vel_filter.set_state(filter_state) filter = FilterChain([DuplicateTimeFilter(), vel_filter]) filtered_piece = apply_filter(piece, filter) filter_state = vel_filter.get_state() filtered_pieces.append(filtered_piece) finder = FindEndpoints(endpoints=endpoints) if findendpoints_state is not None: finder.set_state(findendpoints_state) for i, d in enumerate(filtered_piece): finder.process(d) endpoints = finder.get_endpoints() findendpoints_state = finder.get_state() data = np.vstack(filtered_pieces) print(trajectory_point_to_str(data, _get_point_index(data, endpoints[0]))) print(trajectory_point_to_str(data, _get_point_index(data, endpoints[1]))) assert(len(endpoints) == 2) assert(_get_point_index(data, endpoints[0]) == 5) assert(_get_point_index(data, endpoints[1]) == 122) AtoB_paths = [] BtoA_paths = [] finder_current_route = [] finder_endpoint_index = None for piece in filtered_pieces: finder = RoutesFinder(endpoints) finder.set_state(finder_current_route, finder_endpoint_index) for d in piece: finder.process(d) finder_current_route, finder_endpoint_index = finder.get_state() AtoB_paths_piece, BtoA_paths_piece = finder.get_routes() AtoB_paths += AtoB_paths_piece BtoA_paths += BtoA_paths_piece AtoB_paths_indices = _extract_indices(data, AtoB_paths) BtoA_paths_indices = _extract_indices(data, BtoA_paths) print(AtoB_paths_indices) print(BtoA_paths_indices) assert(AtoB_paths_indices == [[11, 111], [556, 730]]) assert(BtoA_paths_indices == [[288, 387]])