2.2.4.9. utils package

2.2.4.9.1. Submodules

2.2.4.9.2. utils.data_association module

Implements multiple classes and function used for the data association of landmarks

class utils.data_association.Associations(observed_landmarks_n: int | None = None, array: ndarray[Any, dtype[ScalarType]] | None = None, size: int | None = None)

Bases: object

Class to represent a set of established associations between observed and observable landmarks.

The associations are saved in a numpy array. Each index represents an observed landmark identified by its index and the respective value describes the associated observable landmark identified by its index.

array

Numpy array saving the established associations between the observed and observable landmarks.

Type:

npt.NDArray

size

Number of established associations.

Type:

int

Initilization function for a new set of associations.

One either passes the number of observed landmarks in this update step to generate a completely new set or an already existing array to generate a new copy.

Parameters:

• observed_landmarks_n (Optional[int], optional) – Number of observed landmarks in this update step to initialize the numpy array, by default None

• array (npt.NDArray, optional) – Associations array to create a copy of an existing associations object, by default None

static coords_indices_to_flat_indices(indices: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]]

Converts landmark indices to flat numpy array containing the indices of this landmarks for the state vector.

Parameters:

indices (npt.NDArray) – Indices of landmarks to generate the flat numpy array for.

Returns:

Indices of landmarks as flat numpy array.

Return type:

npt.NDArray

property copy: Associations

Gets a deepcopy of the current Associations object by generating a new one with a copy of the array.

Returns:

Copy of the current associations object.

Return type:

Associations

property flat_observable_landmark_indices: ndarray[Any, dtype[ScalarType]]

Gets indices for state vector and state covariance of observable landmarks as flat numpy array.

Returns:

Indices of observable landmarks as flat numpy array.

Return type:

npt.NDArray

property flat_observed_landmark_indices: ndarray[Any, dtype[ScalarType]]

Gets indices of state vector and state covariance observed landmarks as flat numpy array.

Returns:

Indices of observed landmarks as flat numpy array.

Return type:

npt.NDArray

property is_empty: bool

Gets whether at least one association has been established yet.

Returns:

Whether at least one association has been established yet.

Return type:

bool

translate_to_global_mapping(observable_to_global_mapping: List[int] | None = None)

Translates the associations between the observed and observable landmarks to associations between the observed landmarks and all tracked landmarks.

Parameters:

observable_to_global_mapping (Optional[List[int]], optional) – Mapping between the observable landmarks to all tracked landmarks, by default None

class utils.data_association.Cluster(observed_landmark_indices: ndarray[Any, dtype[ScalarType]], individual_compatibility: ndarray[Any, dtype[ScalarType]], observable_landmark_indices: ndarray[Any, dtype[ScalarType]], order: ndarray[Any, dtype[ScalarType]])

Bases: object

Class to represent a cluster of observed landmark that can be searched for the best associations set independently from other landmarks cluster.

Parameters:

• observed_landmark_indices (npt.NDArray) – Numpy boolean array masking which observed landmarks are part of this cluster.

• observable_landmark_indices (npt.NDArray) – Numpy boolean array masking which observable landmarks are part of this cluster.

• order (npt.NDArray) – Numpy array containing the array by which the the compatibility matrix has been ordered for the clustering.

class utils.data_association.ClusterJCBB(observed_landmarks: ndarray[Any, dtype[ScalarType]], observable_landmarks: ndarray[Any, dtype[ScalarType]], observable_state_covariance: ndarray[Any, dtype[ScalarType]], use_kd_tree: bool = True, chi2quantile: float = 0.5, verifies_landmark_id: bool = True, observable_to_global_mapping: List[int] | None = None)

Bases: object

Class to represent and execute the Joint compatibility branch and bound (JCBB) algorithm for a given set of observed and observable landmarks.

Code has been ported from the C++ Implementation of the mobile robot programming toolkit. https://github.com/MRPT/mrpt/blob/develop/libs/slam/src/slam/data_association.cpp There have been made three changes compared to the implementation above. 1. The joint complatibility (mahalanobis distance of all associations) is only calculated at the end for all associations sets with the highest number of established associations to determine the best association set. 2. There has been prepared a feature to calculate in a more efficient way, so we believe: We take into consideration that an observed landmark might not have any individual compatible observable landmark left since all individual compatible observable landmarks have either already been associated or willingly not. 3. This implementation only supports the mahalanobis distance and not the maximum likelihood.

observed_landmarks

Mx(O+1) Numpy array representing the corodinates and the id of the observed landmarks. M being the number of landmarks and O the dimension of coordinates.

Type:

npt.NDArray

observable_landmarks

Nx(O+1) Numpy array representing the coordinates and the id of the observable landmarks. M being the number of landmarks and O the dimension of coordinates.

Type:

npt.NDArray

observable_state_covariance

Covariance matrix of the observable landmarks. Dimension and explanation will follow.

Type:

npt.NDArray

observed_landmarks_n

Number of observed landmarks.

Type:

int

observable_landmarks_n

Number of observable landmarks.

Type:

int

associations_objects

List of Associations objects (sets) that have the highest number of established associations and thus are candidates for the best associations set.

Type:

List[Associations]

best_associations

Best Associations object, gets assigned at the end and must be an element of associations_objects.

Type:

Associations

best_distance

Mahalanobis distance of the currently best associations set.

Type:

float

individual_compatibility

Compatibility matrix between the observed and observable landmarks based on the individual distance and color.

Type:

npt.NDArray

individual_distances

Matrix containing the individual distances between the observed and observable landmarks.

Type:

npt.NDArray

individual_compatibility_counts

Array containing the count of compatible observable landmarks for the respective observed landmark.

Type:

npt.NDArray

nodes_explored_n

Number of explored nodes of the search tree for the best associations set.

Type:

int

association_metric

Which metric to use to compare different associations sets.

Type:

AssociationMetric

use_kd_tree

Option to specify whether a kd tree should be used for evaluating which pairings should be considered.

Type:

bool

verifies_landmark_id

Specified whether the landmark id (color) of observed and observable landmark should match.

Type:

bool

inverse_observable_landmark_covariances

Inverse of the covariance matrix for all observable covariances so that it only has to be calculated once.

Type:

npt.NDArray

chi2thres

Threshold to evaluate whether two landmarks are compatible given their mahalanobis distance.

Type:

float

observable_landmarks_flat_coords

Coordinates of all observable landmarks flattened.

Type:

npt.NDArray

observed_landmarks_flat_coords

Coordinates of all observed landmarks flattened.

Type:

npt.NDArray

clusters

List of all landmark clusters.

Type:

List[Cluster]

Initizalizes the JCBB class.

Parameters:

• observed_landmarks (npt.NDArray) – Mx(O+1) Numpy array representing the coordinates and the id of the observed landmarks. M being the number of landmarks and O the dimension of coordinates.

• observable_landmarks (npt.NDArray) – Nx(O+1) Numpy array representing the coordinates and the id of the observable landmarks. M being the number of landmarks and O the dimension of coordinates.

• observable_state_covariance (npt.NDArray) – Covariance matrix of the observable landmarks. Dimension and explanation will follow.

• use_kd_tree (bool, optional) – Option to specify whether a kd tree should be used for evaluating which pairings should be considered, by default True

• chi2quantile (float, optional) – Chi^2 quantile to calculate the threshold for evaluating the compatibility of two landmarks, by default 0.5

• verifies_landmark_id (bool, optional) – Specified whether the landmark id (color) of observed and observable landmark should match, by default True

• observable_to_global_mapping (Optional[List[int]], optional) – Mapping between the observable landmarks to all tracked landmarks, by default None

associate_with_full_covariance()

Calculate best association between the observed and observable landmarks.

static calculate_metric(observation: ndarray[Any, dtype[ScalarType]], prediction: ndarray[Any, dtype[ScalarType]], covariance: ndarray[Any, dtype[ScalarType]], metric: DataAssociationMetric) → float

Calculate metric between an observation and a prediction scaled by the covariance of the prediction.

Parameters:

• observation (npt.NDArray) – Observed landmark(s).

• prediction (npt.NDArray) – Observable landmark(s).

• covariance (npt.NDArray) – Covariance of the observable landmark(s).

• metric (DataAssociationMetric) – Metric to calculate.

Returns:

Calculated metric.

Return type:

float

find_clusters()

Find clusters of compatible landmarks in which the best associations can be searched for independently.

is_closer(distance1: float, distance2: float) → bool

Returns whether the first distance is smaller than the second distance and thus its landmarks are closer.

Based on the metric to use.

Parameters:

• distance1 (float) – First distance.

• distance2 (float) – First distance.

Returns:

Whether the first distance is smaller than the second distance and thus its landmarks are closer.

Return type:

bool

mahalanobis_distance_of_associations(current_associations: Associations) → float

Mahalanobis distance for a given set of associations. Is equivalent to the joint compatibility.

Parameters:

current_associations (Associations) – Associations set to calculate the joint compatibility for.

Returns:

Mahalanobis distance for the given set of associations.

Return type:

float

prepare()

Function to prepare the data for the JCBB algorithm to save some computation time.

recursive(cluster_observed_landmark_i: int, current_associations: Associations, cluster: Cluster)

Function to search for the best associations set recursivly.

Parameters:

• cluster_observed_landmark_i (int) – Index of the current observed landmark to try all possible not already associated observable landmarks.

• current_associations (Associations) – Current already established associations for this branch of the search tree.

• cluster (Cluster) – Cluster for which the best associations are currently searched for.

verify_landmark_id(observed_landmark_i: int, observable_landmark_i: int) → bool

Verifies whether the landmark id (color) of the given observed and observable landmark match.

Returns always True if it has been specified that the landmark id shouldn’t be verified.

Parameters:

• observed_landmark_i (int) – Index of the observed landmark the landmark id should be verified for.

• observable_landmark_i (int) – Index of the observable landmark the landmark id should be verified for.

Returns:

Whether the landmark id (color) of the given observed and observable landmark match.

Return type:

bool

class utils.data_association.DataAssociationMetric(value)

Bases: Enum

Enum to describe which metric to use to measure the compatilibity of two landmarks.

MAHALANOBIS_DISTANCE = 1

MAXIMUM_LIKELIHOOD = 2

class utils.data_association.JCBB(observed_landmarks: ndarray[Any, dtype[ScalarType]], observable_landmarks: ndarray[Any, dtype[ScalarType]], observable_state_covariance: ndarray[Any, dtype[ScalarType]], use_kd_tree: bool = True, chi2quantile: float = 0.5, verifies_landmark_id: bool = True, observable_to_global_mapping: List[int] | None = None)

Bases: object

Class to represent and execute the Joint compatibility branch and bound (JCBB) algorithm for a given set of observed and observable landmarks.

Code has been ported from the C++ Implementation of the mobile robot programming toolkit. https://github.com/MRPT/mrpt/blob/develop/libs/slam/src/slam/data_association.cpp There have been made three changes compared to the implementation above. 1. The joint complatibility (mahalanobis distance of all associations) is only calculated at the end for all associations sets with the highest number of established associations to determine the best association set. 2. There has been prepared a feature to calculate in a more efficient way, so we believe: We take into consideration that an observed landmark might not have any individual compatible observable landmark left since all individual compatible observable landmarks have either already been associated or willingly not. 3. This implementation only supports the mahalanobis distance and not the maximum likelihood.

observed_landmarks

Mx(O+1) Numpy array representing the corodinates and the id of the observed landmarks. M being the number of landmarks and O the dimension of coordinates.

Type:

npt.NDArray

observable_landmarks

Nx(O+1) Numpy array representing the coordinates and the id of the observable landmarks. M being the number of landmarks and O the dimension of coordinates.

Type:

npt.NDArray

observable_state_covariance

Covariance matrix of the observable landmarks. Dimension and explanation will follow.

Type:

npt.NDArray

observed_landmarks_n

Number of observed landmarks.

Type:

int

observable_landmarks_n

Number of observable landmarks.

Type:

int

associations_objects

List of Associations objects (sets) that have the highest number of established associations and thus are candidates for the best associations set.

Type:

List[Associations]

best_associations

Best Associations object, gets assigned at the end and must be an element of associations_objects.

Type:

Associations

best_distance

Mahalanobis distance of the currently best associations set.

Type:

float

individual_compatibility

Compatibility matrix between the observed and observable landmarks based on the individual distance and color.

Type:

npt.NDArray

individual_distances

Matrix containing the individual distances between the observed and observable landmarks.

Type:

npt.NDArray

individual_compatibility_counts

Array containing the count of compatible observable landmarks for the respective observed landmark.

Type:

npt.NDArray

nodes_explored_n

Number of explored nodes of the search tree for the best associations set.

Type:

int

association_metric

Which metric to use to compare different associations sets.

Type:

AssociationMetric

use_kd_tree

Option to specify whether a kd tree should be used for evaluating which pairings should be considered.

Type:

bool

verifies_landmark_id

Specified whether the landmark id (color) of observed and observable landmark should match.

Type:

bool

inverse_observable_landmark_covariances

Inverse of the covariance matrix for all observable covariances so that it only has to be calculated once.

Type:

npt.NDArray

chi2thres

Threshold to evaluate whether two landmarks are compatible given their mahalanobis distance.

Type:

float

observable_landmarks_flat_coords

Coordinates of all observable landmarks flattened.

Type:

npt.NDArray

observed_landmarks_flat_coords

Coordinates of all observed landmarks flattened.

Type:

npt.NDArray

Initizalizes the JCBB class.

Parameters:

• observed_landmarks (npt.NDArray) – Mx(O+1) Numpy array representing the corodinates and the id of the observed landmarks. M being the number of landmarks and O the dimension of coordinates.

• observable_landmarks (npt.NDArray) – Nx(O+1) Numpy array representing the coordinates and the id of the observable landmarks. M being the number of landmarks and O the dimension of coordinates.

• observable_state_covariance (npt.NDArray) – Covariance matrix of the observable landmarks. Dimension and explanation will follow.

• use_kd_tree (bool, optional) – Option to specify whether a kd tree should be used for evaluating which pairings should be considered, by default True

• chi2quantile (float, optional) – Chi^2 quantile to calculate the threshold for evaluate the compatibility of two landmarks, by default 0.5

• verifies_landmark_id (bool, optional) – Specified whether the landmark id (color) of observed and observable landmark should match., by default True

• observable_to_global_mapping (Optional[List[int]], optional) – Mapping between the observable landmarks to all tracked landmarks., by default None

associate_with_full_covariance()

Calculate best association between the observed and observable landmarks.

static calculate_metric(observation: ndarray[Any, dtype[ScalarType]], prediction: ndarray[Any, dtype[ScalarType]], covariance: ndarray[Any, dtype[ScalarType]], metric: DataAssociationMetric) → float

Calculate metric between an observation and a prediction scaled by the covariance of the prediction.

Parameters:

• observation (npt.NDArray) – Observed landmark(s).

• prediction (npt.NDArray) – Observable landmark(s).

• covariance (npt.NDArray) – Covariance of the observable landmark(s).

• metric (DataAssociationMetric) – Metric to calculate.

Returns:

Calculated metric.

Return type:

float

is_closer(distance1: float, distance2: float) → bool

Returns whether the first distance is smaller than the second distance and thus its landmarks are closer.

Based on the metric to use.

Parameters:

• distance1 (float) – First distance.

• distance2 (float) – First distance.

Returns:

Whether the first distance is smaller than the second distance and thus its landmarks are closer.

Return type:

bool

mahalanobis_distance_of_associations(current_associations: Associations) → float

Mahalanobis distance for a given set of associations. Is equivalent to the joint compatibility.

Parameters:

current_associations (Associations) – Associations set to calculate the joint compatibility for.

Returns:

Mahalanobis distance for the given set of associations.

Return type:

float

prepare()

Function to prepare the data for the JCBB algorithm to save some computation time.

recursive(observed_landmark_i: int, current_associations: Associations)

Function to search for the best associations set recursivly.

Parameters:

• observed_landmark_i (int) – Index of the current observed landmark to try all possible not already associated observable landmarks.

• current_associations (Associations) – Current already established associations for this branch of the search tree.

verify_landmark_id(observed_landmark_i: int, observable_landmark_i: int) → bool

Verifies whether the landmark id (color) of the given observed and observable landmark match.

Returns always True if it has been specified that the landmark id shouldn’t be verified.

Parameters:

• observed_landmark_i (int) – Index of the observed landmark the landmark id should be verified for.

• observable_landmark_i (int) – Index of the observable landmark the landmark id should be verified for.

Returns:

Whether the landmark id (color) of the given observed and observable landmark match.

Return type:

bool

2.2.4.9.3. utils.data_import module

Implementation of all utility function needed for the import of data.

class utils.data_import.RosbagAndMDFImporter(mdf_filename: str | None = None, rotate_acc: bool = True, bag_filename: str | None = None, time_offset: float = 0.0)

Bases: object

Helper Class to import data from a mdf file and if specified also from a bag file.

mdf_filename

Filename of the mdf file.

Type:

str

rotate_acc

Specifies whether the accelerometer should be rotated, by default True.

Type:

bool

bag_filename

Filename of the bag file, specify if you want to merge bag data, by default None.

Type:

Optional[str]

time_offset

Time offset between the mdf and bag file. Might need to adapt vor every combination-, by default 0..

Type:

float

df

Dataframe containing the imported data.

Type:

pd.DataFrame

Initialize object to import a mdf file and convert it to a pandas dataframe.

Import the specified columns and subsequently rotate the accelerometer. If passed also imports a bag file for the cone positions.

Parameters:

• mdf_filename (Optional[str], optional) – Filename of the mdf file, by default None

• rotate_acc (bool, optional) – Specifies whether the accelerometer should be rotated, by default True

• bag_filename (Optional[str], optional) – Filename of the bag file, specify if you want to merge bag data, by default None

• time_offset (float, optional) – Time offset between the mdf and bag file. Might need to adapt vor every combination, by default 0.

calculate_acc_rotation_object() → None

Calculate rotation object for accelerometer.

Uses the mean of the acceleration while standing still to calculate the gravitational acceleration.

import_bag(start: float | None = None, end: float | None = None, offset: float | None = None) → DataFrame

Import the bag file and return it as dataframe.

Bag is being cut, so that it matches with the mdf dataframe.

Parameters:

• start (float, optional) – Start timestamp that should be returned, by default None

• end (float, optional) – End timestamp that should be returned, by default None

• offset (float, optional) – Offset in seconds since 01.01.1970, by default None

Returns:

Dataframe containing the measured cone positions in cartesian coordinates.

Return type:

pd.DataFrame

import_data() → DataFrame

Import the mdf file and optional an additional bag file and return it as dataframe.

If the filename of the bag file is specified, the bag file is merged into the mdf file.

Returns:

mdf file as pandas dataframe.

Return type:

pd.DataFrame

postprocess_data() → None

Postprocess the imported data.

At the moment only rotate the accelerometer.

utils.data_import.import_map_csv(map_csv_filename: str) → DataFrame

Imports the ground truth of the landmark positions from a csv.

CSV should be the export of the CURE intern track creator tool.

Parameters:

map_csv_filename (str) – Filename and location of the map csv.

Returns:

Pandas Dataframe containing the cone positions of the ground truth.

Return type:

pd.DataFrame

2.2.4.9.4. utils.geometry module

utils.geometry.generate_fov_path(fov_min_distance: float = 0.0, fov_max_distance: float = 12.0, fov_angle: float = 1.7453292519943295, fov_scaling: float = 1.0, offset_after_rotation: ndarray[Any, dtype[floating]] = array([0, 0]), offset_before_rotation: ndarray[Any, dtype[floating]] = array([0, 0]), rotation: float = 0.0, point_n: int = 11) → Path

Generate matplotlib path to represent the FoV of a camera sensor.

Parameters:

• fov_min_distance (float, optional) – Minimal distance of the FOV of the camera, by default 0m.

• fov_max_distance (float, optional) – Maximal distance of the field of view of the camera, by default 12,.

• fov_angle (float, optional) – Angle of the field of view of the camera, by default np.radians(100).

• fov_scaling (float, optional) – Scaling factor for the field of view of the camera, by default 1.0.

• offset_after_rotation (npt.NDArray[np.floating], optional) – Offset to move the field of view after rotating it, by default np.array([0, 0]).

• offset_before_rotation (npt.NDArray[np.floating], optional) – Offset to move the field of view before rotating it, by default np.array([0, 0]).

• rotation (float, optional) – Rotation angle to rotate the field of view, by default 0.

• point_n (int, optional) – Count of points to use for constructing the path of the field of view, by default 11.

Returns:

Matplotlib path to describe the field of view of the camera.

Return type:

Path

utils.geometry.limit_coordinates_inside_path(coordinates: ndarray[Any, dtype[ScalarType]], path: Path) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]]]

Limit the given set of coordinates to those that lie within the specified matplotlib path.

Parameters:

• coordinates (npt.NDArray) – Specified coordinates to be limited.

• path (Path) – Specified path to limit the coordinates.

Returns:

Coordinates lieing inside the matplotlib Path, Indices of returned coordinates in respect to the input data.

Return type:

Tuple[npt.NDArray, npt.NDArray]

utils.geometry.rotate_and_move(xy: ndarray[Any, dtype[ScalarType]], rotation: float = 0.0, offset_before_rotation: ndarray[Any, dtype[ScalarType]] = array([0, 0]), offset_before_rotation_lidar: ndarray[Any, dtype[ScalarType]] = array([0, 0]), is_lidar_index: List | None = None, offset_after_rotation: ndarray[Any, dtype[ScalarType]] = array([0, 0])) → ndarray[Any, dtype[ScalarType]]

Function to move and rotate given coordinates.

First moves the coordinates, then rotates them around the origin and then move them again. Using a numpy built rotation matrix.

Parameters:

• xy (npt.NDArray) – Coordinates to move, rotate and move again.

• rotation (float, optional) – Rotation angle to rotate the coordinates around the origin, by default 0.

• offset_before_rotation (npt.NDArray, optional) – Offset (Camera) to move the coordinates before rotating them, by default np.array([0, 0]).

• offset_before_rotation_lidar (npt.NDArray, optional) – Offset (LiDAR) to move the coordinates before rotating them, by default np.array([0, 0]).

• is_lidar_index (List, optional) – List of booleans to indicate if the point is from the lidar or camera, by default None.

• offset_after_rotation (npt.NDArray, optional) – Offset to move the coordinates after rotating them, by default np.array([0, 0]).

Returns:

Moved, rotated and moved coordinates.

Return type:

npt.NDArray

utils.geometry.tranform_cartesian_to_polar_cone_coords(cone_coordinates: ndarray[Any, dtype[ScalarType]]) → ndarray[Any, dtype[ScalarType]]

Transforms 2d cartesian coords to polar coords.

Parameters:

cone_coordinates (npt.NDArray) – 2d position of cones in cartesian coordinates.

Returns:

2d position of cones in polar coordinates.

Return type:

npt.NDArray

2.2.4.9.5. utils.helpers module

Implementation of all misc utility functions.

class utils.helpers.DataAssociation(value)

Bases: Enum

Enum to describe which states to use during the update step.

JCBB uses the just little adapted Jointly Compatibility Branch and Bound algorithm. ClusterJCBB uses the JCBB algorithm for previous formed compatibility clusters.

ClusterJCBB = 2

JCBB = 1

class utils.helpers.LandmarkUpdateStrategy(value)

Bases: Enum

An enumeration.

ALL = 1

DO_NOT_INITIALIZE = 4

DO_NOT_UPDATE = 5

NO_HEADING = 2

NO_POSE = 3

class utils.helpers.Saver(filter: ukf.UKF)

Bases: object

compare(cmp_saver: Saver, exclude: List[str] = [])

Compares the current saver with another saver. Finds prediction step with diverging attributes.

Prints them and the executed steps in the previous and current prediction step.

Parameters:

cmp_saver (Saver) – Saver to compare with

finalize()

Converts the list of all attributes to numpy arrays. Currently unused.

pickle(filename: str = 'saver', test_case: str = 'default') → None

Pickle the saver object for analysis later on.

Parameters:

• filename (str, optional) – Filename the saver object should be saved to, by default ‘saver’.

• test_case (str, optional) – Name of the subfolder where the pickle should be saved to, by default ‘default’.

pickle_parameter_study(filename: str = 'saver', test_case: str = 'default', parameter_study_name: str = 'default') → None

Pickle the saver object for analysis later on.

Parameters:

• filename (str, optional) – Filename the saver object should be saved to, by default ‘saver’.

• test_case (str, optional) – Name of the subfolder where the pickle should be saved to, by default ‘default’.

static read_pickle(filename: str = 'saver', test_case: str = 'default') → Saver | None

Read the pickle of a previous pickled saver object.

Returns None if the file does not exist.

Parameters:

• filename (str, optional) – Filename of the pickle object to be read, by default ‘saver’

• test_case (str, optional) – Name of the subfolder where the pickle should be read from, by default ‘default’

Returns:

Read in saver object.

Return type:

Optional[Saver]

static read_pickle_parameter_study(filename: str = 'saver', test_case: str = 'default', parameter_study_name: str = 'default') → Saver | None

Read the pickle of a previous pickled saver object.

Returns None if the file does not exist.

Parameters:

• filename (str, optional) – Filename of the pickle object to be read, by default ‘saver’

• test_case (str, optional) – Name of the subfolder where the pickle should be read from, by default ‘default’

Returns:

Read in saver object.

Return type:

Optional[Saver]

recover(delete_last_state: bool = True)

Function to recover from a state where the state covariance matrix is not semi-positive definite and thus cannot be updated or predicted anymore

Parameters:

delete_last_state (bool, optional) – Specified whether the last saved state should be deleted before recovering last saved state, by default True

save()

Save the current values of the predefined attributes to be saved of the kalman filter.

saving_attributes = ['x', 'u', 'sensor_z', 'sensor_R', 'current_global_landmarks', 't', 'x_R', 'landmarks', 'mapping', 'x_R_prior', 'x_R_post', 'P_R_prior', 'P_R_post', 'P', 'executed_steps', 'individual_compatability', 'observable_to_global_mapping', 'local_mapping_nodes_explored_n', 'local_mapping_compared_associations_n', 'observed_landmarks', 'observable_landmarks', 'observable_state_covariance']

class utils.helpers.UpdateStrategy(value)

Bases: Enum

Enum to describe which states to use during the update step.

ALL_STATES means that all tracked states will be used for the update step as as part of the sigma points. ONLY_ROBOT_POSE_AND_NECESSARY_STATES means that all robot pose states and the states that can be measured (e.g. by the local mapping) will be used for the update step as as part of the sigma points. ONLY_ROBOT_POSE_OR_ALL_STATES means that either only the robot states will be used for the update step as as part of the sigma points or all states when there is at least one other state that can be measured.

ALL_STATES = 1

ONLY_ROBOT_POSE_AND_NECESSARY_STATES = 2

ONLY_ROBOT_POSE_OR_ALL_STATES = 3

utils.helpers.generate_progressbar(data_length: int) → ProgressBar

Generates a progressbar object with the given data length with more information than the standard one.

Parameters:

data_length (int) – Length of data points to process.

Returns:

Progressbar object with the given data length with more information than the standard one.

Return type:

progressbar.ProgressBar

utils.helpers.local_to_gps(data: ndarray[Any, dtype[floating]], lat_origin: float, lon_origin: float, gamma: float = 0, colorize: bool = False) → DataFrame

Converts numpy array of local coordinates to DataFrame of global coordinates.

Using a transformer object to convert local to global coordinates.

Parameters:

• data (npt.NDArray[np.floating]) – Numpy array containing local x and y data.

• lat_origin (float) – Latitude origin of the transformer object.

• lon_origin (float) – Longitude origin of the transformer object.

• gamma (float, optional) – Azimuth of centerline clockwise from north of the rectified bearing of centre line., by default 0

• colorize (bool, optional) – Specifies whether a color for every datapoint should be calculated., by default False

Returns:

Pandas Dataframe containing global latitude and longitude as columns.

Return type:

pd.DataFrame

2.2.4.9.6. utils.math module

Implementation of all math utility functions.

utils.math.circular_mean(sigmas: ndarray[Any, dtype[ScalarType]], Wm: ndarray[Any, dtype[ScalarType]], normalize: bool = True) → ndarray[Any, dtype[ScalarType]]

Calculate weighted mean of angles.

Since angles are periodic normal mean functions cannot be used, so this function implements one possibility to calculate circular means.

Parameters:

• sigmas (npt.NDArray) – Sigma points for UKF.

• Wm (npt.NDArray) – Weights for the sigma points.

Returns:

Weighted mean.

Return type:

npt.NDArray

utils.math.normalize_degree_angle(angle: ndarray[Any, dtype[floating]]) → ndarray[Any, dtype[floating]]

Normalize angle in unit [degrees].

Since angles are periodic, they have to be normalized.

Parameters:

angle (npt.NDArray[np.floating]) – Input angle.

Returns:

Normalized angle.

Return type:

npt.NDArray[np.floating]

utils.math.normalize_degree_angles(angles: ndarray[Any, dtype[floating]]) → ndarray[Any, dtype[floating]]

Normalize angles in unit [degrees].

Since angles are periodic, they have to be normalized.

Parameters:

angles (npt.NDArray[np.floating]) – Input angle.

Returns:

Normalized angles.

Return type:

npt.NDArray[np.floating]

utils.math.normalize_rad_angle(angle: ndarray[Any, dtype[floating]]) → ndarray[Any, dtype[floating]]

Normalize angle in unit [rad].

Since angles are periodic, they have to be normalized.

Parameters:

angle (npt.NDArray[np.floating]) – Input angle.

Returns:

Normalized angle.

Return type:

npt.NDArray[np.floating]

See also

normalize_rad_angles

Normalize multiple angles in [rad] simultaneously.

utils.math.normalize_rad_angles(angles: ndarray[Any, dtype[floating]]) → ndarray[Any, dtype[floating]]

Normalize angles in unit [rad].

Since angles are periodic, they have to be normalized.

Parameters:

angles (npt.NDArray[np.floating]) – Input angle.

Returns:

Normalized angles.

Return type:

npt.NDArray[np.floating]

2.2.4.9.7. utils.plot module

Implementation of all utility functions regarding the plotting of any data regarding the kalman filter.

class utils.plot.KalmanFilterAnimation(saver: Saver, save_count: int = 1000, step_size: int = 1, map_cones=None, map_x_offset: float = 0, map_y_offset: float = 0, map_rotation: float = 0)

Bases: object

An animated scatter plot using matplotlib.animations.FuncAnimation.

Initializes kalman filter animation.

Parameters:

• saver (helpers.Saver) – Saver containing all data of the kalman filter to animate.

• save_count (int, optional) – Number of frame to presave, by default 1000.

• step_size (int, optional) – Step size of animation, by default 1.

• map_cones (_type_, optional) – Pandas Dataframe containing the ground truth position of the landmarks, by default None.

• map_x_offset (float, optional) – Offset to move the ground truth map along the x axis., by default 0

• map_y_offset (float, optional) – Offset to move the ground truth map along the y axis., by default 0

• map_rotation (float, optional) – Angle in degrees to rotate the ground truth map around after moving it., by default 0

static compute_eigenvalues_and_angle_of_covariance(cov: ndarray[Any, dtype[ScalarType]], n_std: float = 1.0) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]], float]

Compute the eigenvalues and the angle of the covariance matrix. [Kin12]

Parameters:

• cov (npt.NDArray) – Covariance matrix.

• n_std (float, optional) – Number of standard deviations to plot, by default 1.0.

Returns:

Weight, Height and angle of the covariance matrix.

Return type:

Tuple[npt.NDArray, npt.NDArray, float]

create_confidence_ellipse(pos: ndarray[Any, dtype[ScalarType]], cov: ndarray[Any, dtype[ScalarType]], n_std: float = 3.0, facecolor: str = 'none', **kwargs) → Ellipse

Create confidence ellipse to plot position uncertainty.

Using https://matplotlib.org/devdocs/gallery/statistics/confidence_ellipse.html.

Parameters:

• pos (npt.NDArray) – Cartesian coordinates of the position.

• cov (npt.NDArray) – Uncertainty / covariance matrix of the positon.

• n_std (float, optional) – Number of standard deviations to plot, by default 3.0.

• facecolor (str, optional) – Face color of ellipse to plot, by default ‘none’.

Returns:

Confidence Ellipse to plot.

Return type:

Ellipse

get_cones_data(cone_coords: ndarray[Any, dtype[ScalarType]], cone_ids: ndarray[Any, dtype[ScalarType]]) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]]]

Get landmark position, color and size.

Parameters:

• cone_coords (npt.NDArray) – Cartesian oordinates of the cones.

• cone_ids (npt.NDArray) – ID of the cones.

Returns:

Landmark positions, marker color and marker size.

Return type:

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

get_observed_cones_data(time_step: int) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]]]

Get position, color and size for all observed landmarks of the specified time step.

Parameters:

time_step (int) – Time step to be animated.

Returns:

Observed landmark positions, marker color and marker size.

Return type:

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

get_tracked_cones_data(time_step: int) → Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]]]

Get position, color and size for all tracked landmarks of the specified time step.

Parameters:

time_step (int) – Time step to be animated.

Returns:

Tracked landmark positions, marker color and marker size.

Return type:

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

move_map_cones()

Moves and rotates the ground truth map according to the current map transformation parameters.

plot_fov = True

plot_heatmap = False

replot_map_cones()

Replots the ground truth map after changig map transformation parameters.

set_cones_data_for_scatter(scatter: PathCollection, cones_data: Tuple[ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]], ndarray[Any, dtype[ScalarType]]]) → None

Set cones data for a specified scatter plot.

Sets coordinates of scatter points, marker color and size.

Parameters:

• scatter (clt.PathCollection) – Scatter object to be modified.

• cones_data (Tuple[npt.NDArray, npt.NDArray, npt.NDArray]) – Landmark positions, marker color and marker size.

setup_plot()

Setup all plots.

update(i: int)

Function to update the plots of the animation / figure.

Parameters:

i (int) – Current timestep to plot and animate.

update_gps_measurements(i)

update_map_transformation(map_x_offset: float | None = None, map_y_offset: float | None = None, map_rotation: float | None = None)

Uupdate all transformation attributes for the ground truth with the.

Parameters:

• map_x_offset (Optional[float], optional) – Offset to move the ground truth map along the x axis., by default None

• map_y_offset (Optional[float], optional) – Offset to move the ground truth map along the y axis., by default None

• map_rotation (Optional[float], optional) – Angle in degrees to rotate the ground truth map around after moving it., by default None

class utils.plot.Player(fig: Figure, func: Callable, map_update_transmation_func: Callable, fargs: Tuple | None = None, init_func: Callable | None = None, save_count: int | None = None, mini: int = 0, maxi: int = 100, frames=None, pos: Tuple[float, float] = (0.125, 0.92), step_size: int = 1, map_x_offset: float = 0, map_y_offset: float = 0, map_rotation: float = 0, **kwargs)

Bases: FuncAnimation

Wrapper class to extend animation by interactive menu.

fig

Figure that should be animated.

Type:

matplotlib.figure.Figure

func

Function to call to update figure.

Type:

Callable

min

Minimal frame number to be animated.

Type:

int

max

Maximal frame number to be animated.

Type:

int

step_size

Number of frames to skip during animation per step.

Type:

int

i

Current frame number.

Type:

int

forwards

Boolean to specify whether the animation currently runs forward.

Type:

bool

runs

Boolean to specify whether the animation currently is animated / runs.

Type:

bool

Initializes Player with an interactive menu for an animation.

Parameters:

• fig (matplotlib.figure.Figure) – Figure that should be animated.

• func (Callable) – Function to call to update figure.

• map_update_transmation_func (Callable) – Function to call to update the parameters for the ground truth map transformation.

• frames (Optional[Iterable, int, Generator], optional) – No idea yet, by default None.

• init_func (Callable, optional) – Function to initialize figure, by default None.

• args (Optional[Tuple], optional) – args to pass to update function, by default None.

• save_count (int, optional) – Integer how many frames should be presaved, by default None.

• mini (int, optional) – Minimal frame number to be animated, by default 0.

• maxi (int, optional) – Maximal frame number to be animated, by default 100.

• pos (Tuple[float, float], optional) – Position of interactive menu, by default (0.125, 0.92).

• step_size (int, optional) – Number of frames to skip during animation per step, by default 1.

• map_x_offset (float, optional) – Offset to move the ground truth map along the x axis., by default 0

• map_y_offset (float, optional) – Offset to move the ground truth map along the y axis., by default 0

• map_rotation (float, optional) – Angle in degrees to rotate the ground truth map around after moving it., by default 0

backward(event=None)

Play animation backward.

forward(event=None)

Play animation forward.

map_rotation_changed(map_rotation: str)

Callback function to change the rotation angle map transformation parameter.

Parameters:

map_rotation (float, optional) – Angle in degrees to rotate the ground truth map around after moving it.

map_x_offset_changed(map_x_offset: str)

Callback function to change the x offset map transformation parameter.

Parameters:

map_x_offset (float, optional) – Offset to move the ground truth map along the x axis.

map_y_offset_changed(map_y_offset: str)

Callback function to change the y offset map transformation parameter.

Parameters:

map_y_offset (float, optional) – Offset to move the ground truth map along the y axis.

onebackward(event=None)

Step one frame backward by setting direction and calling onestep.

oneforward(event=None)

Step one frame forward by setting direction and calling onestep.

onestep()

Step one frame forward or backward. Direction depends on previous specification.

play()

Play animation.

set_label_for_textbox(textbox)

setup(pos: Tuple[float, float], map_x_offset: float = 0, map_y_offset: float = 0, map_rotation: float = 0)

Setup player and interactive menu.

Parameters:

• pos (Tuple[float, float]) – Position of the interactive menu.

• map_x_offset (float, optional) – Offset to move the ground truth map along the x axis., by default 0

• map_y_offset (float, optional) – Offset to move the ground truth map along the y axis., by default 0

• map_rotation (float, optional) – Angle in degrees to rotate the ground truth map around after moving it., by default 0

start()

Start animation.

stop(event=None)

Stop animation.

class utils.plot.SensorPlotConfig(sensor_name: str, z_i: List[int], x_i: List[int])

Bases: object

Class to specify how to plot the measurements of a sensor.

Specify what the sensor name is, that should be used, which measurements to convert with the hx_inverse function and subsequently which converted states to plot.

sensor_name

Name of the sensor to be plotted.

Type:

str

z_i

Indices of the measurements to be converted.

Type:

List[int]

x_i

Indices of the converted states to be plotted.

Type:

List[int]

Initialize sensor plot config.

…

Parameters:

• sensor_name (str) – Name of the sensor to be plotted.

• z_i (List[int]) – Indices of the measurements to be converted.

• x_i (List[int]) – Indices of the converted states to be plotted.

utils.plot.load_preloading_map(test_day: str = 'default', track_layout: str = 'default') → Tuple[str | None, DataFrame | None]

utils.plot.plot_animation(saver: Saver, filename: str = 'animation', test_case: str = 'default', map_cones: DataFrame | None = None, map_x_offset: float = 0, map_y_offset: float = 0, map_rotation: float = 0) → None

Plot and save animation of the vehicle position and landmark position and the respective uncertainty.

…

Parameters:

• saver (helpers.Saver) – helpers.Saver object containing the estimated data of the KalmanFilter.

• filename (str, optional) – Name of the file where the plot should be saved to, by default ‘track’.

• test_case (str, optional) – Name of the subfolder where the plot should be saved to, by default ‘default’.

• map_cones (pd.DataFrame, optional) – Pandas dataframe containing the ground truth positions of the landmarks of the map, by default None.

• map_x_offset (float, optional) – Offset to move the ground truth map along the x axis., by default 0

• map_y_offset (float, optional) – Offset to move the ground truth map along the y axis., by default 0

• map_rotation (float, optional) – Angle in degrees to rotate the ground truth map around after moving it., by default 0

utils.plot.plot_flow(saver: Saver, test_case: str = 'default')

_summary_

Parameters:

• saver (helpers.Saver) – helpers.Saver object containing the data of the KalmanFilter.

• test_case (str, optional) – Name of the subfolder where the plot should be saved to., by default ‘default’

utils.plot.plot_gps_coordinates(gps_coordinates: DataFrame, lat_key: str = 'lat', lon_key: str = 'lon', filename: str = 'gps', test_case: str = 'default')

Plot GPS Coordinates.

…

Parameters:

• gps_coordinates (pd.DataFrame) – Pandas dataframe containing the gps coordinates.

• lat_key (str, optional) – Key for the latitudes in the pandas dataframe., by default ‘lat’

• lon_key (str, optional) – Key for the longitudes in the pandas dataframe., by default ‘lon’

• filename (str, optional) – Name of the file where the plot should be saved to., by default ‘gps’

• test_case (str, optional) – Name of the subfolder where the plot should be saved to., by default ‘default’

utils.plot.plot_observed_landmark_vs_observable_landmarks(observed_landmarks, observable_landmarks, mapping, test_day='default', track_layout='default')

utils.plot.plot_standard_plots(saver: Saver, start: int = 0, end: int | None = None, test_case: str = 'default', map_cones=None, map_x_offset: float = 0, map_y_offset: float = 0, map_rotation: float = 0) → None

Plot all standard plots for the saver of the KalmanFilter.

Plot the estimated acceleration, angular rate, velocity, position, local and global trajectory

Parameters:

• saver (helpers.Saver) – helpers.Saver with the KalmanFilter data.

• start (int, optional) – Timestamp from where to plot the data., by default 0

• end (Optional[int], optional) – Timestamp until where to plot the data., by default None

• test_case (str, optional) – Name of the test case., by default ‘default’

• map_x_offset (float, optional) – Offset to move the ground truth map along the x axis., by default 0

• map_y_offset (float, optional) – Offset to move the ground truth map along the y axis., by default 0

• map_rotation (float, optional) – Angle in degrees to rotate the ground truth map around after moving it., by default 0

utils.plot.plot_state_measurement_control(saver: Saver, x_i: List[int], u_i: List[int] = [], sensor_configs: List[SensorPlotConfig] = [], start: int = 0, end: int | None = None, filename: str = 'plot', test_case: str = 'default')

Plot some state(s) against some measurement(s) and control input(s).

Uses sensor plot configs to plot measurements. Plot the control inputs on a secondary axis.

Parameters:

• saver (helpers.Saver) – helpers.Saver object containing the data of the KalmanFilter

• x_i (List[int]) – Indices of the state(s) to be plotted.

• u_i (List[int], optional) – Indices of the control input(s) to be plotted., by default []

• sensor_configs (List[SensorPlotConfig], optional) – Sensor configs of the measurements to be plotted., by default []

• start (int, optional) – Timestamp from where to plot the data., by default 0

• end (Optional[int], optional) – Timestamp until where to plot the data., by default None

• filename (str, optional) – Name of the file where the plot should be saved to., by default ‘plot’

• test_case (str, optional) – Name of the subfolder where the plot should be saved to., by default ‘default’

utils.plot.plot_track(saver: Saver, filename='track', test_case: str = 'default')

Plot and save the local estimated trajectory.

…

Parameters:

• saver (helpers.Saver) – helpers.Saver object containing the estimated data of the KalmanFilter.

• filename (str, optional) – Name of the file where the plot should be saved to, by default ‘track’.

• test_case (str, optional) – Name of the subfolder where the plot should be saved to, by default ‘default’.

utils.plot.save_flow_json(flow_dict: Dict[int, List[str]], test_case: str = 'default')

Saves the flow of the kalman filter as json.

Parameters:

• flow_dict (Dict[int, List[str]]) – Dictionary containing the flow of the kalman filter.

• test_case (str, optional) – Name of the subfolder where the plot should be saved to., by default ‘default’

utils.plot.save_plot(fig: Figure, filename: str, test_case: str = 'default', plot_html: bool = True, plot_png: bool = True) → None

Saves the plot.

Saves the plot as html and/or png.

Parameters:

• fig (go.Figure) – Plot to be saved.

• filename (str) – Name of the file where the plot should be saved to.

• test_case (str, optional) – Name of the subfolder where the plot should be saved to., by default ‘default’

• plot_html (bool, optional) – Specifies whether the plot should be saved as html., by default True

• plot_png (bool, optional) – Specifies whether the plot should be saved as png., by default True

utils.plot.save_tracked_map(filter: UKF, gps_filter: UKF | None, test_day: str = 'default', track_layout: str = 'default') → str

utils.plot.title_generator(saver: Saver, x_i: List[int] = [0], u_i: List[int] = [], z_i: List[int] = []) → str

Generate title of plot.

Uses the names of states, control inputs and measurements to generate plot title.

Parameters:

• saver (helpers.Saver) – helpers.Saver object containing the data of the kalman filter.

• x_i (List[int], optional) – Indices of the states being plotted., by default [0]

• u_i (List[int], optional) – Indices of the control inputs being plotted., by default []

• z_i (List[int], optional) – Indices of the measurements being plotted., by default []

Returns:

Title of the plot.

Return type:

str

2.2.4.9.8. utils.post_process_saver module

2.2.4.9.9. utils.vehicle_dynamics module

Implementation of all utility functions regarding the dynamic of the vehicle.

utils.vehicle_dynamics.ackermann_to_radius(ackermann: ndarray[Any, dtype[floating]], wheelbase: float = 1.55) → ndarray[Any, dtype[floating]]

Calculate radius of turn with the ackermann angle.

Using a simple bicycle model that disregards any side slip angle.

Parameters:

• ackermann (npt.NDArray[np.floating]) – Ackermann angle, aka steering angle of the wheel [rad].

• wheelbase (float, optional) – Distance between front and rear axel., by default 1.550

Returns:

Radius of turn.

Return type:

npt.NDArray[np.floating]

utils.vehicle_dynamics.load_steering_model() → dict

utils.vehicle_dynamics.steering_wheel_to_ackermann(steering_wheel_angle: float, model: dict) → float

Calculate the ackermann angle with the steering wheel angle as input.

Using a detailed formula, first implemented by Birk Blumhoff for his study work.

Parameters:

• steering_wheel_angle (float) – Angle of the steering wheel [rad].

• model (dict) –

Returns:

Ackermann angle [rad].

Return type:

float