def get_interpolating_spline_line(self, sample):
entry_line = sample['geometry']['entry_line']
exit_line = sample['geometry']['exit_line']
oneway_entry = float_to_boolean(sample['X'][_feature_types.index('oneway_entry')])
oneway_exit = float_to_boolean(sample['X'][_feature_types.index('oneway_exit')])
if oneway_entry:
distance_entry = 0.
else:
distance_entry = LANE_WIDTH/2
if oneway_exit:
distance_exit = 0.
else:
distance_exit = LANE_WIDTH/2
w = 2*LANE_WIDTH
far_entry_p = get_offset_point_at_distance(entry_line, entry_line.length - 70.0, distance_entry)
entry_p2 = get_offset_point_at_distance(entry_line, entry_line.length - w - 0.1, distance_entry) # Control point to ensure spline orientation with street
entry_p = get_offset_point_at_distance(entry_line, entry_line.length - w, distance_entry)
exit_p = get_offset_point_at_distance(exit_line, w, distance_exit)
exit_p2 = get_offset_point_at_distance(exit_line, w + 0.1, distance_exit)
far_exit_p = get_offset_point_at_distance(exit_line, 70.0, distance_exit)
coords = [ list(far_entry_p.coords)[0],
list(entry_p2.coords)[0],
list(entry_p.coords)[0],
list(exit_p.coords)[0],
list(exit_p2.coords)[0],
list(far_exit_p.coords)[0]]
x, y = zip(*coords)
# Make a parametric quadratic spline with given knot vector
x2, y2 = parametric_combined_spline(x, y, k=2, s=0.0)
interpolating_spline_line = LineString(zip(x2, y2))
desired_entry_distance = sample['X'][_feature_types.index("track_distance_projected_along_normal_entry")]
interpolating_spline_line = rectify_line(interpolating_spline_line, sample['geometry']['entry_line'], sample['geometry']['exit_line'], False, False, desired_entry_distance, 0.)
return interpolating_spline_line
def _get_curved_line(self, A_1, A_2, R_min, sample):
intersection_angle = sample['X'][_feature_types.index('intersection_angle')]
ang_sign = np.sign(intersection_angle)
entry_line = sample['geometry']['entry_line']
entry_lane_line = extend_line(entry_line.parallel_offset(LANE_WIDTH/2, side='right'), 100.0, direction="forward")
exit_line = sample['geometry']['exit_line']
exit_lane_line = extend_line(exit_line.parallel_offset(LANE_WIDTH/2, side='right'), 100.0, direction="backward")
# Entering straight
entry_tangent_vec = get_tangent_vec_at(entry_line, entry_line.length)
entry_straight_line = self._get_straight_line(entry_tangent_vec, 50.0)
# Entering Euler spiral
entry_euler_line = self._get_euler_spiral_line(R_min, ang_sign*A_1, entry_tangent_vec)
# Exiting Euler spiral
exit_tangent_vec = -get_tangent_vec_at(exit_line, 0.)
exit_euler_line = self._get_euler_spiral_line(R_min, -ang_sign*A_2, exit_tangent_vec)
# Constant curvature segment
normal1 = ang_sign*get_normal_vec_at(entry_euler_line, entry_euler_line.length)
normal2 = -ang_sign*get_normal_vec_at(exit_euler_line, exit_euler_line.length)
circular_line = self._get_circular_line(R_min, normal1, normal2)
# Exiting straight line
exit_straight_line = self._get_straight_line(-exit_tangent_vec, 50.0)
# Join segments
curved_line = self._join_segments(entry_straight_line, entry_euler_line, circular_line, exit_euler_line, exit_straight_line)
# Place in intersection
curved_line = self._place_line_in_intersection(curved_line, sample)
return curved_line
def _place_line_in_intersection(self, curved_line, sample):
# Move line to approximate location of intersection
entry_line = sample['geometry']['entry_line']
curve_peak_coord = np.array(curved_line.interpolate(0.5, normalized=True).coords[0])
translation_vec = np.array(entry_line.coords[-1]) - curve_peak_coord
curved_line = shapely.affinity.translate(curved_line, xoff=translation_vec[0], yoff=translation_vec[1])
# Place exactly in intersection
if self.allow_rectification:
desired_entry_distance = sample['X'][_feature_types.index("track_distance_projected_along_normal_entry")]
desired_exit_distance = sample['X'][_feature_types.index("track_distance_projected_along_normal_exit")]
else:
desired_entry_distance = None
desired_exit_distance = None
if self.des_entry_dist != None:
desired_entry_distance = self.des_entry_dist
if self.des_exit_dist != None:
desired_exit_distance = self.des_exit_dist
curved_line = rectify_line(curved_line, sample['geometry']['entry_line'], sample['geometry']['exit_line'], False, False, desired_entry_distance, desired_exit_distance)
return curved_line
def _calculate_curve_parameters(self, f, sample):
intersection_angle = sample['X'][_feature_types.index('intersection_angle')] # intersection angle in radians (different system than Alhajyaseen)
if intersection_angle >= 0.0:
# Right turn in left hand traffic
V_min = 4.49 \
+ 0.072*f['theta'] \
+ 0.0092*f['D_HN_IN'] \
+ 0.105*f['D_HN_OUT'] \
+ 0.38*f['V_in']
R_min = 1.86 \
+ 0.062*f['theta'] \
+ 0.13*f['MIN_D_HN'] \
+ 0.36*V_min
A_1 = - 8.65 \
+ 0.17*f['D_HN_IN'] \
+ 0.29*V_min
A_2 = 3.63 \
+ 0.24*f['D_HN_OUT'] \
+ 0.29*V_min
else:
# Left turn in left hand traffic
V_min = - 1.08 \
+ 0.22*f['R_c'] \
+ 0.14*f['theta'] \
- 1.79*f['heavy_vehicle_dummy'] \
+ 0.84*f['lateral_exit_shoulder_dist'] \
+ 0.091*f['V_in']
R_min = - 6.46 \
+ 0.39*f['R_c'] \
+ 0.13*f['theta'] \
+ 0.86*f['lateral_exit_shoulder_dist']
A_1 = - 1.65 \
+ 0.33*f['R_c'] \
+ 0.0404*f['theta'] \
+ 0.46*f['lateral_exit_shoulder_dist'] \
+ 0.37*V_min
A_2 = 2.33 \
+ 0.34*f['R_c'] \
+ 2.051*f['heavy_vehicle_dummy'] \
+ 1.041*f['lateral_exit_shoulder_dist'] \
+ 0.27*V_min
return A_1, A_2, R_min, V_min
def _calculate_intersection_features(self, sample):
# All variables are calculated as if being in left hand traffic
# Other variables
intersection_angle = sample['X'][_feature_types.index('intersection_angle')] # intersection angle in radians (different system than Alhajyaseen)
# Alhajyaseen features
features = {}
features['R_c'] = 3.0 # Corner radius [m]
features['theta'] = np.degrees(np.pi - np.abs(intersection_angle)) # intersection angle [deg]
features['heavy_vehicle_dummy'] = 0.0 # Passenger car
if self.allow_actual_speed:
features['V_in'] = sample['X'][_feature_types.index("vehicle_speed_entry")] # Approaching speed estimated [km/h]
else:
features['V_in'] = 30.0 # Dummy speed
if intersection_angle >= 0.0:
# Right turn in left hand traffic
features['D_HN_IN'] = INT_DIST # Distance from IP to hard nose at entry [m]
features['D_HN_OUT'] = INT_DIST # Distance from IP to hard nose at exit [m]
features['MIN_D_HN'] = min(features['D_HN_IN'], features['D_HN_OUT']) # Minimum of the two distances
else:
# Left turn in left hand traffic
features['lateral_exit_shoulder_dist'] = LANE_WIDTH/2.0
return features
def plot_sample_intersection_curvature(samples, title="Sample curvature over intersection coordinates", ax=None, color=None):
"""Plot each sample's curvature relative to the intersection distances coordinate system"""
print "Curvature calculation..."
sample_steps = 100
curvatures = np.zeros((len(samples), sample_steps))
line_dists = np.array(curvatures)
for i, s in enumerate(samples):
track_line = s['geometry']['track_line']
entry_line = s['geometry']['entry_line']
exit_line = s['geometry']['exit_line']
try:
half_angle_vec = extract_features.get_half_angle_vec(exit_line, s['X'][_feature_types.index('intersection_angle')])
# Limit path to a set s_di interval at intersection
# _, track_line = split_path_at_line_dist(track_line, entry_line, half_angle_vec, entry_line.length-36.0)
# track_line, _ = split_path_at_line_dist(track_line, exit_line, half_angle_vec, 36.0)
curvature_sample_coords = [track_line.interpolate(dist).coords[0] for dist in np.linspace(0, track_line.length, sample_steps)]
X, Y = zip(*curvature_sample_coords)
way_line, dists = extract_features.set_up_way_line_and_distances(entry_line, exit_line)
way_line = extract_features.extend_line(way_line, 1000.0, direction="both") # Make sure the way_line is not too short to cover the whole track
LineDistances, _ = extract_features.get_distances_from_cartesian(X, Y, way_line, half_angle_vec)
line_dists[i] = LineDistances - 1000.0 - INT_DIST # Shift to the actual coordinate system
curvatures[i] = extract_features.get_line_curvature(track_line, sample_steps)
except extract_features.NoIntersectionError as e:
#plot_helper.plot_intersection(s, additional_lines=[way_line])
print e
continue
# fig = plt.figure()
# sns.plt.hold(True)
for i in range(curvatures.shape[0]):
handle, = ax.plot(line_dists[i], np.degrees(curvatures[i]), color=color, linestyle='-')
return handle # Only need one
def plot_intersection(sample, predicted=[], rgbcolors=[], labels=[], label_methods=[], additional_lines=[], heatmap=None, title=None, block=True, orientation="preserve", reference_lines=False):
# normal_en, neg_normal_en = get_normal_to_line(entry_line, entry_line.length-INT_DIST, normalized=False, direction="both")
# normal_ex, neg_normal_ex = get_normal_to_line(exit_line, INT_DIST, normalized=False, direction="both")
csample = copy.deepcopy(sample)
entry_line = csample['geometry']['entry_line']
exit_line = csample['geometry']['exit_line']
curve_secant = csample['geometry']['curve_secant']
track_line = csample['geometry']['track_line']
half_angle_line = csample['geometry']['half_angle_line']
intersection_angle = csample['X'][_feature_types.index('intersection_angle')]
rotation = (0., (0.,0.)) # rad
if orientation == "curve-secant":
x_axis = LineString([(0,0),(1,0)])
inv_curve_secant = LineString([curve_secant.coords[1], curve_secant.coords[0]])
phi = - get_angle_between_lines(x_axis, curve_secant)
if intersection_angle > 0.:
# With the intersection angle it can be determined how the
# intersection is upright (curve_secant is below entry and exit_line)
phi = np.pi + phi
# rot_c, = curve_secant.interpolate(0.5, normalized=True).coords[:]
rot_c = (0,0)
rotation = (phi, rot_c)
phi, rot_c = rotation
if rotation[0] != 0.:
# Rotate all given LineStrings
# And update in copied sample to be used by submethods
entry_line = affinity.rotate(entry_line, phi, origin=rot_c, use_radians=True)
csample['geometry']['entry_line'] = entry_line
exit_line = affinity.rotate(exit_line, phi, origin=rot_c, use_radians=True)
csample['geometry']['exit_line'] = exit_line
curve_secant = affinity.rotate(curve_secant, phi, origin=rot_c, use_radians=True)
csample['geometry']['curve_secant'] = curve_secant
track_line = affinity.rotate(track_line, phi, origin=rot_c, use_radians=True)
csample['geometry']['track_line'] = track_line
half_angle_line = affinity.rotate(half_angle_line, phi, origin=rot_c, use_radians=True)
csample['geometry']['half_angle_line'] = half_angle_line
additional_lines = [affinity.rotate(al, phi, origin=rot_c, use_radians=True) for al in additional_lines]
handles = []
fig = plt.figure()
plt.hold(True)
plt.axis('equal')
if heatmap != None:
X, Y, D = heatmap
# Rotate if necessary
if rotation[0] != 0.:
rot_phi, rot_c = rotation
target_shape = np.shape(X)
coords = np.transpose(np.vstack((X.ravel(), Y.ravel())))
rot_coords = np.transpose(rotate_xy(coords, rot_phi, rot_c))
X = np.reshape(rot_coords[0], target_shape)
Y = np.reshape(rot_coords[1], target_shape)
ax = plt.gca()
p = ax.pcolormesh(X, Y, D, cmap="Blues")
plot_lines('k', entry_line, exit_line)
if reference_lines:
plot_line('k', curve_secant, half_angle_line)
handles.append( plot_line('r', track_line, 'Measured Track', linestyle='-') )
plot_arrows_along_line('r', track_line)
if predicted:
if rgbcolors == []:
rgbcolors = get_distributed_colors(len(predicted))
if labels == []:
labels = [""]*len(predicted)
if label_methods == []:
label_methods = [sample['label']['selected_method']]*len(predicted)
for pred, color, label, label_method in zip(predicted, rgbcolors, labels, label_methods):
# print sample['y'] - pred
line = get_predicted_line(pred, label_method, csample)
handles.append( plot_line(color, line, label) )
for line in additional_lines:
plot_line('b', line)
if orientation == "curve-secant":
plt.gca().set_ylim([-20,10])
plt.gca().set_xlim([-20,20])
plt.legend(handles=handles)
if title: plt.title(title)
plt.show(block=block)
# select_label_method(sub_samples, 'y_radii')
# # train_samples, validation_samples = automatic_test.get_cross_validation_samples(sub_samples, 4)
# train_samples, validation_samples = automatic_test.get_partitioned_samples(sub_samples, 0.75)
# # Test 01 and 02
# automatic_test.train([whole_algo], train_samples)
# results = automatic_test.predict_all_estimators([whole_algo], validation_samples)
# for sample, prediction, predictions_all_estimators in zip(validation_samples, results[whole_algo]['predictions'], results[whole_algo]['predictions_all_estimators']):
# automatic_test.output_sample_features(sample)
# predicted_distances = [pred[0] for pred in predictions_all_estimators]
# heatmap = get_heatmap_from_polar_all_predictors(predicted_distances, sample['geometry']['curve_secant'], sample['X'][_feature_types.index('intersection_angle')])
# plot_intersection(sample, [prediction], rgbcolors=[cmap(0.6)], labels=[whole_algo.get_name()], heatmap=heatmap, orientation="curve-secant")
select_label_method(sub_samples, 'y_distances')
select_label_method(test_samples, 'y_distances')
# train_samples, validation_samples = automatic_test.get_cross_validation_samples(sub_samples, 4)
train_samples, validation_samples = automatic_test.get_partitioned_samples(sub_samples, 0.75)
# Test 03 and 04
automatic_test.train([whole_algo], train_samples)
# results = automatic_test.predict_all_estimators([whole_algo], test_samples)
results = automatic_test.predict_all_estimators([whole_algo], validation_samples)
for sample, prediction, predictions_all_estimators in zip(validation_samples, results[whole_algo]['predictions'], results[whole_algo]['predictions_all_estimators']):
# for sample, prediction, predictions_all_estimators in zip(test_samples, results[whole_algo]['predictions'], results[whole_algo]['predictions_all_estimators']):
predicted_distances = [pred[0] for pred in predictions_all_estimators]
half_angle_vec = get_half_angle_vec(sample['geometry']['exit_line'], sample['X'][_feature_types.index('intersection_angle')])
heatmap = get_heatmap_from_distances_all_predictors(predicted_distances,
sample['geometry']['entry_line'],
sample['geometry']['exit_line'],
half_angle_vec)
plot_intersection(sample, [prediction], rgbcolors=[cmap(0.6)], labels=[whole_algo.get_name()], heatmap=heatmap, orientation="curve-secant")
extract_features.select_label_method(kitti_samples, 'y_radii')
extract_features.select_label_method(darmstadt_samples, 'y_radii')
random.shuffle(kitti_samples)
random.shuffle(darmstadt_samples)
kitti_train_samples, kitti_test_samples = automatic_test.get_partitioned_samples(kitti_samples, 0.7)
darmstadt_train_samples, darmstadt_test_samples = automatic_test.get_partitioned_samples(darmstadt_samples, 0.7)
train_samples = kitti_train_samples + darmstadt_train_samples
test_samples = kitti_test_samples + darmstadt_test_samples
# samples = automatic_test.load_samples('../data/training_data/samples.pickle')
# # samples = automatic_test.normalize_features(samples)
# select_label_method(samples, 'y_distances')
# train_samples, test_samples = automatic_test.get_partitioned_samples(samples, 0.8)
# #automatic_test.test([rf_algo], train_samples, test_samples, cross_validation=False)
automatic_test.train([rf_algo], train_samples)
results = automatic_test.predict_all_estimators([rf_algo], test_samples)
for sample, prediction, predictions_all_estimators in zip(test_samples, results[rf_algo]['predictions'], results[rf_algo]['predictions_all_estimators']):
predicted_distances = [pred[0] for pred in predictions_all_estimators]
heatmap = get_heatmap_from_polar_all_predictors(predicted_distances, sample['geometry']['curve_secant'], sample['X'][_feature_types.index('intersection_angle')])
# half_angle_vec = get_half_angle_vec(sample['geometry']['exit_line'], sample['X'][_feature_types.index('intersection_angle')])
# heatmap = get_heatmap_from_distances_all_predictors(predicted_distances,
# sample['geometry']['entry_line'],
# sample['geometry']['exit_line'],
# half_angle_vec)
# plot_intersection(sample, predicted_distances, heatmap=heatmap, orientation="curve-secant")
plot_intersection(sample, [prediction], rgbcolors=['b'], heatmap=heatmap, orientation="curve-secant")
import automatic_test
import regressors
import reference_implementations
from extract_features import _feature_types
import extract_features
import numpy as np
import plot_helper
import matplotlib.pyplot as plt
import random
import pickle
kitti_samples = automatic_test.load_samples('../data/training_data/samples_15_10_08/samples.pickle')
darmstadt_samples = automatic_test.load_samples('../data/training_data/samples_15_10_20_darmstadt/samples.pickle')
extract_features.select_label_method(kitti_samples, 'y_distances')
extract_features.select_label_method(darmstadt_samples, 'y_distances')
random.shuffle(kitti_samples)
random.shuffle(darmstadt_samples)
kitti_train_samples, kitti_test_samples = automatic_test.get_partitioned_samples(kitti_samples, 0.7)
darmstadt_train_samples, darmstadt_test_samples = automatic_test.get_partitioned_samples(darmstadt_samples, 0.7)
train_samples = kitti_train_samples + darmstadt_train_samples
test_samples = kitti_test_samples + darmstadt_test_samples
rf_algo = regressors.RandomForestAlgorithm(_feature_types)
results = automatic_test.test([rf_algo], train_samples, test_samples)
angles = np.rad2deg( np.array([s['X'][_feature_types.index('intersection_angle')] for s in test_samples]) )
plt.plot(angles, results[rf_algo]['mse'], 'r.')
plt.show(block=False)
automatic_test.show_intersection_plot(results, test_samples, which_samples="worst-case", orientation="curve-secant")
#!/usr/bin/python
#coding:utf-8
# Extract single estimator regression predictions from RandomForestAlgorithm and display each line separately
import sys
sys.path.append('../')
import automatic_test
import regressors
import reference_implementations
from extract_features import _feature_types, select_label_method
from plot_helper import plot_intersection, get_heatmap_from_polar_all_predictors
feature_list = _feature_types
rf_algo = regressors.RandomForestAlgorithm(feature_list)
samples = automatic_test.load_samples('../data/training_data/samples.pickle')
# samples = automatic_test.normalize_features(samples)
# select_label_method(samples, 'y_distances')
train_samples, test_samples = automatic_test.get_partitioned_samples(samples, 0.8)
#automatic_test.test([rf_algo], train_samples, test_samples, cross_validation=False)
automatic_test.train([rf_algo], train_samples)
results = automatic_test.predict_all_estimators([rf_algo], test_samples)
for sample, predictions_all_estimators in zip(test_samples, results[rf_algo]['predictions_all_estimators']):
predicted_radii = [pred[0] for pred in predictions_all_estimators]
print predicted_radii
heatmap = get_heatmap_from_polar_all_predictors(predicted_radii,
sample['geometry']['curve_secant'],
sample['X'][_feature_types.index('intersection_angle')])
plot_intersection(sample, predicted_radii, heatmap=heatmap, orientation="preserve")
# plot_intersection(sample, predicted_radii, orientation="preserve")
figure4, axes4 = sns.plt.subplots(1, 2, sharey=True)
for i, (name, samples) in enumerate(dataset_samples):
ax = axes4[i]
lanes = list(get_array_from_feature(samples, 'lane_count_entry')) + list(get_array_from_feature(samples, 'lane_count_exit'))
lanes_labels = [label_int(ms) for ms in lanes]
lanes_plot = sns.countplot(lanes_labels, order=["n/a", "1", "2", "4"], ax=ax)
# ax.set_xlabel(u"Fahrstreifenanzahl")
# if i == 0:
# ax.set_ylabel("Anzahl der Kreuzungsarme [-]")
# sns.plt.title("KITTI + Karlsruhe")
ax.set_ylabel("")
sns.plt.show(figure4)
# Track curvatures
figure5, axes5 = sns.plt.subplots(1, 2)
for i, (name, samples) in enumerate(dataset_samples):
l_samples = [s for s in samples if s['X'][_feature_types.index('intersection_angle')] >= 0.]
r_samples = [s for s in samples if s['X'][_feature_types.index('intersection_angle')] < 0.]
ax = axes5[i]
ax.hold(True)
l_handle = plot_sample_intersection_curvature(l_samples, ax=ax, color=(0,0,1,0.5))
r_handle = plot_sample_intersection_curvature(r_samples, ax=ax, color=(1,0,0,0.5))
ax.legend([l_handle, r_handle], ['Linksabb.', 'Rechtsabb.'])
ax.set_xlim([-30,30])
ax.set_ylim([-15,15])
ax.set_ylabel("")
sns.plt.show(figure5)
figure6, axes6 = sns.plt.subplots(2, 2, sharey=True, sharex=True)
for i, (name, samples) in enumerate(dataset_samples):
speed, dist = zip(*[get_average_speed_and_dist_of_sample(s) for s in samples])
print "%s: Durchschnittsgeschwindigkeit %d km/h" % (name, np.mean(speed))
def predict(self, sample):
predicted_line = self.get_modified_geiger_line(sample['geometry']['entry_line'], sample['geometry']['exit_line'])
radii = sample_line(sample['geometry']['curve_secant'],
predicted_line,
sample['X'][_feature_types.index('intersection_angle')])
return radii
#!/usr/bin/python
#coding:utf-8
# Show the curvatures of right turn and left turn samples respectively
import sys
sys.path.append('../')
import automatic_test
import extract_features
from extract_features import _feature_types
import plot_helper
import sample_analysis
samples = automatic_test.load_samples('../data/training_data/samples.pickle')
right_turn_samples = [s for s in samples if s['X'][_feature_types.index('intersection_angle')] <= 0.0]
left_turn_samples = [s for s in samples if s['X'][_feature_types.index('intersection_angle')] > 0.0]
sample_analysis.plot_sample_intersection_curvature(right_turn_samples, "Right turn sample curvature over intersection coordinates")
sample_analysis.plot_sample_intersection_curvature(left_turn_samples, "Left turn sample curvature over intersection coordinates")
def output_sample_features(sample, features=_feature_types):
fn = sample['pickled_filename'].split('/')[-1]
print "=====", fn
for f in features:
print f, ":", sample['X'][_feature_types.index(f)]
