""" Sensor - Core sensor class for optical tactile sensing. This module provides the Sensor class for capturing, processing, and analyzing optical tactile sensor data with support for stitching, optical flow tracking, and Helmholtz-Hodge decomposition. """ # import os # only used by commented-out stitch_cal import time from enum import Enum, auto from pathlib import Path import cv2 import numpy as np from . import calibration as cal_mod from . import configs from .backend_availability import check_cupy_available, check_opencl_available from .force_readout import ( FILTER_PRESETS, ForceReadout, available_filter_presets, ) from .frame_processors import create_frame_processor from .get_depth import Contact3D from .hhd import nHHD # from threading import Thread, Event # from queue import Queue, Empty from .Stitcher import Stitcher from .tracker import Tracker from .capture import ( CaptureConfig, CaptureSource, ReconnectPolicy, is_live_camera, normalize_source, open_capture, ) class Sensor: """ Optical tactile sensor interface with deformation analysis. Provides image capture, stitching, optical flow tracking, and force decomposition. Supports both synchronous and asynchronous (pipelined) processing modes. """ # ========================================================================= # Info Enum (for read_info API) # ========================================================================= class info(Enum): """Data types available through read_info().""" VRAW = auto() VNORMAL = auto() VSHEAR = auto() FPS = auto() TIMESTAMP = auto() FNORMAL = auto() FSHEARX = auto() FSHEARY = auto() FNORMAL_RAW = auto() FSHEARX_RAW = auto() FSHEARY_RAW = auto() CENTROID = auto() CONTOUR = auto() DEPTH = auto() IMG = auto() # ========================================================================= # Initialization # ========================================================================= def __init__(self, vid_src, cuda=True, isstitch=True, verbose=True, debug=False, config_name="prototype", cal_path=None, override=False, box=None, mirror=True, backend='auto', capture_config: CaptureConfig | None = None, auto_reconnect: bool = False, capture_mode: str = "latest", capture_queue_size: int = 1, capture_overflow: str = "drop_oldest", calibration_dir: str | Path | None = None, device_id: str | None = None, **_deprecated_capture_kwargs): """ Initialize Sensor with video source and configuration. Args: vid_src: Video source (int for camera ID, str for file path) cuda: Enable CUDA acceleration (default: True) isstitch: Enable stitching mode for multi-tile sensor (default: False) verbose: Print initialization logs (default: True) config_name: Configuration name (default: "prototype") cal_path: Explicit force_calibration.json path (overrides bundle default) calibration_dir: User-managed root containing /.json. device_id: Manually entered or hardware-reported device identity. override: Skip auto-detection, use provided box (default: False) box: Manual bounding boxes when override=True (default: None) backend: Poisson backend (default: "auto") "auto": Automatically select the best backend based on availability "cupy": Use CuPy for GPU acceleration "opencl": Use OpenCL for GPU acceleration "cpu": Use CPU for computation capture_config: Optional ``CaptureConfig`` overriding capture kwargs below. auto_reconnect: Enable USB disconnect auto-reconnect for live cameras. capture_mode: Deprecated; ignored (latest-frame capture only). capture_queue_size: Deprecated; ignored. capture_overflow: Deprecated; ignored. """ self.CUPY_AVAILABLE = check_cupy_available() self.OPENCL_AVAILABLE = check_opencl_available() if vid_src is None: raise Exception("User must specify a video source.") self._external_only = isinstance(vid_src, str) and vid_src.lower() == "external" # Store settings self.vid_src = normalize_source(vid_src) if not self._external_only else vid_src self.isstitch = isstitch self.cuda = cuda self.verbose = verbose self.debug = debug self.config_name = config_name self.cal_path = cal_path if calibration_dir is not None and device_id is None: raise cal_mod.CalibrationError("device_id is required with calibration_dir") if device_id is not None and calibration_dir is None and cal_path is None: raise cal_mod.CalibrationError("device_id requires calibration_dir or cal_path") if calibration_dir is not None and cal_path is not None: raise cal_mod.CalibrationError("cal_path and calibration_dir are mutually exclusive") self.calibration_dir = calibration_dir self.device_id = device_id self.device_calibration_missing = False self.mirror = mirror self._capture_config_override = capture_config self._auto_reconnect = bool(auto_reconnect) # Load configuration self.__config_cam = None self.__config_stitcher = None self.__config_contact = None self._load_config() # State variables self.stitch_valid = False self.compute_deformation_valid = False self.compute_contact_valid = False # Image Data Variables (public) # self.stitch_calibration = [] self.frame = self.img = None self.initial_img = None self._img_bgr_raw = None self._img_gray_buf = None shape = (self.__config_cam["resolution_stitch"][1], self.__config_cam["resolution_stitch"][0]) self.img_size_raw = None self.img_filtered = None self.tail_frames_count = 0 # Optical Flow Data Variables (public) self.flow = np.zeros(shape + (2,), dtype=np.float32) self.vtemplate = np.zeros_like(self.flow) self.vnormal = self.vtemplate self.vshear = self.vtemplate self.n_boxes = None self.fnormal = self.fshearx = self.fsheary = self.ftor = 0.0 self._force_raw = (0.0, 0.0, 0.0) self.calibration: cal_mod.Calibration | None = None self._force_cal_sensor_name: str | None = None #: Why no calibration was applied, when a bundle existed but carried no #: usable coefficients. ``None`` means the question never came up. self._force_cal_attempts: list[dict] | None = None self._init_force_calibration() try: config_fps = float(self.__config_cam.get("fps", 0.0) or 0.0) except (TypeError, ValueError): config_fps = 0.0 self._force_readout = ForceReadout( self._config.get("filters"), config_fps=config_fps, verbose=self.verbose, ) if self.verbose: if self.calibration is not None and self.calibration.source_path is not None: print(f"Force calibration: {self.calibration.source_path}") elif self._force_cal_attempts is not None: # A bundle was resolved for this sensor type, so "file missing" # would be the wrong story: name the bundle and the roots that # were searched instead. print(self._force_calibration_note()) else: print("Force calibration: raw (no force_calibration.json)") # Contact Data Variables (public) self.centroid = (0, 0) self.centroid_found = False self.contour = np.empty(0) self.depth_map = np.zeros_like(self.img) self.valid_mask_raw = None self.valid_mask = None self.valid_mask_hhd = None # Scratch buffers reused by _compute_flow_means / compute_hhd. self._flow_mag_buf = None self._flow_mask_u8 = None self._hhd_vn_buf = None self._hhd_vs_buf = None self._hhd_flow_ds_buf = None self._hhd_mask_f32 = None self.mask_stats = { "coverage": 0.0, "flow_mean_valid": 0.0, "flow_mean_invalid": 0.0, } # Timing self.last_log_time = time.perf_counter() self._fps = 0 self._last_capture_time = 0.0 self._last_processing_time = 0.0 self._last_stage_times = { # Frame-post-processing roll-ups (seconds). "extract_tiles": 0.0, # sensor units + tile extraction "stitch": 0.0, # feature/feather stitch of the extracted tiles "resize": 0.0, # BGR->gray + resize to resolution_stitch + mirror # Finer breakdown. "store": 0.0, # frame reference/copy "units": 0.0, # compound-eye unit (sub-image) extraction "gray": 0.0, # BGR->gray conversion "mirror": 0.0, # horizontal flip "mask": 0.0, # valid-mask resize/cache update "astype": 0.0, } # Initialize components self.stitcher = ( Stitcher(isstitch=isstitch, config_stitcher=self.__config_stitcher, verbose=self.verbose) if self._is_compound_eye() else None ) self.frame_processor = create_frame_processor(self.sensor_type, stitcher=self.stitcher) self.__tracker = Tracker(adaptive=True, cuda=cuda, verbose=self.verbose, config_tracker=self.__config_tracker) self.__contact3d = Contact3D(config_contact=self.__config_contact, verbose=self.verbose) self.__hhd = None # Integer >= 1: run HHD on flow downsampled by this factor for speed (1 = full res). _raw_ds = self.__config_tracker.get("hhd_downsample", 1) try: self._hhd_downsample = max(1, int(_raw_ds)) if _raw_ds is not None else 1 except (TypeError, ValueError): self._hhd_downsample = 1 self._hhd_grid_shape = None self._hhd_ref_shape = None cfg_backend = self.__config_tracker.get("backend", None) self.poisson_backend = cfg_backend if cfg_backend is not None else backend self.masked_poisson_solver = self.__config_tracker.get("masked_poisson_solver", "sparse") self.masked_poisson_max_boundary = self.__config_tracker.get("masked_poisson_max_boundary", 96) self.masked_poisson_boundary_stride = self.__config_tracker.get("masked_poisson_boundary_stride", 1) self.masked_poisson_max_iters = self.__config_tracker.get("masked_poisson_max_iters", 3) self.masked_poisson_tol = self.__config_tracker.get("masked_poisson_tol", 1e-4) self.masked_poisson_reg = self.__config_tracker.get("masked_poisson_reg", 1e-8) # Legacy PyAV capture state (unused after orisys.capture refactor). # self._use_pyav = False # self._av_container = None # self._av_stream = None # self._av_decoder = None self.cap = None # compat alias for _capture_worker.source.cap self._capture_source: CaptureSource | None = None self._is_live_camera = ( is_live_camera(self.vid_src) and not self._external_only ) # Setup video capture unless this sensor is fed externally. if self._external_only: self.stitch_valid = False if self.verbose: print("Sensor initialized in external-frame mode.") else: self._open_video_source(override, box) if not self._is_live_camera: # A file source is replayed at whatever speed the CPU manages, # so wall-clock intervals do not reflect signal time: keep the # configured camera fps for the force low-pass design. self._force_readout.pin_file_rate() if self.verbose: readout = self._force_readout print(f" Video source: {vid_src}") print(f" Stitch mode: {self.stitch_valid}") print(f" Optical flow algorithm: {self.__config_tracker['algorithm']}") print(f" Poisson backend: {self.poisson_backend}") # print(f" CUDA acceleration: {self.cuda}") print(f" CuPy available: {self.CUPY_AVAILABLE}") print(f" OpenCL available: {self.OPENCL_AVAILABLE}") print( " Signal filters: " f"ema_alpha={readout.flow_ema_alpha} " f"median={readout.force_median_window} " f"lowpass={readout.lowpass_cutoff_hz}Hz@" f"{readout.lowpass_sample_rate_hz:.1f}Hz " f"({readout.sample_rate_source})" ) # ========================================================================= # Public API - Core Methods (Most Frequently Used) # ========================================================================= def _resize_mask(self, mask, size): if mask is None: return None mask_u8 = np.asarray(mask, dtype=np.uint8) resized = cv2.resize(mask_u8, size, interpolation=cv2.INTER_NEAREST) return resized.astype(bool) def _prepare_valid_mask(self, valid_mask, raw_shape, stitched_shape): if valid_mask is None: self.valid_mask_raw = None self.valid_mask = None self.valid_mask_hhd = None self.mask_stats["coverage"] = 0.0 return mask_raw = np.asarray(valid_mask, dtype=bool) if mask_raw.shape != raw_shape: mask_raw = self._resize_mask(mask_raw, (raw_shape[1], raw_shape[0])) if self.mirror: mask_raw = np.fliplr(mask_raw) self.valid_mask_raw = mask_raw self.valid_mask = self._resize_mask(mask_raw, (stitched_shape[1], stitched_shape[0])) self._update_hhd_mask_cache() self.mask_stats["coverage"] = float(np.mean(self.valid_mask)) if self.valid_mask.size else 0.0 def _hhd_grid_size(self): if self.valid_mask is None: return None h, w = self.valid_mask.shape[:2] k = max(1, int(self._hhd_downsample)) if k <= 1: return (w, h) return (max(1, w // k), max(1, h // k)) def _update_hhd_mask_cache(self): """Precompute valid_mask at HHD resolution when the UV domain is fixed.""" if self.valid_mask is None: self.valid_mask_hhd = None return ds_size = self._hhd_grid_size() if ds_size is None: self.valid_mask_hhd = None return if self.valid_mask.shape[1] == ds_size[0] and self.valid_mask.shape[0] == ds_size[1]: self.valid_mask_hhd = self.valid_mask.astype(bool) else: self.valid_mask_hhd = self._resize_mask(self.valid_mask, ds_size) def _compute_flow_means(self, flow): if flow is None: return shape = flow.shape[:2] mag = self._flow_mag_buf if mag is None or mag.shape != shape or mag.dtype != np.float32: mag = np.empty(shape, dtype=np.float32) self._flow_mag_buf = mag # cv2.magnitude + cv2.mean(mask=...) avoid the (H, W) temporaries and the # boolean fancy-index gather that np.linalg.norm + mag[mask].mean() need. cv2.magnitude(flow[:, :, 0], flow[:, :, 1], mag) mask = self.valid_mask if mask is None or mask.shape != shape: self.mask_stats["flow_mean_valid"] = float(cv2.mean(mag)[0]) if mag.size else 0.0 self.mask_stats["flow_mean_invalid"] = 0.0 return mask_u8 = self._flow_mask_u8 if mask_u8 is None or mask_u8.shape != shape or mask_u8.dtype != np.uint8: mask_u8 = np.empty(shape, dtype=np.uint8) self._flow_mask_u8 = mask_u8 np.copyto(mask_u8, mask, casting="unsafe") np.multiply(mask_u8, 255, out=mask_u8) self.mask_stats["flow_mean_valid"] = float(cv2.mean(mag, mask=mask_u8)[0]) np.subtract(255, mask_u8, out=mask_u8) self.mask_stats["flow_mean_invalid"] = float(cv2.mean(mag, mask=mask_u8)[0]) def get_img(self, frame=None, already_processed=False, valid_mask=None, already_gray=False): """ Capture and preprocess a frame. Parameters ---------- frame : np.ndarray | None Optional externally supplied frame. When provided, the sensor skips reading from its own video source and uses this frame instead. already_processed : bool If True, treat `frame` as the final BGR tactile image and skip the configured frame processor. This is useful for pipelines that generate a derived image, such as an unrolled tactile view. Returns: np.ndarray | None: Processed image, or None on capture failure. """ # Simple direct read - buffer size of 1 should minimize frame accumulation # Timing measurement helps identify if capture or processing is the bottleneck t_capture_start = time.perf_counter() if frame is None: ret, frame = self._read_frame() else: ret = frame is not None t_capture_end = time.perf_counter() self._last_capture_time = t_capture_end - t_capture_start if not ret: print("Failed to get_img from video source.") return None # Start processing timing t_processing_start = time.perf_counter() self.frame = frame if (already_processed and isinstance(frame, np.ndarray)) else ( frame.copy() if isinstance(frame, np.ndarray) else frame ) t_after_store = time.perf_counter() if already_processed: img_bgr = frame gray = None if already_gray: if frame.ndim == 2: gray = frame elif frame.ndim == 3 and frame.shape[2] == 1: gray = frame[:, :, 0] else: gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) img_bgr = None else: if img_bgr.ndim == 2: img_bgr = cv2.cvtColor(img_bgr, cv2.COLOR_GRAY2BGR) elif img_bgr.ndim == 3 and img_bgr.shape[2] == 1: img_bgr = cv2.cvtColor(img_bgr[:, :, 0], cv2.COLOR_GRAY2BGR) else: img_bgr = self.frame_processor.process(frame) gray = None t_after_extract = time.perf_counter() if gray is None: self.img_size_raw = img_bgr.shape self._img_bgr_raw = img_bgr gray = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) else: self.img_size_raw = gray.shape self._img_bgr_raw = None t_after_gray = time.perf_counter() res = tuple(self._config_resolution_stitch) if self._img_gray_buf is None or self._img_gray_buf.shape != (res[1], res[0]): self._img_gray_buf = np.empty((res[1], res[0]), dtype=np.uint8) cv2.resize(gray, res, dst=self._img_gray_buf, interpolation=cv2.INTER_LINEAR) t_after_resize = time.perf_counter() if self.mirror: cv2.flip(self._img_gray_buf, 1, dst=self._img_gray_buf) t_after_mirror = time.perf_counter() self.img = self._img_gray_buf self._prepare_valid_mask(valid_mask, gray.shape, self.img.shape) t_processing_end = time.perf_counter() self._last_processing_time = t_processing_end - t_processing_start self._record_stage_times( t_processing_start=t_processing_start, t_after_store=t_after_store, t_after_extract=t_after_extract, t_after_gray=t_after_gray, t_after_resize=t_after_resize, t_after_mirror=t_after_mirror, t_processing_end=t_processing_end, ) # FPS calculation self._fps = 1.0 / (time.perf_counter() - self.last_log_time) self.last_log_time = time.perf_counter() return self.img.copy() def _record_stage_times( self, *, t_processing_start, t_after_store, t_after_extract, t_after_gray, t_after_resize, t_after_mirror, t_processing_end, ): """Record per-stage frame-post-processing timings (seconds). ``extract_tiles``/``stitch`` come from the frame processor, which times its own sub-steps; the remaining keys are measured here. The roll-ups keep the historical key names while no longer collapsing ``stitch`` to zero: previously ``t_after_stitch`` was sampled immediately after ``t_after_extract``, so ``stitch`` was always ~0 s and ``resize`` silently absorbed the whole stitch cost. """ stage = self._last_stage_times processor_timings = getattr(self.frame_processor, "last_timings", None) or {} units_s = float(processor_timings.get("units", 0.0)) extract_s = float(processor_timings.get("extract_tiles", 0.0)) stitch_s = float(processor_timings.get("stitch", 0.0)) processor_total_s = t_after_extract - t_after_store stage["store"] = t_after_store - t_processing_start stage["units"] = units_s stage["gray"] = t_after_gray - t_after_extract stage["resize"] = t_after_resize - t_after_gray stage["mirror"] = t_after_mirror - t_after_resize stage["mask"] = t_processing_end - t_after_mirror if processor_timings: stage["extract_tiles"] = units_s + extract_s stage["stitch"] = stitch_s else: # Non-compound-eye processors are a single opaque step. stage["extract_tiles"] = processor_total_s stage["stitch"] = 0.0 def _get_img_bgr(self): """Produce a resized+mirrored BGR image on demand (for display).""" if self._img_bgr_raw is None: if self.img is None: return None return cv2.cvtColor(self.img, cv2.COLOR_GRAY2BGR) res = tuple(self._config_resolution_stitch) bgr = cv2.resize(self._img_bgr_raw, res, interpolation=cv2.INTER_LINEAR) if self.mirror: cv2.flip(bgr, 1, dst=bgr) return bgr def detect_motion(self, threshold=6): # detect motion by optical flow magnitude mag = np.linalg.norm(self.flow, axis=-1) if self.valid_mask is not None and self.valid_mask.shape == mag.shape: active = self.valid_mask.astype(bool) motion_value = mag[active].mean() if np.any(active) else 0.0 else: motion_value = mag.mean() return motion_value > threshold def compute_deformation(self, decompose=True, check_motion=False, threshold=6, tail_frames=10): """ Compute optical flow and optionally decompose it into force components. Parameters ---------- decompose : bool If True, run HHD decomposition (`compute_hhd`) after flow computation. check_motion : bool If True, gate decomposition with `detect_motion(threshold)`. When motion falls below threshold, decomposition may still run for up to `tail_frames` additional frames to avoid abrupt force drop-off. threshold : float Motion-detection threshold on mean flow magnitude. tail_frames : int Number of extra non-motion frames allowed to keep running HHD when `check_motion=True`. Returns ------- bool | None - `True` : motion is currently detected (`check_motion=True`) - `False` : motion is currently not detected (`check_motion=True`) - `None` : motion gating disabled (`check_motion=False`) """ if self.img is None: raise Exception("Warning: Get image first") self.compute_flow() if not check_motion: self.tail_frames_count = 0 if decompose: self.compute_hhd() return None is_motion = self.detect_motion(threshold=threshold) try: tail_frames = max(0, int(tail_frames)) except (TypeError, ValueError): tail_frames = 0 if is_motion: self.tail_frames_count = 0 if decompose: self.compute_hhd() return True # No motion this frame. Keep HHD alive briefly if requested, but do not # report this as "motion detected". if decompose and self.tail_frames_count < tail_frames: self.tail_frames_count += 1 self.compute_hhd() else: self.tail_frames_count = tail_frames return False def warmup_deformation(self): """Build optical-flow and masked-HHD caches using the current image.""" if self.img is None: raise Exception("Warning: Get image first") self.compute_flow() self.compute_hhd() return True def compute_flow(self, img=None): """Compute the optical flow for one frame. ``img`` optionally supplies the frame to use instead of ``self.img``. StreamPipeline passes it explicitly so a producer thread cannot swap ``self.img`` between the hand-off and the tracker call. Positional callers and existing code are unaffected. """ image = self.img if img is None else img if image is None: raise Exception("Warning: Get image first") flow = self.__tracker.track(image, valid_mask=self.valid_mask) # flow = self.__tracker.track_raft(self.img) if hasattr(flow, 'download'): flow = flow.download() flow = self._force_readout.apply_flow_ema(flow) self.flow = flow self._compute_flow_means(self.flow) @property def force_sample_rate_hz(self) -> float: """Sample rate currently used to design the force low-pass (Hz).""" return self._force_readout.sample_rate_hz @property def force_sample_rate_source(self) -> str: """``config``, ``measured`` or ``config (file)``.""" return self._force_readout.sample_rate_source def compute_hhd(self, flow=None): """Decompose the optical flow and update the force readout. ``flow`` optionally supplies the field to decompose instead of ``self.flow``. StreamPipeline passes its own per-frame copy so the flow stage cannot overwrite ``self.flow`` mid-decomposition. Existing callers are unaffected. """ flow = self.flow if flow is None else flow if flow is None: raise Exception("Warning: Compute flow first") orig_h, orig_w = flow.shape[:2] try: k = max(1, int(self._hhd_downsample)) except (TypeError, ValueError): k = 1 mask_ds = None if self.valid_mask is not None: ds_size = self._hhd_grid_size() if ( self.valid_mask_hhd is not None and ds_size is not None and self.valid_mask_hhd.shape[1] == ds_size[0] and self.valid_mask_hhd.shape[0] == ds_size[1] ): mask_ds = self.valid_mask_hhd else: mask_ds = self.valid_mask.astype(bool) if k > 1: ds_w = max(1, orig_w // k) ds_h = max(1, orig_h // k) flow_ds = self._hhd_flow_ds_buf if flow_ds is None or flow_ds.shape != (ds_h, ds_w, 2) or flow_ds.dtype != np.float32: flow_ds = np.empty((ds_h, ds_w, 2), dtype=np.float32) self._hhd_flow_ds_buf = flow_ds cv2.resize(flow, (ds_w, ds_h), dst=flow_ds, interpolation=cv2.INTER_AREA) if mask_ds is not None and ( mask_ds.shape[1] != ds_w or mask_ds.shape[0] != ds_h ): mask_ds = self._resize_mask(mask_ds, (ds_w, ds_h)) else: flow_ds = flow ds_shape = flow_ds.shape[:2] ref_shape = (orig_h, orig_w) self.valid_mask_hhd = mask_ds if ( self.__hhd is None or self._hhd_grid_shape != ds_shape or self._hhd_ref_shape != ref_shape ): spacings = (18 * k, 18 * k) self.__hhd = nHHD( cuda=self.cuda, grid=ds_shape, spacings=spacings, poisson_backend=self.poisson_backend, debug=self.debug, # Rescale grid-summed force aggregates back onto the full-resolution # flow grid so Sn/Stx/Sty/Stor stay comparable across hhd_downsample. force_reference_grid=ref_shape, masked_poisson_solver=self.masked_poisson_solver, masked_poisson_max_boundary=self.masked_poisson_max_boundary, masked_poisson_boundary_stride=self.masked_poisson_boundary_stride, masked_poisson_max_iters=self.masked_poisson_max_iters, masked_poisson_tol=self.masked_poisson_tol, masked_poisson_reg=self.masked_poisson_reg, ) self._hhd_grid_shape = ds_shape self._hhd_ref_shape = ref_shape _, forces = self.__hhd.get_force(flow_ds, valid_mask=mask_ds) fn, fx, fy, ftor = forces self._force_raw = (float(fn), float(fx), float(fy)) self.ftor = float(ftor) self._force_readout.update_sample_rate() self._apply_force_filter() vnormal_ds = self.__hhd.get_normal() vshear_ds = self.__hhd.get_shear() if k > 1: # Upsample into reusable buffers; the solver keeps owning its own # arrays, so nothing downstream can alias solver state. vn = self._hhd_vn_buf vs = self._hhd_vs_buf if vn is None or vn.shape != (orig_h, orig_w, 2) or vn.dtype != np.float32: vn = np.empty((orig_h, orig_w, 2), dtype=np.float32) vs = np.empty((orig_h, orig_w, 2), dtype=np.float32) self._hhd_vn_buf = vn self._hhd_vs_buf = vs cv2.resize(vnormal_ds, (orig_w, orig_h), dst=vn, interpolation=cv2.INTER_LINEAR) cv2.resize(vshear_ds, (orig_w, orig_h), dst=vs, interpolation=cv2.INTER_LINEAR) self.vnormal = vn self.vshear = vs else: self.vnormal = vnormal_ds.copy() self.vshear = vshear_ds.copy() if self.valid_mask is not None: # In-place multiply by a 0/1 float mask instead of # ``x.copy(); x[~active] = 0`` (copy + boolean gather/scatter). m3 = self._hhd_mask_f32 if m3 is None or m3.shape != (orig_h, orig_w, 1) or m3.dtype != np.float32: m3 = np.empty((orig_h, orig_w, 1), dtype=np.float32) self._hhd_mask_f32 = m3 np.copyto(m3[:, :, 0], self.valid_mask, casting="unsafe") np.multiply(self.vnormal, m3, out=self.vnormal) np.multiply(self.vshear, m3, out=self.vshear) self.compute_deformation_valid = True return True def compute_contact(self, vnormal=None): """ Compute contact information from vnormal field. Args: vnormal: Optional vnormal field. If None, uses self.vnormal. Returns: dict: Contact result with centroid, contour, and depth_map """ multiple_foe = False # Use provided vnormal or fall back to internal state vnormal_to_use = vnormal if vnormal is not None else self.vnormal if vnormal_to_use is None: raise Exception("Warning: vnormal is required for contact computation") if multiple_foe: result = self.__contact3d.compute_multiple_foe(vnormal_to_use, valid_mask=self.valid_mask) else: if self.cuda: result = self.__contact3d.compute_foe_gpu(vnormal_to_use, valid_mask=self.valid_mask) else: result = self.__contact3d.compute_foe(vnormal_to_use, valid_mask=self.valid_mask) self.centroid = result['centroid'] self.centroid_found = result['centroid_found'] if 'contour' in result: self.contour = result['contour'] self.depth_map = result['divergence_map'] if self.valid_mask is not None and self.centroid_found: cx, cy = int(self.centroid[0]), int(self.centroid[1]) inside = ( 0 <= cy < self.valid_mask.shape[0] and 0 <= cx < self.valid_mask.shape[1] and bool(self.valid_mask[cy, cx]) ) if not inside and self.verbose: print("Warning: contact centroid lies outside the valid UV mask.") self.compute_contact_valid = True return result def read_info(self, *args) -> tuple: """ Get sensor data by info type. Args: *args: info enum values (e.g., sensor.info.FPS, sensor.info.FNORMAL) Returns: Tuple: Requested data in order """ if any(arg in [self.info.IMG] for arg in args): if self.img is None: raise Exception("Warning: Get image first") # if any(arg in [self.info.VRAW, self.info.VNORMAL, self.info.VSHEAR, self.info.FNORMAL, self.info.FSHEARX, self.info.FSHEARY] for arg in args): # if not self.compute_deformation_valid: # print("Warning: Compute deformation first") # # raise Exception("Warning: Compute deformation first") # if any(arg in [self.info.CENTROID, self.info.CONTOUR, self.info.DEPTH] for arg in args): # if not self.compute_contact_valid: # print("Warning: Compute contact first") # raise Exception("Warning: Compute contact first") mapping = { # Return copies of arrays to prevent external modification of internal state self.info.VRAW: lambda: self.flow.copy() if self.flow is not None else np.array([]), self.info.VNORMAL: lambda: self.vnormal.copy() if self.vnormal is not None else np.array([]), self.info.VSHEAR: lambda: self.vshear.copy() if self.vshear is not None else np.array([]), # Scalar values are safe to return directly self.info.FNORMAL: lambda: self.fnormal, self.info.FSHEARX: lambda: self.fshearx, self.info.FSHEARY: lambda: self.fsheary, self.info.FNORMAL_RAW: lambda: self._force_raw[0], self.info.FSHEARX_RAW: lambda: self._force_raw[1], self.info.FSHEARY_RAW: lambda: self._force_raw[2], self.info.FPS: lambda: self._fps, self.info.TIMESTAMP: lambda: time.strftime('%H:%M:%S'), # Return copies of arrays to prevent external modification self.info.DEPTH: lambda: self.depth_map.copy() if self.depth_map is not None else np.array([]), # Centroid: can be tuple, list, or array - return a copy to prevent modification # Return None if centroid is not found or empty self.info.CENTROID: lambda: ( np.array(self.centroid).copy() if self.centroid is not None and len(self.centroid) > 0 else None ), self.info.CONTOUR: lambda: self.contour.copy() if self.contour is not None else np.array([]), self.info.IMG: lambda: self._get_img_bgr() if self._img_bgr_raw is not None else np.array([]), } return tuple(mapping.get(arg, lambda: None)() for arg in args if isinstance(arg, self.info)) def reset(self): """Reset optical flow tracker and force-filter state.""" self.__tracker.reset() self._force_readout.reset() # The unfiltered pair belongs to the frame it was computed for: a new # session must not report the previous one's reading. self._force_unfiltered = None self._force_raw_unfiltered = None if self.img is not None: self.initial_img = self.img.copy() def disconnect(self): """Release resources and stop async threads.""" if self._capture_source is not None: self._capture_source.close() self._capture_source = None self.cap = None if self.verbose: print("Sensor disconnected.") # Legacy PyAV / direct-cap fallbacks (unused after orisys.capture refactor). # elif self._use_pyav: # if self._av_container is not None: # self._av_container.close() # self._av_container = None # self._av_stream = None # self._av_decoder = None # if self.verbose: # print("Sensor disconnected.") # elif self.cap is not None: # try: # if self.cap.isOpened(): # self.cap.release() # except Exception: # pass # self.cap = None # if self.verbose: # print("Sensor disconnected.") try: cv2.destroyAllWindows() except cv2.error: # A headless OpenCV build (``opencv-python-headless``) has no highgui, # so there is nothing to close. Failing here would leave the rest of # the shutdown — and every caller that disconnects in a ``finally`` — # reporting an error that says nothing about the sensor. pass # ========================================================================= # Public API - Configuration & Calibration # ========================================================================= def configure( self, *, lowpass_cutoff_hz=None, lowpass_sample_rate_hz=None, lowpass_order=None, lowpass_enabled=None, flow_ema_alpha=None, force_median_window=None, ): """Update runtime-safe settings without reconnecting the camera. Parameters ---------- lowpass_cutoff_hz : float, optional Force low-pass cutoff in Hz. ``0`` disables filtering. A positive value enables filtering unless ``lowpass_enabled=False``. lowpass_sample_rate_hz : float, optional Assumed force sample rate for the Butterworth design (default 30). Set this to the actual output rate of the deployment. lowpass_order : int, optional Butterworth order (default 2). lowpass_enabled : bool, optional Explicit on/off for the causal force filter. flow_ema_alpha : float, optional EMA coefficient applied to the optical-flow field before HHD. ``1.0`` disables it; smaller values smooth more (0.5 is the default and costs about one frame of delay at 30 fps). force_median_window : int, optional Causal sliding-median width applied to the raw force triple before the low-pass. ``<= 1`` disables it; 3 is the default. """ if flow_ema_alpha is not None: alpha = float(flow_ema_alpha) if not (0.0 < alpha <= 1.0): raise ValueError("flow_ema_alpha must be in (0, 1]") if self.calibration is not None: mismatches, _ = cal_mod.filter_mismatches( self.calibration.pipeline.get("filters"), {"measurement": {"flow_ema_alpha": alpha}}, ) if mismatches: raise cal_mod.CalibrationError("; ".join(mismatches)) filter_changed = self._force_readout.configure( lowpass_cutoff_hz=lowpass_cutoff_hz, lowpass_sample_rate_hz=lowpass_sample_rate_hz, lowpass_order=lowpass_order, lowpass_enabled=lowpass_enabled, flow_ema_alpha=flow_ema_alpha, force_median_window=force_median_window, ) if filter_changed and self.compute_deformation_valid: self._apply_force_filter() return self def _bundle_name_candidates(self) -> list[str]: """Product bundle names implied by ``config_name``.""" text = str(self.config_name or "").strip().replace("\\", "/") if not text: return [] candidates: list[str] = [] stem = Path(text).stem if text else "" if stem: candidates.append(stem) parts = [p for p in text.split("/") if p] if "sensors" in parts: idx = parts.index("sensors") if idx + 1 < len(parts): candidates.append(parts[idx + 1]) for name in configs.PRODUCT_BUNDLES: if text == name or text.endswith(f"/sensors/{name}") or text.endswith(f"/sensors/{name}/sensor.json"): candidates.append(name) seen: set[str] = set() ordered: list[str] = [] for candidate in candidates: alias = configs.CONFIG_ALIASES.get(candidate, candidate) for name in (alias, candidate): if name in seen: continue seen.add(name) ordered.append(name) return ordered def _force_calibration_note(self) -> str: """Explain a bundle that resolved but carried no usable coefficients. The old message ("raw (no force_calibration.json)") reads as "the file is missing", which is exactly the wrong story when the lookup found a bundle from another root and stopped there without ever asking whether that root had coefficients. """ if getattr(self, "device_calibration_missing", False): return ( f"Force calibration: raw (no file for {self.device_id!r} " f"under {self.calibration_dir})" ) attempts = self._force_cal_attempts or [] # ``found_root`` is keyed even when nothing was found (valued None), so # test its value, not the key. found = [entry for entry in attempts if entry.get("found_root")] with_cal = [entry for entry in found if entry.get("force_calibration_json")] chosen = (with_cal or found or [None])[0] searched: list = [] for entry in attempts: for root in entry.get("searched") or (): if root not in searched: searched.append(root) lines: list[str] = [] if chosen is None: lines.append("Force calibration: raw (no sensor bundle resolved)") else: root = chosen["found_root"] if chosen.get("force_calibration_json"): lines.append( f"Force calibration: raw (a bundle was found at {root}, but its " "force_calibration.json could not be loaded)" ) else: lines.append( f"Force calibration: raw (bundle found at {root} has no " "force_calibration.json)" ) if searched: lines.append( " roots searched: " + "; ".join(str(root) for root in searched) ) return "\n".join(lines) def _init_force_calibration(self) -> None: """Load the selected force calibration before processing frames.""" self.calibration = None self._force_cal_sensor_name = None self._force_cal_attempts = None family = None pipeline = None if self.device_id is not None: family, pipeline = self._device_calibration_expectations() if self.cal_path is not None and str(self.cal_path).strip(): # Explicit path: missing/invalid raises (no silent fallback). self.calibration = cal_mod.load(self.cal_path, strict=True) if self.device_id is not None: self.calibration = cal_mod.validate_for_device( self.calibration, family, self.device_id, pipeline=pipeline ) return if self.calibration_dir is not None: self.calibration = cal_mod.load_for_device( self.calibration_dir, family, self.device_id, pipeline=pipeline ) self.device_calibration_missing = self.calibration is None return attempts: list[dict] = [] for name in self._bundle_name_candidates(): cal = cal_mod.load(sensor_type=name) if cal is not None: self.calibration = cal self._force_cal_sensor_name = name self._force_cal_attempts = None return try: attempts.append(configs.describe_bundle_resolution(name)) except Exception: # noqa: BLE001, S112 - diagnostics must never break loading continue # Deliberately NOT setting ``_force_cal_sensor_name``: callers read a # non-None value there as "this sensor is calibrated", and a bundle # without coefficients is not that. self._force_cal_attempts = attempts or None def _device_calibration_expectations(self): family = next( (name for name in self._bundle_name_candidates() if name in configs.PRODUCT_BUNDLES), None, ) if family is None: raise cal_mod.CalibrationError( f"cannot determine product family from config_name={self.config_name!r}" ) camera = self._config.get("camera", {}) tracker = self._config.get("tracker", {}) return family, { "stitch_resolution": camera.get("resolution_stitch"), "dis_mode": tracker.get("dis_mode"), "hhd_downsample": tracker.get("hhd_downsample"), "filters": cal_mod.filters_fingerprint( self.filters_state() if hasattr(self, "_force_readout") else self._config.get("filters") ), } @property def force_calibrated(self) -> bool: """True when at least one channel curve is loaded.""" return self.calibration is not None and bool(self.calibration.channels) @property def force_unit(self) -> str: return "N" if self.force_calibrated else "raw" @property def calibrated_axes(self) -> tuple[str, ...]: """Axes with a loaded force-calibration curve.""" channels = self.calibration.channels if self.calibration is not None else {} return tuple(axis for axis in cal_mod.CHANNEL_IDS if axis in channels) @property def force_units(self) -> tuple[str, str, str]: """Units of FN, FX and FY in that order.""" axes = self.calibrated_axes return ( "N" if "fn" in axes else "raw", "N" if "fx" in axes else "raw", "N" if "fy" in axes else "raw", ) def set_calibration( self, cal: "cal_mod.Calibration | str | Path | None", ) -> None: """Attach or replace the active force calibration (live swap after save).""" if cal is None: self.calibration = None self.device_calibration_missing = False self._force_cal_sensor_name = None self._force_cal_attempts = None return if isinstance(cal, cal_mod.Calibration): selected = cal else: selected = cal_mod.load(cal, strict=True) if self.device_id is not None: family, pipeline = self._device_calibration_expectations() selected = cal_mod.validate_for_device( selected, family, self.device_id, pipeline=pipeline ) self.calibration = selected self.device_calibration_missing = False self._force_cal_attempts = None def _apply_force_filter(self): convert = self.calibration.convert if self.calibration is not None else None fn, fx, fy = self._force_readout.apply(self._force_raw, convert) readout = self._force_readout self._force_filtered_raw = readout.filtered_raw self.fnormal, self.fshearx, self.fsheary = fn, fx, fy self._force_raw_unfiltered = readout.raw_unfiltered self._force_unfiltered = readout.unfiltered_newton # ------------------------------------------------------- filter contract def filters_state(self) -> dict: """What the filter chain is **actually** doing, for records and audits. Read this instead of the config file: a config can be overridden at runtime (``configure``) and a value that is present is never silently replaced, so the effective state is the only honest answer. The two groups are kept apart on purpose: * ``measurement`` — stages upstream of the HHD/force computation (``flow_ema_alpha``): turning them on changes *what is measured*, so two recordings with different values are not comparable; * ``readout`` — stages that only smooth the force triple on its way out (median, low-pass): recorded, but they do not change the physical claim. """ tracker_cfg = self._config.get("tracker") or {} state = self._force_readout.state(dis_mode=tracker_cfg.get("dis_mode")) state["source"] = "sensor.filters_state" return state def force_triplets(self) -> dict: """The filtered and unfiltered force triples of the current frame. One call, so a sampling thread reads a single consistent frame: the filtered pair is what ``read_info`` reports and what a recording stores as its primary columns, the unfiltered pair is the same reading with the readout stages bypassed. """ raw_unfiltered = getattr(self, "_force_raw_unfiltered", None) unfiltered = getattr(self, "_force_unfiltered", None) filtered_raw = getattr(self, "_force_filtered_raw", None) return { "filtered": { "raw": tuple(filtered_raw) if filtered_raw is not None else None, "newton": ( float(self.fnormal), float(self.fshearx), float(self.fsheary), ), }, "unfiltered": { "raw": tuple(raw_unfiltered) if raw_unfiltered is not None else None, "newton": tuple(unfiltered) if unfiltered is not None else None, }, } # def export_config(self, path="./config/exported_config.json"): # """Return current configuration dict.""" # configs.export_config(self._config, path) # if self.verbose: # print(f"Config exported to {path}") # return True # def stitch_cal(self): # """ # Load or create stitch calibration. # Returns: # np.ndarray: Stitched calibration image # """ # if self.cal_path is None: # self.cal_path = "./config/cal.npy" # if os.path.exists(self.cal_path): # if self.verbose: # print("Loading calibration from", self.cal_path) # self.stitch_calibration = np.load(self.cal_path, allow_pickle=True) # self.stitcher.stitch_calibration = self.stitch_calibration # return self.stitcher.stitch_rt_optimized() # else: # if self.verbose: # print("Creating new calibration...") # img, self.stitch_calibration = self.stitcher.stitch_cal() # os.makedirs("./config", exist_ok=True) # np.save(self.cal_path, self.stitch_calibration) # if self.verbose: # print("Calibration saved to", self.cal_path) # return img # ========================================================================= # Private - Initialization Helpers # ========================================================================= def _load_config(self): """Load sensor configuration from JSON file.""" self._config = configs.load_config(self.config_name) self.sensor_type = self._config.get( "type", "compound_eye" if "stitcher" in self._config else "single_camera", ) self.__config_cam = self._config["camera"] # Capture resolution/fps now applied via CaptureConfig.from_camera_dict(__config_cam). # self._config_resolution_raw = self.__config_cam["resolution_raw"] # self._config_fps = self.__config_cam["fps"] self._config_units = self.__config_cam.get("units", 1) self._config_resolution_stitch = self.__config_cam["resolution_stitch"] # self._config_dis_preset = self._config["tracker"]["dis_preset"] self.__config_tracker = self._config["tracker"] self.__config_stitcher = self._config.get("stitcher") self.__config_contact = self._config["contact"] if self.verbose: print(f"Config: {self.config_name}") return True def _is_compound_eye(self): return str(self.sensor_type).lower() in ("compound_eye", "compound-eye", "compound") def _open_video_source(self, override, box): """Initialize video capture and calibration.""" if self.verbose: print("Connecting to video source...") if self._capture_config_override is not None: capture_config = self._capture_config_override else: capture_config = CaptureConfig.from_camera_dict( self.vid_src, self.__config_cam, reconnect=ReconnectPolicy(enabled=self._auto_reconnect), verbose=self.verbose, background=False, ) try: self._capture_source = open_capture(capture_config, start_background=False) except Exception as exc: raise Exception("Failed to open video source.") from exc self.cap = self._capture_source.cap if not self._is_source_open(): raise Exception("Failed to open video source.") self._capture_source.warmup_frames(3) # Wait for first valid frame frame = None while frame is None: ret, frame = self._read_frame_raw() if not ret: time.sleep(0.1) if not self._is_compound_eye(): self.stitch_valid = False self.initial_img = self._make_initial_gray(frame) elif override: if box is not None: self.stitcher.isstitch = self.isstitch = False self.stitcher.boxes = box self.stitch_valid = bool(self.isstitch) img = self.frame_processor.process(frame) self.initial_img = self._make_initial_gray(img) else: print("Override=True but no box provided.") else: self._find_stitch_boxes(frame) if not self.stitch_valid: raise RuntimeError( f"Failed to detect {self._config_units} vision tiles for stitching. " "Check camera index, lighting, and that MJPG 1080p is negotiated." ) img = self.frame_processor.process(frame) self.initial_img = self._make_initial_gray(img) # For live cameras, start background capture to avoid latency buildup. if self._is_live_camera and self._capture_source is not None: self._capture_source.start_background() def _make_initial_gray(self, img_bgr): gray = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) self.resolution_stitch = gray.shape initial = cv2.resize(gray, tuple(self._config_resolution_stitch), interpolation=cv2.INTER_CUBIC) if self.mirror: initial = cv2.flip(initial, 1) return initial.astype(np.uint8) def _find_stitch_boxes(self, img_src, show=False): """Detect vision tile boxes with camera stabilization.""" n_boxes, v_mean0, timeout = 0, 0, 5 boxes = [] best_n_boxes = 0 best_boxes = [] start_time = cv2.getTickCount() while n_boxes < self._config_units: ret, frame = self._read_frame_raw() if not ret: continue v_mean = np.sum(cv2.mean(frame)) if abs(v_mean - v_mean0) < 5: n_boxes, boxes = self.stitcher.find_stitch_boxes(frame, show=show) if n_boxes > best_n_boxes: best_n_boxes = n_boxes best_boxes = boxes if (cv2.getTickCount() - start_time) / cv2.getTickFrequency() > timeout: print("Timeout finding vision units.") break v_mean0 = v_mean if n_boxes < self._config_units and best_n_boxes >= self._config_units: n_boxes = best_n_boxes boxes = best_boxes self.n_boxes = n_boxes self.boxes = boxes self.stitch_valid = self.isstitch and n_boxes == self._config_units status = "valid for stitching" if self.stitch_valid else "non-stitch mode" print(f"Sensor: {status}") return n_boxes # Unused after frame_processor owns stitch extract/process. # def _extract_tiles(self, img_src): # """Extract tiles from source image.""" # self.stitcher.get_stitch_unit(img_src) # self.stitcher.extract_tiles() # return True def _is_source_open(self): if self._capture_source is not None: return self._capture_source.is_open() return False # Legacy PyAV / direct-cap fallbacks (unused after orisys.capture refactor). # if self._use_pyav: # return self._av_container is not None # return self.cap is not None and self.cap.isOpened() def _read_frame(self): if self._capture_source is not None and self._capture_source.running: return self._capture_source.read_latest(copy=True, wait_s=1.0) return self._read_frame_raw() def _read_frame_raw(self): if self._capture_source is not None: return self._capture_source.read_raw() return False, None # Legacy PyAV / direct-cap fallbacks (unused after orisys.capture refactor). # if self._use_pyav: # if self._av_decoder is None: # return False, None # try: # frame = next(self._av_decoder) # except StopIteration: # return False, None # except Exception: # return False, None # return True, frame.to_ndarray(format="bgr24") # if self.cap is None: # return False, None # return self.cap.read() @property def capture_status(self): """Return capture health snapshot, or None in external-frame mode.""" if self._capture_source is None: return None return self._capture_source.status @property def _capture_worker(self): """Deprecated alias for ``_capture_source``.""" return self._capture_source # ========================================================================= # Private - Utilities # ========================================================================= def _print_timings(self, name, timings): """Print timing breakdown.""" print(f"\n[{name} timing]") for k, v in timings.items(): if k != 'total': print(f" {k}: {v:7.2f} ms") print(" ─────────────────────────") print(f" TOTAL: {timings['total']:7.2f} ms ({1000/timings['total']:.1f} FPS)")