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

fix(perception): reconstruct rolling occupancy from K1 increments

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-08-05 21:12:47 +03:00
parent c70ad345ea
commit 7aa3ce55c0
16 changed files with 1489 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/perception/threat.py
+115 -60
View File
@@ -10,6 +10,7 @@ from __future__ import annotations
import hashlib
import json
import math
from collections.abc import Iterable
from dataclasses import dataclass
from pathlib import Path
from typing import Final, Protocol
@@ -26,9 +27,14 @@ from .contracts import (
)
from .geometry_math import quaternion_xyzw_to_rotation_matrix
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"
REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v3"
REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v3"
DEFAULT_REPLAY_THREAT_PROFILE_PATH: Final = "config/perception/m4-replay-threat-v3.json"
LEGACY_REPLAY_THREAT_PROFILE_SCHEMA: Final = "missioncore.replay-threat-profile/v2"
LEGACY_REPLAY_THREAT_PROVIDER_ID: Final = "dual-evidence-replay-threat/v2"
type Vector3 = tuple[float, float, float]
type Matrix3 = tuple[Vector3, Vector3, Vector3]
class ReplayThreatError(ValueError):
@@ -38,12 +44,8 @@ class ReplayThreatError(ValueError):
@dataclass(frozen=True, slots=True)
class ReplayBodyFrame:
frame_id: str
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],
]
origin_map_xyz_m: Vector3
basis_map_from_body: Matrix3
sensor_height_m: float
surface_slope_deg: float
forward_source: str
@@ -68,8 +70,22 @@ class ReplayBodyFrame:
)
):
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)
columns: Matrix3 = (
(
self.basis_map_from_body[0][0],
self.basis_map_from_body[1][0],
self.basis_map_from_body[2][0],
),
(
self.basis_map_from_body[0][1],
self.basis_map_from_body[1][1],
self.basis_map_from_body[2][1],
),
(
self.basis_map_from_body[0][2],
self.basis_map_from_body[1][2],
self.basis_map_from_body[2][2],
),
)
if (
not self.forward_source
@@ -87,15 +103,17 @@ class ReplayBodyFrame:
def map_point_to_body(
self,
point_map_xyz_m: tuple[float, float, float],
) -> tuple[float, float, float]:
delta = tuple(point_map_xyz_m[index] - self.origin_map_xyz_m[index] for index in range(3))
point_map_xyz_m: Vector3,
) -> Vector3:
delta = _vector3(
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]:
vector_map_xyz_m: Vector3,
) -> Vector3:
values = tuple(
float(
sum(
@@ -172,14 +190,14 @@ class RecordedReplayBodyFrameResolver:
)
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)
position = _vector3(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])
ground_normal = _vector3(value / normal_norm for value in plane[:3])
if ground_normal[2] < 0.0:
ground_normal = tuple(-value for value in ground_normal)
ground_normal = _vector3(-value for value in ground_normal)
plane = tuple(-value for value in plane)
sensor_height = _dot(position, ground_normal) + plane[3] / normal_norm
if (
@@ -200,15 +218,15 @@ class RecordedReplayBodyFrameResolver:
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(
camera_forward_lidar = _vector3(float(calibration[2, index]) for index in range(3))
camera_forward_map = _vector3(
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(
route = _vector3(
float(
inputs.trajectory_end_position_map[index]
- inputs.trajectory_start_position_map[index]
@@ -235,8 +253,12 @@ class RecordedReplayBodyFrameResolver:
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))
origin = _vector3(position[index] - vertical_height * up[index] for index in range(3))
basis: Matrix3 = (
(forward[0], left[0], up[0]),
(forward[1], left[1], up[1]),
(forward[2], left[2], up[2]),
)
frame = ReplayBodyFrame(
frame_id=frame_id,
origin_map_xyz_m=origin,
@@ -326,7 +348,10 @@ class DualEvidenceReplayThreatProvider:
body_frame_resolver: ReplayBodyFrameResolver,
profile: ReplayThreatProfile,
) -> None:
if profile.provider_id != self.provider_id:
if profile.provider_id not in {
self.provider_id,
LEGACY_REPLAY_THREAT_PROVIDER_ID,
}:
raise ReplayThreatError("threat provider identity changed")
self.body_frame_resolver = body_frame_resolver
self.profile = profile
@@ -354,7 +379,7 @@ class DualEvidenceReplayThreatProvider:
obstacle: TemporalObstacle,
body_frame: ReplayBodyFrame | None,
) -> ThreatAssessment:
if obstacle.state is not TemporalState.CURRENT:
if obstacle.state not in {TemporalState.CURRENT, TemporalState.RETAINED}:
return self._unknown(
frame_id,
obstacle.component_id,
@@ -390,12 +415,21 @@ class DualEvidenceReplayThreatProvider:
rig=self.profile.rig,
corridor=self.profile.corridor,
)
motion_complete = obstacle.motion is not MotionState.UNKNOWN and velocity_body is not None
retained = obstacle.state is TemporalState.RETAINED
motion_complete = (
not retained
and 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
reasons = [
"current-corridor-intersection"
(
"retained-corridor-intersection"
if retained
else "current-corridor-intersection"
)
if current_intersection
else "predicted-corridor-intersection",
"metric-lidar-geometry",
@@ -404,6 +438,14 @@ class DualEvidenceReplayThreatProvider:
intersection = CorridorIntersection.CLEAR
decision = ThreatDecision.NOT_THREAT
reasons = ["predicted-corridor-clear", "metric-lidar-geometry"]
elif retained:
intersection = CorridorIntersection.UNKNOWN
decision = ThreatDecision.UNKNOWN
reasons = [
"retained-map-occupancy-outside-current-corridor",
"metric-lidar-geometry",
"absence-of-republication-is-not-free",
]
else:
intersection = CorridorIntersection.UNKNOWN
decision = ThreatDecision.UNKNOWN
@@ -423,7 +465,9 @@ class DualEvidenceReplayThreatProvider:
horizon_seconds=self.profile.corridor.prediction_horizon_seconds,
)
relative_speed = _closing_speed_mps(centroid_body, velocity_body)
if velocity_body is None:
if retained:
reasons.append("motion-retained-map-increment-no-current-motion")
elif velocity_body is None:
reasons.append(f"motion-{obstacle.motion_reason}")
else:
reasons.append(f"motion-{obstacle.motion.value}")
@@ -465,14 +509,14 @@ class DualEvidenceReplayThreatProvider:
or last.frame_id != current_body_frame.frame_id
):
return None
obstacle_delta = tuple(
obstacle_delta = _vector3(
last.centroid_xyz_m[index] - first.centroid_xyz_m[index] for index in range(3)
)
rig_delta = tuple(
rig_delta = _vector3(
last_body_frame.origin_map_xyz_m[index] - first_body_frame.origin_map_xyz_m[index]
for index in range(3)
)
relative_map = tuple(
relative_map = _vector3(
(obstacle_delta[index] - rig_delta[index]) / span_seconds for index in range(3)
)
body = current_body_frame.map_vector_to_body(relative_map)
@@ -530,11 +574,13 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
},
"replay threat profile",
)
if (
document["schema_version"] != REPLAY_THREAT_PROFILE_SCHEMA
or document["provider_id"] != REPLAY_THREAT_PROVIDER_ID
):
identity = (document["schema_version"], document["provider_id"])
if identity not in {
(REPLAY_THREAT_PROFILE_SCHEMA, REPLAY_THREAT_PROVIDER_ID),
(LEGACY_REPLAY_THREAT_PROFILE_SCHEMA, LEGACY_REPLAY_THREAT_PROVIDER_ID),
}:
raise ReplayThreatError("replay threat profile identity is incompatible")
is_v3 = identity == (REPLAY_THREAT_PROFILE_SCHEMA, REPLAY_THREAT_PROVIDER_ID)
source = _object(document["source"], "threat source")
calibration = _object(document["calibration"], "threat calibration")
body_frame = _object(document["body_frame"], "virtual body frame")
@@ -603,18 +649,17 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
},
"virtual corridor",
)
_exact_keys(
policy,
{
policy_keys = {
"camera_only_decision",
"held_or_stale_decision",
"semantic_class_used",
"detector_identity_used",
"absence_of_points_means_free",
"geometry_only_is_eligible",
},
"threat policy",
)
}
if is_v3:
policy_keys.add("retained_map_intersection_decision")
_exact_keys(policy, policy_keys, "threat policy")
_exact_keys(
authority,
{
@@ -634,14 +679,18 @@ def load_replay_threat_profile(path: Path) -> ReplayThreatProfile:
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
!= {
or policy != {
"camera_only_decision": "unknown",
"held_or_stale_decision": "unknown",
"semantic_class_used": False,
"detector_identity_used": False,
"absence_of_points_means_free": False,
"geometry_only_is_eligible": True,
**(
{"retained_map_intersection_decision": "threat"}
if is_v3
else {}
),
}
or authority
!= {
@@ -936,17 +985,19 @@ def _positive_integer(document: dict[str, object], key: str) -> int:
return value
def _dot(
first: tuple[float, float, float],
second: tuple[float, float, float],
) -> float:
def _vector3(values: Iterable[float]) -> Vector3:
first, second, third = values
return float(first), float(second), float(third)
def _dot(first: Vector3, second: Vector3) -> 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]:
first: Vector3,
second: Vector3,
) -> Vector3:
return (
first[1] * second[2] - first[2] * second[1],
first[2] * second[0] - first[0] * second[2],
@@ -955,25 +1006,29 @@ def _cross(
def _normalize(
value: tuple[float, float, float],
) -> tuple[float, float, float] | None:
value: Vector3,
) -> Vector3 | None:
norm = math.sqrt(_dot(value, value))
if norm < 1e-9:
return None
return tuple(item / norm for item in value)
return value[0] / norm, value[1] / norm, value[2] / norm
def _reject(
value: tuple[float, float, float],
normal: tuple[float, float, float],
) -> tuple[float, float, float]:
value: Vector3,
normal: Vector3,
) -> Vector3:
along = _dot(value, normal)
return tuple(value[index] - along * normal[index] for index in range(3))
return (
value[0] - along * normal[0],
value[1] - along * normal[1],
value[2] - along * normal[2],
)
def _angle_degrees(
first: tuple[float, float, float],
second: tuple[float, float, float],
first: Vector3,
second: Vector3,
) -> float:
return math.degrees(math.acos(max(-1.0, min(1.0, _dot(first, second)))))