torchdrivesim.infractions ========================= .. py:module:: torchdrivesim.infractions Attributes ---------- .. autoapisummary:: torchdrivesim.infractions.logger torchdrivesim.infractions.LANELET_TAGS_TO_EXCLUDE Functions --------- .. autoapisummary:: torchdrivesim.infractions.point_mesh_face_distance torchdrivesim.infractions.point_to_mesh_distance_pt torchdrivesim.infractions.offroad_infraction_loss torchdrivesim.infractions.lanelet_orientation_loss torchdrivesim.infractions.iou_differentiable torchdrivesim.infractions.compute_agent_collisions_metric_pytorch3d torchdrivesim.infractions.compute_agent_collisions_metric torchdrivesim.infractions.bbox2discs torchdrivesim.infractions.get_all_intersections torchdrivesim.infractions.rectangle_vertices torchdrivesim.infractions.collision_detection_with_discs Module Contents --------------- .. py:data:: logger :value: None .. py:data:: LANELET_TAGS_TO_EXCLUDE :value: ['parking'] .. py:function:: point_mesh_face_distance(meshes: pytorch3d.structures.Meshes, pcls: pytorch3d.structures.Pointclouds, reduction: str = 'sum', weighted: bool = False, threshold: float = 0) -> torch.Tensor Computes the distance between a pointcloud and a mesh, defined as the L2 distance from each point to the closest face in the mesh, reduced across the points in the cloud. Runs in batch mode for both mesh and pointcloud. :param meshes: a batch of B meshes :param pcls: a batch of B pointcouds :param reduction: 'none' | 'sum' | 'mean' | 'min' | 'max' :param weighted: weight by the inverse of number of points in the pointcloud :param threshold: reduce result by this amount and clip at zero from below :returns: BxP tensor if reduction is 'none', else Bx1 tensor .. py:function:: point_to_mesh_distance_pt(points: torch.Tensor, tris: torch.Tensor, threshold: float = 0) Computes the distance between points and mesh triangles, defined as the L2 distance from each point to the closest face in the mesh. This function only uses native Pytorch operations and is an alternative version of the function `point_mesh_face_distance` that relies on Pytorch3D. :param points: Bx3 tensor of points :param tris: BxFx3x3 tensor of mesh triangles :param threshold: reduce result by this amount and clip at zero from below :returns: Bx1 tensor .. py:function:: offroad_infraction_loss(agent_states: torch.Tensor, lenwid: torch.Tensor, driving_surface_mesh: Union[pytorch3d.structures.Meshes, torchdrivesim.mesh.BaseMesh], threshold: float = 0, use_pytorch3d: Optional[bool] = None) -> torch.Tensor Calculates off-road infraction loss, defined as the sum of thresholded distances from agent corners to the driving surface mesh. :param agent_states: BxAx4 tensor of agent states x,y,orientation,speed :param lenwid: BxAx2 tensor providing length and width of each agent :param driving_surface_mesh: a batch of B meshes defining driving surface :param threshold: if given, the distance of each corner is thresholded using this value :param use_pytorch3d: whether to use the optimized and differentiable pytorch3d code, or the simpler pure pytorch implementation - defaults to true iff pytorch3d is installed :returns: BxA tensor of offroad losses for each agent .. py:function:: lanelet_orientation_loss(lanelet_maps: List[Optional[torchdrivesim.lanelet2.LaneletMap]], agents_state: torch.Tensor, recenter_offset: Optional[torch.Tensor] = None, direction_angle_threshold: float = np.pi / 2, lanelet_dist_tolerance: float = 1.0) -> torch.Tensor Calculate a loss value of the agent orientation regarding to the direction of the lanelet segment. The loss is computed by finding the direction of the lanelet the agent is currently on, and then taking -cos(δ) given δ is the orientation difference between the lanelet orientation and the agent orientation, unless abs(δ) is smaller or equal to `direction_angle_threshold`, in which case the loss is 0. This normalizes the loss range to [0,1]. In case it is ambiguous which lanelet the agent is on, the one best matching agent orientation is on. :param lanelet_maps: a list of B maps; None is means orientation loss won't be computed for that batch element :param agents_state: BxAx4 tensor of agent states x,t,orientation,speed :param recenter_offset: Bx2 tensor that needs to be added to agent coordinates to match them to map :param direction_angle_threshold: minimal angle between the lane direction and agent direction that's considered an infraction (needs to be at least pi / 2) :param lanelet_dist_tolerance: how far away from a lanelet the agent can be to still be considered inside :returns: BxA tensor of orientation losses for all agents .. py:function:: iou_differentiable(box1: torch.Tensor, box2: torch.Tensor, fast: bool = True) -> torch.Tensor Computes an approximate, differentiable IoU between two oriented bounding boxes. Accepts multiple batch dimensions. :param box1: BxAx5 tensor x,y,length,width,orientation :param box2: BxAx5 tensor :param fast: whether to use faster but less accurate method :returns: tensor of shape (B,A) with IoU values .. py:function:: compute_agent_collisions_metric_pytorch3d(all_rects: torch.Tensor, masks: torch.Tensor) -> torch.Tensor Computes the number of collisions per agent in batch. :param all_rects: BxAx5 tensor of oriented rectangles x,y,length,width,orientation :param masks: BxA boolean tensor, indicating whether to compute collisions for this agent :returns: BxA tensor with collision counts per agent .. py:function:: compute_agent_collisions_metric(all_rects: numpy.ndarray, collision_masks: numpy.ndarray, present_masks: numpy.ndarray) -> numpy.ndarray Returns the number of collisions per agent in a batch. :param all_rects: BxAx5 array of oriented rectangles x,y,length,width,orientation :param collision_masks: BxA boolean array, indicating whether to compute collisions for this agent :param present_masks: BxA boolean array, indicating which rectangles are not padding :returns: BxA array with collision counts per agent .. py:function:: bbox2discs(box: torch.Tensor, num_discs: int = 5, backend: str = 'torch') -> Tuple[torch.Tensor, torch.Tensor] Converts a bounding box to the specified number of equally spaced discs with radius half the width. B is the batch size. A bounding box is described by 5 values in this order: (x, y, length, width, orientation).) :param box: Bx5 tensor or bounding boxes :param num_discs: The number of discs used to represent a vehicle. Must be a positive odd number. (default: 5) :param backend: Either torch or numpy. (default: 'torch') :returns: A tuple (agent_disc_center, agent_r) where agent_disc_center is the coordinates of the center of each disc (B, num_discs, 2) and agent_r is the radius of the discs (B, 1) .. py:function:: get_all_intersections(rects: numpy.ndarray, ego_idx: Optional[int] = None) -> numpy.ndarray Calculates an upper triangular matrix of size equal to the number of rotated rectangles which indicating whether one rectangle intersects with another. :param rects: (agents_count x 5) numpy array :param ego_idx: Optional ego index for ego only mode :returns: An upper triangular matrix of size equal to the number of rotated rectangles .. py:function:: rectangle_vertices(cx: numpy.ndarray, cy: numpy.ndarray, w: numpy.ndarray, h: numpy.ndarray, angle: numpy.ndarray) -> numpy.ndarray Calculate the 4 corners of the rotated rectangles given the center point, the size and the orientation. :param cx: Bx1 array with x coordinate of the center point :param cy: Bx1 array with y coordinate of the center point :param w: Bx1 array with width of the rectangle :param h: Bx1 array with height of the rectangle :param angle: Bx1 array with yaw angle of the rectangle :returns: Bx4 corners of the rectangle .. py:function:: collision_detection_with_discs(box1: torch.Tensor, box2: torch.Tensor, num_discs: int = 5, backend: str = 'torch') Calculate the differentiable collision loss as described in the paper TrafficSim. Accepts multiple batch dimensions. A bounding box is described by 5 values in this order: (x, y, length, width, orientation) :param box1: BxAx5 tensor :param box2: BxAx5 tensor :param num_discs: The number of discs used to represent a vehicle. Must be a positive odd number. :param backend: Either torch or numpy. :returns: tensor of shape (B,A) with collision values between corresponding agents