feat(perception): add full camera-first shadow
This commit is contained in:
DCCONSTRUCTIONS
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,237 @@
|
||||
"""Versioned, fail-closed vehicle-body and sensor-mount geometry."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import re
|
||||
from collections.abc import Mapping
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Final, Literal
|
||||
|
||||
import numpy as np
|
||||
|
||||
RIG_GEOMETRY_SCHEMA: Final = "missioncore.rig-geometry/v1"
|
||||
_SHA256 = re.compile(r"^[a-f0-9]{64}$")
|
||||
_SAFE_ID = re.compile(r"^[a-z0-9][a-z0-9._/-]{0,159}$")
|
||||
_AUTHORITY: Final = {
|
||||
"commands_enabled": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
}
|
||||
|
||||
|
||||
class RigGeometryError(ValueError):
|
||||
"""A rig geometry document is incomplete, ambiguous, or inconsistent."""
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class RigGeometry:
|
||||
"""Validated rig geometry plus its content identity."""
|
||||
|
||||
document: Mapping[str, Any]
|
||||
identity_sha256: str
|
||||
qualification_state: Literal["unbound", "measured", "qualified"]
|
||||
|
||||
@property
|
||||
def metric_body_geometry_available(self) -> bool:
|
||||
return self.qualification_state in {"measured", "qualified"}
|
||||
|
||||
@property
|
||||
def collision_geometry_qualified(self) -> bool:
|
||||
return self.qualification_state == "qualified"
|
||||
|
||||
def collision_contract(self) -> dict[str, object]:
|
||||
if self.collision_geometry_qualified:
|
||||
return {
|
||||
"state": "geometry-qualified",
|
||||
"geometry_profile_sha256": self.identity_sha256,
|
||||
"recent_collision_publishable": False,
|
||||
"reason": "collision-algorithm-and-independent-safety-gate-not-qualified",
|
||||
}
|
||||
return {
|
||||
"state": "unavailable",
|
||||
"geometry_profile_sha256": self.identity_sha256,
|
||||
"recent_collision_publishable": False,
|
||||
"reason": (
|
||||
"vehicle-body-and-lidar-mount-geometry-not-bound"
|
||||
if self.qualification_state == "unbound"
|
||||
else "vehicle-body-and-lidar-mount-geometry-not-qualified"
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
def load_rig_geometry(path: Path) -> RigGeometry:
|
||||
"""Read and validate one regular JSON geometry profile."""
|
||||
|
||||
source = path.expanduser().absolute()
|
||||
if source.is_symlink():
|
||||
raise RigGeometryError("rig geometry profile cannot be a symlink")
|
||||
resolved = source.resolve(strict=True)
|
||||
if not resolved.is_file():
|
||||
raise RigGeometryError("rig geometry profile must be a regular file")
|
||||
try:
|
||||
value = json.loads(resolved.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError) as exc:
|
||||
raise RigGeometryError("rig geometry profile is not valid JSON") from exc
|
||||
if not isinstance(value, dict):
|
||||
raise RigGeometryError("rig geometry profile must be an object")
|
||||
return parse_rig_geometry(value)
|
||||
|
||||
|
||||
def parse_rig_geometry(value: Mapping[str, Any]) -> RigGeometry:
|
||||
"""Validate an in-memory v1 profile without inferring physical values."""
|
||||
|
||||
document = dict(value)
|
||||
profile_id = document.get("profile_id")
|
||||
rig_kind = document.get("rig_kind")
|
||||
qualification = _mapping(document.get("qualification"), "qualification")
|
||||
frames = _mapping(document.get("coordinate_frames"), "coordinate_frames")
|
||||
state = qualification.get("state")
|
||||
reason = qualification.get("reason")
|
||||
evidence_ids = qualification.get("evidence_sha256")
|
||||
if (
|
||||
document.get("schema_version") != RIG_GEOMETRY_SCHEMA
|
||||
or not isinstance(profile_id, str)
|
||||
or _SAFE_ID.fullmatch(profile_id) is None
|
||||
or rig_kind not in {"portable", "vehicle-mounted"}
|
||||
or state not in {"unbound", "measured", "qualified"}
|
||||
or not isinstance(reason, str)
|
||||
or not reason.strip()
|
||||
or len(reason) > 240
|
||||
or not isinstance(evidence_ids, list)
|
||||
or any(
|
||||
not isinstance(item, str) or _SHA256.fullmatch(item) is None
|
||||
for item in evidence_ids
|
||||
)
|
||||
or len(set(evidence_ids)) != len(evidence_ids)
|
||||
or document.get("authority") != _AUTHORITY
|
||||
):
|
||||
raise RigGeometryError("rig geometry profile identity is invalid")
|
||||
|
||||
lidar_frame = frames.get("lidar_frame")
|
||||
body_frame = frames.get("body_frame")
|
||||
body_from_lidar = frames.get("body_from_lidar")
|
||||
body = document.get("vehicle_body")
|
||||
if not isinstance(lidar_frame, str) or _SAFE_ID.fullmatch(lidar_frame) is None:
|
||||
raise RigGeometryError("rig geometry LiDAR frame is invalid")
|
||||
|
||||
if state == "unbound":
|
||||
if (
|
||||
rig_kind != "portable"
|
||||
or body_frame is not None
|
||||
or body_from_lidar is not None
|
||||
or body is not None
|
||||
or evidence_ids
|
||||
):
|
||||
raise RigGeometryError("unbound rig geometry must not contain physical values")
|
||||
else:
|
||||
if (
|
||||
rig_kind != "vehicle-mounted"
|
||||
or not isinstance(body_frame, str)
|
||||
or _SAFE_ID.fullmatch(body_frame) is None
|
||||
):
|
||||
raise RigGeometryError("mounted rig body frame is invalid")
|
||||
_validate_transform(
|
||||
_mapping(body_from_lidar, "body_from_lidar"),
|
||||
body_frame=body_frame,
|
||||
lidar_frame=lidar_frame,
|
||||
)
|
||||
_validate_body(_mapping(body, "vehicle_body"), body_frame=body_frame)
|
||||
if not evidence_ids:
|
||||
raise RigGeometryError("measured rig geometry requires measurement evidence")
|
||||
uncertainty = _mapping(
|
||||
qualification.get("uncertainty"),
|
||||
"qualification uncertainty",
|
||||
)
|
||||
_positive_number(
|
||||
uncertainty.get("translation_1sigma_m"),
|
||||
"translation uncertainty",
|
||||
)
|
||||
_positive_number(
|
||||
uncertainty.get("rotation_1sigma_deg"),
|
||||
"rotation uncertainty",
|
||||
)
|
||||
_positive_number(
|
||||
uncertainty.get("body_dimension_1sigma_m"),
|
||||
"body-dimension uncertainty",
|
||||
)
|
||||
if state == "qualified":
|
||||
qualification_method = qualification.get("qualification_method")
|
||||
if (
|
||||
not isinstance(qualification_method, str)
|
||||
or not qualification_method.strip()
|
||||
or len(qualification_method) > 240
|
||||
):
|
||||
raise RigGeometryError("qualified rig geometry needs a method")
|
||||
|
||||
canonical = _canonical_json(document)
|
||||
return RigGeometry(
|
||||
document=document,
|
||||
identity_sha256=hashlib.sha256(canonical).hexdigest(),
|
||||
qualification_state=state,
|
||||
)
|
||||
|
||||
|
||||
def _validate_transform(
|
||||
value: Mapping[str, Any],
|
||||
*,
|
||||
body_frame: str,
|
||||
lidar_frame: str,
|
||||
) -> None:
|
||||
translation = _vector(value.get("translation_m"), 3, "mount translation")
|
||||
quaternion = _vector(value.get("quaternion_xyzw"), 4, "mount quaternion")
|
||||
if (
|
||||
value.get("from_frame") != lidar_frame
|
||||
or value.get("to_frame") != body_frame
|
||||
or not math.isclose(float(np.linalg.norm(quaternion)), 1.0, abs_tol=1e-6)
|
||||
or np.linalg.norm(translation) > 20.0
|
||||
):
|
||||
raise RigGeometryError("rig mount transform is invalid")
|
||||
|
||||
|
||||
def _validate_body(value: Mapping[str, Any], *, body_frame: str) -> None:
|
||||
minimum = _vector(value.get("minimum_xyz_m"), 3, "body minimum")
|
||||
maximum = _vector(value.get("maximum_xyz_m"), 3, "body maximum")
|
||||
if (
|
||||
value.get("frame") != body_frame
|
||||
or value.get("shape") != "axis-aligned-box"
|
||||
or np.any(maximum <= minimum)
|
||||
or np.any(maximum - minimum > 30.0)
|
||||
):
|
||||
raise RigGeometryError("vehicle body envelope is invalid")
|
||||
|
||||
|
||||
def _mapping(value: object, label: str) -> Mapping[str, Any]:
|
||||
if not isinstance(value, Mapping):
|
||||
raise RigGeometryError(f"{label} must be an object")
|
||||
return value
|
||||
|
||||
|
||||
def _vector(value: object, length: int, label: str) -> np.ndarray:
|
||||
if not isinstance(value, list) or len(value) != length:
|
||||
raise RigGeometryError(f"{label} is invalid")
|
||||
array = np.asarray(value, dtype=np.float64)
|
||||
if not np.isfinite(array).all():
|
||||
raise RigGeometryError(f"{label} is invalid")
|
||||
return array
|
||||
|
||||
|
||||
def _positive_number(value: object, label: str) -> float:
|
||||
if not isinstance(value, (int, float)) or isinstance(value, bool):
|
||||
raise RigGeometryError(f"{label} is invalid")
|
||||
number = float(value)
|
||||
if not math.isfinite(number) or number <= 0.0:
|
||||
raise RigGeometryError(f"{label} is invalid")
|
||||
return number
|
||||
|
||||
|
||||
def _canonical_json(value: object) -> bytes:
|
||||
return json.dumps(
|
||||
value,
|
||||
ensure_ascii=False,
|
||||
sort_keys=True,
|
||||
separators=(",", ":"),
|
||||
allow_nan=False,
|
||||
).encode()
|
||||
@@ -138,6 +138,65 @@ class _GeometryClusterSupport:
|
||||
occupied_source_indices: IntArray
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class CameraGeometryFrameEvaluation:
|
||||
"""One camera-owned semantic frame with LiDAR-owned metric association."""
|
||||
|
||||
semantic_observations: tuple[dict[str, object], ...]
|
||||
geometry_only_occupied: tuple[dict[str, object], ...]
|
||||
claimed_source_indices: frozenset[int]
|
||||
|
||||
|
||||
def evaluate_camera_geometry_frame(
|
||||
*,
|
||||
objects: Iterable[Mapping[str, Any]],
|
||||
projected: ProjectedPointCloud | None,
|
||||
frame_points_map: FloatArray,
|
||||
point_class: npt.NDArray[np.uint8],
|
||||
point_height_m: npt.NDArray[np.float32],
|
||||
sensor_position_map: FloatArray,
|
||||
source_available: bool,
|
||||
surface_valid: bool,
|
||||
profile: CameraGeometryFusionProfile = DEFAULT_CAMERA_GEOMETRY_FUSION_PROFILE,
|
||||
include_geometry_only: bool = True,
|
||||
) -> CameraGeometryFrameEvaluation:
|
||||
"""Apply the canonical E29 association without changing semantic ownership."""
|
||||
|
||||
supports = tuple(
|
||||
_semantic_support(
|
||||
item,
|
||||
projected=projected,
|
||||
frame_points_map=frame_points_map,
|
||||
point_class=point_class,
|
||||
point_height_m=point_height_m,
|
||||
source_available=source_available,
|
||||
surface_valid=surface_valid,
|
||||
profile=profile,
|
||||
)
|
||||
for item in objects
|
||||
)
|
||||
claimed = frozenset(_claimed_indices(supports))
|
||||
geometry = (
|
||||
tuple(
|
||||
_geometry_clusters(
|
||||
points_map=frame_points_map,
|
||||
point_class=point_class,
|
||||
point_height_m=point_height_m,
|
||||
sensor_position_map=sensor_position_map,
|
||||
claimed_source_indices=set(claimed),
|
||||
profile=profile,
|
||||
)
|
||||
)
|
||||
if include_geometry_only
|
||||
else ()
|
||||
)
|
||||
return CameraGeometryFrameEvaluation(
|
||||
semantic_observations=tuple(support.document for support in supports),
|
||||
geometry_only_occupied=geometry,
|
||||
claimed_source_indices=claimed,
|
||||
)
|
||||
|
||||
|
||||
def build_camera_geometry_fusion(
|
||||
*,
|
||||
fusion_frames_path: Path,
|
||||
@@ -229,7 +288,6 @@ def build_camera_geometry_fusion(
|
||||
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]
|
||||
@@ -251,40 +309,32 @@ def build_camera_geometry_fusion(
|
||||
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,
|
||||
evaluation = evaluate_camera_geometry_frame(
|
||||
objects=frame["objects"],
|
||||
projected=projected,
|
||||
frame_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,
|
||||
source_available=source_available,
|
||||
surface_valid=surface_valid,
|
||||
profile=profile,
|
||||
)
|
||||
for support_document in evaluation.semantic_observations:
|
||||
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
|
||||
|
||||
geometry_clusters = list(evaluation.geometry_only_occupied)
|
||||
if geometry_clusters:
|
||||
frames_with_geometry_only += 1
|
||||
geometry_only_clusters_per_frame.append(float(len(geometry_clusters)))
|
||||
@@ -308,7 +358,7 @@ def build_camera_geometry_fusion(
|
||||
"session_seconds": frame["session_seconds"],
|
||||
"source_available": source_available,
|
||||
"local_surface_valid": surface_valid,
|
||||
"semantic_observations": [support.document for support in semantic_supports],
|
||||
"semantic_observations": list(evaluation.semantic_observations),
|
||||
"geometry_only_occupied": geometry_clusters,
|
||||
"policy": {
|
||||
"camera_owns_semantics": True,
|
||||
@@ -741,7 +791,9 @@ def _spatial_cluster(
|
||||
return rows[np.asarray(selected, dtype=np.int64)]
|
||||
|
||||
|
||||
def _projection_profile(source: E10LidarFieldSource) -> Kb4ProjectionProfile:
|
||||
def projection_profile_from_source(source: E10LidarFieldSource) -> Kb4ProjectionProfile:
|
||||
"""Build the source-bound factory projection used by E29 association."""
|
||||
|
||||
identity_projection = source.identity.get("projection")
|
||||
if not isinstance(identity_projection, Mapping):
|
||||
raise SemanticGeometryFusionError("source projection identity is missing")
|
||||
@@ -771,6 +823,10 @@ def _projection_profile(source: E10LidarFieldSource) -> Kb4ProjectionProfile:
|
||||
)
|
||||
|
||||
|
||||
def _projection_profile(source: E10LidarFieldSource) -> Kb4ProjectionProfile:
|
||||
return projection_profile_from_source(source)
|
||||
|
||||
|
||||
def _fusion_frame(
|
||||
line: str,
|
||||
*,
|
||||
|
||||
Reference in New Issue
Block a user