diff --git a/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md index 63fa225..48336e6 100644 --- a/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md +++ b/docs/13_LIDAR_WORKER_PRODUCT_AND_ROADMAP.md @@ -5,7 +5,8 @@ Status: accepted architecture plan; L0/L1 implemented; L2 diagnostic A/B complete; full GOOSE and RELLIS qualification complete; L2.6d K1 replay local-surface temporal qualification, operator triage and prior-plane residual explainability implemented; L2.6e recorded-source-paced bounded shadow -qualified; E28 complete worker replay accepted; dynamic-observation layer next +qualified; E28 complete worker replay accepted; E29 camera-first semantic and +parallel geometry-only replay implemented; operator review and shadow gate next Scope: passively received real-time K1 point/pose evidence, immutable replay and future live shadow processing Explicitly out of scope: K1 firmware modification, a new onboard exporter, new @@ -585,16 +586,36 @@ independent gate without increasing unsafe false-free or false-dynamic output. - [x] Run the complete accepted K1 local-surface profile on the NVIDIA worker. - [ ] Commission physical K1 acquisition separately; do not block replay development on device presence. -- [ ] Fuse K1 geometric evidence with E26 camera evidence as independent +- [x] Fuse K1 geometric evidence with E26 camera evidence as independent sources; a LiDAR-native detector remains optional. -- [ ] Publish `agree`, `single-source`, `conflict` and `unknown`; unknown remains +- [x] Publish `agree`, `single-source`, `conflict` and `unknown`; unknown remains occupied. +- [x] Preserve unassociated L2.6 occupied components as a separate bounded + geometry-only layer without inventing semantic classes or free space. +- [ ] Review E29 conflict, camera-only and geometry-only episodes in the + operator surface and freeze a vehicle-footprint/self-filter contract. +- [ ] Replay the E29 contract at recorded source pace with staleness/deadline + health and exact bounded-queue accounting. - [ ] Measure sensor-to-result latency, deadline misses, drops, memory and GPU headroom on a physical run. Exit: repeatable shadow telemetry only. Navigation and safety acceptance remain false. +LAB E29 binds the complete E26 camera-first result to the complete L2.6 +source-aligned local-surface derivative. It processed `4,489` frames and +`20,513` observations. Of `19,625` current semantic observations, `6,341` +have connected occupied LiDAR support, `13,246` remain explicit camera-only and +`38` are conflicts; `888` held or unavailable observations remain `unknown`. +The independent layer retains `21,321` unassociated occupied components rather +than calling them free or assigning a guessed class. Postprocessing p95 was +`2.517 ms/frame`. These are coverage and runtime measurements on one replay, +not detection accuracy or planner acceptance. The immutable diagnostic result +is +`e29-camera-geometry-421a9d930638bef12cd5eb10979a477917fa4a389e655ed95f73ba4bd62e13dc`; +the detailed interpretation and reproduction contract are in +`experiments/perception/LAB_E29_REPORT_2026-07-26.md`. + ### L5 — local occupancy and Nav2 - [ ] Qualify conservative hit-based occupied/unknown output from the existing diff --git a/experiments/perception/LAB_E29_REPORT_2026-07-26.md b/experiments/perception/LAB_E29_REPORT_2026-07-26.md new file mode 100644 index 0000000..81e319a --- /dev/null +++ b/experiments/perception/LAB_E29_REPORT_2026-07-26.md @@ -0,0 +1,208 @@ +# LAB E29 — camera-first semantics with independent local-surface geometry + +Date: 2026-07-26 +Status: diagnostic replay complete; operator review, navigation and safety +acceptance are false +Immutable result: +`e29-camera-geometry-421a9d930638bef12cd5eb10979a477917fa4a389e655ed95f73ba4bd62e13dc` + +## Decision under test + +E29 implements the product boundary selected after E28: + +1. camera detection/tracking owns semantic class and image-space identity; +2. the passive K1 point stream owns metric range and occupied geometry; +3. L2.6 supplies the source-aligned local-surface reference; +4. object presence publishes `agree`, `single-source-camera`, `conflict` or + `unknown`; +5. unassociated occupied LiDAR components remain a separate + `single-source-geometry` layer with no invented semantic label; +6. absent returns never mean free space; +7. no branch has command, navigation or safety authority. + +This is not a LiDAR-native object detector and it does not replace camera +segmentation. It is the conservative validation layer between the accepted E26 +camera-first result and a future planner-facing occupancy product. + +## Immutable inputs + +- Source recording: `RAVNOVES00`. +- Frames: `4,489`; source-aligned LiDAR/L2.6 frames: `3,928`. +- Source E26 result: + `e10-integrated-perception-459aac93918d8f6414b342986ccc6968fefcef6c1f3a78a5254df0b565255ad2`. +- Source E26 fusion-frame SHA-256: + `a1eb6c87880d283888c25a259167078377cdb0a32f9830bdd8cf3bc6e2a69f5f`. +- L2.6 model: + `k1-local-surface-23762244c8bdb97de26fb721ac957d7a00bc9a63571ac4cfa4be19c4effc7d55`. +- Source pack: + `e10-lidar-pack-576c994a6c814e2592dd6240ace3902a5db94843312c759a73ba0c9166157d2b`. +- Factory projection: calibrated K1 KB4, `camera_1`, `800×600`. +- Pinned profile: + `experiments/perception/e29_camera_geometry_fusion_profile.json`. + +The source point pack and E26 lab copy have different session/result identities +but the same immutable `lidar-pack.npz` payload, timeline, frame indices and +factory calibration. E29 binds every fusion row back to the exact source frame +and session timestamp. It sends no request to K1 and changes no raw or +persistent reconstruction data. + +## Implemented contract + +Each current camera object keeps its E26 class, box, track and motion evidence. +For the same source frame E29: + +1. projects the complete K1 map-frame cloud through the factory KB4 model; +2. selects visible points inside an inset detector box; +3. reads the L2.6 point class relative to the rolling local surface; +4. depth-clusters and spatially clusters only `occupied-above-surface` support; +5. publishes a robust median range only for connected occupied support; +6. preserves camera-only observations when LiDAR support is absent or + insufficient; +7. emits `conflict` only when an object that previously had observed 3D + geometry is currently covered by enough classified points and the L2.6 + result calls that region surface rather than occupied. + +The support gate is not “20 points means an object”. It combines: + +- calibrated image association; +- L2.6 height-over-local-surface classification; +- contiguous depth support; +- spatial connectedness; +- distinct occupied voxels; +- explicit source availability. + +All remaining occupied L2.6 points inside the 10 m local model are grouped into +bounded connected voxel components. A component touched by accepted semantic +support is not repeated as geometry-only. Every retained component is +`occupied/unknown-class`, not a car, person, wall or curb. + +## Full replay result + +### Semantic observations + +E29 processed `20,513` E26 observations; `19,625` were current semantic +observations. + +| geometry status | observations | meaning | +|---|---:|---| +| `agree` | 6,341 | camera semantic has connected L2.6 occupied support | +| `single-source-camera` | 13,246 | camera semantic remains valid, metric geometry is not qualified | +| `conflict` | 38 | prior object geometry and current local-surface class disagree | +| `unknown` | 888 | held observation or source/local-surface evidence unavailable | + +Agreement among current semantic observations is `32.31%`. This is coverage, +not precision or recall. L2.6 classifies only the local 10 m area, objects are +frequently sparse or occluded, and the replay has no independent object ground +truth. + +Per camera group: + +- vehicle: `5,937 agree`, `12,518 camera-only`, `38 conflict`, `814 unknown`; +- person: `363 agree`, `713 camera-only`, `70 unknown`; +- bicycle: `29 agree`, `10 camera-only`; +- motorcycle: `12 agree`, `5 camera-only`, `4 unknown`. + +E29 adds useful evidence to cases that E26 could not settle: + +- `1,764` E26 `insufficient-independent-evidence` observations now have + connected occupied support; +- `1,092` camera-relative-only observations now have occupied metric geometry; +- `235` E26 motion-conflict observations have presence geometry. This confirms + occupied support but deliberately does not resolve the independent motion + conflict. + +Of the `13,246` camera-only observations, `11,259` have no L2.6-classified +point inside the box and `11,688` have no occupied point. Another `1,256` +contain exactly one occupied point, which remains insufficient rather than +being promoted by threshold wishful thinking. + +### Explicit conflicts + +The `38` conflicts belong to nine source tracks and 16 short episodes. All are +vehicle-class observations. The largest concentrations are: + +- track `115`, `69.799–71.398 s`; +- track `470`, `215.324–218.708 s`; +- track `679`, `282.569–283.281 s`; +- track `1011`, `354.937–355.242 s`; +- track `1276`, `402.606–402.995 s`. + +These are review candidates, not proof that the camera is wrong. In those +boxes L2.6 classified 6–38 points, at least 80% as surface and none as +occupied, while prior E26 geometry existed. The episode can therefore expose a +camera false positive, temporal misbinding, projection edge case or a local +surface failure. E29 keeps the disagreement instead of averaging it away. + +### Geometry-only occupied layer + +All `3,928` source-available frames contain at least one unassociated occupied +component: + +- `21,321` bounded components; +- `2,021,343` supporting points; +- `5` components/frame p50, `9` p95, `15` maximum; +- `16` points/component p50, `562` p95; +- nearest range `6.087 m` p50 and `9.477 m` p95. + +This proves the required parallel path exists. It does not prove that every +component is a navigation obstacle. The layer includes structures, vegetation, +vehicles, people, terrain discontinuities, possible scanner-carrier/self +returns and local-surface mistakes. It needs operator episode review and a +vehicle-footprint/self-filter contract before planner qualification. + +## Runtime + +The complete offline build took `8.467 s`: + +- processing mean: `1.826 ms/frame`; +- p50: `1.948 ms/frame`; +- p95: `2.517 ms/frame`; +- maximum: `9.552 ms/frame`. + +This measures only E29 postprocessing on materialized replay arrays. It excludes +camera inference, sensor transport, decoding, L2.6 compute, serialization and +browser rendering. A second invocation returned the same immutable result ID +and validated artifact digests rather than rebuilding the result. + +## Conclusion + +The selected architecture is feasible on the existing K1 recording without a +second scanner and without changing K1 firmware: + +```text +camera detection / segmentation / tracking + │ + ▼ + semantic observation + │ + factory KB4 + source frame/time + │ + ▼ +K1 points ── L2.6 local surface ── occupied support / range + │ + ┌───────────┴───────────┐ + ▼ ▼ +semantic geometry status geometry-only occupied +``` + +E29 is accepted as a diagnostic replay implementation. It is not promoted to +navigation or safety. The next useful gate is operator review of the 16 +conflict episodes, representative camera-only gaps and geometry-only +components, followed by recorded-source-paced shadow delivery with deadline +and staleness health. + +## Reproduction and validation + +```bash +PYTHONPATH=src .venv/bin/python \ + experiments/perception/run_e29_camera_geometry_fusion.py \ + --fusion-result .runtime/compute-experiments/e10/worker-results/e10-integrated-perception-459aac93918d8f6414b342986ccc6968fefcef6c1f3a78a5254df0b565255ad2 \ + --source-pack .runtime/compute-experiments/e10/lidar-packs/e10-lidar-pack-576c994a6c814e2592dd6240ace3902a5db94843312c759a73ba0c9166157d2b \ + --local-surface .runtime/compute-experiments/k1-local-surface-v1/models/k1-local-surface-23762244c8bdb97de26fb721ac957d7a00bc9a63571ac4cfa4be19c4effc7d55 +``` + +- `4` targeted E29 tests pass. +- Ruff passes for the E29 module, runner and tests. +- Strict mypy passes for the E29 module, runner and tests. +- Raw K1 evidence and persistent reconstruction are unchanged. +- Commands, free-space inference, navigation and safety authority remain false. diff --git a/experiments/perception/e29_camera_geometry_fusion_profile.json b/experiments/perception/e29_camera_geometry_fusion_profile.json new file mode 100644 index 0000000..cf570aa --- /dev/null +++ b/experiments/perception/e29_camera_geometry_fusion_profile.json @@ -0,0 +1,17 @@ +{ + "profile_id": "camera-first-local-surface-validation/v1", + "bbox_inset_fraction": 0.03, + "depth_cluster_minimum_gap_m": 0.45, + "depth_cluster_gap_fraction": 0.08, + "spatial_cluster_radius_m": 0.6, + "semantic_minimum_occupied_points": 2, + "semantic_minimum_occupied_voxels": 1, + "semantic_voxel_size_m": 0.35, + "conflict_minimum_classified_points": 6, + "conflict_surface_fraction": 0.8, + "geometry_local_radius_m": 10.0, + "geometry_voxel_size_m": 0.45, + "geometry_minimum_cluster_points": 4, + "geometry_minimum_cluster_voxels": 2, + "maximum_geometry_clusters_per_frame": 64 +} diff --git a/experiments/perception/run_e29_camera_geometry_fusion.py b/experiments/perception/run_e29_camera_geometry_fusion.py new file mode 100644 index 0000000..87c41a3 --- /dev/null +++ b/experiments/perception/run_e29_camera_geometry_fusion.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 +"""Build LAB E29 from immutable E26 and L2.6 replay artifacts.""" + +from __future__ import annotations + +import argparse +import hashlib +import json +from pathlib import Path +from typing import Any + +from k1link.compute.lidar_field_review import E10LidarFieldSource +from k1link.compute.lidar_local_surface import K1LocalSurfaceV1 +from k1link.compute.semantic_geometry_fusion import ( + CameraGeometryFusionProfile, + build_camera_geometry_fusion, +) + + +def _sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + while chunk := stream.read(1024 * 1024): + digest.update(chunk) + return digest.hexdigest() + + +def _profile(path: Path) -> CameraGeometryFusionProfile: + value: Any = json.loads(path.read_text(encoding="utf-8")) + if not isinstance(value, dict): + raise ValueError("LAB E29 profile must be an object") + return CameraGeometryFusionProfile(**value) + + +def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument("--fusion-result", type=Path, required=True) + parser.add_argument("--source-pack", type=Path, required=True) + parser.add_argument("--local-surface", type=Path, required=True) + parser.add_argument( + "--profile", + type=Path, + default=Path("experiments/perception/e29_camera_geometry_fusion_profile.json"), + ) + parser.add_argument( + "--output-root", + type=Path, + default=Path(".runtime/compute-experiments/e29/results"), + ) + args = parser.parse_args() + + fusion_result = args.fusion_result.expanduser().resolve(strict=True) + result_document = json.loads((fusion_result / "result.json").read_text(encoding="utf-8")) + result_id = result_document.get("result_id") + if not isinstance(result_id, str) or result_id != fusion_result.name: + raise ValueError("LAB E29 source result id is invalid") + frames_path = fusion_result / "fusion-frames.jsonl" + source = E10LidarFieldSource(args.source_pack) + surface = K1LocalSurfaceV1(args.local_surface) + try: + build = build_camera_geometry_fusion( + fusion_frames_path=frames_path, + source_result_id=result_id, + source_fusion_frames_sha256=_sha256(frames_path), + source=source, + surface=surface, + output_root=args.output_root, + profile=_profile(args.profile), + ) + finally: + surface.close() + source.close() + summary = { + "result_id": build.result_id, + "result_root": str(build.result_root), + "status": build.report["status"], + "metrics": build.report["metrics"], + "decision": build.report["decision"], + "authority": build.report["authority"], + } + print(json.dumps(summary, ensure_ascii=False, indent=2)) + + +if __name__ == "__main__": + main() diff --git a/src/k1link/compute/semantic_geometry_fusion.py b/src/k1link/compute/semantic_geometry_fusion.py new file mode 100644 index 0000000..c5dbd7c --- /dev/null +++ b/src/k1link/compute/semantic_geometry_fusion.py @@ -0,0 +1,964 @@ +"""Camera-first semantic observations validated by passive LiDAR geometry. + +The module joins an accepted camera-first E26 replay with a source-aligned +``missioncore.k1-local-surface/v1`` derivative. Camera detections retain +semantic ownership; LiDAR supplies range, occupied support and local-surface +evidence. Unassociated occupied geometry is published separately and never +silently converted to free space. +""" + +from __future__ import annotations + +import hashlib +import json +import math +import os +import shutil +import time +from collections import Counter, deque +from collections.abc import Iterable, Mapping +from dataclasses import asdict, dataclass +from datetime import UTC, datetime +from pathlib import Path +from typing import Any, Final + +import numpy as np +import numpy.typing as npt + +from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import ( + Kb4ProjectionProfile, + ProjectedPointCloud, + project_map_points_kb4, +) + +from .lidar_field_review import E10LidarFieldSource +from .lidar_local_surface import ( + POINT_BELOW_SURFACE, + POINT_OCCUPIED, + POINT_SURFACE, + K1LocalSurfaceV1, +) + +CAMERA_GEOMETRY_FUSION_SCHEMA: Final = "missioncore.e29-camera-geometry-fusion/v1" +CAMERA_GEOMETRY_FRAME_SCHEMA: Final = "missioncore.e29-camera-geometry-frame/v1" +CAMERA_GEOMETRY_REPORT_SCHEMA: Final = "missioncore.e29-camera-geometry-report/v1" +CAMERA_GEOMETRY_FRAMES_NAME: Final = "camera-geometry-frames.jsonl" +CAMERA_GEOMETRY_REPORT_NAME: Final = "camera-geometry-report.json" +CAMERA_GEOMETRY_MANIFEST_NAME: Final = "manifest.json" + +FloatArray = npt.NDArray[np.float64] +IntArray = npt.NDArray[np.int64] + + +class SemanticGeometryFusionError(ValueError): + """Raised when the replay or fusion contract is invalid.""" + + +@dataclass(frozen=True, slots=True) +class CameraGeometryFusionProfile: + """Bounded evidence thresholds for camera/LiDAR presence validation.""" + + profile_id: str = "camera-first-local-surface-validation/v1" + bbox_inset_fraction: float = 0.03 + depth_cluster_minimum_gap_m: float = 0.45 + depth_cluster_gap_fraction: float = 0.08 + spatial_cluster_radius_m: float = 0.60 + semantic_minimum_occupied_points: int = 2 + semantic_minimum_occupied_voxels: int = 1 + semantic_voxel_size_m: float = 0.35 + conflict_minimum_classified_points: int = 6 + conflict_surface_fraction: float = 0.80 + geometry_local_radius_m: float = 10.0 + geometry_voxel_size_m: float = 0.45 + geometry_minimum_cluster_points: int = 4 + geometry_minimum_cluster_voxels: int = 2 + maximum_geometry_clusters_per_frame: int = 64 + + def __post_init__(self) -> None: + numeric = ( + self.bbox_inset_fraction, + self.depth_cluster_minimum_gap_m, + self.depth_cluster_gap_fraction, + self.spatial_cluster_radius_m, + self.semantic_voxel_size_m, + self.conflict_surface_fraction, + self.geometry_local_radius_m, + self.geometry_voxel_size_m, + ) + if ( + not self.profile_id.strip() + or len(self.profile_id) > 160 + or not np.isfinite(numeric).all() + or not 0.0 <= self.bbox_inset_fraction < 0.25 + or not 0.05 <= self.depth_cluster_minimum_gap_m <= 5.0 + or not 0.0 <= self.depth_cluster_gap_fraction <= 1.0 + or not 0.05 <= self.spatial_cluster_radius_m <= 5.0 + or not 1 <= self.semantic_minimum_occupied_points <= 64 + or not 1 <= self.semantic_minimum_occupied_voxels <= 32 + or not 0.05 <= self.semantic_voxel_size_m <= 2.0 + or not 1 <= self.conflict_minimum_classified_points <= 256 + or not 0.5 <= self.conflict_surface_fraction <= 1.0 + or not 1.0 <= self.geometry_local_radius_m <= 100.0 + or not 0.05 <= self.geometry_voxel_size_m <= 5.0 + or not 1 <= self.geometry_minimum_cluster_points <= 256 + or not 1 <= self.geometry_minimum_cluster_voxels <= 128 + or not 1 <= self.maximum_geometry_clusters_per_frame <= 512 + ): + raise SemanticGeometryFusionError("camera/geometry fusion profile is invalid") + + def to_dict(self) -> dict[str, object]: + value = asdict(self) + value["schema_version"] = "missioncore.e29-camera-geometry-profile/v1" + value["absence_of_points_means_free"] = False + value["unknown_is_occupied"] = True + value["commands_enabled"] = False + value["navigation_or_safety_accepted"] = False + return value + + +DEFAULT_CAMERA_GEOMETRY_FUSION_PROFILE: Final = CameraGeometryFusionProfile() + + +@dataclass(frozen=True, slots=True) +class SemanticGeometryBuild: + result_root: Path + result_id: str + report: dict[str, Any] + + +@dataclass(frozen=True, slots=True) +class _SemanticSupport: + document: dict[str, object] + occupied_source_indices: IntArray + + +def build_camera_geometry_fusion( + *, + fusion_frames_path: Path, + source_result_id: str, + source_fusion_frames_sha256: str, + source: E10LidarFieldSource, + surface: K1LocalSurfaceV1, + output_root: Path, + profile: CameraGeometryFusionProfile = DEFAULT_CAMERA_GEOMETRY_FUSION_PROFILE, +) -> SemanticGeometryBuild: + """Build one immutable, source-aligned E29 replay result.""" + + _validate_source_binding(source, surface) + fusion_path = fusion_frames_path.expanduser().resolve(strict=True) + if not fusion_path.is_file() or fusion_path.is_symlink(): + raise SemanticGeometryFusionError("fusion frame source must be a regular file") + if _sha256(fusion_path) != source_fusion_frames_sha256: + raise SemanticGeometryFusionError("fusion frame source digest changed") + if not source_result_id.startswith("e10-integrated-perception-"): + raise SemanticGeometryFusionError("source integrated-perception id is invalid") + + profile_document = profile.to_dict() + identity = { + "schema_version": CAMERA_GEOMETRY_FUSION_SCHEMA, + "source_result_id": source_result_id, + "source_fusion_frames_sha256": source_fusion_frames_sha256, + "source_pack_id": source.pack_id, + "local_surface_model_id": surface.model_id, + "frame_count": source.frame_count, + "timeline_start_seconds": float(source.arrays["session_seconds"][0]), + "timeline_end_seconds": float(source.arrays["session_seconds"][-1]), + "profile": profile_document, + "producer_sha256": _sha256(Path(__file__).resolve(strict=True)), + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + identity_sha256 = hashlib.sha256(_canonical_json(identity)).hexdigest() + result_id = f"e29-camera-geometry-{identity_sha256}" + root = output_root.expanduser().resolve() + root.mkdir(mode=0o700, parents=True, exist_ok=True) + output = root / result_id + if output.exists(): + return _read_existing_result(output, identity) + + staging = root / f".{result_id}.{os.getpid()}.incomplete" + staging.mkdir(mode=0o700, exist_ok=False) + started = time.perf_counter() + processing_ms: list[float] = [] + status_counts: Counter[str] = Counter() + group_status_counts: Counter[tuple[str, str]] = Counter() + motion_status_counts: Counter[tuple[str, str]] = Counter() + geometry_cluster_counts: Counter[str] = Counter() + semantic_observations = 0 + semantic_current_observations = 0 + geometry_only_distances: list[float] = [] + geometry_only_cluster_points: list[float] = [] + geometry_only_cluster_voxels: list[float] = [] + geometry_only_clusters_per_frame: list[float] = [] + geometry_only_points = 0 + frames_with_geometry_only = 0 + frame_count = 0 + projection = _projection_profile(source) + arrays = source.arrays + offsets = arrays["cloud_offsets"] + points_map = arrays["cloud_points_map"] + point_class = surface.arrays["point_class"] + point_height = surface.arrays["point_height_m"] + + frames_path = staging / CAMERA_GEOMETRY_FRAMES_NAME + try: + with ( + fusion_path.open("r", encoding="utf-8") as input_stream, + frames_path.open("x", encoding="utf-8") as output_stream, + ): + for expected_frame_index, line in enumerate(input_stream): + frame_started = time.perf_counter() + frame = _fusion_frame( + line, + expected_frame_index=expected_frame_index, + source=source, + ) + frame_count += 1 + start = int(offsets[expected_frame_index]) + end = int(offsets[expected_frame_index + 1]) + frame_points = np.asarray(points_map[start:end], dtype=np.float64) + frame_classes = point_class[start:end] + frame_heights = point_height[start:end] + source_available = bool(arrays["sample_available"][expected_frame_index]) + surface_valid = bool(surface.arrays["frame_valid"][expected_frame_index]) + semantic_supports: list[_SemanticSupport] = [] + if source_available and surface_valid: + position = arrays["pose_positions_map"][expected_frame_index] + orientation = arrays["pose_quaternions_map_from_lidar"][expected_frame_index] + projected = project_map_points_kb4( + frame_points, + position_map_xyz=( + float(position[0]), + float(position[1]), + float(position[2]), + ), + orientation_map_from_lidar_xyzw=( + float(orientation[0]), + float(orientation[1]), + float(orientation[2]), + float(orientation[3]), + ), + profile=projection, + ) + else: + projected = None + + for item in frame["objects"]: + support = _semantic_support( + item, + projected=projected, + frame_points_map=frame_points, + point_class=frame_classes, + point_height_m=frame_heights, + source_available=source_available, + surface_valid=surface_valid, + profile=profile, + ) + semantic_supports.append(support) + semantic_observations += 1 + geometry_status = str(support.document["geometry_status"]) + group = str(support.document["association_group"]) + motion_status = str(support.document["motion_status"]) + status_counts[geometry_status] += 1 + group_status_counts[(group, geometry_status)] += 1 + motion_status_counts[(motion_status, geometry_status)] += 1 + if support.document["semantic_current"] is True: + semantic_current_observations += 1 + + claimed = _claimed_indices(semantic_supports) + geometry_clusters = _geometry_clusters( + points_map=frame_points, + point_class=frame_classes, + point_height_m=frame_heights, + sensor_position_map=np.asarray( + arrays["pose_positions_map"][expected_frame_index], + dtype=np.float64, + ), + claimed_source_indices=claimed, + profile=profile, + ) + if geometry_clusters: + frames_with_geometry_only += 1 + geometry_only_clusters_per_frame.append(float(len(geometry_clusters))) + geometry_cluster_counts["single-source-geometry"] += len(geometry_clusters) + geometry_only_points += sum( + _required_int(cluster["point_count"]) for cluster in geometry_clusters + ) + geometry_only_cluster_points.extend( + float(_required_int(cluster["point_count"])) for cluster in geometry_clusters + ) + geometry_only_cluster_voxels.extend( + float(_required_int(cluster["voxel_count"])) for cluster in geometry_clusters + ) + geometry_only_distances.extend( + _required_float(cluster["nearest_range_m"]) for cluster in geometry_clusters + ) + document = { + "schema_version": CAMERA_GEOMETRY_FRAME_SCHEMA, + "frame_index": expected_frame_index, + "source_frame_index": frame["source_frame_index"], + "session_seconds": frame["session_seconds"], + "source_available": source_available, + "local_surface_valid": surface_valid, + "semantic_observations": [support.document for support in semantic_supports], + "geometry_only_occupied": geometry_clusters, + "policy": { + "camera_owns_semantics": True, + "lidar_owns_metric_geometry": True, + "absence_of_points_means_free": False, + "unknown_is_occupied": True, + }, + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + output_stream.write( + json.dumps( + document, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ) + + "\n" + ) + processing_ms.append((time.perf_counter() - frame_started) * 1_000.0) + if frame_count != source.frame_count: + raise SemanticGeometryFusionError("fusion frame source is incomplete") + + report = { + "schema_version": CAMERA_GEOMETRY_REPORT_SCHEMA, + "result_id": result_id, + "created_at_utc": datetime.now(UTC).isoformat(), + "status": "diagnostic-replay-complete", + "ground_truth": False, + "identity": identity, + "metrics": { + "frames": { + "total": frame_count, + "source_available": int(np.count_nonzero(arrays["sample_available"])), + "local_surface_valid": int(np.count_nonzero(surface.arrays["frame_valid"])), + "with_geometry_only_occupied": frames_with_geometry_only, + }, + "semantic_observations": { + "total": semantic_observations, + "current": semantic_current_observations, + "agreement_fraction_of_current": ( + float(status_counts["agree"] / semantic_current_observations) + if semantic_current_observations + else 0.0 + ), + "geometry_status": dict(sorted(status_counts.items())), + "by_group": _nested_counts(group_status_counts), + "by_source_motion_status": _nested_counts(motion_status_counts), + }, + "geometry_only_occupied": { + "cluster_count": int(geometry_cluster_counts["single-source-geometry"]), + "point_count": geometry_only_points, + "clusters_per_frame": _distribution(geometry_only_clusters_per_frame), + "points_per_cluster": _distribution(geometry_only_cluster_points), + "voxels_per_cluster": _distribution(geometry_only_cluster_voxels), + "nearest_range_m": _distribution(geometry_only_distances), + }, + "runtime": { + "frame_processing_ms": _distribution(processing_ms), + "build_elapsed_ms": (time.perf_counter() - started) * 1_000.0, + }, + }, + "decision": { + "camera_first_contract_implemented": True, + "parallel_geometry_only_layer_implemented": True, + "production_promotion": False, + "next_gate": ( + "operator review of agree/camera-only/conflict and geometry-only " + "episodes before any planner-facing qualification" + ), + }, + "limitations": [ + "recorded host-arrival timing is best-effort rather than hardware time", + "geometry-only clusters have occupied geometry but no semantic class", + "absence of returns remains unknown and is never emitted as free space", + "the replay has no independent object or free-space ground truth", + ], + "authority": { + "commands_enabled": False, + "navigation_or_safety_accepted": False, + }, + } + report_path = staging / CAMERA_GEOMETRY_REPORT_NAME + _write_json(report_path, report) + manifest = { + "schema_version": CAMERA_GEOMETRY_FUSION_SCHEMA, + "result_id": result_id, + "identity_sha256": identity_sha256, + "identity": identity, + "created_at_utc": report["created_at_utc"], + "classification": "private-derived-perception-diagnostic", + "ground_truth": False, + "artifacts": [ + _artifact("camera-geometry-frames", frames_path, "application/x-ndjson"), + _artifact("camera-geometry-report", report_path, "application/json"), + ], + } + _write_json(staging / CAMERA_GEOMETRY_MANIFEST_NAME, manifest) + os.replace(staging, output) + except BaseException: + shutil.rmtree(staging, ignore_errors=True) + raise + return SemanticGeometryBuild(result_root=output, result_id=result_id, report=report) + + +def _semantic_support( + item: Mapping[str, Any], + *, + projected: ProjectedPointCloud | None, + frame_points_map: FloatArray, + point_class: npt.NDArray[np.uint8], + point_height_m: npt.NDArray[np.float32], + source_available: bool, + surface_valid: bool, + profile: CameraGeometryFusionProfile, +) -> _SemanticSupport: + bbox = _bbox(item.get("bbox_xyxy")) + held = "hold" in str(item.get("cuboid_status", "")) + current = bbox is not None and not held + base = { + "source_track_id": _optional_int(item.get("source_track_id")), + "track_id": _optional_int(item.get("track_id")), + "label": str(item.get("label", "object")), + "association_group": str(item.get("association_group", "object")), + "score": _optional_float(item.get("score")), + "bbox_xyxy": None if bbox is None else bbox.tolist(), + "semantic_current": current, + "camera_motion_state": str(item.get("camera_motion_state", "unknown")), + "camera_motion_confidence": _optional_float(item.get("camera_motion_confidence")), + "motion_state": str(item.get("motion_state", "unknown")), + "motion_status": str(item.get("motion_status", "unknown")), + "unknown_is_occupied": True, + "navigation_or_safety_accepted": False, + } + if not current: + base.update(_empty_geometry("unknown", "semantic-observation-not-current")) + return _SemanticSupport(base, np.empty(0, dtype=np.int64)) + if not source_available or not surface_valid or projected is None: + base.update(_empty_geometry("unknown", "source-or-local-surface-unavailable")) + return _SemanticSupport(base, np.empty(0, dtype=np.int64)) + if bbox is None: + raise AssertionError("current semantic observation must have a bbox") + + inset = profile.bbox_inset_fraction + width = float(bbox[2] - bbox[0]) + height = float(bbox[3] - bbox[1]) + inner = np.asarray( + [ + bbox[0] + width * inset, + bbox[1] + height * inset, + bbox[2] - width * inset, + bbox[3] - height * inset, + ], + dtype=np.float64, + ) + pixels = projected.pixels_xy + inside = ( + (pixels[:, 0] >= inner[0]) + & (pixels[:, 0] <= inner[2]) + & (pixels[:, 1] >= inner[1]) + & (pixels[:, 1] <= inner[3]) + ) + projected_rows = np.flatnonzero(inside).astype(np.int64, copy=False) + source_indices = projected.source_indices[projected_rows] + classes = point_class[source_indices] + counts = np.bincount(classes, minlength=4) + occupied_rows = projected_rows[classes == POINT_OCCUPIED] + clustered_rows = _depth_cluster( + occupied_rows, + projected.depths_m, + minimum_gap_m=profile.depth_cluster_minimum_gap_m, + gap_fraction=profile.depth_cluster_gap_fraction, + ) + clustered_rows = _spatial_cluster( + clustered_rows, + projected.source_indices, + frame_points_map, + radius_m=profile.spatial_cluster_radius_m, + ) + occupied_indices = projected.source_indices[clustered_rows] + occupied_points = frame_points_map[occupied_indices] + voxel_count = _voxel_count(occupied_points, profile.semantic_voxel_size_m) + classified = int(counts[POINT_SURFACE] + counts[POINT_OCCUPIED] + counts[POINT_BELOW_SURFACE]) + occupied_count = int(occupied_indices.size) + support_agrees = ( + occupied_count >= profile.semantic_minimum_occupied_points + and voxel_count >= profile.semantic_minimum_occupied_voxels + ) + conflict = ( + not support_agrees + and _has_observed_geometry(item) + and classified >= profile.conflict_minimum_classified_points + and int(counts[POINT_OCCUPIED]) == 0 + and float(counts[POINT_SURFACE] / max(1, classified)) >= profile.conflict_surface_fraction + ) + if support_agrees: + status = "agree" + reason = "camera-semantic-with-connected-occupied-lidar-support" + elif conflict: + status = "conflict" + reason = "camera-object-region-observed-as-local-surface" + else: + status = "single-source-camera" + reason = "camera-semantic-without-qualified-occupied-lidar-support" + if occupied_count: + ranges = projected.depths_m[clustered_rows] + range_m = float(np.median(ranges)) + centroid = np.median(occupied_points, axis=0).astype(np.float64).tolist() + height_range = [ + float(np.min(point_height_m[occupied_indices])), + float(np.max(point_height_m[occupied_indices])), + ] + else: + range_m = None + centroid = None + height_range = None + base.update( + { + "geometry_status": status, + "geometry_reason": reason, + "range_m": range_m, + "occupied_centroid_map_xyz_m": centroid, + "occupied_height_range_m": height_range, + "support": { + "projected_points_in_bbox": int(source_indices.size), + "classified_points_in_bbox": classified, + "surface_points_in_bbox": int(counts[POINT_SURFACE]), + "occupied_points_in_bbox": int(counts[POINT_OCCUPIED]), + "below_surface_points_in_bbox": int(counts[POINT_BELOW_SURFACE]), + "connected_occupied_points": occupied_count, + "connected_occupied_voxels": voxel_count, + }, + } + ) + return _SemanticSupport(base, occupied_indices.astype(np.int64, copy=False)) + + +def _geometry_clusters( + *, + points_map: FloatArray, + point_class: npt.NDArray[np.uint8], + point_height_m: npt.NDArray[np.float32], + sensor_position_map: FloatArray, + claimed_source_indices: set[int], + profile: CameraGeometryFusionProfile, +) -> list[dict[str, object]]: + occupied = np.flatnonzero(point_class == POINT_OCCUPIED).astype(np.int64) + if occupied.size == 0: + return [] + ranges = np.linalg.norm(points_map[occupied] - sensor_position_map, axis=1) + occupied = occupied[ranges <= profile.geometry_local_radius_m] + if occupied.size == 0: + return [] + components = _voxel_components( + points_map[occupied], + occupied, + profile.geometry_voxel_size_m, + ) + documents: list[dict[str, object]] = [] + for indices, voxel_count in components: + if ( + indices.size < profile.geometry_minimum_cluster_points + or voxel_count < profile.geometry_minimum_cluster_voxels + or any(int(value) in claimed_source_indices for value in indices) + ): + continue + values = points_map[indices] + distances = np.linalg.norm(values - sensor_position_map, axis=1) + documents.append( + { + "geometry_status": "single-source-geometry", + "semantic_class": None, + "point_count": int(indices.size), + "voxel_count": voxel_count, + "centroid_map_xyz_m": np.median(values, axis=0).astype(np.float64).tolist(), + "bounds_map_xyz_m": [ + np.min(values, axis=0).astype(np.float64).tolist(), + np.max(values, axis=0).astype(np.float64).tolist(), + ], + "height_range_m": [ + float(np.min(point_height_m[indices])), + float(np.max(point_height_m[indices])), + ], + "nearest_range_m": float(np.min(distances)), + "unknown_is_occupied": True, + "navigation_or_safety_accepted": False, + } + ) + documents.sort( + key=lambda item: ( + _required_float(item["nearest_range_m"]), + -_required_int(item["point_count"]), + ) + ) + return documents[: profile.maximum_geometry_clusters_per_frame] + + +def _voxel_components( + points: FloatArray, + source_indices: IntArray, + voxel_size_m: float, +) -> list[tuple[IntArray, int]]: + cells = np.floor(points / voxel_size_m).astype(np.int64) + cell_points: dict[tuple[int, int, int], list[int]] = {} + for local_index, cell in enumerate(cells): + key = (int(cell[0]), int(cell[1]), int(cell[2])) + cell_points.setdefault(key, []).append(int(source_indices[local_index])) + remaining = set(cell_points) + components: list[tuple[IntArray, int]] = [] + neighbors = tuple( + (dx, dy, dz) + for dx in (-1, 0, 1) + for dy in (-1, 0, 1) + for dz in (-1, 0, 1) + if (dx, dy, dz) != (0, 0, 0) + ) + while remaining: + seed = remaining.pop() + queue: deque[tuple[int, int, int]] = deque([seed]) + cells_in_component = [seed] + while queue: + current = queue.popleft() + for delta in neighbors: + candidate = ( + current[0] + delta[0], + current[1] + delta[1], + current[2] + delta[2], + ) + if candidate in remaining: + remaining.remove(candidate) + queue.append(candidate) + cells_in_component.append(candidate) + indices = np.asarray( + [source_index for cell in cells_in_component for source_index in cell_points[cell]], + dtype=np.int64, + ) + components.append((indices, len(cells_in_component))) + return components + + +def _depth_cluster( + rows: IntArray, + depths: FloatArray, + *, + minimum_gap_m: float, + gap_fraction: float, +) -> IntArray: + if rows.size < 2: + return rows + ordered = rows[np.argsort(depths[rows])] + groups: list[IntArray] = [] + start = 0 + for offset, gap in enumerate(np.diff(depths[ordered]), start=1): + threshold = max( + minimum_gap_m, + gap_fraction * float(depths[ordered[offset - 1]]), + ) + if float(gap) > threshold: + groups.append(ordered[start:offset]) + start = offset + groups.append(ordered[start:]) + return min( + groups, + key=lambda group: (-int(group.size), float(np.median(depths[group]))), + ) + + +def _spatial_cluster( + rows: IntArray, + source_indices: IntArray, + points_map: FloatArray, + *, + radius_m: float, +) -> IntArray: + if rows.size < 2: + return rows + points = points_map[source_indices[rows]] + adjacent = np.sum((points[:, None, :] - points[None, :, :]) ** 2, axis=2) <= radius_m * radius_m + unseen = set(range(rows.size)) + groups: list[list[int]] = [] + while unseen: + seed = unseen.pop() + group = [seed] + pending = [seed] + while pending: + current = pending.pop() + connected = [neighbor for neighbor in tuple(unseen) if adjacent[current, neighbor]] + for neighbor in connected: + unseen.remove(neighbor) + pending.append(neighbor) + group.append(neighbor) + groups.append(group) + selected = min( + groups, + key=lambda group: ( + -len(group), + float(np.median(np.linalg.norm(points[np.asarray(group, dtype=np.int64)], axis=1))), + ), + ) + return rows[np.asarray(selected, dtype=np.int64)] + + +def _projection_profile(source: E10LidarFieldSource) -> Kb4ProjectionProfile: + identity_projection = source.identity.get("projection") + if not isinstance(identity_projection, Mapping): + raise SemanticGeometryFusionError("source projection identity is missing") + intrinsic = source.arrays["intrinsic_fx_fy_cx_cy"] + distortion = source.arrays["distortion_kb4"] + return Kb4ProjectionProfile( + source_id=str(source.identity["source_id"]), + calibration_slot=str(source.identity["camera_slot"]), + width=int(identity_projection["width"]), + height=int(identity_projection["height"]), + intrinsic_fx_fy_cx_cy=( + float(intrinsic[0]), + float(intrinsic[1]), + float(intrinsic[2]), + float(intrinsic[3]), + ), + distortion_kb4=( + float(distortion[0]), + float(distortion[1]), + float(distortion[2]), + float(distortion[3]), + ), + t_camera_from_lidar=np.asarray( + source.arrays["t_camera_from_lidar"], + dtype=np.float64, + ), + ) + + +def _fusion_frame( + line: str, + *, + expected_frame_index: int, + source: E10LidarFieldSource, +) -> dict[str, Any]: + try: + value = json.loads(line) + except json.JSONDecodeError as exc: + raise SemanticGeometryFusionError("fusion frame JSON is invalid") from exc + if not isinstance(value, dict) or not isinstance(value.get("objects"), list): + raise SemanticGeometryFusionError("fusion frame shape is invalid") + expected_source_index = int(source.arrays["source_frame_indices"][expected_frame_index]) + expected_seconds = float(source.arrays["session_seconds"][expected_frame_index]) + if ( + value.get("schema_version") != "missioncore.e10-fusion-frame/v1" + or value.get("frame_index") != expected_frame_index + or value.get("source_frame_index") != expected_source_index + or not math.isclose( + float(value.get("session_seconds", math.nan)), + expected_seconds, + abs_tol=1e-9, + ) + ): + raise SemanticGeometryFusionError("fusion frame is not source-aligned") + return value + + +def _validate_source_binding( + source: E10LidarFieldSource, + surface: K1LocalSurfaceV1, +) -> None: + identity = surface.identity + if ( + identity.get("source_pack_id") != source.pack_id + or identity.get("frame_count") != source.frame_count + or identity.get("point_count") != int(source.identity["point_count"]) + ): + raise SemanticGeometryFusionError("local surface is not bound to source pack") + + +def _bbox(value: object) -> FloatArray | None: + if not isinstance(value, list) or len(value) != 4: + return None + array = np.asarray(value, dtype=np.float64) + if not np.isfinite(array).all() or array[2] <= array[0] or array[3] <= array[1]: + return None + return array + + +def _has_observed_geometry(item: Mapping[str, Any]) -> bool: + return all( + item.get(key) is not None + for key in ( + "observed_cuboid_center_map", + "observed_cuboid_half_size", + "observed_cuboid_quaternion_xyzw", + ) + ) + + +def _empty_geometry(status: str, reason: str) -> dict[str, object]: + return { + "geometry_status": status, + "geometry_reason": reason, + "range_m": None, + "occupied_centroid_map_xyz_m": None, + "occupied_height_range_m": None, + "support": { + "projected_points_in_bbox": 0, + "classified_points_in_bbox": 0, + "surface_points_in_bbox": 0, + "occupied_points_in_bbox": 0, + "below_surface_points_in_bbox": 0, + "connected_occupied_points": 0, + "connected_occupied_voxels": 0, + }, + } + + +def _claimed_indices(supports: Iterable[_SemanticSupport]) -> set[int]: + return { + int(value) + for support in supports + if support.document["geometry_status"] == "agree" + for value in support.occupied_source_indices + } + + +def _voxel_count(points: FloatArray, size_m: float) -> int: + if points.size == 0: + return 0 + return int(np.unique(np.floor(points / size_m).astype(np.int64), axis=0).shape[0]) + + +def _optional_int(value: object) -> int | None: + return int(value) if isinstance(value, int) and not isinstance(value, bool) else None + + +def _optional_float(value: object) -> float | None: + if isinstance(value, (int, float)) and not isinstance(value, bool): + result = float(value) + return result if math.isfinite(result) else None + return None + + +def _required_int(value: object) -> int: + if not isinstance(value, int) or isinstance(value, bool): + raise SemanticGeometryFusionError("expected an integer metric") + return value + + +def _required_float(value: object) -> float: + if not isinstance(value, (int, float)) or isinstance(value, bool): + raise SemanticGeometryFusionError("expected a numeric metric") + result = float(value) + if not math.isfinite(result): + raise SemanticGeometryFusionError("numeric metric is not finite") + return result + + +def _distribution(values: Iterable[float]) -> dict[str, float | int | None]: + array = np.asarray(list(values), dtype=np.float64) + if array.size == 0: + return { + "sample_count": 0, + "minimum": None, + "p50": None, + "p95": None, + "maximum": None, + "mean": None, + } + return { + "sample_count": int(array.size), + "minimum": float(np.min(array)), + "p50": float(np.percentile(array, 50)), + "p95": float(np.percentile(array, 95)), + "maximum": float(np.max(array)), + "mean": float(np.mean(array)), + } + + +def _nested_counts( + counts: Mapping[tuple[str, str], int], +) -> dict[str, dict[str, int]]: + result: dict[str, dict[str, int]] = {} + for (group, status), count in sorted(counts.items()): + result.setdefault(group, {})[status] = count + return result + + +def _canonical_json(value: object) -> bytes: + return json.dumps( + value, + ensure_ascii=False, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode() + + +def _sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + while chunk := stream.read(1024 * 1024): + digest.update(chunk) + return digest.hexdigest() + + +def _artifact(role: str, path: Path, media_type: str) -> dict[str, object]: + return { + "role": role, + "path": path.name, + "media_type": media_type, + "byte_length": path.stat().st_size, + "sha256": _sha256(path), + } + + +def _write_json(path: Path, value: object) -> None: + path.write_bytes(_canonical_json(value)) + + +def _read_existing_result( + output: Path, + expected_identity: Mapping[str, Any], +) -> SemanticGeometryBuild: + manifest_path = output / CAMERA_GEOMETRY_MANIFEST_NAME + report_path = output / CAMERA_GEOMETRY_REPORT_NAME + frames_path = output / CAMERA_GEOMETRY_FRAMES_NAME + try: + manifest = json.loads(manifest_path.read_text(encoding="utf-8")) + report = json.loads(report_path.read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError) as exc: + raise SemanticGeometryFusionError("existing E29 result is invalid") from exc + identity_sha256 = hashlib.sha256(_canonical_json(expected_identity)).hexdigest() + if ( + not isinstance(manifest, dict) + or manifest.get("schema_version") != CAMERA_GEOMETRY_FUSION_SCHEMA + or manifest.get("identity") != expected_identity + or manifest.get("identity_sha256") != identity_sha256 + or output.name != f"e29-camera-geometry-{identity_sha256}" + or not frames_path.is_file() + or report.get("result_id") != output.name + ): + raise SemanticGeometryFusionError("existing E29 result identity changed") + artifacts = manifest.get("artifacts") + if not isinstance(artifacts, list) or len(artifacts) != 2: + raise SemanticGeometryFusionError("existing E29 artifact list is invalid") + for artifact in artifacts: + if not isinstance(artifact, dict): + raise SemanticGeometryFusionError("existing E29 artifact is invalid") + path = output / str(artifact.get("path")) + if ( + not path.is_file() + or path.stat().st_size != artifact.get("byte_length") + or _sha256(path) != artifact.get("sha256") + ): + raise SemanticGeometryFusionError("existing E29 artifact digest changed") + return SemanticGeometryBuild( + result_root=output, + result_id=output.name, + report=report, + ) diff --git a/tests/test_semantic_geometry_fusion.py b/tests/test_semantic_geometry_fusion.py new file mode 100644 index 0000000..d708f63 --- /dev/null +++ b/tests/test_semantic_geometry_fusion.py @@ -0,0 +1,160 @@ +from __future__ import annotations + +import numpy as np +import pytest + +from k1link.compute.semantic_geometry_fusion import ( + CameraGeometryFusionProfile, + SemanticGeometryFusionError, + _geometry_clusters, + _semantic_support, +) +from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import ( + ProjectedPointCloud, +) + + +def _object(*, observed_geometry: bool = False) -> dict[str, object]: + value: dict[str, object] = { + "source_track_id": 42, + "track_id": 4200, + "label": "car", + "association_group": "vehicle", + "score": 0.91, + "bbox_xyxy": [10.0, 10.0, 90.0, 90.0], + "cuboid_status": "rejected-fewer-than-8-clustered-points", + "camera_motion_state": "static", + "camera_motion_confidence": 0.8, + "motion_state": "unknown", + "motion_status": "e26-insufficient-independent-evidence", + } + if observed_geometry: + value.update( + { + "observed_cuboid_center_map": [2.0, 0.0, 0.5], + "observed_cuboid_half_size": [1.0, 0.5, 0.5], + "observed_cuboid_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], + } + ) + return value + + +def _projection(point_count: int) -> ProjectedPointCloud: + return ProjectedPointCloud( + pixels_xy=np.column_stack( + ( + np.linspace(20.0, 80.0, point_count), + np.linspace(20.0, 80.0, point_count), + ) + ).astype(np.float64), + depths_m=np.linspace(2.0, 2.4, point_count).astype(np.float64), + source_indices=np.arange(point_count, dtype=np.int64), + source_point_count=point_count, + camera_front_point_count=point_count, + ) + + +def test_camera_semantic_and_connected_occupied_support_agree() -> None: + points = np.asarray( + [ + [2.0, 0.0, 0.5], + [2.2, 0.0, 0.6], + [2.1, 0.2, 0.0], + [2.3, 0.2, -0.1], + ], + dtype=np.float64, + ) + support = _semantic_support( + _object(), + projected=_projection(4), + frame_points_map=points, + point_class=np.asarray([2, 2, 1, 3], dtype=np.uint8), + point_height_m=np.asarray([0.5, 0.6, 0.0, -0.1], dtype=np.float32), + source_available=True, + surface_valid=True, + profile=CameraGeometryFusionProfile(), + ) + assert support.document["geometry_status"] == "agree" + assert support.document["range_m"] == pytest.approx(2.0666666667) + assert support.document["unknown_is_occupied"] is True + assert support.document["navigation_or_safety_accepted"] is False + assert support.occupied_source_indices.tolist() == [0, 1] + + +def test_camera_only_and_surface_conflict_remain_explicit() -> None: + points = np.column_stack( + ( + np.linspace(2.0, 3.0, 6), + np.zeros(6), + np.zeros(6), + ) + ).astype(np.float64) + camera_only = _semantic_support( + _object(), + projected=_projection(6), + frame_points_map=points, + point_class=np.asarray([2, 1, 1, 1, 1, 1], dtype=np.uint8), + point_height_m=np.asarray([0.5, 0, 0, 0, 0, 0], dtype=np.float32), + source_available=True, + surface_valid=True, + profile=CameraGeometryFusionProfile(), + ) + conflict = _semantic_support( + _object(observed_geometry=True), + projected=_projection(6), + frame_points_map=points, + point_class=np.ones(6, dtype=np.uint8), + point_height_m=np.zeros(6, dtype=np.float32), + source_available=True, + surface_valid=True, + profile=CameraGeometryFusionProfile(), + ) + unknown = _semantic_support( + _object(), + projected=None, + frame_points_map=np.empty((0, 3), dtype=np.float64), + point_class=np.empty(0, dtype=np.uint8), + point_height_m=np.empty(0, dtype=np.float32), + source_available=False, + surface_valid=False, + profile=CameraGeometryFusionProfile(), + ) + assert camera_only.document["geometry_status"] == "single-source-camera" + assert conflict.document["geometry_status"] == "conflict" + assert unknown.document["geometry_status"] == "unknown" + + +def test_unclaimed_occupied_component_is_a_separate_geometry_layer() -> None: + points = np.asarray( + [ + [1.00, 0.00, 0.30], + [1.20, 0.00, 0.35], + [1.45, 0.00, 0.40], + [1.65, 0.00, 0.45], + [4.00, 0.00, 0.30], + [4.20, 0.00, 0.35], + [4.45, 0.00, 0.40], + [4.65, 0.00, 0.45], + ], + dtype=np.float64, + ) + clusters = _geometry_clusters( + points_map=points, + point_class=np.full(8, 2, dtype=np.uint8), + point_height_m=np.linspace(0.3, 0.45, 8).astype(np.float32), + sensor_position_map=np.zeros(3, dtype=np.float64), + claimed_source_indices={0}, + profile=CameraGeometryFusionProfile(), + ) + assert len(clusters) == 1 + assert clusters[0]["geometry_status"] == "single-source-geometry" + assert clusters[0]["semantic_class"] is None + assert clusters[0]["point_count"] == 4 + assert clusters[0]["unknown_is_occupied"] is True + + +def test_profile_rejects_free_space_shortcuts() -> None: + with pytest.raises(SemanticGeometryFusionError): + CameraGeometryFusionProfile( + geometry_minimum_cluster_points=0, + )