diff --git a/experiments/perception/LAB_E26_REPORT_2026-07-24.md b/experiments/perception/LAB_E26_REPORT_2026-07-24.md new file mode 100644 index 0000000..66214de --- /dev/null +++ b/experiments/perception/LAB_E26_REPORT_2026-07-24.md @@ -0,0 +1,261 @@ +# LAB E26 — camera ego-motion evidence and conservative LiDAR fusion + +Date: 2026-07-24 +Status: recorded diagnostic accepted; navigation and safety acceptance are false +Immutable replay: +`LAB E26.1 · Camera + ego-motion · full RAVNOVES00` + +## Objective + +E25 proved that persistent map-frame LiDAR support can stop parked vehicles +from looking dynamic, but it cannot recover motion when the K1 supplies too few +current LiDAR returns. E26 adds an independent camera measurement without +inventing metric velocity: + +1. preserve the original detector track in image space; +2. remove the apparent motion caused by the moving rig; +3. test whether observations from several calibrated camera poses are + consistent with one static point in `k1-map`; +4. fuse the resulting relative camera state with E25 LiDAR evidence; +5. represent disagreement and missing evidence explicitly as `unknown`; +6. keep every unknown object occupied and outside navigation authority. + +The experiment changes the measurement, not the visual smoothing of 3D boxes. +Rerun remains the operator viewer. The perception result is produced before +Rerun and can be consumed independently of it. + +## Immutable inputs + +- Source observation: `20260720T065719Z_viewer_live` (`RAVNOVES00`). +- Published lab session: `lab-e26-1-camera-ego-motion`. +- Camera: `sensor.camera.right`, factory calibration slot `camera_1`. +- Coordinate frame: `k1-map`. +- Frames: 4,489. +- Source timeline: 35.421857292–484.044857292 s. +- Published replay duration: 448.623 s. +- Factory projection model: KB4. +- Calibration SHA-256: + `05f3ad9b38b3a4fc95388a8ec83da83c745e217709e51787b3d5aad0969f6fa9`. +- Camera/detector source result: + `e10-integrated-perception-34ade557b5636717aa497fc00355b84df7063e483a175f2f5d3f04c03df1c898`. +- E25.3 LiDAR-evidence source result: + `e10-integrated-perception-9034a5cf306b379cc248c1da17639f140fc896bb55e412bef43f89974de892a3`. +- Detector: YOLOX-S, COCO-80, 640×640. +- Semantic source: + `tue-mps/cityscapes_semantic_eomt_large_1024`, FP16 autocast. +- E26 profile SHA-256: + `17668cea5a8bddd621271ed0457ca3a91e6d3957133d53213d6705d9b03d039e`. +- E26 benchmark SHA-256: + `c3523c8f1ca22631120a129d20c73a8429be8c908279cc1c3286661f33d5fe57`. + +E26 does not repeat detector or segmentation inference. It reuses immutable +model outputs, E25 LiDAR evidence, rig poses and factory calibration. It writes +a new integrated result and hard-links immutable source payloads where +possible. The source results and raw sensor data are not modified. + +## Camera measurement + +For each original 2D detector track, E26 takes a bottom-centre image point with +a small configurable inset. The point is unprojected through the factory KB4 +model into a three-dimensional camera ray. The calibrated camera-to-rig +extrinsic and the recorded rig pose transform that ray into `k1-map`. + +Over a bounded two-second history, a least-squares ray intersection estimates +the one world point that would explain the observations if the target were +static. The tracker evaluates: + +- median angular residual to the static-world hypothesis; +- p80 angular residual; +- positive-depth fraction; +- estimated range; +- rig baseline covered by the observations; +- observation count, time span, detector score and box size. + +Low residuals support `static`. A well-observed track with residuals beyond the +class thresholds supports `dynamic`. A hypothesis behind the camera can support +`dynamic` only under the stricter score and repeated-observation gate. +Degenerate geometry, insufficient baseline, weak detections, missing pose or an +invalid KB4 edge ray yields `unknown`. + +The camera branch measures relative inconsistency with a static world +hypothesis. It does not measure metric object velocity. Camera-only +`speed_mps` and `velocity_map_mps` are therefore deliberately null. + +## Bounded streaming contract + +The implementation is a single forward pass with no lookahead: + +- history: at most 2.0 s; +- maximum 32 observations per camera track; +- maximum 256 live tracks; +- maximum idle time: 2.5 s; +- minimum observations: 5; +- minimum observation span: 0.4 s; +- minimum rig baseline: 0.25 m; +- maximum normal-matrix condition: 1,000,000; +- minimum box diagonal: 35 px; +- minimum median detector score: 0.60; +- dynamic confirmation: 2 frames; +- static confirmation: 4 frames; +- evidence decay: 1 frame. + +The run reached only 34 simultaneous tracks. Five unprojectable KB4 edge rays +were rejected fail-soft and counted; detector and LiDAR objects remained in the +result as unavailable camera evidence. + +## Fusion contract + +Camera and LiDAR are treated as independent evidence sources: + +- camera and LiDAR agree: publish the common state with conservative + confidence; +- camera and LiDAR disagree: publish explicit `unknown`, clear velocity and + increment the conflict counter; +- camera only: publish relative `static` or `dynamic` with confidence scaled by + 0.65, but without metric velocity; +- LiDAR only: preserve the metric E25 state and velocity; +- neither source qualifies: publish `unknown`. + +Every branch carries: + +- `unknown_is_occupied=true`; +- `camera_only_metric_velocity_valid=false`; +- `navigation_or_safety_accepted=false`. + +There is no command output and no planner authority in E26. + +## Operator-reviewed benchmark + +The benchmark contains eight dynamic target windows and three parked-vehicle +control windows. The dynamic windows are bound to reviewed source detector +track IDs, so a nearby parked object cannot satisfy a moving-object event. + +Dynamic windows: + +- woman with dog at 54–62 s: 24 dynamic hits, 82.76% evidence coverage; +- person loading a car at 62–69 s: best run 4 hits over 0.896 s, 81.82% coverage; +- adult and child at 111–120 s: 26 hits over 4.387 s, 91.49% coverage; +- person on the road at 124–132 s: 11 hits over 1.0 s, 76.19% coverage; +- stroller group at 151–161 s: best run 5 hits over 0.498 s, 84.0% coverage; +- oncoming vehicle at 167–173 s: 3 hits over 0.172 s, 73.33% coverage; +- vulnerable-road-user group at 170–179 s: best run 5 hits over 0.38 s, + 72.09% coverage; +- same-direction vehicle at 180–188 s: 12 hits over 1.734 s, 100% coverage. + +Parked-vehicle controls: + +- 70–90 s: 853 source observations, 732 evidence observations, + 57.51% classified, 418 static / 311 unknown / 3 dynamic, + false-dynamic fraction 0.00410; +- 132–148 s: 820 source observations, 746 evidence observations, + 78.42% classified, 585 static / 161 unknown / 0 dynamic, + false-dynamic fraction 0.0; +- 161–166 s: 151 source observations, 120 evidence observations, + 62.5% classified, 75 static / 45 unknown / 0 dynamic, + false-dynamic fraction 0.0. + +Result: 11/11 benchmark windows pass. + +This is a useful regression result, not independent ground truth. The windows +come from operator timestamps and reviewed detector-track binding on the same +recording used to develop the experiment. The result does not measure global +precision, recall, ID-switch rate, distance error or safety performance outside +those windows. + +## Runtime and resource result + +Camera ego-motion postprocessor: + +- mean: 1.982 ms/frame; +- p50: 2.059 ms/frame; +- p95: 3.499 ms/frame; +- maximum: 106.171 ms/frame; +- camera observations: 20,513; +- created tracks: 1,466; +- peak live tracks: 34; +- pose-unavailable frames: 561; +- invalid camera rays: 5; +- published camera states: 6,105 static, 1,117 dynamic, 13,291 unknown; +- explicit camera/LiDAR conflicts: 374. + +The p95 satisfies the configured 12 ms budget for this postprocessor. It is not +an end-to-end real-time measurement: detector inference, semantic inference, +sensor transport, decoding, synchronization, Rerun serialization, network +transfer and browser rendering are not included. + +## Replay acceptance + +The saved session is `ready`, `replayable=true`, and visible through the single +existing server on `http://127.0.0.1:8000/`. + +An acceptance request through the same perception endpoint used by the browser +returned: + +- HTTP 200; +- media type `application/vnd.rerun.rrd`; +- Rerun magic `RRF2`; +- payload: 69,735,518 bytes; +- SHA-256: + `0d7978468f829acb6afe2aa14f6e2f2f96d55ee35e21d8439cdd3ec3bdd107b1`; +- cold response time: 47.172 s. + +The 47.172 s cold Rerun materialization is an operator-path cost, not perception +compute. It remains too slow for first connection in the field and must be +addressed separately through prebuilt/cached, chunked or live incremental +delivery. It does not invalidate the 3.499 ms motion-evidence p95, and the +3.499 ms number does not excuse the 47.172 s operator wait. + +The server was not stopped, restarted or moved to another port during +acceptance. + +## Validation + +- Ruff passes for `src`, `tests` and `experiments/perception`. +- Strict mypy passes for the E26 compute, publication, calibration and CLI + modules. +- 44 targeted E10/E24/E25/E26 and KB4 projection tests pass. +- The complete repository `pytest` suite passes. +- The on-disk implementation, profile and benchmark SHA-256 values exactly + match those embedded in the immutable E26.1 report. +- The saved-session API reports `ready`, `replayable=true` and the expected + result ID. +- The single localhost service remains available on port 8000. + +## Conclusion + +E26 closes the specific E25 evidence gap on the reviewed recording: moving +people and the two moving vehicles can be detected from calibrated camera +parallax when current LiDAR support is sparse, while parked-vehicle false +dynamic remains near zero. The implementation is bounded and has no lookahead, +so its state structure is compatible with a later live shadow deployment. + +The result is still not a planner-ready world model: + +- camera-only motion has no metric velocity; +- 13,291 published camera observations remain unknown; +- 374 camera/LiDAR conflicts require explicit downstream handling; +- the benchmark is small and not independent; +- the detector and track association still inherit COCO-domain errors; +- no end-to-end live latency or dropped-frame envelope has been measured; +- the cold operator replay still takes approximately 47 s. + +## E27 gate + +E27 should not be another threshold-tuning pass. It should turn E26 into a +measurable world-model qualification: + +1. create frame-level independent labels for the reviewed moving objects, + parked controls, ID switches and visibility; +2. quantify camera association integrity, dynamic/static precision and recall, + time-to-confirm and conflict duration; +3. add metric range/velocity only when LiDAR, ground contact or another + observable depth source supports it; +4. preserve occupancy and uncertainty independently of cuboid presentation; +5. replay the bounded stages at sensor cadence and record end-to-end deadline + misses, queue depth and dropped-frame policy; +6. keep the first live step shadow-only, with no navigation or safety authority; +7. prebuild or incrementally stream the operator product so connecting to the + vehicle never waits for a full 69 MB Rerun serialization. + +E26.1 is accepted as a diagnostic evidence-fusion laboratory result. It is not +accepted for autonomous navigation, collision avoidance or safety decisions. diff --git a/experiments/perception/e26_camera_ego_motion_profile.json b/experiments/perception/e26_camera_ego_motion_profile.json new file mode 100644 index 0000000..6d6ffde --- /dev/null +++ b/experiments/perception/e26_camera_ego_motion_profile.json @@ -0,0 +1,77 @@ +{ + "schema_version": "missioncore.e26-camera-ego-motion-profile/v1", + "profile_id": "lab-e26-kb4-multiview-static-hypothesis-v1", + "mode": "recorded-streaming-qualification", + "source": { + "source_id": "sensor.camera.right", + "coordinate_frame": "k1-map", + "calibration_slot": "camera_1", + "calibration_sha256": "05f3ad9b38b3a4fc95388a8ec83da83c745e217709e51787b3d5aad0969f6fa9" + }, + "camera_evidence": { + "history_seconds": 2.0, + "maximum_observations_per_track": 32, + "minimum_observations": 5, + "minimum_span_seconds": 0.4, + "minimum_ego_baseline_m": 0.25, + "maximum_normal_matrix_condition": 1000000.0, + "footpoint_inset_fraction": 0.03, + "minimum_bbox_diagonal_px": 35.0, + "minimum_median_detector_score": 0.6, + "minimum_negative_depth_detector_score": 0.7, + "minimum_valid_positive_depth_fraction": 0.8, + "negative_depth_dynamic_fraction": 0.6, + "minimum_static_hypothesis_range_m": 0.8, + "maximum_static_hypothesis_range_m": 80.0, + "dynamic_median_angular_residual_degrees": { + "person": 1.8, + "bicycle": 1.8, + "motorcycle": 1.8, + "vehicle": 3.5, + "default": 3.5 + }, + "dynamic_p80_angular_residual_degrees": { + "person": 2.8, + "bicycle": 2.8, + "motorcycle": 2.8, + "vehicle": 5.0, + "default": 5.0 + }, + "static_median_angular_residual_degrees": { + "person": 1.0, + "bicycle": 1.0, + "motorcycle": 1.0, + "vehicle": 1.4, + "default": 1.4 + }, + "static_p80_angular_residual_degrees": { + "person": 2.0, + "bicycle": 2.0, + "motorcycle": 2.0, + "vehicle": 2.5, + "default": 2.5 + }, + "dynamic_confirmation_frames": 2, + "static_confirmation_frames": 4, + "evidence_decay_frames": 1 + }, + "fusion": { + "camera_only_confidence_scale": 0.65, + "conflict_state": "unknown", + "unknown_is_occupied": true, + "camera_only_metric_velocity_valid": false + }, + "bounds": { + "maximum_tracks": 256, + "maximum_track_idle_seconds": 2.5 + }, + "acceptance": { + "maximum_processing_p95_ms": 12.0, + "maximum_tracks_observed": 256, + "maximum_static_control_false_dynamic_fraction": 0.1 + }, + "authority": { + "commands_enabled": false, + "navigation_or_safety_accepted": false + } +} diff --git a/experiments/perception/e26_motion_benchmark.json b/experiments/perception/e26_motion_benchmark.json new file mode 100644 index 0000000..665e9ef --- /dev/null +++ b/experiments/perception/e26_motion_benchmark.json @@ -0,0 +1,131 @@ +{ + "schema_version": "missioncore.e26-motion-benchmark/v1", + "benchmark_id": "ravnoves00-camera-ego-reviewed-anchors-v1", + "source_session_id": "20260720T065719Z_viewer_live", + "timeline": "session_seconds", + "annotation_status": "operator-approximate-with-reviewed-detector-track-binding", + "events": [ + { + "id": "dynamic-person-woman-dog-left-58s", + "kind": "target-motion", + "window_seconds": [54.0, 62.0], + "class_group": "person", + "target_source_track_ids": [83], + "expected_motion": "dynamic", + "minimum_hits": 3, + "minimum_span_seconds": 0.2, + "minimum_coverage_fraction": 0.08 + }, + { + "id": "dynamic-person-loading-car-right-65s", + "kind": "target-motion", + "window_seconds": [62.0, 69.0], + "class_group": "person", + "target_source_track_ids": [112], + "expected_motion": "dynamic", + "minimum_hits": 3, + "minimum_span_seconds": 0.2, + "minimum_coverage_fraction": 0.1 + }, + { + "id": "static-vehicles-before-moving-70-90s", + "kind": "static-control", + "window_seconds": [70.0, 90.0], + "class_group": "vehicle", + "expected_motion": "static", + "minimum_source_observations": 80, + "minimum_coverage_fraction": 0.2, + "minimum_classified_fraction": 0.2, + "maximum_false_dynamic_fraction": 0.1 + }, + { + "id": "dynamic-person-adult-child-left-115s", + "kind": "target-motion", + "window_seconds": [111.0, 120.0], + "class_group": "person", + "target_source_track_ids": [229], + "expected_motion": "dynamic", + "minimum_hits": 3, + "minimum_span_seconds": 0.2, + "minimum_coverage_fraction": 0.08 + }, + { + "id": "dynamic-person-road-right-127s", + "kind": "target-motion", + "window_seconds": [124.0, 132.0], + "class_group": "person", + "target_source_track_ids": [253], + "expected_motion": "dynamic", + "minimum_hits": 2, + "minimum_span_seconds": 0.15, + "minimum_coverage_fraction": 0.08 + }, + { + "id": "static-vehicles-mid-run-132-148s", + "kind": "static-control", + "window_seconds": [132.0, 148.0], + "class_group": "vehicle", + "expected_motion": "static", + "minimum_source_observations": 80, + "minimum_coverage_fraction": 0.2, + "minimum_classified_fraction": 0.2, + "maximum_false_dynamic_fraction": 0.1 + }, + { + "id": "dynamic-person-stroller-group-left-155s", + "kind": "target-motion", + "window_seconds": [151.0, 161.0], + "class_group": "person", + "target_source_track_ids": [322, 328], + "expected_motion": "dynamic", + "minimum_hits": 3, + "minimum_span_seconds": 0.2, + "minimum_coverage_fraction": 0.08 + }, + { + "id": "static-vehicles-before-oncoming-161-166s", + "kind": "static-control", + "window_seconds": [161.0, 166.0], + "class_group": "vehicle", + "expected_motion": "static", + "minimum_source_observations": 25, + "minimum_coverage_fraction": 0.15, + "minimum_classified_fraction": 0.15, + "maximum_false_dynamic_fraction": 0.1 + }, + { + "id": "dynamic-vehicle-oncoming-right-170s", + "kind": "target-motion", + "window_seconds": [167.0, 173.0], + "class_group": "vehicle", + "target_source_track_ids": [365], + "expected_motion": "dynamic", + "minimum_hits": 3, + "minimum_span_seconds": 0.15, + "minimum_coverage_fraction": 0.1 + }, + { + "id": "dynamic-person-bike-group-left-173s", + "kind": "target-motion", + "window_seconds": [170.0, 179.0], + "class_group": "vulnerable_road_user", + "target_source_track_ids": [379, 386, 388], + "expected_motion": "dynamic", + "minimum_hits": 3, + "minimum_span_seconds": 0.2, + "minimum_coverage_fraction": 0.08 + }, + { + "id": "dynamic-vehicle-same-direction-right-183s", + "kind": "target-motion", + "window_seconds": [180.0, 188.0], + "class_group": "vehicle", + "target_source_track_ids": [373], + "binding_review": "frame-and-2d-track-reviewed; 391 is a parked vehicle", + "expected_motion": "dynamic", + "minimum_hits": 3, + "minimum_span_seconds": 0.2, + "minimum_coverage_fraction": 0.1 + } + ] +} diff --git a/src/k1link/compute/__init__.py b/src/k1link/compute/__init__.py index dbfbd4f..28f393f 100644 --- a/src/k1link/compute/__init__.py +++ b/src/k1link/compute/__init__.py @@ -35,6 +35,7 @@ from .jobs import ( validate_camera_compute_job, ) from .lab_instances import ( + PublishedCameraEgoMotionLabInstance, PublishedIntegratedLabInstance, PublishedPersistentSupportLabInstance, PublishedTemporalLabInstance, @@ -44,6 +45,7 @@ from .lab_instances import ( publish_e23_lab_instance, publish_e24_lab_instance, publish_e25_lab_instance, + publish_e26_lab_instance, publish_integrated_lab_instance, ) from .live_perception import ( @@ -122,6 +124,7 @@ __all__ = [ "LivePerceptionIngress", "IntegratedPerceptionOverlayStore", "PublishedIntegratedLabInstance", + "PublishedCameraEgoMotionLabInstance", "PublishedPersistentSupportLabInstance", "PublishedTemporalLabInstance", "PublishedWorldMotionLabInstance", @@ -158,6 +161,7 @@ __all__ = [ "publish_e23_lab_instance", "publish_e24_lab_instance", "publish_e25_lab_instance", + "publish_e26_lab_instance", "publish_integrated_lab_instance", "validate_multirate_perception_qualification_result", "prepare_recorded_qualification_slice", diff --git a/src/k1link/compute/camera_ego_motion.py b/src/k1link/compute/camera_ego_motion.py new file mode 100644 index 0000000..8686e51 --- /dev/null +++ b/src/k1link/compute/camera_ego_motion.py @@ -0,0 +1,1249 @@ +"""Camera/ego-motion evidence fused conservatively with LAB E25 LiDAR state. + +LAB E26 does not infer metric velocity from a monocular box. It tests whether +the bottom-center rays of one detector track remain compatible with a single +static world point while the calibrated rig moves. A failed static hypothesis +is relative-motion evidence. LiDAR remains the metric range/occupancy source; +conflicting sources produce ``unknown`` rather than a blended answer. +""" + +from __future__ import annotations + +import hashlib +import json +import math +import os +import secrets +import shutil +import time +from collections import Counter, defaultdict, deque +from collections.abc import Mapping +from dataclasses import dataclass, field +from datetime import UTC, datetime +from pathlib import Path +from typing import Any + +import numpy as np + +from k1link.artifacts import write_json_atomic +from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import ( + CalibratedProjectionError, + Kb4ProjectionProfile, + quaternion_xyzw_to_rotation_matrix, + unproject_pixels_kb4, +) +from k1link.sessions import SessionIntegrityError + +from .integrated_perception import IntegratedPerceptionResult +from .jobs import CameraComputeJob +from .world_motion import ( + _artifact, + _box_arrays, + _canonical, + _percentiles, + _read_jsonl, + _read_object, + _rewrite_npz_boxes, + _sha256, + _write_jsonl, +) + +PROFILE_SCHEMA = "missioncore.e26-camera-ego-motion-profile/v1" +BENCHMARK_SCHEMA = "missioncore.e26-motion-benchmark/v1" +PIPELINE_ID = "kb4-multiview-static-hypothesis-lidar-fusion/v1" + + +@dataclass(frozen=True, slots=True) +class _RayObservation: + session_seconds: float + origin_map: np.ndarray + direction_map: np.ndarray + detector_score: float + bbox_diagonal_px: float + + +@dataclass(slots=True) +class _CameraTrack: + source_track_id: int + group: str + observations: deque[_RayObservation] = field(default_factory=deque) + motion_state: str = "unknown" + dynamic_evidence: int = 0 + static_evidence: int = 0 + last_observed_seconds: float = 0.0 + + +@dataclass(frozen=True, slots=True) +class _StaticHypothesis: + point_map: np.ndarray + median_angular_residual_degrees: float + p80_angular_residual_degrees: float + positive_depth_fraction: float + median_range_m: float + ego_baseline_m: float + normal_matrix_condition: float + median_detector_score: float + median_bbox_diagonal_px: float + + +@dataclass(frozen=True, slots=True) +class CameraEgoMotionBuild: + result_root: Path + profile_sha256: str + benchmark_sha256: str + report: dict[str, Any] + + +class CameraEgoMotionTracker: + """Bounded per-detector-track static-world-hypothesis evaluator.""" + + def __init__( + self, + profile: dict[str, Any], + projection: Kb4ProjectionProfile, + ) -> None: + self.profile = profile + self.projection = projection + transform = np.asarray(projection.t_camera_from_lidar, dtype=np.float64) + if transform.shape != (4, 4) or not np.isfinite(transform).all(): + raise SessionIntegrityError("LAB E26 camera transform is invalid") + self.rotation_camera_from_lidar = transform[:3, :3] + self.camera_origin_lidar = ( + -self.rotation_camera_from_lidar.T @ transform[:3, 3] + ) + self.tracks: dict[int, _CameraTrack] = {} + self.created_tracks = 0 + self.peak_tracks = 0 + self.current_hypotheses = 0 + self.published_states: Counter[str] = Counter() + self.conflicts = 0 + self.invalid_camera_rays = 0 + + def observe( + self, + *, + source_track_id: int, + group: str, + session_seconds: float, + bbox_xyxy: list[float], + detector_score: float, + position_map_xyz: np.ndarray, + orientation_map_from_lidar_xyzw: np.ndarray, + ) -> dict[str, Any]: + self._prune(session_seconds) + try: + ray = self._ray_map( + bbox_xyxy, + position_map_xyz, + orientation_map_from_lidar_xyzw, + ) + except CalibratedProjectionError: + self.invalid_camera_rays += 1 + self.published_states["unknown"] += 1 + return _camera_evidence(None, "unknown", 0.0) + track = self.tracks.get(source_track_id) + if track is None: + track = _CameraTrack( + source_track_id=source_track_id, + group=group, + last_observed_seconds=session_seconds, + ) + self.tracks[source_track_id] = track + self.created_tracks += 1 + config = self.profile["camera_evidence"] + diagonal = math.hypot( + float(bbox_xyxy[2]) - float(bbox_xyxy[0]), + float(bbox_xyxy[3]) - float(bbox_xyxy[1]), + ) + track.observations.append( + _RayObservation( + session_seconds=session_seconds, + origin_map=ray[0], + direction_map=ray[1], + detector_score=detector_score, + bbox_diagonal_px=diagonal, + ) + ) + track.last_observed_seconds = session_seconds + self._trim_observations(track, session_seconds) + hypothesis = self._static_hypothesis(track) + if hypothesis is None: + self.published_states["unknown"] += 1 + self.peak_tracks = max(self.peak_tracks, len(self.tracks)) + return _camera_evidence(None, "unknown", 0.0) + self.current_hypotheses += 1 + raw_state = self._raw_state(track.group, hypothesis) + decay = int(config["evidence_decay_frames"]) + if raw_state == "dynamic": + track.dynamic_evidence += 1 + track.static_evidence = max(0, track.static_evidence - decay) + elif raw_state == "static": + track.static_evidence += 1 + track.dynamic_evidence = max(0, track.dynamic_evidence - decay) + else: + track.dynamic_evidence = max(0, track.dynamic_evidence - decay) + track.static_evidence = max(0, track.static_evidence - decay) + if track.dynamic_evidence >= int(config["dynamic_confirmation_frames"]): + track.motion_state = "dynamic" + elif track.static_evidence >= int(config["static_confirmation_frames"]): + track.motion_state = "static" + elif raw_state == "unknown": + track.motion_state = "unknown" + confidence = _camera_confidence( + track, + hypothesis, + config, + ) + self.published_states[track.motion_state] += 1 + self.peak_tracks = max(self.peak_tracks, len(self.tracks)) + return _camera_evidence(hypothesis, track.motion_state, confidence) + + def snapshot(self) -> dict[str, Any]: + return { + "active_tracks": len(self.tracks), + "created_tracks": self.created_tracks, + "peak_tracks": self.peak_tracks, + "current_hypotheses": self.current_hypotheses, + "published_camera_motion_states": dict(self.published_states), + "fusion_conflicts": self.conflicts, + "invalid_camera_rays": self.invalid_camera_rays, + } + + def note_conflict(self) -> None: + self.conflicts += 1 + + def _ray_map( + self, + bbox_xyxy: list[float], + position_map_xyz: np.ndarray, + orientation_map_from_lidar_xyzw: np.ndarray, + ) -> tuple[np.ndarray, np.ndarray]: + if ( + len(bbox_xyxy) != 4 + or not np.isfinite(np.asarray(bbox_xyxy, dtype=np.float64)).all() + or not np.isfinite(position_map_xyz).all() + or not np.isfinite(orientation_map_from_lidar_xyzw).all() + ): + raise SessionIntegrityError("LAB E26 camera observation is invalid") + x1, y1, x2, y2 = (float(value) for value in bbox_xyxy) + if not x1 < x2 or not y1 < y2: + raise SessionIntegrityError("LAB E26 detector box is invalid") + inset = float(self.profile["camera_evidence"]["footpoint_inset_fraction"]) + pixel = np.asarray( + [[(x1 + x2) * 0.5, y2 - inset * (y2 - y1)]], + dtype=np.float64, + ) + direction_camera = unproject_pixels_kb4( + pixel, + profile=self.projection, + )[0] + direction_lidar = self.rotation_camera_from_lidar.T @ direction_camera + rotation_map_from_lidar = quaternion_xyzw_to_rotation_matrix( + ( + float(orientation_map_from_lidar_xyzw[0]), + float(orientation_map_from_lidar_xyzw[1]), + float(orientation_map_from_lidar_xyzw[2]), + float(orientation_map_from_lidar_xyzw[3]), + ) + ) + origin_map = ( + np.asarray(position_map_xyz, dtype=np.float64) + + rotation_map_from_lidar @ self.camera_origin_lidar + ) + direction_map = rotation_map_from_lidar @ direction_lidar + direction_map /= np.linalg.norm(direction_map) + return origin_map, direction_map + + def _static_hypothesis( + self, + track: _CameraTrack, + ) -> _StaticHypothesis | None: + config = self.profile["camera_evidence"] + observations = list(track.observations) + if ( + len(observations) < int(config["minimum_observations"]) + or observations[-1].session_seconds - observations[0].session_seconds + < float(config["minimum_span_seconds"]) + ): + return None + baseline = max( + float( + np.linalg.norm( + observations[-1].origin_map - observation.origin_map + ) + ) + for observation in observations + ) + if baseline < float(config["minimum_ego_baseline_m"]): + return None + identity = np.eye(3, dtype=np.float64) + normal = np.zeros((3, 3), dtype=np.float64) + right = np.zeros(3, dtype=np.float64) + for observation in observations: + projector = identity - np.outer( + observation.direction_map, + observation.direction_map, + ) + normal += projector + right += projector @ observation.origin_map + condition = float(np.linalg.cond(normal)) + if ( + not math.isfinite(condition) + or condition > float(config["maximum_normal_matrix_condition"]) + ): + return None + try: + point = np.linalg.solve(normal, right) + except np.linalg.LinAlgError: + return None + ranges = np.asarray( + [ + float( + observation.direction_map + @ (point - observation.origin_map) + ) + for observation in observations + ], + dtype=np.float64, + ) + perpendicular = np.asarray( + [ + float( + np.linalg.norm( + np.cross( + observation.direction_map, + point - observation.origin_map, + ) + ) + ) + for observation in observations + ], + dtype=np.float64, + ) + angular = np.degrees( + np.arctan2( + perpendicular, + np.maximum(ranges, 1e-6), + ) + ) + return _StaticHypothesis( + point_map=point, + median_angular_residual_degrees=float(np.median(angular)), + p80_angular_residual_degrees=float(np.percentile(angular, 80)), + positive_depth_fraction=float(np.mean(ranges > 0.0)), + median_range_m=float(np.median(ranges)), + ego_baseline_m=baseline, + normal_matrix_condition=condition, + median_detector_score=float( + np.median( + [observation.detector_score for observation in observations] + ) + ), + median_bbox_diagonal_px=float( + np.median( + [observation.bbox_diagonal_px for observation in observations] + ) + ), + ) + + def _raw_state( + self, + group: str, + hypothesis: _StaticHypothesis, + ) -> str: + config = self.profile["camera_evidence"] + large_enough = ( + hypothesis.median_bbox_diagonal_px + >= float(config["minimum_bbox_diagonal_px"]) + ) + negative_depth_dynamic = ( + large_enough + and hypothesis.positive_depth_fraction + < float(config["negative_depth_dynamic_fraction"]) + and hypothesis.median_detector_score + >= float(config["minimum_negative_depth_detector_score"]) + ) + residual_dynamic = ( + large_enough + and hypothesis.positive_depth_fraction + >= float(config["minimum_valid_positive_depth_fraction"]) + and hypothesis.median_range_m + >= float(config["minimum_static_hypothesis_range_m"]) + and hypothesis.median_detector_score + >= float(config["minimum_median_detector_score"]) + and hypothesis.median_angular_residual_degrees + > _group_value( + config["dynamic_median_angular_residual_degrees"], + group, + ) + and hypothesis.p80_angular_residual_degrees + > _group_value( + config["dynamic_p80_angular_residual_degrees"], + group, + ) + ) + if negative_depth_dynamic or residual_dynamic: + return "dynamic" + static = ( + hypothesis.positive_depth_fraction >= 0.9 + and float(config["minimum_static_hypothesis_range_m"]) + <= hypothesis.median_range_m + <= float(config["maximum_static_hypothesis_range_m"]) + and hypothesis.median_angular_residual_degrees + < _group_value( + config["static_median_angular_residual_degrees"], + group, + ) + and hypothesis.p80_angular_residual_degrees + < _group_value( + config["static_p80_angular_residual_degrees"], + group, + ) + ) + return "static" if static else "unknown" + + def _trim_observations( + self, + track: _CameraTrack, + session_seconds: float, + ) -> None: + config = self.profile["camera_evidence"] + history_seconds = float(config["history_seconds"]) + while ( + track.observations + and session_seconds - track.observations[0].session_seconds + > history_seconds + ): + track.observations.popleft() + maximum = int(config["maximum_observations_per_track"]) + while len(track.observations) > maximum: + track.observations.popleft() + + def _prune(self, session_seconds: float) -> None: + maximum_idle = float( + self.profile["bounds"]["maximum_track_idle_seconds"] + ) + expired = [ + track_id + for track_id, track in self.tracks.items() + if session_seconds - track.last_observed_seconds > maximum_idle + ] + for track_id in expired: + del self.tracks[track_id] + maximum_tracks = int(self.profile["bounds"]["maximum_tracks"]) + if len(self.tracks) <= maximum_tracks: + return + overflow = len(self.tracks) - maximum_tracks + for track in sorted( + self.tracks.values(), + key=lambda value: ( + value.last_observed_seconds, + value.source_track_id, + ), + )[:overflow]: + del self.tracks[track.source_track_id] + + +def read_camera_ego_motion_profile( + path: Path, +) -> tuple[dict[str, Any], str]: + resolved = path.expanduser().resolve(strict=True) + profile = _read_object(resolved) + camera = profile.get("camera_evidence") + bounds = profile.get("bounds") + if ( + profile.get("schema_version") != PROFILE_SCHEMA + or profile.get("mode") != "recorded-streaming-qualification" + or profile.get("authority") + != {"commands_enabled": False, "navigation_or_safety_accepted": False} + or not isinstance(camera, dict) + or not isinstance(bounds, dict) + or not 0.1 <= float(camera.get("history_seconds", 0.0)) <= 5.0 + or not 3 <= int(camera.get("minimum_observations", 0)) <= 32 + or not 1 <= int(camera.get("maximum_observations_per_track", 0)) <= 64 + or not 1 <= int(bounds.get("maximum_tracks", 0)) <= 512 + or float(camera.get("negative_depth_dynamic_fraction", 1.0)) + >= float(camera.get("minimum_valid_positive_depth_fraction", 0.0)) + ): + raise SessionIntegrityError("LAB E26 camera/ego-motion profile is invalid") + return profile, _sha256(resolved) + + +def read_camera_ego_motion_benchmark( + path: Path, +) -> tuple[dict[str, Any], str]: + resolved = path.expanduser().resolve(strict=True) + benchmark = _read_object(resolved) + events = benchmark.get("events") + if ( + benchmark.get("schema_version") != BENCHMARK_SCHEMA + or benchmark.get("source_session_id") + != "20260720T065719Z_viewer_live" + or not isinstance(events, list) + or not events + ): + raise SessionIntegrityError("LAB E26 motion benchmark is invalid") + identifiers: set[str] = set() + for event in events: + if ( + not isinstance(event, dict) + or not isinstance(event.get("id"), str) + or event["id"] in identifiers + or event.get("kind") not in {"target-motion", "static-control"} + or event.get("expected_motion") not in {"static", "dynamic"} + or event.get("class_group") + not in { + "person", + "vehicle", + "bicycle", + "vulnerable_road_user", + } + or not isinstance(event.get("window_seconds"), list) + or len(event["window_seconds"]) != 2 + or float(event["window_seconds"][0]) + >= float(event["window_seconds"][1]) + ): + raise SessionIntegrityError("LAB E26 benchmark event is invalid") + if event["kind"] == "target-motion": + targets = event.get("target_source_track_ids") + if ( + not isinstance(targets, list) + or not targets + or not all( + isinstance(value, int) + and not isinstance(value, bool) + and value > 0 + for value in targets + ) + ): + raise SessionIntegrityError("LAB E26 target binding is invalid") + identifiers.add(event["id"]) + return benchmark, _sha256(resolved) + + +def evaluate_camera_ego_motion_benchmark( + source_rows: list[dict[str, Any]], + fusion_rows: list[dict[str, Any]], + benchmark: dict[str, Any], +) -> dict[str, Any]: + outcomes: list[dict[str, Any]] = [] + for event in benchmark["events"]: + start, end = (float(value) for value in event["window_seconds"]) + targets = { + int(value) for value in event.get("target_source_track_ids", []) + } + source_keys: set[tuple[int, int]] = set() + for row in source_rows: + timestamp = float(row["session_seconds"]) + if not start <= timestamp <= end: + continue + for item in row["objects"]: + source_id = int(item.get("source_track_id", item["track_id"])) + if _event_class_matches(item, event) and ( + event["kind"] == "static-control" + or source_id in targets + ): + source_keys.add((int(row["frame_index"]), source_id)) + evidence: dict[tuple[int, int], tuple[float, dict[str, Any]]] = {} + duplicates = 0 + for row in fusion_rows: + timestamp = float(row["session_seconds"]) + if not start <= timestamp <= end: + continue + for item in row["objects"]: + source_id = item.get("source_track_id") + if ( + isinstance(source_id, int) + and item.get("camera_evidence_current") is True + and _event_class_matches(item, event) + and ( + event["kind"] == "static-control" + or source_id in targets + ) + ): + key = (int(row["frame_index"]), source_id) + if key in evidence: + duplicates += 1 + evidence[key] = (timestamp, item) + matched = sorted(evidence.values(), key=lambda value: value[0]) + source_observations = len(source_keys) + coverage = len(matched) / max(1, source_observations) + if event["kind"] == "static-control": + counts = Counter( + str(item["motion_state"]) for _, item in matched + ) + classified = counts["static"] + counts["dynamic"] + classified_fraction = classified / max(1, len(matched)) + false_dynamic_fraction = counts["dynamic"] / max(1, len(matched)) + passed = ( + source_observations + >= int(event["minimum_source_observations"]) + and coverage >= float(event["minimum_coverage_fraction"]) + and classified_fraction + >= float(event["minimum_classified_fraction"]) + and false_dynamic_fraction + <= float(event["maximum_false_dynamic_fraction"]) + ) + outcomes.append( + { + "id": event["id"], + "kind": event["kind"], + "expected_motion": event["expected_motion"], + "class_group": event["class_group"], + "window_seconds": [start, end], + "passed": passed, + "source_observations": source_observations, + "evidence_observations": len(matched), + "duplicate_evidence_observations": duplicates, + "coverage_fraction": coverage, + "classified_fraction": classified_fraction, + "false_dynamic_fraction": false_dynamic_fraction, + "state_counts": dict(counts), + } + ) + continue + by_source: dict[int, list[float]] = defaultdict(list) + for timestamp, item in matched: + if item.get("motion_state") == event["expected_motion"]: + by_source[int(item["source_track_id"])].append(timestamp) + best_source: int | None = None + best_hits = 0 + best_span = 0.0 + for source_id, timestamps in by_source.items(): + span = max(timestamps) - min(timestamps) + if (len(timestamps), span) > (best_hits, best_span): + best_source = source_id + best_hits = len(timestamps) + best_span = span + passed = ( + coverage >= float(event["minimum_coverage_fraction"]) + and best_hits >= int(event["minimum_hits"]) + and best_span >= float(event["minimum_span_seconds"]) + ) + outcomes.append( + { + "id": event["id"], + "kind": event["kind"], + "expected_motion": event["expected_motion"], + "class_group": event["class_group"], + "target_source_track_ids": sorted(targets), + "window_seconds": [start, end], + "passed": passed, + "source_observations": source_observations, + "evidence_observations": len(matched), + "duplicate_evidence_observations": duplicates, + "coverage_fraction": coverage, + "best_source_track_id": best_source, + "best_hits": best_hits, + "best_span_seconds": best_span, + } + ) + return { + "schema_version": "missioncore.e26-motion-benchmark-result/v1", + "passed": all(item["passed"] for item in outcomes), + "passed_events": sum(item["passed"] for item in outcomes), + "total_events": len(outcomes), + "events": outcomes, + } + + +def build_camera_ego_motion_result( + *, + lidar_source: IntegratedPerceptionResult, + camera_source: IntegratedPerceptionResult, + lab_job: CameraComputeJob, + lab_pack: Path, + results_root: Path, + profile_path: Path, + benchmark_path: Path, +) -> CameraEgoMotionBuild: + """Build one immutable E26 result without rerunning model inference.""" + + if not lidar_source.accepted or not camera_source.accepted: + raise SessionIntegrityError("LAB E26 source result is not accepted") + profile, profile_sha256 = read_camera_ego_motion_profile(profile_path) + benchmark, benchmark_sha256 = read_camera_ego_motion_benchmark( + benchmark_path + ) + if camera_source.job.session_id != benchmark["source_session_id"]: + raise SessionIntegrityError("LAB E26 benchmark source session differs") + lidar_document = _read_object(lidar_source.result_root / "result.json") + camera_document = _read_object(camera_source.result_root / "result.json") + lidar_configuration = lidar_document["identity"].get("configuration") + if ( + not isinstance(lidar_configuration, dict) + or lidar_configuration.get("source_result_id") + != camera_source.result_id + ): + raise SessionIntegrityError( + "LAB E26 LiDAR result is not derived from the camera source" + ) + camera_rows = _read_jsonl(camera_source.fusion_path) + lidar_rows = _read_jsonl(lidar_source.fusion_path) + lidar_world_rows = _read_jsonl(lidar_source.world_path) + if not ( + len(camera_rows) + == len(lidar_rows) + == len(lidar_world_rows) + == camera_source.frame_count + ): + raise SessionIntegrityError("LAB E26 source timelines are incomplete") + + with np.load(lab_pack / "lidar-pack.npz", allow_pickle=False) as arrays: + pose_positions = np.asarray( + arrays["pose_positions_map"], + dtype=np.float64, + ).copy() + pose_quaternions = np.asarray( + arrays["pose_quaternions_map_from_lidar"], + dtype=np.float64, + ).copy() + projection = Kb4ProjectionProfile( + source_id=lab_job.source_id, + calibration_slot=str(profile["source"]["calibration_slot"]), + width=800, + height=600, + intrinsic_fx_fy_cx_cy=( + float(arrays["intrinsic_fx_fy_cx_cy"][0]), + float(arrays["intrinsic_fx_fy_cx_cy"][1]), + float(arrays["intrinsic_fx_fy_cx_cy"][2]), + float(arrays["intrinsic_fx_fy_cx_cy"][3]), + ), + distortion_kb4=( + float(arrays["distortion_kb4"][0]), + float(arrays["distortion_kb4"][1]), + float(arrays["distortion_kb4"][2]), + float(arrays["distortion_kb4"][3]), + ), + t_camera_from_lidar=np.asarray( + arrays["t_camera_from_lidar"], + dtype=np.float64, + ).copy(), + ) + if ( + pose_positions.shape != (camera_source.frame_count, 3) + or pose_quaternions.shape != (camera_source.frame_count, 4) + ): + raise SessionIntegrityError("LAB E26 pose timeline shape changed") + + tracker = CameraEgoMotionTracker(profile, projection) + fusion_rows: list[dict[str, Any]] = [] + world_rows: list[dict[str, Any]] = [] + frame_ms: list[float] = [] + pose_unavailable_frames = 0 + camera_observations = 0 + for index, (camera_row, lidar_row, lidar_world_row) in enumerate( + zip(camera_rows, lidar_rows, lidar_world_rows, strict=True) + ): + started = time.perf_counter() + if ( + camera_row["frame_index"] != lidar_row["frame_index"] + or camera_row["source_frame_index"] + != lidar_row["source_frame_index"] + ): + raise SessionIntegrityError("LAB E26 source timelines diverged") + position = pose_positions[index] + quaternion = pose_quaternions[index] + pose_available = bool( + np.isfinite(position).all() and np.isfinite(quaternion).all() + ) + if not pose_available: + pose_unavailable_frames += 1 + lidar_by_source = _lidar_objects_by_source(lidar_row["objects"]) + normalized_objects: list[dict[str, Any]] = [] + for camera_object in camera_row["objects"]: + source_track_id = int( + camera_object.get( + "source_track_id", + camera_object["track_id"], + ) + ) + evidence = _camera_evidence(None, "unknown", 0.0) + if pose_available: + evidence = tracker.observe( + source_track_id=source_track_id, + group=str(camera_object["association_group"]), + session_seconds=float(camera_row["session_seconds"]), + bbox_xyxy=[ + float(value) + for value in camera_object["bbox_xyxy"] + ], + detector_score=float(camera_object["score"]), + position_map_xyz=position, + orientation_map_from_lidar_xyzw=quaternion, + ) + camera_observations += 1 + normalized_objects.append( + _fuse_camera_lidar_object( + camera_object, + lidar_by_source.get(source_track_id), + evidence, + profile, + tracker, + ) + ) + normalized_fusion = json.loads(json.dumps(camera_row)) + normalized_fusion["objects"] = normalized_objects + normalized_fusion["world_motion"] = { + "schema_version": "missioncore.e26-camera-ego-motion-frame/v1", + "coordinate_frame": "k1-map", + "lookahead_frames": 0, + "camera_measurement": "kb4-multiview-static-world-hypothesis", + "metric_measurement": "e25-persistent-lidar-support", + "unknown_is_occupied": True, + } + normalized_world = json.loads(json.dumps(lidar_world_row)) + normalized_world["objects"] = _update_world_objects( + normalized_world["objects"], + normalized_objects, + ) + normalized_world["object_count"] = len(normalized_world["objects"]) + elapsed = (time.perf_counter() - started) * 1000.0 + delivery = normalized_world.setdefault("delivery", {}) + delivery["world_motion"] = "e26-camera-ego-lidar-conservative-fusion" + delivery["world_motion_processing_ms"] = elapsed + fusion_rows.append(normalized_fusion) + world_rows.append(normalized_world) + frame_ms.append(elapsed) + + benchmark_result = evaluate_camera_ego_motion_benchmark( + camera_rows, + fusion_rows, + benchmark, + ) + runtime = _percentiles(frame_ms) + tracker_metrics = tracker.snapshot() + checks = { + "source_payloads_immutable": True, + "diagnostic_authority_only": True, + "single_pass_no_lookahead": True, + "track_state_bound": tracker_metrics["peak_tracks"] + <= int(profile["acceptance"]["maximum_tracks_observed"]), + "processing_p95_ms": runtime["p95"] + <= float(profile["acceptance"]["maximum_processing_p95_ms"]), + } + accepted = all(checks.values()) + camera_identity = camera_document["identity"] + result_configuration = { + "pipeline": PIPELINE_ID, + "profile_sha256": profile_sha256, + "profile": profile, + "implementation_sha256": _sha256(Path(__file__).resolve()), + "benchmark_sha256": benchmark_sha256, + "benchmark": benchmark, + "lidar_source_result_id": lidar_source.result_id, + "lidar_source_result_sha256": _sha256( + lidar_source.result_root / "result.json" + ), + "source_result_id": camera_source.result_id, + "source_result_sha256": _sha256( + camera_source.result_root / "result.json" + ), + "source_session_id": camera_source.job.session_id, + "processing": "single-pass-bounded-state-no-lookahead", + } + identity = { + "schema_version": "missioncore.e10-integrated-perception-identity/v1", + "job_id": lab_job.job_id, + "input_sha256": lab_job.input_sha256, + "session_id": lab_job.session_id, + "source_id": lab_job.source_id, + "lidar_pack_id": lab_pack.name, + "selection": json.loads(json.dumps(camera_identity["selection"])), + "configuration": result_configuration, + "models": camera_identity["models"], + } + identity_sha256 = hashlib.sha256(_canonical(identity)).hexdigest() + result_id = f"e10-integrated-perception-{identity_sha256}" + destination = results_root / result_id + created_at = ( + datetime.now(UTC) + .isoformat(timespec="milliseconds") + .replace("+00:00", "Z") + ) + report = { + "schema_version": "missioncore.e10-integrated-perception-report/v1", + "result_id": result_id, + "created_at_utc": created_at, + "state": "accepted" if accepted else "rejected", + "ground_truth": False, + "identity": identity, + "acceptance": { + "accepted": accepted, + "navigation_or_safety_accepted": False, + "checks": checks, + }, + "metrics": { + "runtime": { + "camera_ego_motion_frame_processing_ms": runtime, + "camera_observations": camera_observations, + "pose_unavailable_frames": pose_unavailable_frames, + **tracker_metrics, + }, + "benchmark": benchmark_result, + }, + "runtime": { + "executor": "local-deterministic-postprocessor", + "numpy": np.__version__, + "streaming_equivalent": True, + "lookahead_frames": 0, + }, + "limitations": [ + "Recorded diagnostic qualification only; no navigation or command authority.", + ( + "Operator timestamps and detector-track bindings are reviewed " + "annotations, not independent ground truth." + ), + ( + "Camera-only dynamic is relative-motion evidence and does not " + "create a metric velocity or free-space claim." + ), + ( + "LiDAR/camera disagreement is explicit unknown; unknown remains " + "occupied for planner-facing consumers." + ), + ( + "Raw LiDAR, ground returns, camera media, calibration and model " + "outputs are unchanged." + ), + ], + } + if destination.exists(): + existing = _read_object(destination / "result.json") + if existing.get("identity") != identity: + raise SessionIntegrityError("LAB E26 result id collides") + return CameraEgoMotionBuild( + destination, + profile_sha256, + benchmark_sha256, + _read_object(destination / "run-report.json"), + ) + + results_root.mkdir(mode=0o700, parents=True, exist_ok=True) + staging = results_root / f".{result_id}.{secrets.token_hex(12)}.tmp" + staging.mkdir(mode=0o700) + try: + semantic_path = staging / "semantic-frames.jsonl" + fusion_path = staging / "fusion-frames.jsonl" + world_path = staging / "world-state.jsonl" + arrays_path = staging / "transient-perception.npz" + gpu_path = staging / "gpu-telemetry.jsonl" + report_path = staging / "run-report.json" + os.link(camera_source.semantic_path, semantic_path, follow_symlinks=False) + _write_jsonl(fusion_path, fusion_rows) + _write_jsonl(world_path, world_rows) + box_offsets, centers, half_sizes, quaternions, box_colors = _box_arrays( + fusion_rows + ) + _rewrite_npz_boxes( + lidar_source.arrays_path, + arrays_path, + { + "box_offsets": box_offsets, + "box_centers": centers, + "box_half_sizes": half_sizes, + "box_quaternions": quaternions, + "box_colors": box_colors, + }, + ) + os.link( + camera_source.result_root / "gpu-telemetry.jsonl", + gpu_path, + follow_symlinks=False, + ) + write_json_atomic(report_path, report) + artifacts = [ + _artifact( + "e10-semantic-frames", + semantic_path, + "missioncore.e10-semantic-frame/v1", + ), + _artifact( + "e10-fusion-frames", + fusion_path, + "missioncore.e10-fusion-frame/v1", + ), + _artifact( + "e10-world-state", + world_path, + "missioncore.live-perception-world-state/v1", + ), + _artifact("e10-transient-perception", arrays_path, None), + _artifact("worker-gpu-telemetry", gpu_path, None), + _artifact( + "e10-run-report", + report_path, + "missioncore.e10-integrated-perception-report/v1", + ), + ] + write_json_atomic( + staging / "result.json", + { + "schema_version": "missioncore.e10-integrated-perception-result/v1", + "result_id": result_id, + "identity_sha256": identity_sha256, + "identity": identity, + "created_at_utc": created_at, + "ground_truth": False, + "publication_scope": ( + "recorded-integrated-realtime-qualification-only" + ), + "acceptance_state": "accepted" if accepted else "rejected", + "frames_processed": camera_source.frame_count, + "artifacts": artifacts, + }, + ) + os.replace(staging, destination) + finally: + if staging.exists(): + shutil.rmtree(staging) + return CameraEgoMotionBuild( + destination, + profile_sha256, + benchmark_sha256, + report, + ) + + +def _lidar_objects_by_source( + objects: list[dict[str, Any]], +) -> dict[int, dict[str, Any]]: + result: dict[int, dict[str, Any]] = {} + for item in objects: + source_id = item.get("source_track_id") + if not isinstance(source_id, int): + continue + previous = result.get(source_id) + if previous is None or _lidar_rank(item) > _lidar_rank(previous): + result[source_id] = item + return result + + +def _lidar_rank(item: dict[str, Any]) -> tuple[int, int, float, int]: + return ( + int(item.get("occupancy_evidence_current") is True), + int(item.get("occupancy_cell_count", 0)), + float(item.get("motion_confidence", 0.0)), + -int(item.get("track_id", 0)), + ) + + +def _fuse_camera_lidar_object( + camera_object: dict[str, Any], + lidar_object: dict[str, Any] | None, + camera_evidence: dict[str, Any], + profile: dict[str, Any], + tracker: CameraEgoMotionTracker, +) -> dict[str, Any]: + item: dict[str, Any] = json.loads( + json.dumps(lidar_object if lidar_object is not None else camera_object) + ) + source_track_id = int( + camera_object.get("source_track_id", camera_object["track_id"]) + ) + for key in ( + "association_group", + "bbox_xyxy", + "label", + "score", + ): + item[key] = json.loads(json.dumps(camera_object[key])) + item["source_track_id"] = source_track_id + item.update(camera_evidence) + lidar_state = ( + str(lidar_object.get("motion_state", "unknown")) + if lidar_object is not None + else "unknown" + ) + lidar_confidence = ( + float(lidar_object.get("motion_confidence", 0.0)) + if lidar_object is not None + else 0.0 + ) + camera_state = str(camera_evidence["camera_motion_state"]) + camera_confidence = float(camera_evidence["camera_motion_confidence"]) + item["lidar_motion_state"] = lidar_state + item["lidar_motion_confidence"] = lidar_confidence + if ( + camera_state in {"static", "dynamic"} + and lidar_state in {"static", "dynamic"} + and camera_state != lidar_state + ): + item["motion_state"] = str(profile["fusion"]["conflict_state"]) + item["motion_confidence"] = 0.0 + item["motion_status"] = "e26-explicit-camera-lidar-conflict" + item["speed_mps"] = None + item["velocity_map_mps"] = None + tracker.note_conflict() + elif ( + camera_state in {"static", "dynamic"} + and camera_state == lidar_state + ): + item["motion_state"] = camera_state + item["motion_confidence"] = min( + camera_confidence, + lidar_confidence, + ) + item["motion_status"] = "e26-camera-lidar-agree" + elif camera_state in {"static", "dynamic"}: + item["motion_state"] = camera_state + item["motion_confidence"] = ( + camera_confidence + * float(profile["fusion"]["camera_only_confidence_scale"]) + ) + item["motion_status"] = "e26-camera-relative-only-no-metric-velocity" + item["speed_mps"] = None + item["velocity_map_mps"] = None + elif lidar_state in {"static", "dynamic"}: + item["motion_state"] = lidar_state + item["motion_confidence"] = lidar_confidence + item["motion_status"] = "e26-lidar-only" + else: + item["motion_state"] = "unknown" + item["motion_confidence"] = 0.0 + item["motion_status"] = "e26-insufficient-independent-evidence" + item["speed_mps"] = None + item["velocity_map_mps"] = None + item["unknown_is_occupied"] = True + item["camera_only_metric_velocity_valid"] = False + item["navigation_or_safety_accepted"] = False + return item + + +def _update_world_objects( + world_objects: list[dict[str, Any]], + fusion_objects: list[dict[str, Any]], +) -> list[dict[str, Any]]: + by_track = { + int(item["track_id"]): item + for item in fusion_objects + if isinstance(item.get("track_id"), int) + and item.get("cuboid_status") + } + result: list[dict[str, Any]] = [] + for source in world_objects: + item = json.loads(json.dumps(source)) + fusion = by_track.get(int(item["track_id"])) + if fusion is None: + result.append(item) + continue + for key in ( + "motion_state", + "motion_confidence", + "motion_status", + "speed_mps", + "velocity_map_mps", + "camera_motion_state", + "camera_motion_confidence", + "camera_evidence_current", + "camera_static_residual_median_degrees", + "camera_static_residual_p80_degrees", + "camera_static_positive_depth_fraction", + "camera_static_hypothesis_range_m", + "camera_ego_baseline_m", + "camera_lidar_evidence_relation", + "lidar_motion_state", + "lidar_motion_confidence", + "unknown_is_occupied", + "camera_only_metric_velocity_valid", + "navigation_or_safety_accepted", + ): + if key in fusion: + item[key] = fusion[key] + item["velocity_status"] = fusion["motion_status"] + result.append(item) + return result + + +def _camera_evidence( + hypothesis: _StaticHypothesis | None, + motion_state: str, + confidence: float, +) -> dict[str, Any]: + if hypothesis is None: + return { + "camera_motion_state": "unknown", + "camera_motion_confidence": 0.0, + "camera_evidence_current": False, + "camera_static_residual_median_degrees": None, + "camera_static_residual_p80_degrees": None, + "camera_static_positive_depth_fraction": None, + "camera_static_hypothesis_range_m": None, + "camera_ego_baseline_m": None, + "camera_normal_matrix_condition": None, + } + return { + "camera_motion_state": motion_state, + "camera_motion_confidence": confidence, + "camera_evidence_current": True, + "camera_static_residual_median_degrees": ( + hypothesis.median_angular_residual_degrees + ), + "camera_static_residual_p80_degrees": ( + hypothesis.p80_angular_residual_degrees + ), + "camera_static_positive_depth_fraction": ( + hypothesis.positive_depth_fraction + ), + "camera_static_hypothesis_range_m": hypothesis.median_range_m, + "camera_ego_baseline_m": hypothesis.ego_baseline_m, + "camera_normal_matrix_condition": hypothesis.normal_matrix_condition, + "camera_median_detector_score": hypothesis.median_detector_score, + "camera_median_bbox_diagonal_px": ( + hypothesis.median_bbox_diagonal_px + ), + } + + +def _camera_confidence( + track: _CameraTrack, + hypothesis: _StaticHypothesis, + config: Mapping[str, Any], +) -> float: + if track.motion_state == "dynamic": + confirmed = track.dynamic_evidence / max( + 1, + int(config["dynamic_confirmation_frames"]), + ) + residual = hypothesis.p80_angular_residual_degrees / max( + 1e-6, + _group_value( + config["dynamic_p80_angular_residual_degrees"], + track.group, + ), + ) + return min(1.0, 0.5 * confirmed + 0.5 * min(1.0, residual)) + if track.motion_state == "static": + confirmed = track.static_evidence / max( + 1, + int(config["static_confirmation_frames"]), + ) + residual_limit = _group_value( + config["static_p80_angular_residual_degrees"], + track.group, + ) + quality = max( + 0.0, + 1.0 - hypothesis.p80_angular_residual_degrees / residual_limit, + ) + return min(1.0, 0.6 * confirmed + 0.4 * quality) + return 0.0 + + +def _event_class_matches( + item: dict[str, Any], + event: dict[str, Any], +) -> bool: + group = str( + item.get( + "association_group", + item.get("class", item.get("label", "object")), + ) + ) + if event["class_group"] == "vulnerable_road_user": + return group in {"person", "bicycle", "motorcycle"} + return group == str(event["class_group"]) + + +def _group_value(values: Mapping[str, Any], group: str) -> float: + return float(values.get(group, values["default"])) diff --git a/src/k1link/compute/lab_instances.py b/src/k1link/compute/lab_instances.py index 5184d9d..b8ca4bd 100644 --- a/src/k1link/compute/lab_instances.py +++ b/src/k1link/compute/lab_instances.py @@ -22,6 +22,10 @@ from k1link.sessions import ( publish_lab_replay_cache, ) +from .camera_ego_motion import ( + CameraEgoMotionBuild, + build_camera_ego_motion_result, +) from .inline_temporal import StreamingSemanticStabilizer, read_inline_profile from .integrated_perception import ( IntegratedPerceptionResult, @@ -71,6 +75,14 @@ class PublishedPersistentSupportLabInstance: build: PersistentSupportBuild +@dataclass(frozen=True, slots=True) +class PublishedCameraEgoMotionLabInstance: + binding: LabSessionBinding + job: CameraComputeJob + result: IntegratedPerceptionResult + build: CameraEgoMotionBuild + + def publish_integrated_lab_instance( *, repository_root: Path, @@ -709,6 +721,144 @@ def publish_e25_lab_instance( ) +def publish_e26_lab_instance( + *, + repository_root: Path, + lidar_result_root: Path, + camera_result_root: Path, + profile_path: Path, + benchmark_path: Path, + lab_session_id: str, + lab_id: str, + display_name: str, +) -> PublishedCameraEgoMotionLabInstance: + """Derive and publish one bounded camera/ego-motion evidence LAB run.""" + + root = repository_root.expanduser().resolve(strict=True) + jobs_root = root / ".runtime" / "compute-jobs" + results_root = root / ".runtime" / "compute-experiments" / "e10" / "worker-results" + packs_root = root / ".runtime" / "compute-experiments" / "e10" / "lidar-packs" + + def source(path: Path, label: str) -> IntegratedPerceptionResult: + resolved = path.expanduser().resolve(strict=True) + document = _read_object(resolved / "result.json", resolved) + identity = document.get("identity") + if not isinstance(identity, dict) or not isinstance(identity.get("job_id"), str): + raise SessionIntegrityError(f"E26 {label} source has no job identity") + validated = validate_integrated_perception_result( + jobs_root / identity["job_id"], + resolved, + packs_root, + ) + if not validated.accepted: + raise SessionIntegrityError(f"E26 {label} source result is not accepted") + return validated + + lidar_source = source(lidar_result_root, "LiDAR") + camera_source = source(camera_result_root, "camera") + if lidar_source.job.source_id != camera_source.job.source_id: + raise SessionIntegrityError("E26 source camera identities differ") + + lab_job = _publish_lab_job(lidar_source.job, jobs_root, lab_session_id) + lab_pack = _publish_lab_pack( + lidar_source, + lab_job, + packs_root, + lab_session_id, + ) + build = build_camera_ego_motion_result( + lidar_source=lidar_source, + camera_source=camera_source, + lab_job=lab_job, + lab_pack=lab_pack, + results_root=results_root, + profile_path=profile_path, + benchmark_path=benchmark_path, + ) + validated = validate_integrated_perception_result( + lab_job.job_root, + build.result_root, + packs_root, + ) + if not validated.accepted: + failed = [ + name + for name, accepted in build.report["acceptance"]["checks"].items() + if not accepted + ] + raise SessionIntegrityError( + f"E26 camera/ego-motion artifact acceptance failed: {', '.join(failed)}" + ) + + store = SessionStore(root) + source_lab = store.get_lab_instance(lidar_source.job.session_id) + source_session_id = ( + lidar_source.job.session_id + if source_lab is None + else source_lab.source_session_id + ) + publish_lab_replay_cache( + store.data_dir, + source_session_id=source_session_id, + lab_session_id=lab_session_id, + timeline_start_ns=round( + validated.timeline_start_seconds * 1_000_000_000 + ), + timeline_end_ns=round( + validated.timeline_end_seconds * 1_000_000_000 + ), + ) + metrics = build.report["metrics"] + binding = store.publish_lab_instance( + session_id=lab_session_id, + source_session_id=source_session_id, + display_name=display_name, + lab_id=lab_id, + result_kind="e26-camera-ego-motion-fusion", + result_id=validated.result_id, + source_result_id=lidar_source.result_id, + config_sha256=build.profile_sha256, + run_created_at_utc=validated.created_at_utc, + duration_seconds=( + validated.timeline_end_seconds + - validated.timeline_start_seconds + ), + include_recorded_media=False, + provenance={ + "schema_version": "missioncore.e26-lab-publication/v1", + "storage_mode": ( + "bounded-camera-ego-motion-and-immutable-lidar-source-replay" + ), + "source_result_id": lidar_source.result_id, + "camera_source_result_id": camera_source.result_id, + "source_lab_session_id": ( + None if source_lab is None else source_lab.session_id + ), + "source_payloads_mutated": False, + "coordinate_frame": "k1-map", + "camera_measurement": "kb4-multiview-static-world-hypothesis", + "metric_measurement": "e25-persistent-lidar-support", + "lookahead_frames": 0, + "benchmark_sha256": build.benchmark_sha256, + "benchmark_passed": metrics["benchmark"]["passed"], + "benchmark_passed_events": metrics["benchmark"]["passed_events"], + "benchmark_total_events": metrics["benchmark"]["total_events"], + "camera_ego_motion_processing_p95_ms": metrics["runtime"][ + "camera_ego_motion_frame_processing_ms" + ]["p95"], + "peak_tracks": metrics["runtime"]["peak_tracks"], + "camera_only_metric_velocity_valid": False, + "navigation_or_safety_accepted": False, + }, + ) + return PublishedCameraEgoMotionLabInstance( + binding=binding, + job=lab_job, + result=validated, + build=build, + ) + + def _validate_e23_inputs( worker_root: Path, source_report_path: Path, diff --git a/src/k1link/device_plugins/xgrids_k1/analyze/__init__.py b/src/k1link/device_plugins/xgrids_k1/analyze/__init__.py index 614aed7..931431b 100644 --- a/src/k1link/device_plugins/xgrids_k1/analyze/__init__.py +++ b/src/k1link/device_plugins/xgrids_k1/analyze/__init__.py @@ -13,6 +13,7 @@ from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import ( map_points_to_lidar, project_map_points_kb4, quaternion_xyzw_to_rotation_matrix, + unproject_pixels_kb4, ) from k1link.device_plugins.xgrids_k1.analyze.stream_summary import ( DEFAULT_STREAM_SUMMARY_MAX_PAYLOAD_BYTES, @@ -47,5 +48,6 @@ __all__ = [ "quaternion_xyzw_to_rotation_matrix", "run_calibrated_overlay_experiment", "summarize_mqtt_streams", + "unproject_pixels_kb4", "validate_k1_valid_fov_mask", ] diff --git a/src/k1link/device_plugins/xgrids_k1/analyze/calibrated_projection.py b/src/k1link/device_plugins/xgrids_k1/analyze/calibrated_projection.py index 6453522..e04ea92 100644 --- a/src/k1link/device_plugins/xgrids_k1/analyze/calibrated_projection.py +++ b/src/k1link/device_plugins/xgrids_k1/analyze/calibrated_projection.py @@ -205,6 +205,70 @@ def project_map_points_kb4( ) +def unproject_pixels_kb4( + pixels_xy: npt.ArrayLike, + *, + profile: Kb4ProjectionProfile, +) -> FloatArray: + """Return unit camera-frame rays for finite pixels under the KB4 model.""" + + pixels = np.asarray(pixels_xy, dtype=np.float64) + if ( + pixels.ndim != 2 + or pixels.shape[1:] != (2,) + or not np.isfinite(pixels).all() + ): + raise CalibratedProjectionError("camera pixels must have shape (N, 2)") + if pixels.size == 0: + return np.empty((0, 3), dtype=np.float64) + fx, fy, cx, cy = profile.intrinsic_fx_fy_cx_cy + if not all(math.isfinite(value) and value > 0.0 for value in (fx, fy)): + raise CalibratedProjectionError("camera focal lengths must be positive") + distorted_x = (pixels[:, 0] - cx) / fx + distorted_y = (pixels[:, 1] - cy) / fy + theta_distorted = np.hypot(distorted_x, distorted_y) + theta = theta_distorted.copy() + k1, k2, k3, k4 = profile.distortion_kb4 + for _ in range(12): + squared = theta * theta + polynomial = ( + 1.0 + + k1 * squared + + k2 * squared**2 + + k3 * squared**3 + + k4 * squared**4 + ) + derivative = ( + 1.0 + + 3.0 * k1 * squared + + 5.0 * k2 * squared**2 + + 7.0 * k3 * squared**3 + + 9.0 * k4 * squared**4 + ) + if np.any(np.abs(derivative) < 1e-9): + raise CalibratedProjectionError("KB4 inverse derivative became singular") + theta -= (theta * polynomial - theta_distorted) / derivative + if not np.isfinite(theta).all() or np.any(theta < 0.0) or np.any(theta >= math.pi): + raise CalibratedProjectionError("KB4 inverse produced invalid angles") + scale = np.divide( + np.sin(theta), + theta_distorted, + out=np.ones_like(theta), + where=theta_distorted > 1e-12, + ) + directions = np.column_stack( + ( + distorted_x * scale, + distorted_y * scale, + np.cos(theta), + ) + ) + norms = np.linalg.norm(directions, axis=1) + if not np.isfinite(norms).all() or np.any(norms < 1e-9): + raise CalibratedProjectionError("KB4 inverse produced a zero camera ray") + return np.asarray(directions / norms[:, None], dtype=np.float64) + + def depth_colors(depths_m: npt.ArrayLike) -> npt.NDArray[np.uint8]: """Return deterministic blue→cyan→green→yellow→red diagnostic colors.""" diff --git a/src/k1link/device_plugins/xgrids_k1/cli.py b/src/k1link/device_plugins/xgrids_k1/cli.py index 3bea462..93587a9 100644 --- a/src/k1link/device_plugins/xgrids_k1/cli.py +++ b/src/k1link/device_plugins/xgrids_k1/cli.py @@ -24,6 +24,7 @@ from k1link.compute import ( publish_e23_lab_instance, publish_e24_lab_instance, publish_e25_lab_instance, + publish_e26_lab_instance, publish_integrated_lab_instance, ) from k1link.device_plugins.xgrids_k1.analyze import ( @@ -743,6 +744,93 @@ def publish_e25_lab( ) +@lab_app.command("publish-e26") +def publish_e26_lab( + lidar_result: Annotated[ + Path, + typer.Option( + "--lidar-result", + exists=True, + file_okay=False, + readable=True, + resolve_path=True, + help="Accepted E25 persistent-support result.", + ), + ], + camera_result: Annotated[ + Path, + typer.Option( + "--camera-result", + exists=True, + file_okay=False, + readable=True, + resolve_path=True, + help="Accepted pre-world-motion result containing every 2D track.", + ), + ], + profile: Annotated[ + Path, + typer.Option( + exists=True, + dir_okay=False, + readable=True, + resolve_path=True, + help="Bounded E26 camera/ego-motion profile.", + ), + ], + benchmark: Annotated[ + Path, + typer.Option( + exists=True, + dir_okay=False, + readable=True, + resolve_path=True, + help="Spatially bound E26 motion benchmark.", + ), + ], + session_id: Annotated[ + str, + typer.Option("--session-id", help="New immutable LAB session id."), + ], + lab_id: Annotated[ + str, + typer.Option("--lab-id", help="LAB marker, for example 'LAB E26.1'."), + ], + display_name: Annotated[ + str, + typer.Option("--display-name", help="Operator-facing saved-session title."), + ], +) -> None: + """Build and publish calibrated camera/ego-motion evidence.""" + + repository_root = Path(__file__).resolve().parents[4] + try: + published = publish_e26_lab_instance( + repository_root=repository_root, + lidar_result_root=lidar_result, + camera_result_root=camera_result, + profile_path=profile, + benchmark_path=benchmark, + lab_session_id=session_id, + lab_id=lab_id, + display_name=display_name, + ) + except (OSError, SessionIntegrityError, RuntimeError, ValueError) as exc: + console.print(f"[red]E26 LAB publication failed:[/red] {exc}") + raise typer.Exit(code=2) from exc + metrics = published.build.report["metrics"] + console.print( + "[green]E26 LAB instance published.[/green] " + f"session={published.binding.session_id}; " + f"source={published.binding.source_session_id}; " + f"result={published.binding.result_id}; " + f"benchmark={metrics['benchmark']['passed_events']}/" + f"{metrics['benchmark']['total_events']}; " + f"p95={metrics['runtime']['camera_ego_motion_frame_processing_ms']['p95']:.3f}ms; " + "camera_metric_velocity=false; source_payloads_mutated=false" + ) + + @app.command("serve") def serve_console( port: Annotated[ diff --git a/tests/test_e26_camera_ego_motion.py b/tests/test_e26_camera_ego_motion.py new file mode 100644 index 0000000..488b3bb --- /dev/null +++ b/tests/test_e26_camera_ego_motion.py @@ -0,0 +1,308 @@ +from __future__ import annotations + +from pathlib import Path + +import numpy as np + +from k1link.compute.camera_ego_motion import ( + CameraEgoMotionTracker, + _camera_evidence, + _fuse_camera_lidar_object, + evaluate_camera_ego_motion_benchmark, + read_camera_ego_motion_benchmark, + read_camera_ego_motion_profile, +) +from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import ( + Kb4ProjectionProfile, + project_map_points_kb4, +) + +ROOT = Path(__file__).parents[1] + + +def _profile() -> dict[str, object]: + profile, digest = read_camera_ego_motion_profile( + ROOT + / "experiments" + / "perception" + / "e26_camera_ego_motion_profile.json" + ) + assert len(digest) == 64 + return profile + + +def _projection() -> Kb4ProjectionProfile: + transform = np.eye(4, dtype=np.float64) + transform.setflags(write=False) + return Kb4ProjectionProfile( + source_id="sensor.camera.right", + calibration_slot="camera_1", + width=800, + height=600, + intrinsic_fx_fy_cx_cy=(100.0, 100.0, 400.0, 300.0), + distortion_kb4=(0.0, 0.0, 0.0, 0.0), + t_camera_from_lidar=transform, + ) + + +def _bbox_for_world_point( + point_map: np.ndarray, + position_map: np.ndarray, + *, + width: float = 50.0, + height: float = 60.0, +) -> list[float]: + projected = project_map_points_kb4( + [point_map], + position_map_xyz=( + float(position_map[0]), + float(position_map[1]), + float(position_map[2]), + ), + orientation_map_from_lidar_xyzw=(0.0, 0.0, 0.0, 1.0), + profile=_projection(), + ) + u, v = projected.pixels_xy[0] + inset = float( + _profile()["camera_evidence"]["footpoint_inset_fraction"] # type: ignore[index] + ) + bottom = float(v) + inset * height + return [ + float(u) - width * 0.5, + bottom - height, + float(u) + width * 0.5, + bottom, + ] + + +def _observe_track( + tracker: CameraEgoMotionTracker, + *, + moving: bool, +) -> dict[str, object]: + evidence: dict[str, object] = {} + for index in range(18): + session_seconds = index * 0.1 + position = np.asarray([index * 0.1, 0.0, 0.0], dtype=np.float64) + point = np.asarray( + [index * 0.35 if moving else 0.0, 0.0, 6.0], + dtype=np.float64, + ) + evidence = tracker.observe( + source_track_id=7, + group="person", + session_seconds=session_seconds, + bbox_xyxy=_bbox_for_world_point(point, position), + detector_score=0.9, + position_map_xyz=position, + orientation_map_from_lidar_xyzw=np.asarray( + [0.0, 0.0, 0.0, 1.0], + dtype=np.float64, + ), + ) + return evidence + + +def test_e26_profile_and_benchmark_are_pinned_and_diagnostic_only() -> None: + profile = _profile() + benchmark, digest = read_camera_ego_motion_benchmark( + ROOT / "experiments" / "perception" / "e26_motion_benchmark.json" + ) + assert profile["authority"] == { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + } + assert profile["fusion"]["unknown_is_occupied"] is True + assert profile["fusion"]["camera_only_metric_velocity_valid"] is False + assert benchmark["benchmark_id"] == ( + "ravnoves00-camera-ego-reviewed-anchors-v1" + ) + assert len(benchmark["events"]) == 11 + assert len(digest) == 64 + + +def test_e26_multiview_rays_accept_one_static_world_hypothesis() -> None: + tracker = CameraEgoMotionTracker(_profile(), _projection()) + evidence = _observe_track(tracker, moving=False) + assert evidence["camera_evidence_current"] is True + assert evidence["camera_motion_state"] == "static" + assert float(evidence["camera_static_residual_p80_degrees"]) < 1e-6 + assert float(evidence["camera_static_positive_depth_fraction"]) == 1.0 + assert tracker.snapshot()["peak_tracks"] == 1 + + +def test_e26_multiview_rays_reject_a_moving_world_hypothesis() -> None: + tracker = CameraEgoMotionTracker(_profile(), _projection()) + evidence = _observe_track(tracker, moving=True) + assert evidence["camera_evidence_current"] is True + assert evidence["camera_motion_state"] == "dynamic" + assert float(evidence["camera_motion_confidence"]) > 0.0 + + +def test_e26_invalid_fisheye_ray_is_unavailable_not_a_run_failure() -> None: + tracker = CameraEgoMotionTracker(_profile(), _projection()) + evidence = tracker.observe( + source_track_id=99, + group="vehicle", + session_seconds=1.0, + bbox_xyxy=[100000.0, 100000.0, 100100.0, 100100.0], + detector_score=0.9, + position_map_xyz=np.zeros(3, dtype=np.float64), + orientation_map_from_lidar_xyzw=np.asarray( + [0.0, 0.0, 0.0, 1.0], + dtype=np.float64, + ), + ) + assert evidence["camera_evidence_current"] is False + assert tracker.snapshot()["invalid_camera_rays"] == 1 + assert tracker.snapshot()["active_tracks"] == 0 + + +def test_e26_camera_lidar_conflict_is_unknown_not_averaged() -> None: + profile = _profile() + tracker = CameraEgoMotionTracker(profile, _projection()) + camera_object = { + "track_id": 9, + "association_group": "vehicle", + "bbox_xyxy": [100.0, 100.0, 160.0, 160.0], + "label": "car", + "score": 0.9, + } + lidar_object = { + **camera_object, + "track_id": 240009, + "source_track_id": 9, + "motion_state": "static", + "motion_confidence": 0.8, + "speed_mps": 0.0, + "velocity_map_mps": [0.0, 0.0, 0.0], + "cuboid_status": "accepted-world-tracked-e24-v1", + } + evidence = { + **_camera_evidence(None, "unknown", 0.0), + "camera_motion_state": "dynamic", + "camera_motion_confidence": 0.9, + "camera_evidence_current": True, + } + fused = _fuse_camera_lidar_object( + camera_object, + lidar_object, + evidence, + profile, + tracker, + ) + assert fused["motion_state"] == "unknown" + assert fused["motion_status"] == "e26-explicit-camera-lidar-conflict" + assert fused["speed_mps"] is None + assert fused["velocity_map_mps"] is None + assert tracker.snapshot()["fusion_conflicts"] == 1 + + +def test_e26_camera_only_dynamic_does_not_invent_metric_velocity() -> None: + profile = _profile() + tracker = CameraEgoMotionTracker(profile, _projection()) + camera_object = { + "track_id": 12, + "association_group": "person", + "bbox_xyxy": [100.0, 100.0, 160.0, 190.0], + "label": "person", + "score": 0.9, + "speed_mps": 42.0, + "velocity_map_mps": [42.0, 0.0, 0.0], + } + evidence = { + **_camera_evidence(None, "unknown", 0.0), + "camera_motion_state": "dynamic", + "camera_motion_confidence": 0.8, + "camera_evidence_current": True, + } + fused = _fuse_camera_lidar_object( + camera_object, + None, + evidence, + profile, + tracker, + ) + assert fused["motion_state"] == "dynamic" + assert fused["motion_status"] == ( + "e26-camera-relative-only-no-metric-velocity" + ) + assert fused["speed_mps"] is None + assert fused["velocity_map_mps"] is None + assert fused["camera_only_metric_velocity_valid"] is False + + +def test_e26_benchmark_separates_dynamic_hits_and_static_false_alarm() -> None: + benchmark = { + "events": [ + { + "id": "moving", + "kind": "target-motion", + "window_seconds": [0.0, 1.0], + "class_group": "person", + "target_source_track_ids": [7], + "expected_motion": "dynamic", + "minimum_hits": 2, + "minimum_span_seconds": 0.1, + "minimum_coverage_fraction": 0.5, + }, + { + "id": "parked", + "kind": "static-control", + "window_seconds": [0.0, 1.0], + "class_group": "vehicle", + "expected_motion": "static", + "minimum_source_observations": 2, + "minimum_coverage_fraction": 0.5, + "minimum_classified_fraction": 0.5, + "maximum_false_dynamic_fraction": 0.1, + }, + ] + } + source_rows = [] + fusion_rows = [] + for index, timestamp in enumerate((0.0, 0.2)): + source_rows.append( + { + "frame_index": index, + "session_seconds": timestamp, + "objects": [ + { + "track_id": 7, + "association_group": "person", + }, + { + "track_id": 8, + "association_group": "vehicle", + }, + ], + } + ) + fusion_rows.append( + { + "frame_index": index, + "session_seconds": timestamp, + "objects": [ + { + "source_track_id": 7, + "association_group": "person", + "camera_evidence_current": True, + "motion_state": "dynamic", + }, + { + "source_track_id": 8, + "association_group": "vehicle", + "camera_evidence_current": True, + "motion_state": "static", + }, + ], + } + ) + result = evaluate_camera_ego_motion_benchmark( + source_rows, + fusion_rows, + benchmark, + ) + assert result["passed"] is True + assert result["passed_events"] == 2 + assert result["events"][1]["false_dynamic_fraction"] == 0.0 diff --git a/tests/test_xgrids_calibrated_projection.py b/tests/test_xgrids_calibrated_projection.py index 9e3566b..ccde26a 100644 --- a/tests/test_xgrids_calibrated_projection.py +++ b/tests/test_xgrids_calibrated_projection.py @@ -13,6 +13,7 @@ from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import ( map_points_to_lidar, project_map_points_kb4, quaternion_xyzw_to_rotation_matrix, + unproject_pixels_kb4, ) from k1link.device_plugins.xgrids_k1.calibration_schema import ( parse_k1_factory_calibration, @@ -68,6 +69,19 @@ def test_kb4_projection_uses_theta_polynomial_and_rejects_behind_camera() -> Non assert projected.source_indices.tolist() == [0, 1] +def test_kb4_unprojection_round_trips_camera_directions() -> None: + profile = _identity_profile() + directions = unproject_pixels_kb4( + [[400.0, 300.0], [400.0 + 100.0 * math.pi / 4.0, 300.0]], + profile=profile, + ) + np.testing.assert_allclose( + directions, + [[0.0, 0.0, 1.0], [math.sqrt(0.5), 0.0, math.sqrt(0.5)]], + atol=1e-12, + ) + + def test_factory_profile_binds_right_main_camera_and_scales_intrinsics() -> None: calibration = parse_k1_factory_calibration( (FIXTURES / "camera.yaml").read_bytes(),