feat: add camera-first lidar geometry fusion
This commit is contained in:
DCCONSTRUCTIONS
@@ -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
|
||||
|
||||
@@ -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.
|
||||
@@ -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
|
||||
}
|
||||
@@ -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()
|
||||
@@ -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,
|
||||
)
|
||||
@@ -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,
|
||||
)
|
||||
Reference in New Issue
Block a user