From dfc2a76381f91f32e15619d6b3a1934f58b6d0df Mon Sep 17 00:00:00 2001 From: styu12 Date: Mon, 3 Aug 2026 23:25:34 +0900 Subject: [PATCH] feat(ovision): add stereo-inertial camera adapter --- pyproject.toml | 6 +- src/syncfield/adapters/ovision_calibration.py | 249 +++++++ src/syncfield/adapters/ovision_camera.py | 668 ++++++++++++++++++ src/syncfield/adapters/ovision_metadata.py | 360 ++++++++++ tests/unit/adapters/test_ovision_metadata.py | 207 ++++++ uv.lock | 8 +- 6 files changed, 1496 insertions(+), 2 deletions(-) create mode 100644 src/syncfield/adapters/ovision_calibration.py create mode 100644 src/syncfield/adapters/ovision_camera.py create mode 100644 src/syncfield/adapters/ovision_metadata.py create mode 100644 tests/unit/adapters/test_ovision_metadata.py diff --git a/pyproject.toml b/pyproject.toml index 85fb7a1..1075006 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -23,7 +23,7 @@ classifiers = [ # Default install ships the canonical first-run stack: a UVC webcam adapter, # the browser viewer, and the sounddevice-backed chirp / countdown player # (so the 3/2/1 ticks and start/stop chirps are audible out of the box). -# Heavier / platform-specific adapters (BLE, OAK, off-host cameras, +# Heavier / platform-specific adapters (BLE, OAK, OVISION, off-host cameras, # multi-host control plane) stay opt-in via the optional-dependencies # block below. dependencies = [ @@ -44,6 +44,9 @@ ble = ["bleak>=0.21", "pyserial>=3.5"] # pyserial: OGLO USB CDC wired transport # for RVC4). PyAV writes the MP4s (also via _video_encoder, hence numpy). # Install `syncfield[oak]` for the full OAK capture path. oak = ["depthai>=2.30", "av>=12.0.0", "numpy>=1.21"] +# OVISION per-unit flash calibration is a Kalibr YAML payload read through +# the Linux UVC Extension Unit. Video/metadata parsing remains dependency-free. +ovision = ["PyYAML>=6.0"] # mDNS-based multi-host session rendezvous (syncfield.multihost). Required # only when you want a leader/follower session discovered automatically # on the local network — single-host sessions do not need this. @@ -70,6 +73,7 @@ all = [ "depthai>=3.0.0", "zeroconf>=0.130", "httpx>=0.25.0", + "PyYAML>=6.0", ] [project.urls] diff --git a/src/syncfield/adapters/ovision_calibration.py b/src/syncfield/adapters/ovision_calibration.py new file mode 100644 index 0000000..92fef80 --- /dev/null +++ b/src/syncfield/adapters/ovision_calibration.py @@ -0,0 +1,249 @@ +"""Read and validate per-unit OVISION calibration over the Linux UVC XU. + +This is a narrow implementation of HAMPO's V3 customer protocol. It exposes +only read-only calibration and output-mode queries; firmware recovery and +control mutation deliberately remain outside the capture adapter. +""" + +from __future__ import annotations + +import ctypes +import hashlib +import os +import struct +import zlib +from dataclasses import dataclass +from pathlib import Path +from typing import Any + + +XU_UNIT_ID = 0x0A +SELECTOR_CALIB_INFO = 0x02 +SELECTOR_CALIB_TRANSFER = 0x03 +SELECTOR_CALIB_COMMAND = 0x04 +SELECTOR_OUTPUT_MODE = 0x06 +UVC_SET_CUR = 0x01 +UVC_GET_CUR = 0x81 + + +class OvisionCalibrationError(RuntimeError): + """Per-unit calibration could not be read or failed integrity checks.""" + + +@dataclass(frozen=True) +class OvisionCalibration: + blob: bytes + yaml_text: str + serial_number: str + schema_version: int + calibration_version: int + blob_crc32: int + payload_sha256: str + output_mode: str + + def parsed_yaml(self) -> dict[str, Any]: + try: + import yaml # type: ignore[import-not-found] + except ImportError as exc: # pragma: no cover - optional extra guard + raise OvisionCalibrationError( + "OVISION calibration parsing requires PyYAML" + ) from exc + data = yaml.safe_load(self.yaml_text) + if not isinstance(data, dict): + raise OvisionCalibrationError("Kalibr payload is not a mapping") + validate_kalibr_camchain(data) + return data + + def capture_document( + self, + *, + usb_serial: str | None = None, + native_eye_resolution: tuple[int, int] = (1920, 1080), + ) -> dict[str, Any]: + """Build the lossless production sidecar used by downstream VIO.""" + + data = self.parsed_yaml() + streams: dict[str, Any] = {} + for source_name, target_name in (("cam0", "left"), ("cam1", "right")): + camera = data[source_name] + source_resolution = tuple(int(v) for v in camera["resolution"]) + sx = native_eye_resolution[0] / source_resolution[0] + sy = native_eye_resolution[1] / source_resolution[1] + fx, fy, cx, cy = (float(v) for v in camera["intrinsics"]) + streams[target_name] = { + "socket": source_name, + "camera_model": camera["camera_model"], + "distortion_model": camera["distortion_model"], + "distortion_coeffs": camera["distortion_coeffs"], + "resolution": list(native_eye_resolution), + "calibration_resolution": list(source_resolution), + "intrinsics": [ + [fx * sx, 0.0, cx * sx], + [0.0, fy * sy, cy * sy], + [0.0, 0.0, 1.0], + ], + "T_cam_imu": camera["T_cam_imu"], + "timeshift_cam_imu_s": float(camera["timeshift_cam_imu"]), + } + # Legacy consumers call the primary pinhole stream ``rgb``. OVISION's + # packed primary is not itself a pinhole image, so explicitly alias the + # left eye rather than inventing intrinsics for 3840x1080. + streams["rgb"] = {**streams["left"], "alias_of": "left"} + + return { + "schema": "syncfield.ovision_calibration.v1", + "device": { + "name": "OVISION HAMPO-USB-3290-V1.0", + "usb_serial": usb_serial, + "calibration_serial": self.serial_number, + "calibration_schema_version": self.schema_version, + "calibration_version": self.calibration_version, + "calibration_blob_crc32": self.blob_crc32, + "calibration_payload_sha256": self.payload_sha256, + "output_mode": self.output_mode, + }, + "stereo": { + "layout": "side_by_side", + "packed_resolution": [native_eye_resolution[0] * 2, native_eye_resolution[1]], + "eye_order": ["left", "right"], + "T_right_left": data["cam1"]["T_cn_cnm1"], + "synchronization": "internal_fsync", + }, + "streams": streams, + "imu": { + "model": "TDK ICM-42688-P", + "sample_rate_hz": 500, + "accelerometer_range_g": 4, + "accelerometer_lsb_per_g": 8192, + "gyroscope_range_dps": 1000, + "gyroscope_lsb_per_dps": 32.768, + "accelerometer_noise_density_m_s2_sqrt_hz": 70e-6 * 9.80665, + "gyroscope_noise_density_rad_s_sqrt_hz": 2.8e-3 * 3.141592653589793 / 180.0, + "intrinsic_source": "sensor_datasheet_not_per_unit_bias_calibration", + }, + "magnetometer": { + "model": "MEMSIC MMC5633NJL", + "sample_rate_hz": 100, + "raw_preserved": True, + "tesla_per_lsb_assuming_20_bit": 1e-4 / 16384.0, + "scale_status": "datasheet_mode_inferred_not_firmware_declared", + }, + "raw_kalibr_yaml": self.yaml_text, + } + + +class _UvcXuControlQuery(ctypes.Structure): + _fields_ = [ + ("unit", ctypes.c_uint8), + ("selector", ctypes.c_uint8), + ("query", ctypes.c_uint8), + ("size", ctypes.c_uint16), + ("data", ctypes.POINTER(ctypes.c_uint8)), + ] + + +def _uvcioc_ctrl_query() -> int: + # Linux _IOWR('u', 0x21, struct uvc_xu_control_query). + return (3 << 30) | (ord("u") << 8) | 0x21 | (ctypes.sizeof(_UvcXuControlQuery) << 16) + + +def _xu_query(path: str | Path, selector: int, query: int, size: int, payload: bytes | None = None) -> bytes: + try: + import fcntl + except ImportError as exc: # pragma: no cover - non-Linux + raise OvisionCalibrationError("OVISION XU readback requires Linux") from exc + buf = (ctypes.c_uint8 * max(size, 1))() + if payload is not None: + if len(payload) != size: + raise OvisionCalibrationError("XU payload length mismatch") + for index, value in enumerate(payload): + buf[index] = value + control = _UvcXuControlQuery(XU_UNIT_ID, selector, query, size, buf) + fd = os.open(os.fspath(path), os.O_RDWR) + try: + fcntl.ioctl(fd, _uvcioc_ctrl_query(), control, True) + except OSError as exc: + raise OvisionCalibrationError( + f"UVC XU query selector 0x{selector:02x} failed on {path}: {exc}" + ) from exc + finally: + os.close(fd) + return bytes(buf[:size]) + + +def _crc16_modbus(data: bytes) -> int: + crc = 0xFFFF + for byte in data: + crc ^= byte + for _ in range(8): + crc = (crc >> 1) ^ 0xA001 if crc & 1 else crc >> 1 + return crc & 0xFFFF + + +def validate_kalibr_camchain(data: dict[str, Any]) -> None: + for camera_name in ("cam0", "cam1"): + camera = data.get(camera_name) + if not isinstance(camera, dict): + raise OvisionCalibrationError(f"{camera_name} missing from Kalibr payload") + for field in ( + "T_cam_imu", "camera_model", "distortion_model", + "distortion_coeffs", "intrinsics", "resolution", "timeshift_cam_imu", + ): + if field not in camera: + raise OvisionCalibrationError(f"{camera_name}.{field} missing") + if len(camera["intrinsics"]) != 4 or len(camera["resolution"]) != 2: + raise OvisionCalibrationError(f"{camera_name} intrinsics/resolution malformed") + matrix = camera["T_cam_imu"] + if len(matrix) != 4 or any(len(row) != 4 for row in matrix): + raise OvisionCalibrationError(f"{camera_name}.T_cam_imu malformed") + baseline = data["cam1"].get("T_cn_cnm1") + if not isinstance(baseline, list) or len(baseline) != 4 or any(len(row) != 4 for row in baseline): + raise OvisionCalibrationError("cam1.T_cn_cnm1 missing or malformed") + + +def read_ovision_calibration(video_device: str | Path = "/dev/video0") -> OvisionCalibration: + """Read, CRC-check and parse the active per-unit schema-v2 blob.""" + + info_raw = _xu_query(video_device, SELECTOR_CALIB_INFO, UVC_GET_CUR, 16) + schema, version, expected_crc, total_length = struct.unpack_from(" "_SidecarSinks": + directory.mkdir(parents=True, exist_ok=True) + return cls( + directory, + stem, + (directory / f"{stem}.stereo.jsonl").open("w", encoding="utf-8"), + (directory / f"{stem}.imu.jsonl").open("w", encoding="utf-8"), + (directory / f"{stem}.accel.jsonl").open("w", encoding="utf-8"), + (directory / f"{stem}.gyro.jsonl").open("w", encoding="utf-8"), + (directory / f"{stem}.mag.jsonl").open("w", encoding="utf-8"), + ) + + def close(self) -> None: + for file in (self.frame_meta, self.imu, self.accel, self.gyro, self.mag): + file.flush() + file.close() + + +class OvisionCameraStream(StreamBase): + """One physical OVISION camera as one composite stereo-inertial stream.""" + + _discovery_kind = "video" + _discovery_adapter_type = "ovision_camera" + + def __init__( + self, + id: str, + output_dir: str | Path, + *, + video_device: str | Path = "/dev/video0", + usb_serial: str | None = None, + width: int = 3840, + height: int = 1080, + fps: float = 30.0, + preview_interval_s: float = 0.5, + ) -> None: + super().__init__( + id=id, + kind="video", + capabilities=StreamCapabilities( + provides_audio_track=False, + supports_precise_timestamps=True, + is_removable=True, + produces_file=True, + target_hz=fps, + ), + ) + if (width, height) != (3840, 1080): + raise ValueError("OVISION production mode is fixed at 3840x1080") + self._output_dir = Path(output_dir) + self._video_device = Path(video_device) + self._usb_serial = usb_serial + self._width = width + self._height = height + self._fps = float(fps) + self._preview_interval_s = float(preview_interval_s) + self._file_path = self._output_dir / f"{id}.mp4" + self._input: Any = None + self._writer: PassthroughWriter | None = None + self._sinks: _SidecarSinks | None = None + self._prepared: tuple[Path, PassthroughWriter, _SidecarSinks] | None = None + self._calibration: OvisionCalibration | None = None + self._calibration_document: dict[str, Any] | None = None + self._thread: threading.Thread | None = None + self._writer_thread: threading.Thread | None = None + self._packet_queue: queue.Queue[tuple[Any, bytes, int, bool]] = queue.Queue(maxsize=3600) + self._stop_event = threading.Event() + # READY means more than "V4L2 opened": at least one live H.264 packet + # must carry a valid manufacturer SEI payload with stereo timing and + # IMU samples. The kiosk waits on this latch before enabling record. + self._ready_event = threading.Event() + self._recording_lock = threading.Lock() + self._rotation_condition = threading.Condition(self._recording_lock) + self._sink_lock = threading.Lock() + self._recording = False + self._frame_count = 0 + self._first_at: int | None = None + self._last_at: int | None = None + self._prev_capture_ns: int | None = None + self._intervals_ns: list[int] = [] + self._capture_error: str | None = None + self._recorded_artifacts: tuple[OvisionArtifact, ...] = () + self._frame_lock = threading.Lock() + self._latest_frame: Any = None + self._last_preview_at = 0.0 + self._preview_wake = threading.Event() + self._preview_packet: bytes | None = None + self._preview_thread: threading.Thread | None = None + self._started_on_keyframe = False + self._rotation_requested = False + self._rotation_keyframe: tuple[Any, bytes, int, bool] | None = None + + @property + def output_name(self) -> str: + return self.id + + @property + def device_key(self) -> tuple[str, str]: + return ("ovision_camera", self._usb_serial or str(self._video_device)) + + @property + def latest_frame(self) -> Any: + with self._frame_lock: + return self._latest_frame + + def capture_ready(self) -> bool: + """Whether live video plus strict stereo-inertial metadata is valid.""" + return ( + self._ready_event.is_set() + and self._thread is not None + and self._thread.is_alive() + and self._capture_error is None + ) + + def prepare(self) -> None: + if self._input is not None: + return + calibration = read_ovision_calibration(self._video_device) + if calibration.output_mode != "internal": + raise RuntimeError( + f"OVISION must use INTERNAL stereo FSYNC, got {calibration.output_mode}" + ) + document = calibration.capture_document(usb_serial=self._usb_serial) + options = { + "video_size": f"{self._width}x{self._height}", + "framerate": str(int(round(self._fps))), + "input_format": "h264", + "fflags": "nobuffer+flush_packets", + "flags": "low_delay", + "analyzeduration": "0", + "max_delay": "0", + } + self._input = av.open(str(self._video_device), format="v4l2", options=options) + stream = self._input.streams.video[0] + if (stream.codec_context.width, stream.codec_context.height) != (self._width, self._height): + self._input.close() + self._input = None + raise RuntimeError("OVISION did not negotiate 3840x1080 H.264") + self._calibration = calibration + self._calibration_document = document + + def connect(self) -> None: + if self._thread is not None and self._thread.is_alive(): + return + if self._input is None: + self.prepare() + self._stop_event.clear() + self._ready_event.clear() + self._capture_error = None + self._thread = threading.Thread(target=self._capture_loop, name=f"ovision-{self.id}", daemon=True) + self._thread.start() + self._writer_thread = threading.Thread( + target=self._writer_loop, name=f"ovision-writer-{self.id}", daemon=True + ) + self._writer_thread.start() + self._preview_thread = threading.Thread( + target=self._preview_loop, name=f"ovision-preview-{self.id}", daemon=True + ) + self._preview_thread.start() + + def _write_calibration(self, directory: Path) -> None: + assert self._calibration is not None and self._calibration_document is not None + (directory / f"{self.id}.calibration.json").write_text( + json.dumps(self._calibration_document, indent=2) + "\n", encoding="utf-8" + ) + (directory / f"{self.id}.calibration.yaml").write_text( + self._calibration.yaml_text, encoding="utf-8" + ) + (directory / f"{self.id}.calibration.bin").write_bytes(self._calibration.blob) + + def start_recording(self, session_clock: SessionClock) -> None: + self._begin_recording_window(session_clock) + if self._thread is None or not self._thread.is_alive(): + self.connect() + self._output_dir.mkdir(parents=True, exist_ok=True) + self._write_calibration(self._output_dir) + writer = PassthroughWriter.open(self._file_path, template_stream=self._input.streams.video[0]) + sinks = _SidecarSinks.open(self._output_dir, self.id) + with self._sink_lock: + self._frame_count = 0 + self._first_at = None + self._last_at = None + self._prev_capture_ns = None + self._intervals_ns = [] + self._capture_error = None + self._started_on_keyframe = False + self._writer = writer + self._sinks = sinks + with self._recording_lock: + self._recording = True + + def _capture_loop(self) -> None: + packet_iter = None + try: + while not self._stop_event.is_set(): + if packet_iter is None: + packet_iter = self._input.demux(video=0) + try: + packet = next(packet_iter) + except StopIteration: + return + except OSError as exc: + if exc.errno in (None, 4, 11, 35): + packet_iter = None + self._stop_event.wait(0.001) + continue + raise + if packet.size <= 0: + continue + capture_ns = time.monotonic_ns() + encoded = bytes(packet) + is_keyframe = bool(getattr(packet, "is_keyframe", False)) + with self._rotation_condition: + if self._recording: + item = (packet, encoded, capture_ns, is_keyframe) + if self._rotation_requested and is_keyframe: + self._rotation_keyframe = item + self._rotation_condition.notify_all() + while self._rotation_requested and not self._stop_event.is_set(): + self._rotation_condition.wait(timeout=0.2) + else: + try: + self._packet_queue.put_nowait(item) + except queue.Full: + self._capture_error = "OVISION writer queue overflow" + self._emit_health(HealthEvent( + stream_id=self.id, + kind=HealthEventKind.ERROR, + at_ns=capture_ns, + detail=self._capture_error, + )) + if not self._recording: + if not self._ready_event.is_set(): + try: + metadata = parse_ovision_h264_metadata(encoded) + if not metadata.accel or not metadata.gyro: + raise OvisionMetadataError( + "live frame has no accelerometer/gyroscope samples" + ) + self._capture_error = None + self._ready_event.set() + except OvisionMetadataError as exc: + self._capture_error = ( + f"invalid OVISION live metadata: {exc}" + ) + self._queue_preview(encoded, is_keyframe) + except Exception as exc: # noqa: BLE001 + self._capture_error = f"{type(exc).__name__}: {exc}" + self._emit_health(HealthEvent( + stream_id=self.id, + kind=HealthEventKind.ERROR, + at_ns=time.monotonic_ns(), + detail=f"OVISION capture loop ended: {self._capture_error}", + )) + + def _writer_loop(self) -> None: + while not self._stop_event.is_set() or not self._packet_queue.empty(): + try: + packet, encoded, capture_ns, is_keyframe = self._packet_queue.get(timeout=0.2) + except queue.Empty: + continue + try: + metadata = parse_ovision_h264_metadata(encoded) + with self._sink_lock: + if self._writer is not None and self._sinks is not None: + self._record_packet(packet, metadata, capture_ns, is_keyframe) + except OvisionMetadataError as exc: + self._capture_error = f"invalid OVISION frame metadata: {exc}" + self._emit_health(HealthEvent( + stream_id=self.id, + kind=HealthEventKind.ERROR, + at_ns=capture_ns, + detail=self._capture_error, + )) + except Exception as exc: # noqa: BLE001 + self._capture_error = f"OVISION writer failed: {type(exc).__name__}: {exc}" + self._emit_health(HealthEvent( + stream_id=self.id, + kind=HealthEventKind.ERROR, + at_ns=capture_ns, + detail=self._capture_error, + )) + finally: + self._packet_queue.task_done() + + def _record_packet( + self, + packet: Any, + meta: OvisionFrameMetadata, + capture_ns: int, + is_keyframe: bool, + ) -> None: + assert self._writer is not None and self._sinks is not None + # MP4 cannot begin on a predictive frame. Keep every artifact aligned + # by opening the recording window only at the first complete IDR. + if not self._started_on_keyframe: + if not is_keyframe: + return + self._started_on_keyframe = True + frame_number = self._frame_count + device_ns = meta.left_exposure_start_pts_us * 1000 + self._writer.write_packet(packet, capture_ns) + self._observe_first_frame(capture_ns, device_ns) + if self._first_at is None: + self._first_at = capture_ns + self._last_at = capture_ns + if self._prev_capture_ns is not None: + self._intervals_ns.append(capture_ns - self._prev_capture_ns) + self._prev_capture_ns = capture_ns + self._frame_count += 1 + self._sinks.frame_count += 1 + self._sinks.frame_meta.write(json.dumps({ + "frame_number": frame_number, + "capture_ns": capture_ns, + "clock_source": "device_monotonic", + "device_timestamp_ns": device_ns, + "left_exposure_start_ns": meta.left_exposure_start_pts_us * 1000, + "right_exposure_start_ns": meta.right_exposure_start_pts_us * 1000, + "stereo_skew_us": meta.stereo_exposure_start_skew_us, + "left_start_line_rx_ns": meta.left_start_line_rx_pts_us * 1000, + "right_start_line_rx_ns": meta.right_start_line_rx_pts_us * 1000, + "left_exposure_time_us": meta.left_exposure_time_us, + "right_exposure_time_us": meta.right_exposure_time_us, + "left_gpio_trigger_index": meta.left_gpio_trigger_index, + "right_gpio_trigger_index": meta.right_gpio_trigger_index, + "user_data_seq": meta.user_data_seq, + "frame_meta_generation": meta.frame_meta_generation, + }, separators=(",", ":")) + "\n") + self._write_imu(meta, capture_ns) + self._write_mag(meta, capture_ns) + self._emit_sample(SampleEvent( + stream_id=self.id, + frame_number=frame_number, + capture_ns=capture_ns, + uncertainty_ns=500_000, + device_ns=device_ns, + )) + + @staticmethod + def _host_time_for_device(sample_us: int, frame_us: int, capture_ns: int) -> int: + return capture_ns + (sample_us - frame_us) * 1000 + + def _write_imu(self, meta: OvisionFrameMetadata, capture_ns: int) -> None: + assert self._sinks is not None + gyro = {sample.device_timestamp_us: sample for sample in meta.gyro} + accel = {sample.device_timestamp_us: sample for sample in meta.accel} + if gyro.keys() != accel.keys(): + raise OvisionMetadataError("gyro/accelerometer timestamp sets differ") + for timestamp_us in sorted(gyro): + g = gyro[timestamp_us] + a = accel[timestamp_us] + gx, gy, gz = g.gyro_rad_s() + ax, ay, az = a.accel_m_s2() + sample_capture_ns = self._host_time_for_device( + timestamp_us, meta.left_exposure_start_pts_us, capture_ns + ) + common = { + "frame_number": self._sinks.imu_count, + "capture_ns": sample_capture_ns, + "clock_source": "device_monotonic", + "uncertainty_ns": 500_000, + "device_timestamp_ns": timestamp_us * 1000, + } + self._sinks.imu.write(json.dumps({ + **common, + "accel_unit": "m_s2", + "accel_kind": "raw_specific_force_includes_gravity", + "gyro_unit": "rad_s", + "channels": { + "gyro_x": gx, "gyro_y": gy, "gyro_z": gz, + "accel_x": ax, "accel_y": ay, "accel_z": az, + "gyro_temperature_raw": g.temperature_raw, + "accel_temperature_raw": a.temperature_raw, + }, + }, separators=(",", ":")) + "\n") + # Standard raw VIO sidecars. Acceleration follows the existing + # OG-Skill raw-accelerometer contract (g); gyro is rad/s. Keeping + # these separate prevents OVISION specific force from ever being + # mistaken for CoreMotion gravity-free DeviceMotion acceleration. + self._sinks.accel.write(json.dumps({ + **common, + "frame_number": self._sinks.accel_count, + "unit": "g", + "kind": "raw_specific_force_includes_gravity", + "channels": { + "accel_x": ax / 9.80665, + "accel_y": ay / 9.80665, + "accel_z": az / 9.80665, + "temperature_raw": a.temperature_raw, + }, + }, separators=(",", ":")) + "\n") + self._sinks.gyro.write(json.dumps({ + **common, + "frame_number": self._sinks.gyro_count, + "unit": "rad_s", + "channels": { + "gyro_x": gx, "gyro_y": gy, "gyro_z": gz, + "temperature_raw": g.temperature_raw, + }, + }, separators=(",", ":")) + "\n") + self._sinks.imu_count += 1 + self._sinks.accel_count += 1 + self._sinks.gyro_count += 1 + + def _write_mag(self, meta: OvisionFrameMetadata, capture_ns: int) -> None: + assert self._sinks is not None + for sample in meta.mag: + tx, ty, tz = sample.tesla_20_bit() + self._sinks.mag.write(json.dumps({ + "frame_number": self._sinks.mag_count, + "capture_ns": self._host_time_for_device( + sample.device_timestamp_us, meta.left_exposure_start_pts_us, capture_ns + ), + "clock_source": "device_monotonic", + "uncertainty_ns": 500_000, + "device_timestamp_ns": sample.device_timestamp_us * 1000, + "channels": { + "mag_x_raw": sample.xyz_raw[0], + "mag_y_raw": sample.xyz_raw[1], + "mag_z_raw": sample.xyz_raw[2], + "mag_x_tesla_20bit_assumption": tx, + "mag_y_tesla_20bit_assumption": ty, + "mag_z_tesla_20bit_assumption": tz, + "tout_raw": sample.tout_raw, + "temperature_milli_c": sample.temperature_milli_c, + }, + }, separators=(",", ":")) + "\n") + self._sinks.mag_count += 1 + + def _queue_preview(self, encoded: bytes, is_keyframe: bool) -> None: + now = time.monotonic() + if not is_keyframe or now - self._last_preview_at < self._preview_interval_s: + return + self._last_preview_at = now + # Single-slot handoff: capture never waits for a multi-megapixel decode. + self._preview_packet = encoded + self._preview_wake.set() + + def _preview_loop(self) -> None: + while not self._stop_event.is_set(): + if not self._preview_wake.wait(timeout=0.5): + continue + self._preview_wake.clear() + encoded, self._preview_packet = self._preview_packet, None + if encoded is None: + continue + try: + decoder = av.CodecContext.create("h264", "r") + frames = decoder.decode(av.Packet(encoded)) + if frames: + packed = frames[-1].to_ndarray(format="bgr24") + with self._frame_lock: + self._latest_frame = packed[:, : self._width // 2] + except Exception: # noqa: BLE001 + logger.debug("OVISION preview decode failed", exc_info=True) + + def _artifacts(self, sinks: _SidecarSinks) -> tuple[OvisionArtifact, ...]: + return ( + OvisionArtifact(f"{self.id}.stereo", "sensor", sinks.directory / f"{self.id}.stereo.jsonl", sinks.frame_count), + OvisionArtifact(f"{self.id}.imu", "sensor", sinks.directory / f"{self.id}.imu.jsonl", sinks.imu_count), + OvisionArtifact(f"{self.id}.accel", "sensor", sinks.directory / f"{self.id}.accel.jsonl", sinks.accel_count), + OvisionArtifact(f"{self.id}.gyro", "sensor", sinks.directory / f"{self.id}.gyro.jsonl", sinks.gyro_count), + OvisionArtifact(f"{self.id}.mag", "sensor", sinks.directory / f"{self.id}.mag.jsonl", sinks.mag_count), + ) + + def stop_recording(self) -> FinalizationReport: + with self._recording_lock: + self._recording = False + # The producer can no longer enqueue after the lock handoff above. + # Drain every already-dequeued frame before closing any sink. + self._packet_queue.join() + with self._sink_lock: + writer, sinks = self._writer, self._sinks + self._writer = None + self._sinks = None + if writer is not None: + writer.close() + if sinks is not None: + sinks.close() + self._recorded_artifacts = self._artifacts(sinks) + jitter_p95, jitter_p99 = compute_jitter_percentiles(self._intervals_ns) + status = "completed" if self._frame_count and not self._capture_error else "failed" + return FinalizationReport( + stream_id=self.id, + status=status, + frame_count=self._frame_count, + file_path=self._file_path if self._frame_count else None, + first_sample_at_ns=self._first_at, + last_sample_at_ns=self._last_at, + health_events=list(self._collected_health), + error=self._capture_error, + jitter_p95_ns=jitter_p95, + jitter_p99_ns=jitter_p99, + recording_anchor=self._recording_anchor(), + ) + + def prepare_segment_rotation(self, next_output_dir: Path) -> None: + next_output_dir.mkdir(parents=True, exist_ok=True) + self._write_calibration(next_output_dir) + path = next_output_dir / f"{self.id}.mp4" + writer = PassthroughWriter.open(path, template_stream=self._input.streams.video[0]) + sinks = _SidecarSinks.open(next_output_dir, self.id) + self._prepared = (path, writer, sinks) + + def abort_segment_rotation(self) -> None: + prepared, self._prepared = self._prepared, None + if prepared is not None: + _, writer, sinks = prepared + writer.close() + sinks.close() + + def commit_segment_rotation( + self, + boundary_monotonic_ns: int, + swap_persistence: Any = None, + next_session_clock: SessionClock | None = None, + ) -> FinalizationReport: + if self._prepared is None: + raise RuntimeError("OVISION segment rotation was not prepared") + # Cut on the next IDR: all earlier packets drain to the old MP4, the IDR + # itself becomes frame zero in the new MP4. This avoids both packet loss + # and undecodable segment starts. + with self._rotation_condition: + self._rotation_requested = True + self._rotation_keyframe = None + deadline = time.monotonic() + 3.0 + while self._rotation_keyframe is None: + remaining = deadline - time.monotonic() + if remaining <= 0: + self._rotation_requested = False + self._rotation_condition.notify_all() + raise TimeoutError("OVISION did not emit an IDR within 3 seconds") + self._rotation_condition.wait(timeout=remaining) + self._packet_queue.join() + with self._sink_lock: + old = ( + self._writer, self._sinks, self._file_path, self._frame_count, + self._first_at, self._last_at, self._intervals_ns, self._recording_anchor(), + ) + self._file_path, self._writer, self._sinks = self._prepared + self._output_dir = self._file_path.parent + self._prepared = None + self._frame_count = 0 + self._first_at = None + self._last_at = None + self._prev_capture_ns = None + self._intervals_ns = [] + self._started_on_keyframe = False + if swap_persistence is not None: + swap_persistence() + if next_session_clock is not None: + self._begin_recording_window(next_session_clock) + self._packet_queue.put_nowait(self._rotation_keyframe) + self._rotation_keyframe = None + self._rotation_requested = False + self._rotation_condition.notify_all() + writer, sinks, path, count, first, last, intervals, anchor = old + if writer is not None: + writer.close() + if sinks is not None: + sinks.close() + self._recorded_artifacts = self._artifacts(sinks) + p95, p99 = compute_jitter_percentiles(intervals) + return FinalizationReport( + stream_id=self.id, + status="completed" if count else "failed", + frame_count=count, + file_path=path if count else None, + first_sample_at_ns=first, + last_sample_at_ns=last, + health_events=list(self._collected_health), + error=None if count else "No OVISION frames arrived during segment", + jitter_p95_ns=p95, + jitter_p99_ns=p99, + recording_anchor=anchor, + ) + + def recorded_artifacts(self) -> tuple[OvisionArtifact, ...]: + return self._recorded_artifacts + + def disconnect(self) -> None: + self._stop_event.set() + self._ready_event.clear() + with self._rotation_condition: + self._rotation_requested = False + self._rotation_condition.notify_all() + self._preview_wake.set() + if self._thread is not None: + self._thread.join(timeout=3) + self._thread = None + if self._writer_thread is not None: + self._writer_thread.join(timeout=3) + self._writer_thread = None + if self._preview_thread is not None: + self._preview_thread.join(timeout=3) + self._preview_thread = None + if self._input is not None: + self._input.close() + self._input = None + + def start(self, session_clock: SessionClock) -> None: + self.connect() + self.start_recording(session_clock) + + def stop(self) -> FinalizationReport: + report = self.stop_recording() + self.disconnect() + return report diff --git a/src/syncfield/adapters/ovision_metadata.py b/src/syncfield/adapters/ovision_metadata.py new file mode 100644 index 0000000..a5539b3 --- /dev/null +++ b/src/syncfield/adapters/ovision_metadata.py @@ -0,0 +1,360 @@ +"""Parse HAMPO/ZXCZ ``YCTC`` V2 metadata embedded by OVISION cameras. + +The SC233HGS stereo module transports frame timing, ICM-42688-P gyro and +accelerometer samples, and MMC5633NJL magnetometer samples in each compressed +video frame. This module follows the manufacturer's V3 wire protocol. All +timestamps share the camera's monotonic microsecond clock. +""" + +from __future__ import annotations + +import math +import struct +from dataclasses import dataclass + + +YCTC_MAGIC = b"YCTC" +YCTC_VERSION = 2 +YCTC_V2_HEADER_SIZE = 72 + +SENSOR_PRESENT = 0x01 +START_LINE_RX_PTS_VALID = 0x02 +EXPOSURE_START_PTS_VALID = 0x04 +EXPOSURE_TIME_VALID = 0x08 +GPIO_TRIGGER_INDEX_VALID = 0x10 +_KNOWN_SENSOR_FLAGS = 0x1F + +IMU_MAX_SAMPLES = 128 +MAG_MAX_SAMPLES = 16 +_IMU_HEADER_SIZE = 8 +_IMU_SAMPLE_SIZE = 18 +_MAG_HEADER_SIZE = 8 +_MAG_SAMPLE_SIZE = 28 + +# Vendor-stated firmware configuration: accel +/-4 g, gyro +/-1000 dps. +STANDARD_GRAVITY_M_S2 = 9.80665 +ACCEL_M_S2_PER_LSB = 4.0 * STANDARD_GRAVITY_M_S2 / 32768.0 +GYRO_RAD_S_PER_LSB = math.radians(1000.0) / 32768.0 + +# MMC5633NJL 20-bit sensitivity. The wire protocol does not identify the +# configured resolution, so production artifacts must retain xyz_raw even +# when this convenience conversion is used. +MAG_TESLA_PER_LSB_20_BIT = 1e-4 / 16384.0 + + +class OvisionMetadataError(ValueError): + """The compressed frame contains no valid, complete OVISION metadata.""" + + +@dataclass(frozen=True) +class OvisionImuSample: + """One gyro or accelerometer sample in the IMU sensor frame.""" + + xyz_raw: tuple[int, int, int] + temperature_raw: int + device_timestamp_us: int + + def gyro_rad_s(self) -> tuple[float, float, float]: + return tuple(value * GYRO_RAD_S_PER_LSB for value in self.xyz_raw) + + def accel_m_s2(self) -> tuple[float, float, float]: + return tuple(value * ACCEL_M_S2_PER_LSB for value in self.xyz_raw) + + +@dataclass(frozen=True) +class OvisionMagSample: + """One MMC5633NJL sample in the magnetometer sensor frame.""" + + xyz_raw: tuple[int, int, int] + tout_raw: int + temperature_milli_c: int + device_timestamp_us: int + + def tesla_20_bit(self) -> tuple[float, float, float]: + """Convert assuming the firmware's empirically identified 20-bit mode.""" + + return tuple(value * MAG_TESLA_PER_LSB_20_BIT for value in self.xyz_raw) + + +@dataclass(frozen=True) +class OvisionFrameMetadata: + """Validated V2 timing and sensor batches from one video frame.""" + + version: int + payload_size: int + left_start_line_rx_pts_us: int + right_start_line_rx_pts_us: int + left_exposure_time_us: int + right_exposure_time_us: int + left_gpio_trigger_index: int + right_gpio_trigger_index: int + left_exposure_start_pts_us: int + right_exposure_start_pts_us: int + user_data_seq: int + frame_meta_generation: int + left_valid_flags: int + right_valid_flags: int + left_vi_pipe: int + right_vi_pipe: int + imu_generation: int | None + gyro: tuple[OvisionImuSample, ...] + accel: tuple[OvisionImuSample, ...] + mag_generation: int | None + mag: tuple[OvisionMagSample, ...] + + @property + def stereo_exposure_start_skew_us(self) -> int: + """Signed right-minus-left exposure-start skew.""" + + return self.right_exposure_start_pts_us - self.left_exposure_start_pts_us + + +def extract_yctc_app15(jpeg: bytes) -> bytes: + """Return the ``YCTC`` APP15 payload from a complete JPEG frame.""" + + if len(jpeg) < 4 or jpeg[:2] != b"\xff\xd8": + raise OvisionMetadataError("not a JPEG SOI stream") + + offset = 2 + while offset + 2 <= len(jpeg): + if jpeg[offset] != 0xFF: + raise OvisionMetadataError(f"invalid JPEG marker prefix at byte {offset}") + marker = jpeg[offset + 1] + offset += 2 + if marker == 0xDA: + break + if marker in (0xD8, 0xD9) or 0xD0 <= marker <= 0xD7: + continue + if offset + 2 > len(jpeg): + raise OvisionMetadataError("truncated JPEG segment length") + segment_size = int.from_bytes(jpeg[offset : offset + 2], "big") + if segment_size < 2: + raise OvisionMetadataError("invalid JPEG segment length") + end = offset + segment_size + if end > len(jpeg): + raise OvisionMetadataError("truncated JPEG segment") + payload = jpeg[offset + 2 : end] + if marker == 0xEF and payload.startswith(YCTC_MAGIC): + return payload + offset = end + raise OvisionMetadataError("YCTC APP15 segment missing") + + +def _annex_b_nalus(data: bytes): + starts: list[tuple[int, int]] = [] + offset = 0 + while offset + 3 <= len(data): + if data.startswith(b"\x00\x00\x00\x01", offset): + starts.append((offset, 4)) + offset += 4 + elif data.startswith(b"\x00\x00\x01", offset): + starts.append((offset, 3)) + offset += 3 + else: + offset += 1 + for index, (start, prefix_size) in enumerate(starts): + body_start = start + prefix_size + body_end = starts[index + 1][0] if index + 1 < len(starts) else len(data) + if body_end > body_start: + yield data[body_start:body_end] + + +def _unescape_h264_rbsp(escaped: bytes) -> bytes: + out = bytearray() + zero_count = 0 + for byte in escaped: + if zero_count >= 2 and byte == 0x03: + # Count encoded bytes. The skipped byte breaks the encoded zero run. + zero_count = 0 + continue + out.append(byte) + zero_count = zero_count + 1 if byte == 0 else 0 + return bytes(out) + + +def extract_yctc_h264_sei(access_unit: bytes) -> bytes: + """Return the ``YCTC`` message from an Annex-B H.264 access unit.""" + + saw_sei = False + for nal in _annex_b_nalus(access_unit): + if not nal or nal[0] & 0x1F != 6: + continue + saw_sei = True + rbsp = _unescape_h264_rbsp(nal[1:]) + offset = 0 + while offset < len(rbsp): + if rbsp[offset] == 0x80 and offset + 1 == len(rbsp): + break + payload_type = 0 + while offset < len(rbsp) and rbsp[offset] == 0xFF: + payload_type += 0xFF + offset += 1 + if offset >= len(rbsp): + raise OvisionMetadataError("truncated H.264 SEI payload type") + payload_type += rbsp[offset] + offset += 1 + payload_size = 0 + while offset < len(rbsp) and rbsp[offset] == 0xFF: + payload_size += 0xFF + offset += 1 + if offset >= len(rbsp): + raise OvisionMetadataError("truncated H.264 SEI payload size") + payload_size += rbsp[offset] + offset += 1 + end = offset + payload_size + if end > len(rbsp): + raise OvisionMetadataError(f"truncated H.264 SEI payload type {payload_type}") + payload = rbsp[offset:end] + if payload.startswith(YCTC_MAGIC): + return payload + offset = end + detail = "YCTC SEI message missing" if saw_sei else "H.264 SEI NAL missing" + raise OvisionMetadataError(detail) + + +def _validate_sensor_fields( + side: str, + flags: int, + vi_pipe: int, + start_line_pts: int, + exposure_start_pts: int, + exposure_time: int, + trigger_index: int, +) -> None: + if flags & ~_KNOWN_SENSOR_FLAGS: + raise OvisionMetadataError(f"{side} sensor has unknown validity flags 0x{flags:02x}") + fields = ( + (START_LINE_RX_PTS_VALID, start_line_pts, "start-line timestamp"), + (EXPOSURE_START_PTS_VALID, exposure_start_pts, "exposure-start timestamp"), + (EXPOSURE_TIME_VALID, exposure_time, "exposure time"), + (GPIO_TRIGGER_INDEX_VALID, trigger_index, "trigger index"), + ) + for flag, value, name in fields: + if not flags & flag and value != 0: + raise OvisionMetadataError(f"{side} {name} is nonzero while invalid") + if not flags & SENSOR_PRESENT and (vi_pipe != 0 or flags != 0): + raise OvisionMetadataError(f"{side} absent sensor contains nonzero metadata") + + +def parse_yctc_payload(payload: bytes) -> OvisionFrameMetadata: + """Strictly parse one manufacturer V3 ``YCTC`` V2 payload.""" + + if len(payload) < YCTC_V2_HEADER_SIZE: + raise OvisionMetadataError( + f"YCTC payload too short: {len(payload)} < {YCTC_V2_HEADER_SIZE}" + ) + if payload[:4] != YCTC_MAGIC: + raise OvisionMetadataError("YCTC magic missing") + + version, declared_size = struct.unpack_from(" IMU_MAX_SAMPLES or accel_count > IMU_MAX_SAMPLES: + raise OvisionMetadataError("YCTC IMU sample count exceeds protocol maximum") + if _IMU_HEADER_SIZE + (gyro_count + accel_count) * _IMU_SAMPLE_SIZE != imu_payload_len: + raise OvisionMetadataError("YCTC IMU counts do not consume IMU payload") + offset += _IMU_HEADER_SIZE + + def read_imu(count: int) -> list[OvisionImuSample]: + nonlocal offset + samples: list[OvisionImuSample] = [] + for _ in range(count): + x, y, z, temperature, timestamp_us = struct.unpack_from(" MAG_MAX_SAMPLES: + raise OvisionMetadataError("YCTC MAG sample count exceeds protocol maximum") + if _MAG_HEADER_SIZE + mag_count * _MAG_SAMPLE_SIZE != mag_payload_len: + raise OvisionMetadataError("YCTC MAG count does not consume MAG payload") + offset += _MAG_HEADER_SIZE + for _ in range(mag_count): + x, y, z, tout = struct.unpack_from(" OvisionFrameMetadata: + return parse_yctc_payload(extract_yctc_app15(jpeg)) + + +def parse_ovision_h264_metadata(access_unit: bytes) -> OvisionFrameMetadata: + return parse_yctc_payload(extract_yctc_h264_sei(access_unit)) diff --git a/tests/unit/adapters/test_ovision_metadata.py b/tests/unit/adapters/test_ovision_metadata.py new file mode 100644 index 0000000..f373a63 --- /dev/null +++ b/tests/unit/adapters/test_ovision_metadata.py @@ -0,0 +1,207 @@ +"""Strict parser tests for OVISION's MJPEG-embedded YCTC metadata.""" + +from __future__ import annotations + +import struct + +import pytest + +from syncfield.adapters.ovision_metadata import ( + ACCEL_M_S2_PER_LSB, + GYRO_RAD_S_PER_LSB, + OvisionMetadataError, + extract_yctc_app15, + extract_yctc_h264_sei, + parse_ovision_h264_metadata, + parse_ovision_mjpeg_metadata, + parse_yctc_payload, +) + + +def _payload(*, count_a: int = 2, count_b: int = 2, count_low: int = 1) -> bytes: + imu_len = 8 + 18 * (count_a + count_b) + mag_len = 8 + 28 * count_low + size = 72 + imu_len + mag_len + out = bytearray(size) + struct.pack_into( + "<4sHHQQIIIIQQIIBBBBHH", + out, + 0, + b"YCTC", + 2, + size, + 1_000_000, + 1_000_000, + 10_003, + 10_013, + 29, + 29, + 989_600, + 989_609, + 1, + 1, + 0x1F, + 0x1F, + 0, + 1, + imu_len, + mag_len, + ) + offset = 72 + struct.pack_into( + " bytes: + app0 = b"\xff\xe0" + struct.pack(">H", 4) + b"JF" + app15 = b"\xff\xef" + struct.pack(">H", len(payload) + 2) + payload + return b"\xff\xd8" + app0 + app15 + b"\xff\xda" + b"entropy\xff\xd9" + + +def _escape_h264_rbsp(data: bytes) -> bytes: + out = bytearray() + zeros = 0 + for byte in data: + if zeros >= 2 and byte <= 3: + out.append(3) + zeros = 0 + out.append(byte) + zeros = zeros + 1 if byte == 0 else 0 + return bytes(out) + + +def _sei_message(payload_type: int, payload: bytes) -> bytes: + header = bytearray() + while payload_type >= 255: + header.append(255) + payload_type -= 255 + header.append(payload_type) + size = len(payload) + while size >= 255: + header.append(255) + size -= 255 + header.append(size) + return bytes(header) + payload + + +def _h264(payload: bytes) -> bytes: + # One unrelated SEI proves that selection is by YCTC magic, not position. + rbsp = _sei_message(5, b"encoder") + _sei_message(240, payload) + b"\x80" + sei = b"\x00\x00\x00\x01\x06" + _escape_h264_rbsp(rbsp) + idr = b"\x00\x00\x01\x65encoded" + return sei + idr + + +def test_extract_and_parse_yctc_metadata(): + parsed = parse_ovision_mjpeg_metadata(_jpeg(_payload())) + + assert parsed.version == 2 + assert parsed.left_start_line_rx_pts_us == parsed.right_start_line_rx_pts_us == 1_000_000 + assert parsed.left_gpio_trigger_index == parsed.right_gpio_trigger_index == 29 + assert parsed.stereo_exposure_start_skew_us == 9 + assert parsed.user_data_seq == 1 + assert parsed.frame_meta_generation == 1 + assert parsed.imu_generation == 1 + assert [s.xyz_raw for s in parsed.gyro] == [(-40, -20, 5), (-39, -20, 5)] + assert parsed.accel[0].xyz_raw == (-3_580, -240, -7_200) + assert parsed.gyro[1].device_timestamp_us == 972_000 + assert parsed.mag_generation == 1 + assert parsed.mag[0].xyz_raw == (-471, -1_941, 6_299) + assert parsed.mag[0].tout_raw == 255 + assert parsed.mag[0].device_timestamp_us == 975_000 + assert parsed.gyro[0].gyro_rad_s()[0] == pytest.approx(-40 * GYRO_RAD_S_PER_LSB) + assert parsed.accel[0].accel_m_s2()[0] == pytest.approx(-3_580 * ACCEL_M_S2_PER_LSB) + + +def test_extract_and_parse_h264_yctc_sei(): + payload = _payload(count_a=17, count_b=17, count_low=4) + access_unit = _h264(payload) + + assert extract_yctc_h264_sei(access_unit) == payload + parsed = parse_ovision_h264_metadata(access_unit) + assert parsed.payload_size == 812 + assert len(parsed.gyro) == 17 + assert len(parsed.accel) == 17 + assert len(parsed.mag) == 4 + + +def test_h264_parser_rejects_missing_and_truncated_sei(): + with pytest.raises(OvisionMetadataError, match="SEI NAL missing"): + extract_yctc_h264_sei(b"\x00\x00\x01\x65encoded") + with pytest.raises(OvisionMetadataError, match="truncated"): + extract_yctc_h264_sei(b"\x00\x00\x01\x06\xf0\xff") + + +@pytest.mark.parametrize("counts", [(16, 16, 3), (16, 16, 4), (17, 17, 3), (17, 17, 4)]) +def test_real_device_sample_count_shapes_are_accepted(counts): + a, b, low = counts + parsed = parse_yctc_payload(_payload(count_a=a, count_b=b, count_low=low)) + assert len(parsed.gyro) == a + assert len(parsed.accel) == b + assert len(parsed.mag) == low + + +def test_extract_rejects_non_jpeg_and_missing_metadata(): + with pytest.raises(OvisionMetadataError, match="SOI"): + extract_yctc_app15(b"not jpeg") + with pytest.raises(OvisionMetadataError, match="missing"): + extract_yctc_app15(b"\xff\xd8\xff\xdaentropy\xff\xd9") + + +def test_parser_rejects_size_version_and_stereo_mismatch(): + bad_size = bytearray(_payload()) + struct.pack_into("