2.2.5.2. filtering_lib module

Library for track filtering to calculate centerline and track widths based on the landmarks estimated by SLAM.

class filtering_lib.ConeID(value)[source]

Bases: Enum

Enum to describe the different cone IDs.

BLUE = 1
ORANGE = 2
ORANGE_BIG = 3
YELLOW = 0
class filtering_lib.LogLevel(value)[source]

Bases: Enum

Enum to describe which LogLevel to use.

TRACE - the most fine-grained information only used in rare cases where you need the full visibility of what is happening in your application and inside the third-party libraries that you use. You can expect the TRACE logging level to be very verbose. You can use it for example to annotate each step in the algorithm or each individual query with parameters in your code.

DEBUG - less granular compared to the TRACE level, but it is more than you will need in everyday use. The DEBUG log level should be used for information that may be needed for diagnosing issues and troubleshooting or when running application in the test environment for the purpose of making sure everything is running correctly

INFO - the standard log level indicating that something happened, the application entered a certain state, etc. For example, a controller of your authorization API may include an INFO log level with information on which user requested authorization if the authorization was successful or not. The information logged using the INFO log level should be purely informative and not looking into them on a regular basis shouldnt result in missing any important information.

WARN - the log level that indicates that something unexpected happened in the application, a problem, or a situation that might disturb one of the processes. But that doesnt mean that the application failed. The WARN level should be used in situations that are unexpected, but the code can continue the work. For example, a parsing error occurred that resulted in a certain document not being processed.

ERROR - the log level that should be used when the application hits an issue preventing one or more functionalities from properly functioning. The ERROR log level can be used when one of the payment systems is not available, but there is still the option to check out the basket in the e-commerce application or when your social media logging option is not working for some reason.

FATAL - the log level that tells that the application encountered an event or entered a state in which one of the crucial business functionality is no longer working. A FATAL log level may be used when the application is not able to connect to a crucial data store like a database or all the payment systems are not available and users cant checkout their baskets in your e-commerce.

DEBUG = 1
ERROR = 4
FATAL = 5
INFO = 2
TRACE = 0
WARN = 3
class filtering_lib.TrackFiltering(local_fov_angle_rad: float, local_fov_range_m: float, log_level: LogLevel = LogLevel.FATAL, track_width_min: float = 3.0)[source]

Bases: object

TrackFiltering instance implementing functionality to calculate centerline and track widths for given landmarks.

log_level

Log level to use.

Type:

LogLevel

track_width_min

Expected minimal track width.

Type:

float

Initialize TrackFiltering object.

Parameters:

  • local_fov_angle_rad (float) – Field of view angle [rad] for limiting landmarks for local track filtering.

  • local_fov_range_m (float) – Field of view range [m] limiting landmarks for local track filtering.

  • log_level (LogLevel, optional) – Log level to use, by default LogLevel.FATAL

  • track_width_min (float, optional) – Expected minimal track width, by default 3.

  • local_fov_angle_rad – Field of view angle [rad] for limiting landmarks for local track filtering.

  • local_fov_range_m – Field of view range [m] limiting landmarks for local track filtering.

  • vehicle_pose (Optional[npt.NDArray]) – Current vehicle pose (x, y, psi).

  • left_cones (Optional[npt.NDArray]) – All cones which are on the left side of the track, shape: (n, 2) with n equal number of left cones.

  • right_cones (Optional[npt.NDArray]) – All cones which are on the right side of the track, shape: (m, 2) with m equal number of right cones.

  • orange_cones (Optional[npt.NDArray]) – All small cones, shape: (k, 2) with k equal number of small orange cones.

  • left_not_empty (Optional[bool]) – Indicates whether there are no left cones.

  • right_not_empty (Optional[bool]) – Indicates whether there are no right cones.

  • orange_cones_not_empty (Optional[bool]) – Indicates whether there are no small orange cones.

  • global_track (Optional[bool]) – Indicates whether the inputed cones build a closed track.

  • runtime (Optional[float]) – Time it took to run filtering.

  • centerpoints (Optional[npt.NDArray]) – Calculated centerpoints of the filtered track, shape: (c, 2) with c equal number of centerpoints.

  • track_widths (Optional[npt.NDArray]) – Calculated trackwidths of the filtered track, shape: (c, 2) with c equal number of centerpoints.

  • curvature_centerline (Optional[npt.NDArray]) – Curvature of the calculated centerpoints of the filtered track, shape: (c, 2) with c equal number of centerpoints.

  • points_unorganized (Optional[npt.NDArray]) – Helper class attribute for ordering points, buffers the points which are not ordered yet, shape: (i, 2)

  • min_distance (Optional[npt.NDArray]) – Helper class attribute for ordering points, buffers the minimal distances (??)m shape: (l, )

advanced_dynamic_window(cone_id: int) → bool[source]

Generate midpoints if there is only one single cone detected.

Calculate the normalvector on the support vector to the cone. With the the minimum half track width one can estimate the midpoint.

Name of the methode is based on the ‘dynamic window approach’ where a robot finds its path by avoiding a crash with each obstacle.

Note

See implementation of dynamic window

Todo

What is the difference to the dynamic window approach.

Parameters:

cone_id – ID of one and only detected cone.

Returns:

Indicates whether algorithm was successful

Return type:

bool

border_shift(cone_id: int) → bool[source]

Shifts trackborder points to the estimated middle of the track.

Resulting in new centerline and a legal trackwidth for each centerpoint.

Todo

Reqrite generateTree function.

Todo

Add Polyfit and CubicSpline Methode.

Parameters:

cone_id – ID of the detected cones.

Returns:

Indicates whether algorithm was successful.

Return type:

bool

convert_local_to_global(local_points: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]

Convert local coordinates (rear axle) back to global (map) coordinates.

Rotate and translate vehicle coordinate system to global coordinate system.

Todo

Check Rotation from scipy.spatial.transform instead of own implementation.

Parameters:

local_points (npt.NDArray) – Points in local (rear axle) coordinates system.

Returns:

Points in global (map) coordinate system.

Return type:

npt.NDArray

cubic_spline(points: ndarray[Any, dtype[ScalarType]], factor: int = 1, endpoint: bool = True) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]]][source]

Calculate cubic spline of the input points.

Also calculates the curvature of the spline.

Todo

What’s the smallest number of points to get the first and second deriviate.

Parameters:

  • points (npt.NDArray) – Array if points as input for cubicspline.

  • factor (int, optional) – Factor for interpolation between points, by default 1

  • endpoint (bool, optional) – Whether to use the last point or not, by default True

Returns:

new points of the spline interpolation (shape: (m, 2) with m equal (factor * n) + 1) and normal vectors of the spline interpolation (shape: (m, 2) with m equal (factor * n) + 1).

Return type:

Tuple[npt.NDArray, npt.NDArray]

delaunay()[source]

Run delaunex triangulation. Works for local and global tracks.

draw()[source]

Plot the final results.

dynamic_window(cone_id: int) → bool[source]

Generate midpoints if there is only one single cone detected.

Calculate the normalvector on the support vector to the cone. With the the minimum half track width one can estimate the midpoint.

Name of the methode is based on the ‘dynamic window approach’ where a robot finds its path by avoiding a crash with each obstacle.

Note

See implementation of advanced dynamic window

Parameters:

cone_id – ID of one and only detected cone.

Returns:

Indicates whether algorithm was successful.

Return type:

bool

filtering_main(left_cones: ndarray[Any, dtype[ScalarType]], right_cones: ndarray[Any, dtype[ScalarType]], orange_cones: ndarray[Any, dtype[ScalarType]], x_m: float, y_m: float, psi_rad: float, global_track: bool) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]], bool][source]

Filter given cones to calculate centerline and track widths.

Parameters:

  • left_cones (npt.NDArray) – All cones which are on the left side of the track, shape: (n, 2) with n equal number of left cones.

  • right_cones (npt.NDArray) – All cones which are on the right side of the track, shape: (m, 2) with m equal number of right cones.

  • orange_cones (npt.NDArray) – All small cones, shape: (k, 2) with k equal number of small orange cones.

  • x_m (float) – X position [m] of the vehicle.

  • y_m (float) – Y position [m] of the vehicle.

  • psi_rad (float) – Heading [rad] of the vehicle.

  • global_track (bool) – Indicates whether the inputed cones build a closed track.

Returns:

Centerline (shape: (l, 2) with l equal number of centerpoints), Track widths (shape: (l, 2) with l equal number of centerpoints) and Closed track indicating whether the resulting centerline is closed.

Return type:

Tuple[npt.NDArray, npt.NDArray, bool]

find_nearest_point(origin: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]][source]

Find nearest point to last origin point. Delete used points from points_unorgaized class attribute.

Parameters:

origin (npt.NDArray) – Position of last known point, shape: (2, ).

Returns:

Position of next ordered point.

Return type:

npt.NDArray

full_path_filter() → bool[source]

Filter the full path.

Todo

Improve documentation of this method.

Returns:

Indicates whether algorithm was successful.

Return type:

bool

gate_fallback() → bool[source]

Calculate centerpoint between one yellow and one blue cone.

Todo

One could return False if cones are inverted (vehicle has to turn 180 degree to pass gate).

Returns:

Indicates whether algorithm was successful.

Return type:

bool

generate_tree(points: ndarray[Any, dtype[ScalarType]], start_point: ndarray[Any, dtype[ScalarType]], centerpoints: bool = False) → ndarray[Any, dtype[ScalarType]][source]

Bring points intro right order.

Start with vehicle position and try to find nearest points for each upcoming point.

Parameters:

  • points (npt.NDArray) – Array containing position of points user wants to order, shape: (n, 2) with n equal number of points.

  • start_point (npt.NDArray) – Position of first point, e.g. vehicle position, shape: (2, ).

  • centerpoints (bool, optional) – Indicates whether points are centerpoints, by default False

Returns:

Order points, shape: (n, 2) with n equal number of points.

Return type:

npt.NDArray

get_cones_in_local_fov()[source]

Limit all cones to the visuals ones according to the field of view.

Also rotate coordinate system to vehicle coordinate system.

Todo

Check Rotation from scipy.spatial.transform instead of own implementation.

global_filtering() → bool[source]

Filter track globally.

Input are left and right cones, Output are centerline of filtered track and legal area of the track.

Algorithm has the following steps:

  1. Find midpoints with delauney triangulation.

  2. Sort mitpoints with generate tree algorithm (first is the one nearest to start and finish line, second one have to calculated with the direction).

  3. Calculate trackwidth to left and right for each midpoint.

Returns:

Indicates whether filtering was successful.

Return type:

bool

local_filtering() → bool[source]

Filter track locally.

Input are left and right cones, Output are centerline of filtered track and legal area of the track.

The algorithm follows 3 steps (steps 2 & 3 are currently part of motion planning):

  1. Find the midpoints of the track. algorithm seperates between the secen cases below.

  2. Interpolate and approximate centerline from fround mitpoints. Calculate normalvectors.

  3. Defines the legal track width for each point and calculates borderlines on the right and left.

For the first step the algorithm has to decide which kind of midpoint generator should be used.

  1. Only one cone: a. Only one cone on the left: DynamicWindow b. Only one cone on the right: DynamicWindow

  2. Only one color: a. Only left cones: BorderShift a. Only right cones: BorderShift

  3. Two colors: a. One cone each: GateFallback b. One cone on one side: BorderShift c. All other: delauney

Todo

For case 3.b the threshold should be modified. Also a other stepsize for interpolating and approximation could serve better results.

Returns:

Indicates whether filtering was successful.

Return type:

bool

polyfit(points: ndarray[Any, dtype[ScalarType]], order: int = 5, factor: int = 1) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]]][source]

Calculate a polyfit for a given set of points. Also calculate normal vectors for each point.

Parameters:

  • points (npt.NDArray) – Array containing the position of every cone in the right order, shape: (n, 2) with n equal number of points.

  • order (int, optional) – Highest order of ponynomial, needs to be odd number, by default 5

  • factor (int, optional) – Scaling factor for approximation, by default 1

Returns:

new points of the fitted polynomial (shape: (m, 2) with m equal (factor * n) + 1) and normal vectors of the fitted polynomial (shape: (m, 2) with m equal (factor * n) + 1).

Return type:

Tuple[npt.NDArray, npt.NDArray]

set_fov(local_fov_angle_rad: float, local_fov_range_m: float)[source]

Set field of view for local track filtering.

Parameters:

  • local_fov_angle_rad (float) – Field of view angle [rad] for limiting landmarks for local track filtering.

  • local_fov_range_m (float) – Field of view range [m] limiting landmarks for local track filtering.