SILVER/NODEDC_MISSION_CORE
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

feat(perception): add camera ego-motion evidence

Browse Source
This commit is contained in:
DCCONSTRUCTIONS
2026-07-24 13:37:39 +03:00
parent 230cba4b21
commit 7fba39a629
11 changed files with 2348 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/k1link/compute/__init__.py
+4
View File
@@ -35,6 +35,7 @@ from .jobs import (
validate_camera_compute_job,
)
from .lab_instances import (
PublishedCameraEgoMotionLabInstance,
PublishedIntegratedLabInstance,
PublishedPersistentSupportLabInstance,
PublishedTemporalLabInstance,
@@ -44,6 +45,7 @@ from .lab_instances import (
publish_e23_lab_instance,
publish_e24_lab_instance,
publish_e25_lab_instance,
publish_e26_lab_instance,
publish_integrated_lab_instance,
)
from .live_perception import (
@@ -122,6 +124,7 @@ __all__ = [
"LivePerceptionIngress",
"IntegratedPerceptionOverlayStore",
"PublishedIntegratedLabInstance",
"PublishedCameraEgoMotionLabInstance",
"PublishedPersistentSupportLabInstance",
"PublishedTemporalLabInstance",
"PublishedWorldMotionLabInstance",
@@ -158,6 +161,7 @@ __all__ = [
"publish_e23_lab_instance",
"publish_e24_lab_instance",
"publish_e25_lab_instance",
"publish_e26_lab_instance",
"publish_integrated_lab_instance",
"validate_multirate_perception_qualification_result",
"prepare_recorded_qualification_slice",
src/k1link/compute/camera_ego_motion.py
+1249
View File
File diff suppressed because it is too large Load Diff
src/k1link/compute/lab_instances.py
+150
View File
@@ -22,6 +22,10 @@ from k1link.sessions import (
publish_lab_replay_cache,
)
from .camera_ego_motion import (
CameraEgoMotionBuild,
build_camera_ego_motion_result,
)
from .inline_temporal import StreamingSemanticStabilizer, read_inline_profile
from .integrated_perception import (
IntegratedPerceptionResult,
@@ -71,6 +75,14 @@ class PublishedPersistentSupportLabInstance:
build: PersistentSupportBuild
@dataclass(frozen=True, slots=True)
class PublishedCameraEgoMotionLabInstance:
binding: LabSessionBinding
job: CameraComputeJob
result: IntegratedPerceptionResult
build: CameraEgoMotionBuild
def publish_integrated_lab_instance(
*,
repository_root: Path,
@@ -709,6 +721,144 @@ def publish_e25_lab_instance(
)
def publish_e26_lab_instance(
*,
repository_root: Path,
lidar_result_root: Path,
camera_result_root: Path,
profile_path: Path,
benchmark_path: Path,
lab_session_id: str,
lab_id: str,
display_name: str,
) -> PublishedCameraEgoMotionLabInstance:
"""Derive and publish one bounded camera/ego-motion evidence LAB run."""
root = repository_root.expanduser().resolve(strict=True)
jobs_root = root / ".runtime" / "compute-jobs"
results_root = root / ".runtime" / "compute-experiments" / "e10" / "worker-results"
packs_root = root / ".runtime" / "compute-experiments" / "e10" / "lidar-packs"
def source(path: Path, label: str) -> IntegratedPerceptionResult:
resolved = path.expanduser().resolve(strict=True)
document = _read_object(resolved / "result.json", resolved)
identity = document.get("identity")
if not isinstance(identity, dict) or not isinstance(identity.get("job_id"), str):
raise SessionIntegrityError(f"E26 {label} source has no job identity")
validated = validate_integrated_perception_result(
jobs_root / identity["job_id"],
resolved,
packs_root,
)
if not validated.accepted:
raise SessionIntegrityError(f"E26 {label} source result is not accepted")
return validated
lidar_source = source(lidar_result_root, "LiDAR")
camera_source = source(camera_result_root, "camera")
if lidar_source.job.source_id != camera_source.job.source_id:
raise SessionIntegrityError("E26 source camera identities differ")
lab_job = _publish_lab_job(lidar_source.job, jobs_root, lab_session_id)
lab_pack = _publish_lab_pack(
lidar_source,
lab_job,
packs_root,
lab_session_id,
)
build = build_camera_ego_motion_result(
lidar_source=lidar_source,
camera_source=camera_source,
lab_job=lab_job,
lab_pack=lab_pack,
results_root=results_root,
profile_path=profile_path,
benchmark_path=benchmark_path,
)
validated = validate_integrated_perception_result(
lab_job.job_root,
build.result_root,
packs_root,
)
if not validated.accepted:
failed = [
name
for name, accepted in build.report["acceptance"]["checks"].items()
if not accepted
]
raise SessionIntegrityError(
f"E26 camera/ego-motion artifact acceptance failed: {', '.join(failed)}"
)
store = SessionStore(root)
source_lab = store.get_lab_instance(lidar_source.job.session_id)
source_session_id = (
lidar_source.job.session_id
if source_lab is None
else source_lab.source_session_id
)
publish_lab_replay_cache(
store.data_dir,
source_session_id=source_session_id,
lab_session_id=lab_session_id,
timeline_start_ns=round(
validated.timeline_start_seconds * 1_000_000_000
),
timeline_end_ns=round(
validated.timeline_end_seconds * 1_000_000_000
),
)
metrics = build.report["metrics"]
binding = store.publish_lab_instance(
session_id=lab_session_id,
source_session_id=source_session_id,
display_name=display_name,
lab_id=lab_id,
result_kind="e26-camera-ego-motion-fusion",
result_id=validated.result_id,
source_result_id=lidar_source.result_id,
config_sha256=build.profile_sha256,
run_created_at_utc=validated.created_at_utc,
duration_seconds=(
validated.timeline_end_seconds
- validated.timeline_start_seconds
),
include_recorded_media=False,
provenance={
"schema_version": "missioncore.e26-lab-publication/v1",
"storage_mode": (
"bounded-camera-ego-motion-and-immutable-lidar-source-replay"
),
"source_result_id": lidar_source.result_id,
"camera_source_result_id": camera_source.result_id,
"source_lab_session_id": (
None if source_lab is None else source_lab.session_id
),
"source_payloads_mutated": False,
"coordinate_frame": "k1-map",
"camera_measurement": "kb4-multiview-static-world-hypothesis",
"metric_measurement": "e25-persistent-lidar-support",
"lookahead_frames": 0,
"benchmark_sha256": build.benchmark_sha256,
"benchmark_passed": metrics["benchmark"]["passed"],
"benchmark_passed_events": metrics["benchmark"]["passed_events"],
"benchmark_total_events": metrics["benchmark"]["total_events"],
"camera_ego_motion_processing_p95_ms": metrics["runtime"][
"camera_ego_motion_frame_processing_ms"
]["p95"],
"peak_tracks": metrics["runtime"]["peak_tracks"],
"camera_only_metric_velocity_valid": False,
"navigation_or_safety_accepted": False,
},
)
return PublishedCameraEgoMotionLabInstance(
binding=binding,
job=lab_job,
result=validated,
build=build,
)
def _validate_e23_inputs(
worker_root: Path,
source_report_path: Path,
src/k1link/device_plugins/xgrids_k1/analyze/__init__.py
+2
View File
@@ -13,6 +13,7 @@ from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
map_points_to_lidar,
project_map_points_kb4,
quaternion_xyzw_to_rotation_matrix,
unproject_pixels_kb4,
)
from k1link.device_plugins.xgrids_k1.analyze.stream_summary import (
DEFAULT_STREAM_SUMMARY_MAX_PAYLOAD_BYTES,
@@ -47,5 +48,6 @@ __all__ = [
"quaternion_xyzw_to_rotation_matrix",
"run_calibrated_overlay_experiment",
"summarize_mqtt_streams",
"unproject_pixels_kb4",
"validate_k1_valid_fov_mask",
]
src/k1link/device_plugins/xgrids_k1/analyze/calibrated_projection.py
+64
View File
@@ -205,6 +205,70 @@ def project_map_points_kb4(
)
def unproject_pixels_kb4(
pixels_xy: npt.ArrayLike,
*,
profile: Kb4ProjectionProfile,
) -> FloatArray:
"""Return unit camera-frame rays for finite pixels under the KB4 model."""
pixels = np.asarray(pixels_xy, dtype=np.float64)
if (
pixels.ndim != 2
or pixels.shape[1:] != (2,)
or not np.isfinite(pixels).all()
):
raise CalibratedProjectionError("camera pixels must have shape (N, 2)")
if pixels.size == 0:
return np.empty((0, 3), dtype=np.float64)
fx, fy, cx, cy = profile.intrinsic_fx_fy_cx_cy
if not all(math.isfinite(value) and value > 0.0 for value in (fx, fy)):
raise CalibratedProjectionError("camera focal lengths must be positive")
distorted_x = (pixels[:, 0] - cx) / fx
distorted_y = (pixels[:, 1] - cy) / fy
theta_distorted = np.hypot(distorted_x, distorted_y)
theta = theta_distorted.copy()
k1, k2, k3, k4 = profile.distortion_kb4
for _ in range(12):
squared = theta * theta
polynomial = (
1.0
+ k1 * squared
+ k2 * squared**2
+ k3 * squared**3
+ k4 * squared**4
)
derivative = (
1.0
+ 3.0 * k1 * squared
+ 5.0 * k2 * squared**2
+ 7.0 * k3 * squared**3
+ 9.0 * k4 * squared**4
)
if np.any(np.abs(derivative) < 1e-9):
raise CalibratedProjectionError("KB4 inverse derivative became singular")
theta -= (theta * polynomial - theta_distorted) / derivative
if not np.isfinite(theta).all() or np.any(theta < 0.0) or np.any(theta >= math.pi):
raise CalibratedProjectionError("KB4 inverse produced invalid angles")
scale = np.divide(
np.sin(theta),
theta_distorted,
out=np.ones_like(theta),
where=theta_distorted > 1e-12,
)
directions = np.column_stack(
(
distorted_x * scale,
distorted_y * scale,
np.cos(theta),
)
)
norms = np.linalg.norm(directions, axis=1)
if not np.isfinite(norms).all() or np.any(norms < 1e-9):
raise CalibratedProjectionError("KB4 inverse produced a zero camera ray")
return np.asarray(directions / norms[:, None], dtype=np.float64)
def depth_colors(depths_m: npt.ArrayLike) -> npt.NDArray[np.uint8]:
"""Return deterministic blue→cyan→green→yellow→red diagnostic colors."""
src/k1link/device_plugins/xgrids_k1/cli.py
+88
View File
@@ -24,6 +24,7 @@ from k1link.compute import (
publish_e23_lab_instance,
publish_e24_lab_instance,
publish_e25_lab_instance,
publish_e26_lab_instance,
publish_integrated_lab_instance,
)
from k1link.device_plugins.xgrids_k1.analyze import (
@@ -743,6 +744,93 @@ def publish_e25_lab(
)
@lab_app.command("publish-e26")
def publish_e26_lab(
lidar_result: Annotated[
Path,
typer.Option(
"--lidar-result",
exists=True,
file_okay=False,
readable=True,
resolve_path=True,
help="Accepted E25 persistent-support result.",
),
],
camera_result: Annotated[
Path,
typer.Option(
"--camera-result",
exists=True,
file_okay=False,
readable=True,
resolve_path=True,
help="Accepted pre-world-motion result containing every 2D track.",
),
],
profile: Annotated[
Path,
typer.Option(
exists=True,
dir_okay=False,
readable=True,
resolve_path=True,
help="Bounded E26 camera/ego-motion profile.",
),
],
benchmark: Annotated[
Path,
typer.Option(
exists=True,
dir_okay=False,
readable=True,
resolve_path=True,
help="Spatially bound E26 motion benchmark.",
),
],
session_id: Annotated[
str,
typer.Option("--session-id", help="New immutable LAB session id."),
],
lab_id: Annotated[
str,
typer.Option("--lab-id", help="LAB marker, for example 'LAB E26.1'."),
],
display_name: Annotated[
str,
typer.Option("--display-name", help="Operator-facing saved-session title."),
],
) -> None:
"""Build and publish calibrated camera/ego-motion evidence."""
repository_root = Path(__file__).resolve().parents[4]
try:
published = publish_e26_lab_instance(
repository_root=repository_root,
lidar_result_root=lidar_result,
camera_result_root=camera_result,
profile_path=profile,
benchmark_path=benchmark,
lab_session_id=session_id,
lab_id=lab_id,
display_name=display_name,
)
except (OSError, SessionIntegrityError, RuntimeError, ValueError) as exc:
console.print(f"[red]E26 LAB publication failed:[/red] {exc}")
raise typer.Exit(code=2) from exc
metrics = published.build.report["metrics"]
console.print(
"[green]E26 LAB instance published.[/green] "
f"session={published.binding.session_id}; "
f"source={published.binding.source_session_id}; "
f"result={published.binding.result_id}; "
f"benchmark={metrics['benchmark']['passed_events']}/"
f"{metrics['benchmark']['total_events']}; "
f"p95={metrics['runtime']['camera_ego_motion_frame_processing_ms']['p95']:.3f}ms; "
"camera_metric_velocity=false; source_payloads_mutated=false"
)
@app.command("serve")
def serve_console(
port: Annotated[