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)]