SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

fix(perception): stabilize replay body frame

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-05 20:04:08 +03:00
parent de19229895
commit c70ad345ea
12 changed files with 783 additions and 256 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/perception/threat.py
+349 -101
View File
@@ -26,11 +26,9 @@ from .contracts import (
)
from .geometry_math import quaternion_xyzw_to_rotation_matrix
REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v1"
REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v1"
DEFAULT_REPLAY_THREAT_PROFILE_PATH: Final = (
"config/perception/m4-replay-threat-v1.json"
)
REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v2"
REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v2"
DEFAULT_REPLAY_THREAT_PROFILE_PATH: Final = "config/perception/m4-replay-threat-v2.json"
class ReplayThreatError(ValueError):
@@ -38,72 +36,242 @@ class ReplayThreatError(ValueError):
@dataclass(frozen=True, slots=True)
class ReplayPose:
class ReplayBodyFrame:
frame_id: str
position_map_xyz_m: tuple[float, float, float]
orientation_map_from_lidar_xyzw: tuple[float, float, float, float]
origin_map_xyz_m: tuple[float, float, float]
basis_map_from_body: tuple[
tuple[float, float, float],
tuple[float, float, float],
tuple[float, float, float],
]
sensor_height_m: float
surface_slope_deg: float
forward_source: str
camera_forward_alignment_deg: float
def __post_init__(self) -> None:
if not self.frame_id:
raise ReplayThreatError("replay pose frame id is empty")
raise ReplayThreatError("replay body frame id is empty")
if (
len(self.position_map_xyz_m) != 3
or len(self.orientation_map_from_lidar_xyzw) != 4
len(self.origin_map_xyz_m) != 3
or len(self.basis_map_from_body) != 3
or any(len(row) != 3 for row in self.basis_map_from_body)
or not all(
math.isfinite(value)
for value in (
*self.position_map_xyz_m,
*self.orientation_map_from_lidar_xyzw,
*self.origin_map_xyz_m,
*(value for row in self.basis_map_from_body for value in row),
self.sensor_height_m,
self.surface_slope_deg,
self.camera_forward_alignment_deg,
)
)
):
raise ReplayThreatError("replay pose is not finite")
norm = math.sqrt(sum(value * value for value in self.orientation_map_from_lidar_xyzw))
if norm < 1e-9:
raise ReplayThreatError("replay pose orientation has no usable norm")
raise ReplayThreatError("replay body frame is not finite")
columns = tuple(
tuple(self.basis_map_from_body[row][column] for row in range(3)) for column in range(3)
)
if (
not self.forward_source
or self.sensor_height_m <= 0.0
or self.surface_slope_deg < 0.0
or self.camera_forward_alignment_deg < 0.0
or any(abs(_dot(column, column) - 1.0) > 1e-6 for column in columns)
or any(
abs(_dot(columns[first], columns[second])) > 1e-6
for first, second in ((0, 1), (0, 2), (1, 2))
)
or _dot(_cross(columns[0], columns[1]), columns[2]) < 1.0 - 1e-6
):
raise ReplayThreatError("replay body frame basis is invalid")
def map_point_to_body(
self,
point_map_xyz_m: tuple[float, float, float],
) -> tuple[float, float, float]:
rotation = quaternion_xyzw_to_rotation_matrix(
self.orientation_map_from_lidar_xyzw
)
delta = tuple(
point_map_xyz_m[index] - self.position_map_xyz_m[index]
for index in range(3)
)
delta = tuple(point_map_xyz_m[index] - self.origin_map_xyz_m[index] for index in range(3))
return self.map_vector_to_body(delta)
def map_vector_to_body(
self,
vector_map_xyz_m: tuple[float, float, float],
) -> tuple[float, float, float]:
values = tuple(
float(sum(delta[row] * rotation[row, column] for row in range(3)))
float(
sum(
vector_map_xyz_m[row] * self.basis_map_from_body[row][column]
for row in range(3)
)
)
for column in range(3)
)
return values[0], values[1], values[2]
class ReplayPoseResolver(Protocol):
def pose_for_frame(self, frame_id: str) -> ReplayPose | None: ...
class ReplayBodyFrameResolver(Protocol):
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame | None: ...
class RecordedReplayPoseResolver:
"""Adapt the verified geometry store to the source-neutral pose seam."""
class RecordedReplayBodyFrameResolver:
"""Derive a ground-level virtual body frame from verified replay evidence."""
def __init__(self, store: object) -> None:
method = getattr(store, "pose_values_for_frame", None)
def __init__(self, store: object, *, profile: VirtualBodyFrameProfile) -> None:
method = getattr(store, "replay_body_frame_inputs", None)
if not callable(method):
raise ReplayThreatError("recorded pose store does not expose verified poses")
self._pose_values_for_frame = method
raise ReplayThreatError("recorded geometry store lacks body-frame evidence")
available = getattr(store, "available_frame_indices", None)
if not callable(available):
raise ReplayThreatError("recorded geometry store lacks availability evidence")
self._inputs_for_frame = method
self._available_frame_indices = available
self.profile = profile
self._cache: dict[str, tuple[ReplayBodyFrame | None, str]] = {}
def pose_for_frame(self, frame_id: str) -> ReplayPose | None:
values = self._pose_values_for_frame(frame_id)
if values is None:
return None
position, orientation = values
return ReplayPose(
frame_id=frame_id,
position_map_xyz_m=position,
orientation_map_from_lidar_xyzw=orientation,
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame | None:
return self._resolve(frame_id)[0]
def qualified_frame_indices(self) -> tuple[int, ...]:
return tuple(
index
for index in self._available_frame_indices()
if self.body_frame_for_frame(f"frame-{index:06d}") is not None
)
def qualification_summary(self) -> dict[str, object]:
indices = self._available_frame_indices()
for index in indices:
self._resolve(f"frame-{index:06d}")
reasons: dict[str, int] = {}
frames: list[ReplayBodyFrame] = []
for frame, reason in self._cache.values():
reasons[reason] = reasons.get(reason, 0) + 1
if frame is not None:
frames.append(frame)
alignments = sorted(item.camera_forward_alignment_deg for item in frames)
return {
"available": len(indices),
"qualified": len(frames),
"rejected": len(indices) - len(frames),
"reason_counts": dict(sorted(reasons.items())),
"camera_forward_alignment_deg": {
"maximum": max(alignments) if alignments else None,
"p95": _percentile(alignments, 0.95),
},
"origin": self.profile.origin,
"up": self.profile.up,
"forward": self.profile.forward,
}
def _resolve(self, frame_id: str) -> tuple[ReplayBodyFrame | None, str]:
cached = self._cache.get(frame_id)
if cached is not None:
return cached
inputs = self._inputs_for_frame(
frame_id,
trajectory_half_window_frames=self.profile.trajectory_half_window_frames,
)
if inputs is None:
return self._store(frame_id, None, "source-or-surface-unavailable")
position = tuple(float(value) for value in inputs.sensor_position_map)
plane = tuple(float(value) for value in inputs.ground_plane_coefficients_map)
normal_norm = math.sqrt(sum(value * value for value in plane[:3]))
if normal_norm < 1e-9:
return self._store(frame_id, None, "ground-normal-invalid")
ground_normal = tuple(value / normal_norm for value in plane[:3])
if ground_normal[2] < 0.0:
ground_normal = tuple(-value for value in ground_normal)
plane = tuple(-value for value in plane)
sensor_height = _dot(position, ground_normal) + plane[3] / normal_norm
if (
abs(sensor_height - inputs.sensor_height_m) > 0.05
or abs(sensor_height - self.profile.nominal_sensor_height_m)
> self.profile.maximum_sensor_height_deviation_m
):
return self._store(frame_id, None, "sensor-height-unqualified")
if inputs.surface_slope_deg > self.profile.maximum_surface_slope_deg:
return self._store(frame_id, None, "surface-slope-unqualified")
# The collision corridor lives in a gravity-stable base_footprint frame.
# Local terrain locates that footprint vertically but must not rotate the
# SLAM world when a handheld or mounted sensor rolls and pitches.
up = (0.0, 0.0, 1.0)
vertical_denominator = _dot(ground_normal, up)
if vertical_denominator < 1e-6:
return self._store(frame_id, None, "ground-projection-invalid")
vertical_height = sensor_height / vertical_denominator
rotation = quaternion_xyzw_to_rotation_matrix(inputs.sensor_orientation_map_from_lidar_xyzw)
calibration = inputs.t_camera_from_lidar
camera_forward_lidar = tuple(float(calibration[2, index]) for index in range(3))
camera_forward_map = tuple(
float(sum(rotation[row, column] * camera_forward_lidar[column] for column in range(3)))
for row in range(3)
)
camera_forward = _normalize(_reject(camera_forward_map, up))
if camera_forward is None:
return self._store(frame_id, None, "camera-forward-invalid")
route = tuple(
float(
inputs.trajectory_end_position_map[index]
- inputs.trajectory_start_position_map[index]
)
for index in range(3)
)
route_on_ground = _reject(route, up)
if (
math.sqrt(_dot(route_on_ground, route_on_ground))
>= self.profile.minimum_trajectory_displacement_m
):
forward = _normalize(route_on_ground)
assert forward is not None
forward_source = "smoothed-trajectory-tangent"
alignment = _angle_degrees(forward, camera_forward)
if alignment > self.profile.maximum_camera_route_misalignment_deg:
return self._store(frame_id, None, "camera-route-misaligned")
else:
forward = camera_forward
forward_source = "calibrated-camera-forward-fallback"
alignment = 0.0
left = _normalize(_cross(up, forward))
if left is None:
return self._store(frame_id, None, "body-left-invalid")
forward = _normalize(_cross(left, up))
assert forward is not None
origin = tuple(position[index] - vertical_height * up[index] for index in range(3))
basis = tuple((forward[row], left[row], up[row]) for row in range(3))
frame = ReplayBodyFrame(
frame_id=frame_id,
origin_map_xyz_m=origin,
basis_map_from_body=basis,
sensor_height_m=sensor_height,
surface_slope_deg=float(inputs.surface_slope_deg),
forward_source=forward_source,
camera_forward_alignment_deg=alignment,
)
return self._store(frame_id, frame, "qualified")
def _store(
self,
frame_id: str,
frame: ReplayBodyFrame | None,
reason: str,
) -> tuple[ReplayBodyFrame | None, str]:
value = (frame, reason)
self._cache[frame_id] = value
return value
@dataclass(frozen=True, slots=True)
class VirtualBodyFrameProfile:
schema_version: str
origin: str
up: str
forward: str
trajectory_half_window_frames: int
minimum_trajectory_displacement_m: float
maximum_camera_route_misalignment_deg: float
maximum_sensor_height_deviation_m: float
maximum_surface_slope_deg: float
nominal_sensor_height_m: float
@dataclass(frozen=True, slots=True)
class VirtualRigProfile:
@@ -141,6 +309,7 @@ class ReplayThreatProfile:
source_pack_sha256: str
calibration_id: str
calibration_content_sha256: str
body_frame: VirtualBodyFrameProfile
rig: VirtualRigProfile
corridor: VirtualCorridorProfile
profile_sha256: str
@@ -154,12 +323,12 @@ class DualEvidenceReplayThreatProvider:
def __init__(
self,
*,
pose_resolver: ReplayPoseResolver,
body_frame_resolver: ReplayBodyFrameResolver,
profile: ReplayThreatProfile,
) -> None:
if profile.provider_id != self.provider_id:
raise ReplayThreatError("threat provider identity changed")
self.pose_resolver = pose_resolver
self.body_frame_resolver = body_frame_resolver
self.profile = profile
def assess(self, obstacle_map: LocalObstacleMap) -> tuple[ThreatAssessment, ...]:
@@ -168,9 +337,9 @@ class DualEvidenceReplayThreatProvider:
or obstacle_map.session_id != self.profile.session_id
):
raise ReplayThreatError("obstacle map escaped the threat profile")
pose = self.pose_resolver.pose_for_frame(obstacle_map.frame_id)
body_frame = self.body_frame_resolver.body_frame_for_frame(obstacle_map.frame_id)
assessments = [
self._metric_or_stale(obstacle_map.frame_id, obstacle, pose)
self._metric_or_stale(obstacle_map.frame_id, obstacle, body_frame)
for obstacle in (*obstacle_map.occupied, *obstacle_map.unknown)
]
assessments.extend(
@@ -183,7 +352,7 @@ class DualEvidenceReplayThreatProvider:
self,
frame_id: str,
obstacle: TemporalObstacle,
pose: ReplayPose | None,
body_frame: ReplayBodyFrame | None,
) -> ThreatAssessment:
if obstacle.state is not TemporalState.CURRENT:
return self._unknown(
@@ -191,16 +360,16 @@ class DualEvidenceReplayThreatProvider:
obstacle.component_id,
("stale-evidence", f"temporal-state-{obstacle.state.value}"),
)
if pose is None or obstacle.last_centroid_xyz_m is None or not obstacle.cells:
if body_frame is None or obstacle.last_centroid_xyz_m is None or not obstacle.cells:
return self._unknown(
frame_id,
obstacle.component_id,
("current-pose-or-metric-geometry-unavailable",),
)
centroid_body = pose.map_point_to_body(obstacle.last_centroid_xyz_m)
centroid_body = body_frame.map_point_to_body(obstacle.last_centroid_xyz_m)
cells_body = tuple(
pose.map_point_to_body(
body_frame.map_point_to_body(
(
(cell.x + 0.5) * self.profile.corridor.occupied_voxel_size_m,
(cell.y + 0.5) * self.profile.corridor.occupied_voxel_size_m,
@@ -209,7 +378,7 @@ class DualEvidenceReplayThreatProvider:
)
for cell in obstacle.cells
)
velocity_body = self._relative_velocity_body(obstacle, pose)
velocity_body = self._relative_velocity_body(obstacle, body_frame)
corridor_entry = _first_corridor_entry_seconds(
cells_body,
velocity_body,
@@ -221,9 +390,7 @@ class DualEvidenceReplayThreatProvider:
rig=self.profile.rig,
corridor=self.profile.corridor,
)
motion_complete = (
obstacle.motion is not MotionState.UNKNOWN and velocity_body is not None
)
motion_complete = obstacle.motion is not MotionState.UNKNOWN and velocity_body is not None
if current_intersection or (motion_complete and corridor_entry is not None):
intersection = CorridorIntersection.INTERSECTS
decision = ThreatDecision.THREAT
@@ -281,7 +448,7 @@ class DualEvidenceReplayThreatProvider:
def _relative_velocity_body(
self,
obstacle: TemporalObstacle,
current_pose: ReplayPose,
current_body_frame: ReplayBodyFrame,
) -> tuple[float, float] | None:
if len(obstacle.history) < 2:
return None
@@ -290,29 +457,25 @@ class DualEvidenceReplayThreatProvider:
span_seconds = (last.evidence_time_ns - first.evidence_time_ns) / 1_000_000_000
if span_seconds < self.profile.corridor.minimum_motion_span_seconds:
return None
first_pose = self.pose_resolver.pose_for_frame(first.frame_id)
last_pose = self.pose_resolver.pose_for_frame(last.frame_id)
if first_pose is None or last_pose is None or last.frame_id != current_pose.frame_id:
first_body_frame = self.body_frame_resolver.body_frame_for_frame(first.frame_id)
last_body_frame = self.body_frame_resolver.body_frame_for_frame(last.frame_id)
if (
first_body_frame is None
or last_body_frame is None
or last.frame_id != current_body_frame.frame_id
):
return None
obstacle_delta = tuple(
last.centroid_xyz_m[index] - first.centroid_xyz_m[index]
for index in range(3)
last.centroid_xyz_m[index] - first.centroid_xyz_m[index] for index in range(3)
)
rig_delta = tuple(
last_pose.position_map_xyz_m[index] - first_pose.position_map_xyz_m[index]
last_body_frame.origin_map_xyz_m[index] - first_body_frame.origin_map_xyz_m[index]
for index in range(3)
)
relative_map = tuple(
(obstacle_delta[index] - rig_delta[index]) / span_seconds
for index in range(3)
)
rotation = quaternion_xyzw_to_rotation_matrix(
current_pose.orientation_map_from_lidar_xyzw
)
body = tuple(
float(sum(relative_map[row] * rotation[row, column] for row in range(3)))
for column in range(3)
(obstacle_delta[index] - rig_delta[index]) / span_seconds for index in range(3)
)
body = current_body_frame.map_vector_to_body(relative_map)
return body[0], body[1]
def _camera_only(self, frame_id: str, proposal_id: str) -> ThreatAssessment:
@@ -359,6 +522,7 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
"provider_id",
"source",
"calibration",
"body_frame",
"virtual_rig",
"corridor",
"policy",
@@ -373,6 +537,7 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
raise ReplayThreatError("replay threat profile identity is incompatible")
source = _object(document["source"], "threat source")
calibration = _object(document["calibration"], "threat calibration")
body_frame = _object(document["body_frame"], "virtual body frame")
rig = _object(document["virtual_rig"], "virtual rig")
corridor = _object(document["corridor"], "virtual corridor")
policy = _object(document["policy"], "threat policy")
@@ -398,6 +563,21 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
{"calibration_id", "content_identity_sha256", "usage"},
"threat calibration",
)
_exact_keys(
body_frame,
{
"schema_version",
"origin",
"up",
"forward",
"trajectory_half_window_frames",
"minimum_trajectory_displacement_m",
"maximum_camera_route_misalignment_deg",
"maximum_sensor_height_deviation_m",
"maximum_surface_slope_deg",
},
"virtual body frame",
)
_exact_keys(
rig,
{
@@ -448,7 +628,11 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
"threat authority",
)
if (
calibration.get("usage") != "projection-binding-only"
calibration.get("usage") != "projection-and-forward-axis-binding"
or body_frame.get("schema_version") != "missioncore.replay-body-frame-profile/v1"
or body_frame.get("origin") != "local-surface-vertical-projection"
or body_frame.get("up") != "vendor-slam-map-gravity-axis"
or body_frame.get("forward") != "smoothed-slam-trajectory-validated-by-camera-axis"
or rig.get("physical_mount_claimed") is not False
or policy
!= {
@@ -477,18 +661,34 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
lidar_reference=_string(rig, "lidar_reference"),
nominal_sensor_height_m=_positive_number(rig, "nominal_sensor_height_m"),
)
body_frame_profile = VirtualBodyFrameProfile(
schema_version=_string(body_frame, "schema_version"),
origin=_string(body_frame, "origin"),
up=_string(body_frame, "up"),
forward=_string(body_frame, "forward"),
trajectory_half_window_frames=_positive_integer(
body_frame, "trajectory_half_window_frames"
),
minimum_trajectory_displacement_m=_positive_number(
body_frame, "minimum_trajectory_displacement_m"
),
maximum_camera_route_misalignment_deg=_positive_number(
body_frame, "maximum_camera_route_misalignment_deg"
),
maximum_sensor_height_deviation_m=_positive_number(
body_frame, "maximum_sensor_height_deviation_m"
),
maximum_surface_slope_deg=_positive_number(body_frame, "maximum_surface_slope_deg"),
nominal_sensor_height_m=virtual_rig.nominal_sensor_height_m,
)
virtual_corridor = VirtualCorridorProfile(
profile_id=_string(corridor, "profile_id"),
forward_length_m=_positive_number(corridor, "forward_length_m"),
rear_margin_m=_nonnegative_number(corridor, "rear_margin_m"),
lateral_clearance_m=_nonnegative_number(corridor, "lateral_clearance_m"),
prediction_horizon_seconds=_positive_number(
corridor, "prediction_horizon_seconds"
),
prediction_horizon_seconds=_positive_number(corridor, "prediction_horizon_seconds"),
occupied_voxel_size_m=_positive_number(corridor, "occupied_voxel_size_m"),
minimum_motion_span_seconds=_positive_number(
corridor, "minimum_motion_span_seconds"
),
minimum_motion_span_seconds=_positive_number(corridor, "minimum_motion_span_seconds"),
)
if virtual_rig.lidar_reference != "virtual-body-center":
raise ReplayThreatError("virtual LiDAR reference is unsupported")
@@ -515,6 +715,7 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
source_pack_sha256=_string(source, "source_pack_sha256"),
calibration_id=_string(calibration, "calibration_id"),
calibration_content_sha256=_string(calibration, "content_identity_sha256"),
body_frame=body_frame_profile,
rig=virtual_rig,
corridor=virtual_corridor,
profile_sha256=hashlib.sha256(raw).hexdigest(),
@@ -575,10 +776,7 @@ def _first_corridor_entry_seconds(
return None
expansion = corridor.occupied_voxel_size_m * math.sqrt(2) / 2
bounds = _corridor_bounds(rig, corridor, expansion_m=expansion)
entries = (
_ray_box_entry((point[0], point[1]), velocity_body, bounds)
for point in cells_body
)
entries = (_ray_box_entry((point[0], point[1]), velocity_body, bounds) for point in cells_body)
valid = [
entry
for entry in entries
@@ -602,8 +800,7 @@ def _first_body_entry_seconds(
valid = [
entry
for point in cells_body
if (entry := _ray_box_entry((point[0], point[1]), velocity_body, bounds))
is not None
if (entry := _ray_box_entry((point[0], point[1]), velocity_body, bounds)) is not None
and entry <= horizon_seconds
]
return None if not valid else round(min(valid), 12)
@@ -652,10 +849,7 @@ def _closest_body_clearance_m(
horizon_seconds,
max(
0.0,
-(
point[0] * velocity_body[0]
+ point[1] * velocity_body[1]
)
-(point[0] * velocity_body[0] + point[1] * velocity_body[1])
/ speed_squared,
),
)
@@ -665,12 +859,8 @@ def _closest_body_clearance_m(
(point[1], velocity_body[1], bounds[2], bounds[3]),
):
if abs(speed) > 1e-12:
candidates.add(
min(horizon_seconds, max(0.0, (lower - coordinate) / speed))
)
candidates.add(
min(horizon_seconds, max(0.0, (upper - coordinate) / speed))
)
candidates.add(min(horizon_seconds, max(0.0, (lower - coordinate) / speed)))
candidates.add(min(horizon_seconds, max(0.0, (upper - coordinate) / speed)))
velocity = velocity_body or (0.0, 0.0)
clearance = min(
_point_box_distance(
@@ -702,11 +892,7 @@ def _closing_speed_mps(
if distance < 1e-9:
return round(math.hypot(*velocity_body), 12)
return round(
-(
centroid_body[0] * velocity_body[0]
+ centroid_body[1] * velocity_body[1]
)
/ distance,
-(centroid_body[0] * velocity_body[0] + centroid_body[1] * velocity_body[1]) / distance,
12,
)
@@ -743,6 +929,67 @@ def _positive_number(document: dict[str, object], key: str) -> float:
return value
def _positive_integer(document: dict[str, object], key: str) -> int:
value = document.get(key)
if not isinstance(value, int) or isinstance(value, bool) or value <= 0:
raise ReplayThreatError(f"{key} must be a positive integer")
return value
def _dot(
first: tuple[float, float, float],
second: tuple[float, float, float],
) -> float:
return sum(first[index] * second[index] for index in range(3))
def _cross(
first: tuple[float, float, float],
second: tuple[float, float, float],
) -> tuple[float, float, float]:
return (
first[1] * second[2] - first[2] * second[1],
first[2] * second[0] - first[0] * second[2],
first[0] * second[1] - first[1] * second[0],
)
def _normalize(
value: tuple[float, float, float],
) -> tuple[float, float, float] | None:
norm = math.sqrt(_dot(value, value))
if norm < 1e-9:
return None
return tuple(item / norm for item in value)
def _reject(
value: tuple[float, float, float],
normal: tuple[float, float, float],
) -> tuple[float, float, float]:
along = _dot(value, normal)
return tuple(value[index] - along * normal[index] for index in range(3))
def _angle_degrees(
first: tuple[float, float, float],
second: tuple[float, float, float],
) -> float:
return math.degrees(math.acos(max(-1.0, min(1.0, _dot(first, second)))))
def _percentile(values: list[float], fraction: float) -> float | None:
if not values:
return None
position = fraction * (len(values) - 1)
lower = math.floor(position)
upper = math.ceil(position)
if lower == upper:
return values[lower]
weight = position - lower
return values[lower] * (1.0 - weight) + values[upper] * weight
def _nonnegative_number(document: dict[str, object], key: str) -> float:
value = _number(document, key)
if value < 0.0:
@@ -771,11 +1018,12 @@ __all__ = [
"DualEvidenceReplayThreatProvider",
"REPLAY_THREAT_PROFILE_SCHEMA",
"REPLAY_THREAT_PROVIDER_ID",
"RecordedReplayPoseResolver",
"ReplayPose",
"ReplayPoseResolver",
"RecordedReplayBodyFrameResolver",
"ReplayBodyFrame",
"ReplayBodyFrameResolver",
"ReplayThreatError",
"ReplayThreatProfile",
"VirtualBodyFrameProfile",
"VirtualCorridorProfile",
"VirtualRigProfile",
"load_replay_threat_profile",