smarts.core.utils.core_math module

class smarts.core.utils.core_math.CubicPolynomial(a: float, b: float, c: float, d: float)

A cubic polynomial.

a: float

b: float

c: float

d: float

eval(ds: float) → float

Evaluate a value along the polynomial.

classmethod from_list(coefficients: List[float])

Generates CubicPolynomial. :param coefficients: The list of coefficients [a, b, c, d]

Returns:

A new CubicPolynomial.

smarts.core.utils.core_math.batches(list_, n)

Split an indexable container into n batches.

Parameters:

• list – The iterable to split into parts

• n – The number of batches

smarts.core.utils.core_math.circular_mean(vectors: Sequence[ndarray]) → float

Given a sequence of equal-length 2D vectors (e.g., unit vectors), returns their circular mean in radians, but with +y = 0 rad. See: https://en.wikipedia.org/wiki/Circular_mean

smarts.core.utils.core_math.clip(val, min_val, max_val)

Constrain a value between a min and max by clamping exterior values to the extremes.

smarts.core.utils.core_math.comb(n, k)

Binomial coefficient

smarts.core.utils.core_math.combination_pairs_with_unique_indices(first_group, second_group, second_group_default=None)

Generates sets of combinations that use up all of the first group and at least as many of the second group. If len(first_group) > len(second_group) the second group is padded. Groups are combined using only unique indices per result. The value at an index in one group is matched to the value at an index in from the second group. Duplicate results only appear when element values repeat one of the base groups.

ordered_combinations(‘ab’, ‘123’) -> (a1 b2) (a1 b3) (a2 b1) (a2 b3) (a3 b1) (a3 b2) ordered_combinations(‘ab’, ‘1’, default=”k”) -> (a1 bk) (ak b1)

Parameters:

• first_group (Sequence) – A sequence of values.

• second_group (Sequence) – Another sequence of values which may be padded.

• default (Any, optional) – The default used to pad the second group. Defaults to None.

Returns:

Unique index to index pairings using up all elements of the first group and as many of the second group.

Return type:

Generator[Tuple[Tuple[Any, Any], …], None, None]

smarts.core.utils.core_math.constrain_angle(angle: float) → float

Constrain an angle within the inclusive range [-pi, pi]

smarts.core.utils.core_math.evaluate_bezier(points, total)

Generate the approximated points of a bezier curve given a series of control points. :param points: The bezier control points. :param total: The number of points generated from approximating the curve.

Returns:

An approximation of the bezier curve.

smarts.core.utils.core_math.fast_quaternion_from_angle(angle: float) → ndarray

Converts a float to a quaternion. :param angle: An angle in radians.

Returns:

np.array([x, y, z, w])

Return type:

(np.ndarray)

smarts.core.utils.core_math.get_bezier_curve(points)

Get the curve function given a series of points. :returns: 1] into the curve. :rtype: A curve function that takes a normalized offset [0

smarts.core.utils.core_math.get_linear_segments_for_range(s_start: float, s_end: float, segment_size: float) → List[float]

Given a range from s_start to s_end, give a linear segment of size segment_size.

smarts.core.utils.core_math.inplace_unwrap(wp_array)

Unwraps an array in place.

smarts.core.utils.core_math.is_close(a: float, b: float, rel_tolerance: float = 1e-09, abs_tolerance: float = 0.0) → bool

Determines if two values are close as defined by the inputs. :param a: The first value. :param b: The other value. :param rel_tolerance: Difference required to be close relative to the magnitude :param abs_tolerance: Absolute different allowed to be close.

Returns:

If the two values are “close”.

smarts.core.utils.core_math.is_power_of_2(value: int) → bool

Test if the given value is a power of 2 greater than 2**0. (e.g. 2**4)

smarts.core.utils.core_math.lerp(a, b, p)

Linear interpolation between a and b with p .. math:: a * (1.0 - p) + b * p :param a: interpolated values :param b: interpolated values :param p: [0..1] float describing the weight of a to b

smarts.core.utils.core_math.line_intersect(a, b, c, d) → ndarray | None

Check if the lines [a, b] and [c, d] intersect, and return the intersection point if so. Otherwise, return None.

smarts.core.utils.core_math.line_intersect_vectorized(a: ndarray, b: ndarray, C: ndarray, D: ndarray, ignore_start_pt: bool = False) → bool

Vectorized version of line_intersect(…), where C and D represent the segment points for an entire line, and a and b are points of a single line segment to be tested against. If ignore_start_pt is True, then two diverging lines that only intersect at their starting points will cause this to return False.

smarts.core.utils.core_math.low_pass_filter(input_value, previous_filter_state, filter_constant, time_step, lower_bound=-1, raw_value=0)

Filters out large value jumps by taking a filter state and returning a filter state. This is generally intended for filtering out high frequencies from raw signal values. :param input_value: The raw signal value. :param previous_filter_state: The last generated value from the filter. :param filter_constant: The scale of the filter :param time_step: The length of time between the previously processed signal and the current signal. :param lower_bound: The lowest possible value allowed. :param raw_value: A scalar addition to the signal value.

Returns:

The processed raw signal value.

smarts.core.utils.core_math.min_angles_difference_signed(first, second) → float

The minimum signed difference between angles(radians).

smarts.core.utils.core_math.mult_quat(q1, q2)

Specialized quaternion multiplication as required by the unique attributes of quaternions. :returns: The product of the quaternions.

smarts.core.utils.core_math.position_to_ego_frame(position, ego_position, ego_heading)

Get the position in ego vehicle frame given the pose (of either a vehicle or some point) in global frame. Egocentric frame: The ego position becomes origin, and ego heading direction is positive x-axis. :param position: [x,y,z] :param ego_position: Ego vehicle [x,y,z] :param ego_heading: Ego vehicle heading in radians

Returns:

The position [x,y,z] in egocentric view

Return type:

list

smarts.core.utils.core_math.radians_to_vec(radians) → ndarray

Convert a radian value to a unit directional vector. 0 rad relates to [0x, 1y] with counter-clockwise rotation.

smarts.core.utils.core_math.ray_boundary_intersect(ray_start, ray_end, boundary_pts, early_return=True) → ndarray | None

Iterate over the boundary segments, returning the nearest intersection point if a ray intersection is found. If early_return is True, this will return the first intersection point that is found.

smarts.core.utils.core_math.rotate_cw_around_point(point, radians, origin=(0, 0)) → ndarray

Rotate a point clockwise around a given origin.

smarts.core.utils.core_math.rotate_quat(quat, vect)

Rotate a vector with the rotation defined by a quaternion.

smarts.core.utils.core_math.round_param_for_dt(dt: float) → int

For a given dt, returns what to pass as the second parameter to the round() function in order to not lose precision. Note that for whole numbers, like 100, the result will be negative. For example, round_param_for_dt(100) == -2, such that round(190, -2) = 200.

smarts.core.utils.core_math.rounder_for_dt(dt: float) → Callable[[float], float]

Return a rounding function appropriate for time-stepping.

smarts.core.utils.core_math.running_mean(prev_mean: float, prev_step: int, new_val: float) → Tuple[float, int]

Returns a new running mean value, when given previous mean, previous step count, and new value,

Parameters:

• prev_mean (float) – Previous mean value.

• prev_step (int) – Previous step count.

• new_val (float) – New value to be averaged.

Returns:

Updated mean and step count.

Return type:

Tuple[float, int]

smarts.core.utils.core_math.safe_division(n: float, d: float, default=inf)

This method uses a short circuit form where and converts right side to true|false (as 1|0)

Note

The cases are: 1) True and <value> == <value> 2) False and NaN == False

smarts.core.utils.core_math.sign(x) → int

Finds the sign of a numeric type. :param x: A signed numeric type

Returns:

The sign [-1|1] of the input number

smarts.core.utils.core_math.signed_dist_to_line(point, line_point, line_dir_vec) → float

Computes the signed distance to a directed line The signed of the distance is:

• negative if point is on the right of the line

• positive if point is on the left of the line

..code-block:: python

>>> import numpy as np
>>> signed_dist_to_line(np.array([2, 0]), np.array([0, 0]), np.array([0, 1.]))
-2.0
>>> signed_dist_to_line(np.array([-1.5, 0]), np.array([0, 0]), np.array([0, 1.]))
1.5

smarts.core.utils.core_math.slope(horizontal, vertical, default=inf)

Safely converts a 2 dimensional direction vector to a slope as y/x.

Parameters:

• horizontal (float) – The x growth rate.

• vertical (float) – The y growth rate.

• default (float) – The default if the value approaches infinity. Defaults to 1e10.

Returns:

The slope of the direction vector.

Return type:

float

smarts.core.utils.core_math.squared_dist(a, b) → float

Computes the squared distance between a and b. :param a: same dimensions shaped numpy arrays. :param b: same dimensions shaped numpy arrays.

Returns:

dist**2

Return type:

float

smarts.core.utils.core_math.vec_2d(v) → ndarray

Converts a higher order vector to a 2D vector.

smarts.core.utils.core_math.vec_to_radians(v) → float

Converts a vector to a radian value. [0x,+y] is 0 rad with counter-clockwise rotation.

smarts.core.utils.core_math.welford() → Tuple[Callable[[float], None], Callable[[], float], Callable[[], float], Callable[[], int]]

Welford’s online mean and std computation.

Reference

• https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#On-line_algorithm

• https://www.adamsmith.haus/python/answers/how-to-find-a-running-standard-deviation-in-python

Returns:

Callable functions to update, get mean, get std, and get steps.

Return type:

Tuple[ Callable[[float], None], Callable[[], float], Callable[[], float], Callable[[], int] ]

smarts.core.utils.core_math.world_position_from_ego_frame(position, ego_world_position, ego_world_heading)

Restore the position from ego given the pose (of either a vehicle or some point) in world frame. world frame: The world (0, 0, 0) becomes origin. :param position: [x,y,z] :param ego_world_position: Ego vehicle [x,y,z] :param ego_world_heading: Ego vehicle heading in radians

Returns:

The position [x,y,z] in world frame

Return type:

list

smarts.core.utils.core_math.wrap_value(value: int | float, _min: float, _max: float) → float

Wraps the value around if it goes over max or under min.

smarts.core.utils.core_math.yaw_from_quaternion(quaternion) → float

Converts a quaternion to the yaw value. :param quaternion: np.array([x, y, z, w]) :type quaternion: np.ndarray

Returns:

A angle in radians.

Return type:

float