From a76e55ca0de75da681f9f13cd4f321a76f5a943f Mon Sep 17 00:00:00 2001
From: Yangge Li <li213@illinois.edu>
Date: Fri, 13 May 2022 21:08:35 -0500
Subject: [PATCH] working on lane segments
---
src/scene_verifier/map/lane.py | 28 ++
src/scene_verifier/map/lane_map.py | 27 +-
src/scene_verifier/map/lane_segment.py | 226 ++++++++++++++-
src/scene_verifier/utils/__init__.py | 0
src/scene_verifier/utils/utils.py | 378 +++++++++++++++++++++++++
5 files changed, 639 insertions(+), 20 deletions(-)
create mode 100644 src/scene_verifier/map/lane.py
create mode 100644 src/scene_verifier/utils/__init__.py
create mode 100644 src/scene_verifier/utils/utils.py
diff --git a/src/scene_verifier/map/lane.py b/src/scene_verifier/map/lane.py
new file mode 100644
index 00000000..b6c25c24
--- /dev/null
+++ b/src/scene_verifier/map/lane.py
@@ -0,0 +1,28 @@
+from typing import List
+
+import numpy as np
+
+from src.scene_verifier.map.lane_segment import AbstractLane
+
+class Lane():
+ def __init__(self, id, seg_list: List[AbstractLane]):
+ self.id = id
+ self.segment_list: List[AbstractLane] = seg_list
+
+ def get_lane_segment(self, position:np.ndarray) -> AbstractLane:
+ for segment in self.segment_list:
+ logitudinal, lateral = segment.local_coordinates(position)
+ is_on = 0 <= logitudinal < segment.length
+ if is_on:
+ return segment
+ return None
+
+ def get_longitudinal_error(self, position:np.ndarray) -> float:
+ segment = self.get_lane_segment(position)
+ longituinal, lateral = segment.local_coordinates(position)
+ return longituinal
+
+ def get_lateral_error(self, position:np.ndarray) -> float:
+ segment = self.get_lane_segment(position)
+ longituinal, lateral = segment.local_coordinates(position)
+ return lateral
diff --git a/src/scene_verifier/map/lane_map.py b/src/scene_verifier/map/lane_map.py
index 9e543232..5a8b0dfa 100644
--- a/src/scene_verifier/map/lane_map.py
+++ b/src/scene_verifier/map/lane_map.py
@@ -1,12 +1,15 @@
from typing import Dict, List
import copy
-from enum import Enum,auto
+from enum import Enum
-from src.scene_verifier.map.lane_segment import LaneSegment
+import numpy as np
+
+from src.scene_verifier.map.lane_segment import AbstractLane
+from src.scene_verifier.map.lane import Lane
class LaneMap:
- def __init__(self, lane_seg_list:List[LaneSegment] = []):
- self.lane_segment_dict:Dict[str, LaneSegment] = {}
+ def __init__(self, lane_seg_list:List[Lane] = []):
+ self.lane_segment_dict:Dict[str, Lane] = {}
self.left_lane_dict:Dict[str, List[str]] = {}
self.right_lane_dict:Dict[str, List[str]] = {}
for lane_seg in lane_seg_list:
@@ -14,7 +17,7 @@ class LaneMap:
self.left_lane_dict[lane_seg.id] = []
self.right_lane_dict[lane_seg.id] = []
- def add_lanes(self, lane_seg_list:List[LaneSegment]):
+ def add_lanes(self, lane_seg_list:List[AbstractLane]):
for lane_seg in lane_seg_list:
self.lane_segment_dict[lane_seg.id] = lane_seg
self.left_lane_dict[lane_seg.id] = []
@@ -61,11 +64,15 @@ class LaneMap:
lane_idx = lane_idx.name
return self.lane_segment_dict[lane_idx].get_geometry()
- def get_longitudinal_error(self, lane_idx, agent_state):
- raise NotImplementedError
-
- def get_lateral_error(self, lane_idx, agent_state):
- raise NotImplementedError
+ def get_longitudinal_error(self, lane_idx:str, agent_state) -> float:
+ lane = self.lane_segment_dict[lane_idx]
+ position = np.array([agent_state[0], agent_state[1]])
+ return lane.get_longitudinal_error(position)
+
+ def get_lateral_error(self, lane_idx:str, agent_state) -> float:
+ lane = self.lane_segment_dict[lane_idx]
+ position = np.array([agent_state[0], agent_state[1]])
+ return lane.get_longitudinal_error(position)
def get_altitude_error(self, lane_idx, agent_state):
raise NotImplementedError
\ No newline at end of file
diff --git a/src/scene_verifier/map/lane_segment.py b/src/scene_verifier/map/lane_segment.py
index 33143c69..c49e3529 100644
--- a/src/scene_verifier/map/lane_segment.py
+++ b/src/scene_verifier/map/lane_segment.py
@@ -1,15 +1,221 @@
from typing import List
+import numpy as np
+from abc import ABCMeta, abstractmethod
+from typing import Tuple, List, Optional, Union
-class LaneSegment:
- def __init__(self, id, lane_parameter = None):
+from src.scene_verifier.utils.utils import wrap_to_pi, Vector, get_class_path, class_from_path,to_serializable
+
+class LineType:
+
+ """A lane side line type."""
+
+ NONE = 0
+ STRIPED = 1
+ CONTINUOUS = 2
+ CONTINUOUS_LINE = 3
+
+class AbstractLane(object):
+
+ """A lane on the road, described by its central curve."""
+
+ metaclass__ = ABCMeta
+ DEFAULT_WIDTH: float = 4
+ VEHICLE_LENGTH: float = 5
+ length: float = 0
+ line_types: List["LineType"]
+
+ def __init__(self, id:str):
+ self.id = id
+
+ @abstractmethod
+ def position(self, longitudinal: float, lateral: float) -> np.ndarray:
+ """
+ Convert local lane coordinates to a world position.
+
+ :param longitudinal: longitudinal lane coordinate [m]
+ :param lateral: lateral lane coordinate [m]
+ :return: the corresponding world position [m]
+ """
+ raise NotImplementedError()
+
+ @abstractmethod
+ def local_coordinates(self, position: np.ndarray) -> Tuple[float, float]:
+ """
+ Convert a world position to local lane coordinates.
+
+ :param position: a world position [m]
+ :return: the (longitudinal, lateral) lane coordinates [m]
+ """
+ raise NotImplementedError()
+
+ @abstractmethod
+ def heading_at(self, longitudinal: float) -> float:
+ """
+ Get the lane heading at a given longitudinal lane coordinate.
+
+ :param longitudinal: longitudinal lane coordinate [m]
+ :return: the lane heading [rad]
+ """
+ raise NotImplementedError()
+
+ @abstractmethod
+ def width_at(self, longitudinal: float) -> float:
+ """
+ Get the lane width at a given longitudinal lane coordinate.
+
+ :param longitudinal: longitudinal lane coordinate [m]
+ :return: the lane width [m]
+ """
+ raise NotImplementedError()
+
+ @classmethod
+ def from_config(cls, config: dict):
+ """
+ Create lane instance from config
+
+ :param config: json dict with lane parameters
+ """
+ raise NotImplementedError()
+
+ @abstractmethod
+ def to_config(self) -> dict:
+ """
+ Write lane parameters to dict which can be serialized to json
+
+ :return: dict of lane parameters
+ """
+ raise NotImplementedError()
+
+ def on_lane(self, position: np.ndarray, longitudinal: float = None, lateral: float = None, margin: float = 0) \
+ -> bool:
+ """
+ Whether a given world position is on the lane.
+
+ :param position: a world position [m]
+ :param longitudinal: (optional) the corresponding longitudinal lane coordinate, if known [m]
+ :param lateral: (optional) the corresponding lateral lane coordinate, if known [m]
+ :param margin: (optional) a supplementary margin around the lane width
+ :return: is the position on the lane?
+ """
+ if longitudinal is None or lateral is None:
+ longitudinal, lateral = self.local_coordinates(position)
+ is_on = np.abs(lateral) <= self.width_at(longitudinal) / 2 + margin and \
+ -self.VEHICLE_LENGTH <= longitudinal < self.length + self.VEHICLE_LENGTH
+ return is_on
+
+ def is_reachable_from(self, position: np.ndarray) -> bool:
+ """
+ Whether the lane is reachable from a given world position
+
+ :param position: the world position [m]
+ :return: is the lane reachable?
+ """
+ if self.forbidden:
+ return False
+ longitudinal, lateral = self.local_coordinates(position)
+ is_close = np.abs(lateral) <= 2 * self.width_at(longitudinal) and \
+ 0 <= longitudinal < self.length + self.VEHICLE_LENGTH
+ return is_close
+
+ def after_end(self, position: np.ndarray, longitudinal: float = None, lateral: float = None) -> bool:
+ if not longitudinal:
+ longitudinal, _ = self.local_coordinates(position)
+ return longitudinal > self.length - self.VEHICLE_LENGTH / 2
+
+ def distance(self, position: np.ndarray):
+ """Compute the L1 distance [m] from a position to the lane."""
+ s, r = self.local_coordinates(position)
+ return abs(r) + max(s - self.length, 0) + max(0 - s, 0)
+
+ def distance_with_heading(self, position: np.ndarray, heading: Optional[float], heading_weight: float = 1.0):
+ """Compute a weighted distance in position and heading to the lane."""
+ if heading is None:
+ return self.distance(position)
+ s, r = self.local_coordinates(position)
+ angle = np.abs(wrap_to_pi(heading - self.heading_at(s)))
+ return abs(r) + max(s - self.length, 0) + max(0 - s, 0) + heading_weight*angle
+
+class StraightLane(AbstractLane):
+
+ """A lane going in straight line."""
+
+ def __init__(self,
+ id: str,
+ start: Vector,
+ end: Vector,
+ width: float = AbstractLane.DEFAULT_WIDTH,
+ line_types: Tuple[LineType, LineType] = None,
+ forbidden: bool = False,
+ speed_limit: float = 20,
+ priority: int = 0) -> None:
+ """
+ New straight lane.
+
+ :param start: the lane starting position [m]
+ :param end: the lane ending position [m]
+ :param width: the lane width [m]
+ :param line_types: the type of lines on both sides of the lane
+ :param forbidden: is changing to this lane forbidden
+ :param priority: priority level of the lane, for determining who has right of way
+ """
self.id = id
- # self.left_lane:List[str] = left_lane
- # self.right_lane:List[str] = right_lane
- # self.next_segment:int = next_segment
+ self.start = np.array(start)
+ self.end = np.array(end)
+ self.width = width
+ self.heading = np.arctan2(self.end[1] - self.start[1], self.end[0] - self.start[0])
+ self.length = np.linalg.norm(self.end - self.start)
+ self.line_types = line_types or [LineType.STRIPED, LineType.STRIPED]
+ self.direction = (self.end - self.start) / self.length
+ self.direction_lateral = np.array([-self.direction[1], self.direction[0]])
+ self.forbidden = forbidden
+ self.priority = priority
+ self.speed_limit = speed_limit
+
+ def position(self, longitudinal: float, lateral: float) -> np.ndarray:
+ return self.start + longitudinal * self.direction + lateral * self.direction_lateral
+
+ def heading_at(self, longitudinal: float) -> float:
+ return self.heading
+
+ def width_at(self, longitudinal: float) -> float:
+ return self.width
+
+ def local_coordinates(self, position: np.ndarray) -> Tuple[float, float]:
+ delta = position - self.start
+ longitudinal = np.dot(delta, self.direction)
+ lateral = np.dot(delta, self.direction_lateral)
+ return float(longitudinal), float(lateral)
+
+ @classmethod
+ def from_config(cls, config: dict):
+ config["start"] = np.array(config["start"])
+ config["end"] = np.array(config["end"])
+ return cls(**config)
+
+ def to_config(self) -> dict:
+ return {
+ "class_path": get_class_path(self.__class__),
+ "config": {
+ "start": to_serializable(self.start),
+ "end": to_serializable(self.end),
+ "width": self.width,
+ "line_types": self.line_types,
+ "forbidden": self.forbidden,
+ "speed_limit": self.speed_limit,
+ "priority": self.priority
+ }
+ }
+
+# class LaneSegment:
+# def __init__(self, id, lane_parameter = None):
+# self.id = id
+# # self.left_lane:List[str] = left_lane
+# # self.right_lane:List[str] = right_lane
+# # self.next_segment:int = next_segment
- self.lane_parameter = None
- if lane_parameter is not None:
- self.lane_parameter = lane_parameter
+# self.lane_parameter = None
+# if lane_parameter is not None:
+# self.lane_parameter = lane_parameter
- def get_geometry(self):
- return self.lane_parameter
\ No newline at end of file
+# def get_geometry(self):
+# return self.lane_parameter
\ No newline at end of file
diff --git a/src/scene_verifier/utils/__init__.py b/src/scene_verifier/utils/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/src/scene_verifier/utils/utils.py b/src/scene_verifier/utils/utils.py
new file mode 100644
index 00000000..255cb521
--- /dev/null
+++ b/src/scene_verifier/utils/utils.py
@@ -0,0 +1,378 @@
+import copy
+import importlib
+import itertools
+from typing import Tuple, Dict, Callable, List, Optional, Union, Sequence
+
+import numpy as np
+
+# Useful types
+Vector = Union[np.ndarray, Sequence[float]]
+Matrix = Union[np.ndarray, Sequence[Sequence[float]]]
+Interval = Union[np.ndarray,
+ Tuple[Vector, Vector],
+ Tuple[Matrix, Matrix],
+ Tuple[float, float],
+ List[Vector],
+ List[Matrix],
+ List[float]]
+
+def to_serializable(arg: Union[np.ndarray, List]) -> List:
+ if isinstance(arg, np.ndarray):
+ return arg.tolist()
+ return arg
+
+def do_every(duration: float, timer: float) -> bool:
+ return duration < timer
+
+
+def lmap(v: float, x: Interval, y: Interval) -> float:
+ """Linear map of value v with range x to desired range y."""
+ return y[0] + (v - x[0]) * (y[1] - y[0]) / (x[1] - x[0])
+
+
+def get_class_path(cls: Callable) -> str:
+ return cls.__module__ + "." + cls.__qualname__
+
+
+def class_from_path(path: str) -> Callable:
+ module_name, class_name = path.rsplit(".", 1)
+ class_object = getattr(importlib.import_module(module_name), class_name)
+ return class_object
+
+
+def constrain(x: float, a: float, b: float) -> np.ndarray:
+ return np.clip(x, a, b)
+
+
+def not_zero(x: float, eps: float = 1e-2) -> float:
+ if abs(x) > eps:
+ return x
+ elif x >= 0:
+ return eps
+ else:
+ return -eps
+
+
+def wrap_to_pi(x: float) -> float:
+ return ((x + np.pi) % (2 * np.pi)) - np.pi
+
+
+def point_in_rectangle(point: Vector, rect_min: Vector, rect_max: Vector) -> bool:
+ """
+ Check if a point is inside a rectangle
+
+ :param point: a point (x, y)
+ :param rect_min: x_min, y_min
+ :param rect_max: x_max, y_max
+ """
+ return rect_min[0] <= point[0] <= rect_max[0] and rect_min[1] <= point[1] <= rect_max[1]
+
+
+def point_in_rotated_rectangle(point: np.ndarray, center: np.ndarray, length: float, width: float, angle: float) \
+ -> bool:
+ """
+ Check if a point is inside a rotated rectangle
+
+ :param point: a point
+ :param center: rectangle center
+ :param length: rectangle length
+ :param width: rectangle width
+ :param angle: rectangle angle [rad]
+ :return: is the point inside the rectangle
+ """
+ c, s = np.cos(angle), np.sin(angle)
+ r = np.array([[c, -s], [s, c]])
+ ru = r.dot(point - center)
+ return point_in_rectangle(ru, (-length/2, -width/2), (length/2, width/2))
+
+
+def point_in_ellipse(point: Vector, center: Vector, angle: float, length: float, width: float) -> bool:
+ """
+ Check if a point is inside an ellipse
+
+ :param point: a point
+ :param center: ellipse center
+ :param angle: ellipse main axis angle
+ :param length: ellipse big axis
+ :param width: ellipse small axis
+ :return: is the point inside the ellipse
+ """
+ c, s = np.cos(angle), np.sin(angle)
+ r = np.matrix([[c, -s], [s, c]])
+ ru = r.dot(point - center)
+ return np.sum(np.square(ru / np.array([length, width]))) < 1
+
+
+def rotated_rectangles_intersect(rect1: Tuple[Vector, float, float, float],
+ rect2: Tuple[Vector, float, float, float]) -> bool:
+ """
+ Do two rotated rectangles intersect?
+
+ :param rect1: (center, length, width, angle)
+ :param rect2: (center, length, width, angle)
+ :return: do they?
+ """
+ return has_corner_inside(rect1, rect2) or has_corner_inside(rect2, rect1)
+
+
+def rect_corners(center: np.ndarray, length: float, width: float, angle: float,
+ include_midpoints: bool = False, include_center: bool = False) -> List[np.ndarray]:
+ """
+ Returns the positions of the corners of a rectangle.
+ :param center: the rectangle center
+ :param length: the rectangle length
+ :param width: the rectangle width
+ :param angle: the rectangle angle
+ :param include_midpoints: include middle of edges
+ :param include_center: include the center of the rect
+ :return: a list of positions
+ """
+ center = np.array(center)
+ half_l = np.array([length/2, 0])
+ half_w = np.array([0, width/2])
+ corners = [- half_l - half_w,
+ - half_l + half_w,
+ + half_l + half_w,
+ + half_l - half_w]
+ if include_center:
+ corners += [[0, 0]]
+ if include_midpoints:
+ corners += [- half_l, half_l, -half_w, half_w]
+
+ c, s = np.cos(angle), np.sin(angle)
+ rotation = np.array([[c, -s], [s, c]])
+ return (rotation @ np.array(corners).T).T + np.tile(center, (len(corners), 1))
+
+
+def has_corner_inside(rect1: Tuple[Vector, float, float, float],
+ rect2: Tuple[Vector, float, float, float]) -> bool:
+ """
+ Check if rect1 has a corner inside rect2
+
+ :param rect1: (center, length, width, angle)
+ :param rect2: (center, length, width, angle)
+ """
+ return any([point_in_rotated_rectangle(p1, *rect2)
+ for p1 in rect_corners(*rect1, include_midpoints=True, include_center=True)])
+
+
+def project_polygon(polygon: Vector, axis: Vector) -> Tuple[float, float]:
+ min_p, max_p = None, None
+ for p in polygon:
+ projected = p.dot(axis)
+ if min_p is None or projected < min_p:
+ min_p = projected
+ if max_p is None or projected > max_p:
+ max_p = projected
+ return min_p, max_p
+
+
+def interval_distance(min_a: float, max_a: float, min_b: float, max_b: float):
+ """
+ Calculate the distance between [minA, maxA] and [minB, maxB]
+ The distance will be negative if the intervals overlap
+ """
+ return min_b - max_a if min_a < min_b else min_a - max_b
+
+
+def are_polygons_intersecting(a: Vector, b: Vector,
+ displacement_a: Vector, displacement_b: Vector) \
+ -> Tuple[bool, bool, Optional[np.ndarray]]:
+ """
+ Checks if the two polygons are intersecting.
+
+ See https://www.codeproject.com/Articles/15573/2D-Polygon-Collision-Detection
+
+ :param a: polygon A, as a list of [x, y] points
+ :param b: polygon B, as a list of [x, y] points
+ :param displacement_a: velocity of the polygon A
+ :param displacement_b: velocity of the polygon B
+ :return: are intersecting, will intersect, translation vector
+ """
+ intersecting = will_intersect = True
+ min_distance = np.inf
+ translation, translation_axis = None, None
+ for polygon in [a, b]:
+ for p1, p2 in zip(polygon, polygon[1:]):
+ normal = np.array([-p2[1] + p1[1], p2[0] - p1[0]])
+ normal /= np.linalg.norm(normal)
+ min_a, max_a = project_polygon(a, normal)
+ min_b, max_b = project_polygon(b, normal)
+
+ if interval_distance(min_a, max_a, min_b, max_b) > 0:
+ intersecting = False
+
+ velocity_projection = normal.dot(displacement_a - displacement_b)
+ if velocity_projection < 0:
+ min_a += velocity_projection
+ else:
+ max_a += velocity_projection
+
+ distance = interval_distance(min_a, max_a, min_b, max_b)
+ if distance > 0:
+ will_intersect = False
+ if not intersecting and not will_intersect:
+ break
+ if abs(distance) < min_distance:
+ min_distance = abs(distance)
+ d = a[:-1].mean(axis=0) - b[:-1].mean(axis=0) # center difference
+ translation_axis = normal if d.dot(normal) > 0 else -normal
+
+ if will_intersect:
+ translation = min_distance * translation_axis
+ return intersecting, will_intersect, translation
+
+
+def confidence_ellipsoid(data: Dict[str, np.ndarray], lambda_: float = 1e-5, delta: float = 0.1, sigma: float = 0.1,
+ param_bound: float = 1.0) -> Tuple[np.ndarray, np.ndarray, float]:
+ """
+ Compute a confidence ellipsoid over the parameter theta, where y = theta^T phi
+
+ :param data: a dictionary {"features": [phi_0,...,phi_N], "outputs": [y_0,...,y_N]}
+ :param lambda_: l2 regularization parameter
+ :param delta: confidence level
+ :param sigma: noise covariance
+ :param param_bound: an upper-bound on the parameter norm
+ :return: estimated theta, Gramian matrix G_N_lambda, radius beta_N
+ """
+ phi = np.array(data["features"])
+ y = np.array(data["outputs"])
+ g_n_lambda = 1/sigma * np.transpose(phi) @ phi + lambda_ * np.identity(phi.shape[-1])
+ theta_n_lambda = np.linalg.inv(g_n_lambda) @ np.transpose(phi) @ y / sigma
+ d = theta_n_lambda.shape[0]
+ beta_n = np.sqrt(2*np.log(np.sqrt(np.linalg.det(g_n_lambda) / lambda_ ** d) / delta)) + \
+ np.sqrt(lambda_*d) * param_bound
+ return theta_n_lambda, g_n_lambda, beta_n
+
+
+def confidence_polytope(data: dict, parameter_box: np.ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray, float]:
+ """
+ Compute a confidence polytope over the parameter theta, where y = theta^T phi
+
+ :param data: a dictionary {"features": [phi_0,...,phi_N], "outputs": [y_0,...,y_N]}
+ :param parameter_box: a box [theta_min, theta_max] containing the parameter theta
+ :return: estimated theta, polytope vertices, Gramian matrix G_N_lambda, radius beta_N
+ """
+ param_bound = np.amax(np.abs(parameter_box))
+ theta_n_lambda, g_n_lambda, beta_n = confidence_ellipsoid(data, param_bound=param_bound)
+
+ values, pp = np.linalg.eig(g_n_lambda)
+ radius_matrix = np.sqrt(beta_n) * np.linalg.inv(pp) @ np.diag(np.sqrt(1 / values))
+ h = np.array(list(itertools.product([-1, 1], repeat=theta_n_lambda.shape[0])))
+ d_theta = np.array([radius_matrix @ h_k for h_k in h])
+
+ # Clip the parameter and confidence region within the prior parameter box.
+ theta_n_lambda = np.clip(theta_n_lambda, parameter_box[0], parameter_box[1])
+ for k, _ in enumerate(d_theta):
+ d_theta[k] = np.clip(d_theta[k], parameter_box[0] - theta_n_lambda, parameter_box[1] - theta_n_lambda)
+ return theta_n_lambda, d_theta, g_n_lambda, beta_n
+
+
+def is_valid_observation(y: np.ndarray, phi: np.ndarray, theta: np.ndarray, gramian: np.ndarray,
+ beta: float, sigma: float = 0.1) -> bool:
+ """
+ Check if a new observation (phi, y) is valid according to a confidence ellipsoid on theta.
+
+ :param y: observation
+ :param phi: feature
+ :param theta: estimated parameter
+ :param gramian: Gramian matrix
+ :param beta: ellipsoid radius
+ :param sigma: noise covariance
+ :return: validity of the observation
+ """
+ y_hat = np.tensordot(theta, phi, axes=[0, 0])
+ error = np.linalg.norm(y - y_hat)
+ eig_phi, _ = np.linalg.eig(phi.transpose() @ phi)
+ eig_g, _ = np.linalg.eig(gramian)
+ error_bound = np.sqrt(np.amax(eig_phi) / np.amin(eig_g)) * beta + sigma
+ return error < error_bound
+
+
+def is_consistent_dataset(data: dict, parameter_box: np.ndarray = None) -> bool:
+ """
+ Check whether a dataset {phi_n, y_n} is consistent
+
+ The last observation should be in the confidence ellipsoid obtained by the N-1 first observations.
+
+ :param data: a dictionary {"features": [phi_0,...,phi_N], "outputs": [y_0,...,y_N]}
+ :param parameter_box: a box [theta_min, theta_max] containing the parameter theta
+ :return: consistency of the dataset
+ """
+ train_set = copy.deepcopy(data)
+ y, phi = train_set["outputs"].pop(-1), train_set["features"].pop(-1)
+ y, phi = np.array(y)[..., np.newaxis], np.array(phi)[..., np.newaxis]
+ if train_set["outputs"] and train_set["features"]:
+ theta, _, gramian, beta = confidence_polytope(train_set, parameter_box=parameter_box)
+ return is_valid_observation(y, phi, theta, gramian, beta)
+ else:
+ return True
+
+
+def near_split(x, num_bins=None, size_bins=None):
+ """
+ Split a number into several bins with near-even distribution.
+
+ You can either set the number of bins, or their size.
+ The sum of bins always equals the total.
+ :param x: number to split
+ :param num_bins: number of bins
+ :param size_bins: size of bins
+ :return: list of bin sizes
+ """
+ if num_bins:
+ quotient, remainder = divmod(x, num_bins)
+ return [quotient + 1] * remainder + [quotient] * (num_bins - remainder)
+ elif size_bins:
+ return near_split(x, num_bins=int(np.ceil(x / size_bins)))
+
+
+def distance_to_circle(center, radius, direction):
+ scaling = radius * np.ones((2, 1))
+ a = np.linalg.norm(direction / scaling) ** 2
+ b = -2 * np.dot(np.transpose(center), direction / np.square(scaling))
+ c = np.linalg.norm(center / scaling) ** 2 - 1
+ root_inf, root_sup = solve_trinom(a, b, c)
+ if root_inf and root_inf > 0:
+ distance = root_inf
+ elif root_sup and root_sup > 0:
+ distance = 0
+ else:
+ distance = np.infty
+ return distance
+
+
+def distance_to_rect(line: Tuple[np.ndarray, np.ndarray], rect: List[np.ndarray]):
+ """
+ Compute the intersection between a line segment and a rectangle.
+
+ See https://math.stackexchange.com/a/2788041.
+ :param line: a line segment [R, Q]
+ :param rect: a rectangle [A, B, C, D]
+ :return: the distance between R and the intersection of the segment RQ with the rectangle ABCD
+ """
+ r, q = line
+ a, b, c, d = rect
+ u = b - a
+ v = d - a
+ u, v = u/np.linalg.norm(u), v/np.linalg.norm(v)
+ rqu = (q - r) @ u
+ rqv = (q - r) @ v
+ interval_1 = [(a - r) @ u / rqu, (b - r) @ u / rqu]
+ interval_2 = [(a - r) @ v / rqv, (d - r) @ v / rqv]
+ interval_1 = interval_1 if rqu >= 0 else list(reversed(interval_1))
+ interval_2 = interval_2 if rqv >= 0 else list(reversed(interval_2))
+ if interval_distance(*interval_1, *interval_2) <= 0 \
+ and interval_distance(0, 1, *interval_1) <= 0 \
+ and interval_distance(0, 1, *interval_2) <= 0:
+ return max(interval_1[0], interval_2[0]) * np.linalg.norm(q - r)
+ else:
+ return np.inf
+
+
+def solve_trinom(a, b, c):
+ delta = b ** 2 - 4 * a * c
+ if delta >= 0:
+ return (-b - np.sqrt(delta)) / (2 * a), (-b + np.sqrt(delta)) / (2 * a)
+ else:
+ return None, None
\ No newline at end of file
--
GitLab