ykcheungab
0784eacf51
滤波预设、EMA、中值、低通与标定换算离开 Sensor；对外 configure / filters_state 不变。已有 signal_processing、filter_presets、calibration 测试通过。
1458 lines
60 KiB
Python
1458 lines
60 KiB
Python
"""
|
|
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 <product>/<device_id>.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)")
|
|
|