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