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/geometry.py
+84 -7
View File
@@ -38,9 +38,7 @@ from .recorded_source import RECORDED_SOURCE_PACK_ID, RecordedFrameReference
GEOMETRY_PROFILE_SCHEMA: Final = "missioncore.geometry-association-profile/v1"
GEOMETRY_PROVIDER_ID: Final = "ravnoves00-geometry-association/v1"
DEFAULT_GEOMETRY_PROFILE_PATH: Final = Path(
"config/perception/m4-geometry-association-v1.json"
)
DEFAULT_GEOMETRY_PROFILE_PATH: Final = Path("config/perception/m4-geometry-association-v1.json")
FloatArray = npt.NDArray[np.float64]
UInt8Array = npt.NDArray[np.uint8]
@@ -85,6 +83,20 @@ class GeometryFrame:
return int(self.points_map.shape[0])
@dataclass(frozen=True, slots=True)
class ReplayBodyFrameInputs:
"""Verified inputs required to derive one replay-only virtual body frame."""
sensor_position_map: FloatArray
sensor_orientation_map_from_lidar_xyzw: FloatArray
ground_plane_coefficients_map: FloatArray
sensor_height_m: float
surface_slope_deg: float
trajectory_start_position_map: FloatArray
trajectory_end_position_map: FloatArray
t_camera_from_lidar: FloatArray
@dataclass(frozen=True, slots=True)
class GeometryProviderSnapshot:
input_frames: int
@@ -279,13 +291,78 @@ class RecordedGeometryStore:
),
)
def replay_body_frame_inputs(
self,
frame_id: str,
*,
trajectory_half_window_frames: int,
) -> ReplayBodyFrameInputs | None:
"""Return source-bound pose, surface and route evidence without inventing axes."""
if trajectory_half_window_frames < 1:
raise GeometryProviderError("trajectory half-window must be positive")
prefix = "frame-"
if not frame_id.startswith(prefix) or not frame_id[len(prefix) :].isdigit():
raise GeometryProviderError("replay body frame identity is invalid")
frame_index = int(frame_id[len(prefix) :])
if not 0 <= frame_index < self.profile.frame_count:
raise GeometryProviderError("replay body frame is outside the source profile")
if not bool(self._source["sample_available"][frame_index]) or not bool(
self._surface["frame_valid"][frame_index]
):
return None
required = {
"plane_coefficients_map",
"sensor_height_m",
"slope_deg",
}
if not required.issubset(self._surface):
raise GeometryProviderError("local surface lacks replay body-frame evidence")
first = max(0, frame_index - trajectory_half_window_frames)
last = min(self.profile.frame_count, frame_index + trajectory_half_window_frames + 1)
available = np.flatnonzero(self._source["sample_available"][first:last]) + first
if available.size == 0:
return None
values = ReplayBodyFrameInputs(
sensor_position_map=np.asarray(
self._source["pose_positions_map"][frame_index], dtype=np.float64
),
sensor_orientation_map_from_lidar_xyzw=np.asarray(
self._source["pose_quaternions_map_from_lidar"][frame_index],
dtype=np.float64,
),
ground_plane_coefficients_map=np.asarray(
self._surface["plane_coefficients_map"][frame_index],
dtype=np.float64,
),
sensor_height_m=float(self._surface["sensor_height_m"][frame_index]),
surface_slope_deg=float(self._surface["slope_deg"][frame_index]),
trajectory_start_position_map=np.asarray(
self._source["pose_positions_map"][int(available[0])], dtype=np.float64
),
trajectory_end_position_map=np.asarray(
self._source["pose_positions_map"][int(available[-1])], dtype=np.float64
),
t_camera_from_lidar=np.asarray(self._source["t_camera_from_lidar"], dtype=np.float64),
)
if not all(
np.isfinite(value).all()
for value in (
values.sensor_position_map,
values.sensor_orientation_map_from_lidar_xyzw,
values.ground_plane_coefficients_map,
values.trajectory_start_position_map,
values.trajectory_end_position_map,
values.t_camera_from_lidar,
)
) or not math.isfinite(values.sensor_height_m + values.surface_slope_deg):
raise GeometryProviderError("replay body-frame evidence is not finite")
return values
def available_frame_indices(self) -> tuple[int, ...]:
"""Expose the immutable availability partition for deterministic sampling."""
return tuple(
int(index)
for index in np.flatnonzero(self._source["sample_available"])
)
return tuple(int(index) for index in np.flatnonzero(self._source["sample_available"]))
def _validate(self) -> None:
source_required = {
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",
src/k1link/perception/threat_replay.py
+124 -80
View File
@@ -35,7 +35,6 @@ from .contracts import (
from .detector_replay_contracts import DetectorReplayResult
from .detector_replay_result import read_detector_replay_result
from .geometry import RecordedGeometryStore
from .geometry_math import quaternion_xyzw_to_rotation_matrix
from .geometry_replay import GeometryReplayResult, read_geometry_replay_result
from .providers import SourcePacket
from .recorded_source import RecordedRavnoves00Source, ReplayPacing
@@ -43,8 +42,8 @@ from .temporal_replay import TemporalReplayResult, read_temporal_replay_result
from .threat import (
DEFAULT_REPLAY_THREAT_PROFILE_PATH,
DualEvidenceReplayThreatProvider,
RecordedReplayPoseResolver,
ReplayPose,
RecordedReplayBodyFrameResolver,
ReplayBodyFrame,
ReplayThreatProfile,
load_replay_threat_profile,
)
@@ -62,6 +61,7 @@ THREAT_REPLAY_REPORT_NAME: Final = "report.json"
THREAT_REPLAY_MANIFEST_NAME: Final = "manifest.json"
VISUAL_FRAME_COUNT: Final = 32
VISUAL_POINT_LIMIT: Final = 4_000
VISUAL_GEOMETRY_REGRESSION_SEQUENCES: Final = (138, 274)
class ThreatReplayError(RuntimeError):
@@ -87,25 +87,26 @@ def build_threat_replay(
output_root: Path,
) -> ThreatReplayResult:
repository = repository_root.resolve()
profile = load_replay_threat_profile(
repository / DEFAULT_REPLAY_THREAT_PROFILE_PATH
)
profile = load_replay_threat_profile(repository / DEFAULT_REPLAY_THREAT_PROFILE_PATH)
temporal = read_temporal_replay_result(temporal_result_root)
geometry = read_geometry_replay_result(geometry_result_root)
detector = read_detector_replay_result(detector_result_root)
_validate_upstream(profile, temporal, geometry, detector)
store = RecordedGeometryStore.from_repository(repository)
pose_resolver = RecordedReplayPoseResolver(store)
body_frame_resolver = RecordedReplayBodyFrameResolver(
store,
profile=profile.body_frame,
)
provider = DualEvidenceReplayThreatProvider(
pose_resolver=pose_resolver,
body_frame_resolver=body_frame_resolver,
profile=profile,
)
source = RecordedRavnoves00Source.from_repository(
repository,
pacing=ReplayPacing.UNCAPPED,
)
visual_sequences = _visual_sequences(store.available_frame_indices())
visual_sequences = _visual_sequences(body_frame_resolver.qualified_frame_indices())
root = output_root.expanduser().absolute()
root.mkdir(mode=0o700, parents=True, exist_ok=True)
@@ -189,13 +190,11 @@ def build_threat_replay(
)
frame_started_ns = time.perf_counter_ns()
assessments = provider.assess(obstacle_map)
latencies_ms.append(
(time.perf_counter_ns() - frame_started_ns) / 1_000_000
)
latencies_ms.append((time.perf_counter_ns() - frame_started_ns) / 1_000_000)
by_id = {item.component_id: item for item in assessments}
expected_ids = {
item.component_id for item in (*current, *unknown)
} | {item.proposal_id for item in camera_uncertainty}
expected_ids = {item.component_id for item in (*current, *unknown)} | {
item.proposal_id for item in camera_uncertainty
}
if set(by_id) != expected_ids:
raise ThreatReplayError("threat assessment coverage is incomplete")
camera_rows = _camera_rows(
@@ -204,8 +203,7 @@ def build_threat_replay(
by_id,
)
metric_rows = [
_metric_row(item, by_id[item.component_id])
for item in (*current, *unknown)
_metric_row(item, by_id[item.component_id]) for item in (*current, *unknown)
]
for item in assessments:
assessment_counts[item.decision.value] += 1
@@ -222,10 +220,8 @@ def build_threat_replay(
"sequence": frame_count,
"frame_id": packet.envelope.frame_id,
"source_time_ns": packet.envelope.timestamps.source_ns,
"source_available": (
packet.envelope.registered_point_increment.available
),
"pose_available": pose_resolver.pose_for_frame(
"source_available": (packet.envelope.registered_point_increment.available),
"body_frame_available": body_frame_resolver.body_frame_for_frame(
packet.envelope.frame_id
)
is not None,
@@ -247,7 +243,9 @@ def build_threat_replay(
_visual_frame(
packet=packet,
store=store,
pose=pose_resolver.pose_for_frame(packet.envelope.frame_id),
body_frame=body_frame_resolver.body_frame_for_frame(
packet.envelope.frame_id
),
metric_rows=metric_rows,
camera_rows=camera_rows,
profile=profile,
@@ -277,6 +275,7 @@ def build_threat_replay(
elapsed_ns=elapsed_ns,
visual_count=visual_count,
fixtures=fixtures,
body_frame=body_frame_resolver.qualification_summary(),
)
requirements = _requirements(metrics, fixtures)
accepted = all(value is True for value in requirements.values())
@@ -300,6 +299,12 @@ def build_threat_replay(
"source_pack_sha256": profile.source_pack_sha256,
"calibration_id": profile.calibration_id,
"calibration_content_sha256": profile.calibration_content_sha256,
"body_frame": {
"schema_version": profile.body_frame.schema_version,
"origin": profile.body_frame.origin,
"up": profile.body_frame.up,
"forward": profile.body_frame.forward,
},
"rig_profile_id": profile.rig.profile_id,
"corridor_profile_id": profile.corridor.profile_id,
"producer_sha256": _producer_hashes(repository),
@@ -326,13 +331,20 @@ def build_threat_replay(
profile.rig.body_width_m,
],
"nominal_sensor_height_m": profile.rig.nominal_sensor_height_m,
"body_frame": {
"origin": profile.body_frame.origin,
"up": profile.body_frame.up,
"forward": profile.body_frame.forward,
},
"forward_corridor_m": profile.corridor.forward_length_m,
"prediction_horizon_seconds": (
profile.corridor.prediction_horizon_seconds
),
"prediction_horizon_seconds": (profile.corridor.prediction_horizon_seconds),
},
"limitations": [
"The body and corridor are replay-simulated, not a measured physical mount.",
(
"The replay base_footprint uses SLAM trajectory and map gravity; "
"a mounted vehicle replaces it with calibrated T_body_from_sensor."
),
"The LiDAR archive is the vendor mapped point increment, not every raw beam.",
"TTC uses bounded constant-relative-velocity replay extrapolation.",
"Camera-only evidence remains unknown and cannot establish metric clearance.",
@@ -398,9 +410,7 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult:
):
raise ThreatReplayError("threat replay identity changed")
artifacts = _array(manifest.get("artifacts"), "threat artifacts")
by_role = {
_object(item, "threat artifact").get("role"): item for item in artifacts
}
by_role = {_object(item, "threat artifact").get("role"): item for item in artifacts}
expected = {
"threat-replay-frames": (THREAT_REPLAY_FRAMES_NAME, "frames_sha256"),
"threat-visual-frames": (THREAT_REPLAY_VISUALS_NAME, "visuals_sha256"),
@@ -420,9 +430,7 @@ def read_threat_replay_result(root: Path) -> ThreatReplayResult:
raise ThreatReplayError("threat artifact identity changed")
report = _read_json(paths["threat-replay-report"])
metrics = _object(identity.get("metrics"), "threat metrics")
requirements = _object(
identity.get("acceptance_requirements"), "threat requirements"
)
requirements = _object(identity.get("acceptance_requirements"), "threat requirements")
fixtures = _read_json(paths["threat-deterministic-fixtures"])
accepted = all(value is True for value in requirements.values())
if (
@@ -505,9 +513,7 @@ def _metric_row(
"motion_reason": obstacle.motion_reason,
"semantic_hint": obstacle.semantic_hint,
"centroid_map_xyz_m": (
None
if obstacle.last_centroid_xyz_m is None
else list(obstacle.last_centroid_xyz_m)
None if obstacle.last_centroid_xyz_m is None else list(obstacle.last_centroid_xyz_m)
),
"cells": [item.to_dict() for item in obstacle.cells],
"history": [item.to_dict() for item in obstacle.history],
@@ -552,9 +558,7 @@ def _camera_rows(
"occupied_support": geometry["occupied_support"],
"range_m": geometry["range_m"],
"geometry_reason_codes": geometry["reason_codes"],
"threat_decision": (
None if assessment is None else assessment.decision.value
),
"threat_decision": (None if assessment is None else assessment.decision.value),
"threat_reason_codes": (
[] if assessment is None else list(assessment.reason_codes)
),
@@ -567,21 +571,19 @@ def _visual_frame(
*,
packet: SourcePacket,
store: RecordedGeometryStore,
pose: ReplayPose | None,
body_frame: ReplayBodyFrame | None,
metric_rows: list[dict[str, object]],
camera_rows: list[dict[str, object]],
profile: ReplayThreatProfile,
) -> dict[str, object]:
if pose is None:
raise ThreatReplayError("visual frame has no source pose")
if body_frame is None:
raise ThreatReplayError("visual frame has no qualified body frame")
points = store.current_points(packet)
if points is None:
raise ThreatReplayError("visual frame has no current point cloud")
rotation = quaternion_xyzw_to_rotation_matrix(
pose.orientation_map_from_lidar_xyzw
)
position = np.asarray(pose.position_map_xyz_m, dtype=np.float64)
points_body = (points - position) @ rotation
basis = np.asarray(body_frame.basis_map_from_body, dtype=np.float64)
origin = np.asarray(body_frame.origin_map_xyz_m, dtype=np.float64)
points_body = (points - origin) @ basis
stride = max(1, math.ceil(points_body.shape[0] / VISUAL_POINT_LIMIT))
sampled = points_body[::stride][:VISUAL_POINT_LIMIT]
metric_visuals = []
@@ -590,7 +592,7 @@ def _visual_frame(
cells = row.get("cells")
if not isinstance(centroid, list) or not isinstance(cells, list):
continue
centroid_body = pose.map_point_to_body(
centroid_body = body_frame.map_point_to_body(
(float(centroid[0]), float(centroid[1]), float(centroid[2]))
)
cell_centers = []
@@ -602,11 +604,7 @@ def _visual_frame(
for key in ("x", "y", "z")
)
cell_centers.append(
list(
pose.map_point_to_body(
(point_map[0], point_map[1], point_map[2])
)
)
list(body_frame.map_point_to_body((point_map[0], point_map[1], point_map[2])))
)
metric_visuals.append(
{
@@ -628,6 +626,14 @@ def _visual_frame(
"point_cloud_sample_count": int(sampled.shape[0]),
"metric_obstacles": metric_visuals,
"camera_proposals": camera_rows,
"body_frame": {
"origin_map_xyz_m": list(body_frame.origin_map_xyz_m),
"basis_map_from_body": [list(row) for row in body_frame.basis_map_from_body],
"sensor_height_m": body_frame.sensor_height_m,
"surface_slope_deg": body_frame.surface_slope_deg,
"forward_source": body_frame.forward_source,
"camera_forward_alignment_deg": body_frame.camera_forward_alignment_deg,
},
"rig": {
"length_m": profile.rig.body_length_m,
"width_m": profile.rig.body_width_m,
@@ -636,30 +642,29 @@ def _visual_frame(
"corridor": {
"forward_length_m": profile.corridor.forward_length_m,
"rear_margin_m": profile.corridor.rear_margin_m,
"half_width_m": (
profile.rig.body_width_m / 2
+ profile.corridor.lateral_clearance_m
),
"prediction_horizon_seconds": (
profile.corridor.prediction_horizon_seconds
),
"half_width_m": (profile.rig.body_width_m / 2 + profile.corridor.lateral_clearance_m),
"prediction_horizon_seconds": (profile.corridor.prediction_horizon_seconds),
},
"authority": _false_authority(),
}
class _FixturePoses:
def pose_for_frame(self, frame_id: str) -> ReplayPose:
return ReplayPose(
class _FixtureBodyFrames:
def body_frame_for_frame(self, frame_id: str) -> ReplayBodyFrame:
return ReplayBodyFrame(
frame_id=frame_id,
position_map_xyz_m=(0.0, 0.0, 0.0),
orientation_map_from_lidar_xyzw=(0.0, 0.0, 0.0, 1.0),
origin_map_xyz_m=(0.0, 0.0, 0.0),
basis_map_from_body=((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0)),
sensor_height_m=1.25,
surface_slope_deg=0.0,
forward_source="fixture",
camera_forward_alignment_deg=0.0,
)
def _fixture_document(profile: ReplayThreatProfile) -> dict[str, object]:
provider = DualEvidenceReplayThreatProvider(
pose_resolver=_FixturePoses(),
body_frame_resolver=_FixtureBodyFrames(),
profile=profile,
)
frame_id = "frame-000002"
@@ -783,8 +788,7 @@ def _fixture_document(profile: ReplayThreatProfile) -> dict[str, object]:
"cases": cases,
"critical_case_count": sum(item["critical"] is True for item in cases),
"critical_false_not_threat_count": sum(
item["critical"] is True and item["actual"] == "not-threat"
for item in cases
item["critical"] is True and item["actual"] == "not-threat" for item in cases
),
"passed_count": sum(item["passed"] is True for item in cases),
"total_count": len(cases),
@@ -816,9 +820,7 @@ def _fixture_obstacle(
last_centroid_xyz_m=None if state is TemporalState.EXPIRED else last.centroid_xyz_m,
motion=motion if state is TemporalState.CURRENT else MotionState.UNKNOWN,
motion_confidence=(
0.0
if state is not TemporalState.CURRENT or motion is MotionState.UNKNOWN
else 1.0
0.0 if state is not TemporalState.CURRENT or motion is MotionState.UNKNOWN else 1.0
),
motion_reason=(
"stale-support"
@@ -912,6 +914,7 @@ def _metrics(
elapsed_ns: int,
visual_count: int,
fixtures: dict[str, object],
body_frame: dict[str, object],
) -> dict[str, object]:
values = np.asarray(latencies_ms, dtype=np.float64)
return {
@@ -920,6 +923,7 @@ def _metrics(
"decisions": dict(sorted(assessment_counts.items())),
"motion_decisions": dict(sorted(motion_decisions.items())),
"reason_counts": dict(sorted(reason_counts.items())),
"body_frame": body_frame,
"visual_evidence": {
"frame_count": visual_count,
"point_limit_per_frame": VISUAL_POINT_LIMIT,
@@ -928,14 +932,14 @@ def _metrics(
"point_cloud_available": True,
"metric_distance_available": True,
"virtual_corridor_available": True,
"qualified_base_footprint_available": True,
"geometry_regression_sequences": list(VISUAL_GEOMETRY_REGRESSION_SEQUENCES),
},
"fixtures": {
"passed": fixtures["passed_count"],
"total": fixtures["total_count"],
"critical": fixtures["critical_case_count"],
"critical_false_not_threat": fixtures[
"critical_false_not_threat_count"
],
"critical_false_not_threat": fixtures["critical_false_not_threat_count"],
},
"runtime": {
"elapsed_ns": elapsed_ns,
@@ -955,12 +959,9 @@ def _requirements(
evidence = _object(metrics.get("evidence"), "evidence metrics")
decisions = _object(metrics.get("decisions"), "decision metrics")
visual = _object(metrics.get("visual_evidence"), "visual metrics")
total_evidence = sum(
_integer(value, "evidence count") for value in evidence.values()
)
total_decisions = sum(
_integer(value, "decision count") for value in decisions.values()
)
body_frame = _object(metrics.get("body_frame"), "body frame metrics")
total_evidence = sum(_integer(value, "evidence count") for value in evidence.values())
total_decisions = sum(_integer(value, "decision count") for value in decisions.values())
cases = _array(fixtures.get("cases"), "fixture cases")
camera_case = next(
(
@@ -984,8 +985,7 @@ def _requirements(
),
"camera_only_is_unknown_never_safe": camera_case.get("actual") == "unknown",
"held_and_stale_are_unknown_never_safe": (
len(stale_cases) == 2
and all(item.get("actual") == "unknown" for item in stale_cases)
len(stale_cases) == 2 and all(item.get("actual") == "unknown" for item in stale_cases)
),
"geometry_only_evidence_is_assessed": (
_integer(
@@ -1012,8 +1012,29 @@ def _requirements(
"point_cloud_available",
"metric_distance_available",
"virtual_corridor_available",
"qualified_base_footprint_available",
)
)
and visual.get("geometry_regression_sequences")
== list(VISUAL_GEOMETRY_REGRESSION_SEQUENCES)
),
"body_frame_is_grounded_gravity_stable_and_route_aligned": (
body_frame.get("available")
== _integer(body_frame.get("qualified"), "qualified body frames")
+ _integer(body_frame.get("rejected"), "rejected body frames")
and _integer(body_frame.get("qualified"), "qualified body frames")
>= math.ceil(_integer(body_frame.get("available"), "available body frames") * 0.95)
and body_frame.get("origin") == "local-surface-vertical-projection"
and body_frame.get("up") == "vendor-slam-map-gravity-axis"
and body_frame.get("forward") == "smoothed-slam-trajectory-validated-by-camera-axis"
and _number_value(
_object(
body_frame.get("camera_forward_alignment_deg"),
"body alignment metrics",
).get("maximum"),
"maximum body alignment",
)
<= 25.0
),
"physical_collision_and_actuation_authority_remain_false": (
fixtures.get("authority") == _false_authority()
@@ -1062,6 +1083,23 @@ def _visual_sequences(available: tuple[int, ...]) -> frozenset[int]:
available[round(index * (len(available) - 1) / (VISUAL_FRAME_COUNT - 1))]
for index in range(VISUAL_FRAME_COUNT)
}
available_set = frozenset(available)
for anchor in VISUAL_GEOMETRY_REGRESSION_SEQUENCES:
if anchor not in available_set:
raise ThreatReplayError("geometry regression frame is not qualified")
if anchor in selected:
continue
replaceable = selected.difference(
{
available[0],
available[-1],
*VISUAL_GEOMETRY_REGRESSION_SEQUENCES,
}
)
if not replaceable:
raise ThreatReplayError("visual regression sample cannot be inserted")
selected.remove(min(replaceable, key=lambda value: abs(value - anchor)))
selected.add(anchor)
if len(selected) != VISUAL_FRAME_COUNT:
raise ThreatReplayError("visual sample selection is not unique")
return frozenset(selected)
@@ -1189,6 +1227,12 @@ def _integer(value: object, label: str) -> int:
return value
def _number_value(value: object, label: str) -> float:
if not isinstance(value, int | float) or isinstance(value, bool) or not math.isfinite(value):
raise ThreatReplayError(f"{label} is not finite")
return float(value)
def _signed_integer(value: object, label: str) -> int:
if not isinstance(value, int) or isinstance(value, bool):
raise ThreatReplayError(f"{label} must be an integer")