def get_average_speed_and_dist_of_sample(sample): fn = sample['pickled_filename'] with open(fn, 'r') as f: int_sit = pickle.load(f) track = extract_features.transform_track_to_cartesian(int_sit['track']) track_line = sample['geometry']['track_line'] entry_line = sample['geometry']['entry_line'] exit_line = sample['geometry']['exit_line'] dist_p1 = extended_interpolate(entry_line, entry_line.length-INT_DIST) normal1 = extend_line(get_normal_to_line(entry_line, entry_line.length-INT_DIST), 1000.0, direction="both") track_p1 = find_closest_intersection(normal1, dist_p1, track_line) track_i1 = find_nearest_coord_index(track_line, track_p1) dist_p2 = extended_interpolate(exit_line, INT_DIST) normal2 = extend_line(get_normal_to_line(exit_line, INT_DIST), 1000.0, direction="both") track_p2 = find_closest_intersection(normal2, dist_p2, track_line) track_i2 = find_nearest_coord_index(track_line, track_p2) time_delta = (track[track_i2][2] - track[track_i1][2]).total_seconds() min_i, max_i = min(track_i1, track_i2), max(track_i1, track_i2) dist = np.sum(np.linalg.norm(np.diff(np.array([(x, y) for (x, y, _) in track[min_i: max_i+1]]), axis=0), axis=1)) return abs(dist/time_delta*3.6), dist
curr_distance = 0. for i in range(len(track)-1): this_p = np.array([track[i][0], track[i][1]]) next_p = np.array([track[i+1][0], track[i+1][1]]) this_dist = np.linalg.norm(next_p - this_p) time_diff = (track[i+1][2] - track[i][2]).total_seconds() curr_distance += this_dist velocities.append( this_dist/time_diff*3.6) # velocities.append( this_dist/time_diff*3.6/1.5) distance.append(curr_distance) plt.plot(distance, velocities) plt.show() files = ['2010_03_09_drive_0019_1.pickle','2010_03_09_drive_0019_2.pickle'] path = '../data/prepared_data/KITTI_and_Karlsruhe/' kitti_samples = automatic_test.load_samples('../data/training_data/samples_kitti/samples.pickle') selected_samples = [s for s in kitti_samples if s['pickled_filename'].split('/')[-1] in files] for s in selected_samples: automatic_test.output_sample_features(s) for fn in files: fn = path + fn with open(fn, 'r') as f: int_sit = pickle.load(f) int_sit['track'] = extract_features.transform_track_to_cartesian(int_sit['track']) show_velocity_profile(int_sit)