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)
