diff --git a/src/orisys/force_readout.py b/src/orisys/force_readout.py new file mode 100644 index 0000000..0d3e1a2 --- /dev/null +++ b/src/orisys/force_readout.py @@ -0,0 +1,394 @@ +"""Force readout for FN/FX/FY: EMA, median, low-pass, and optional convert. + +Any sensor that produces a raw (fn, fx, fy) triple can use this. Calibration +coefficients stay outside: pass ``convert`` into :meth:`ForceReadout.apply`. +""" + +from __future__ import annotations + +from collections.abc import Callable + +import numpy as np + +from .signal_processing import ( + DEFAULT_SAMPLE_RATE_HZ, + CausalMedianFilter, + LowpassFilter, + SampleRateEstimator, +) + +# Default signal-conditioning settings (config: top-level "filters" block). +# +# A key a config does not state is **off**, not "whatever we thought was +# sensible": the file has to name what runs, or a device can be filtering a +# measurement nobody asked it to filter and no reader of the file could tell. +# The shipped bundles state all three values explicitly, so they are unaffected +# by these defaults; a config with no "filters" block at all filters nothing. +# +# Which settings are worth asking for is measured, not guessed. On dd02-0703 at +# 280x300 (see the noise analysis in the perf notes) flow EMA 0.5 + median 3 +# costs ~2 frames at 30 fps and cuts steady-press force jitter by roughly half +# while also stabilising vnormal/contact (which a force-level filter cannot do); +# that combination is the "balanced" preset. The force low-pass stays off there +# too: on top of the EMA it added no measurable noise benefit at the same +# latency, so it is an opt-in stage for sites with different noise +# characteristics (the "low_noise" preset). +DEFAULT_FORCE_LOWPASS_HZ = 0.0 +DEFAULT_FORCE_MEDIAN_WINDOW = 1 +DEFAULT_FLOW_EMA_ALPHA = 1.0 + +#: Named filter presets, so a recommendation is *executable* instead of a +#: comment in a JSON file (``json.load`` cannot carry comments). A config may +#: write ``"filters": {"preset": "balanced"}`` and override any single key on +#: top of it; the resolved values are what ``Sensor.filters_state()`` reports. +#: The latency/noise trade-offs behind these numbers are measured in +#: ``docs/performance/perf-measurements/`` (moved out of the once-gitignored +#: ``orisys-sdk/_perf/`` on 2026-09-27) and tabulated in ``docs/filters.md``. +FILTER_PRESETS = { + # Everything off: the file says what runs, and the raw reading is the record. + "off": { + "flow_ema_alpha": 1.0, + "force_median_window": 1, + "force_lowpass_cutoff_hz": 0.0, + }, + # One frame of latency @30 fps: median only. The EMA changes the + # measurement, so it stays off when latency matters more than jitter. + "low_latency": { + "flow_ema_alpha": 1.0, + "force_median_window": 3, + "force_lowpass_cutoff_hz": 0.0, + }, + # Two frames @30 fps: the EMA removes the spatially-coherent flow jitter + # that no force-level filter can reach, the median removes its spikes. + "balanced": { + "flow_ema_alpha": 0.5, + "force_median_window": 3, + "force_lowpass_cutoff_hz": 0.0, + }, + # Three frames @30 fps and the quietest readout: as ``balanced`` plus the + # force low-pass on top of the median. + "low_noise": { + "flow_ema_alpha": 0.3, + "force_median_window": 3, + "force_lowpass_cutoff_hz": 10.0, + }, +} + + +def available_filter_presets() -> dict: + """The presets, for UIs that want to offer them (the studio's gear dialog).""" + return {name: dict(values) for name, values in FILTER_PRESETS.items()} + + +ConvertFn = Callable[[float, float, float], tuple[float, float, float]] + + +class ForceReadout: + """Median → low-pass → convert for a three-channel force triple.""" + + def __init__( + self, + filters_cfg: dict | None = None, + *, + config_fps: float = DEFAULT_SAMPLE_RATE_HZ, + verbose: bool = False, + ) -> None: + # These settings change the reported force values, so they are part of + # the measurement-pipeline identity and are recorded in the calibration + # metadata / mismatch check alongside dis_mode and hhd_downsample. + # + # Every stage is switched by its **own value**, so the config states what + # runs: ``flow_ema_alpha >= 1``, ``force_median_window <= 1`` and + # ``force_lowpass_cutoff_hz <= 0`` each mean "stage off". Values are never + # overridden: there is no master switch that could make the file say 0.5 + # while the device runs 1.0. An invalid value falls back to **off**, never + # to a default that filters. ``enabled`` is kept only as a deprecated + # alias for "turn every stage off" and is honoured when it is explicitly + # false. ``state()`` reports what actually took effect. + if not isinstance(filters_cfg, dict): + filters_cfg = {} + legacy_off = filters_cfg.get("enabled", None) is False + preset_name = filters_cfg.get("preset", None) + preset_values = FILTER_PRESETS.get(str(preset_name), {}) if preset_name else {} + self.preset = str(preset_name) if preset_name in FILTER_PRESETS else None + + def _cfg(key, default): + if legacy_off: + return default + value = filters_cfg.get(key, None) + if value is None: + value = preset_values.get(key, None) + return default if value is None else value + + try: + self.config_fps = float(config_fps or 0.0) + except (TypeError, ValueError): + self.config_fps = 0.0 + if self.config_fps <= 0: + self.config_fps = float(DEFAULT_SAMPLE_RATE_HZ) + + self.lowpass_order = 2 + self.lowpass_sample_rate_hz = self.config_fps + self.lowpass_cutoff_hz = 0.0 + self.lowpass_rate_auto = True + try: + self.lowpass_order = max(1, int(_cfg("force_lowpass_order", 2))) + except (TypeError, ValueError): + self.lowpass_order = 2 + rate_cfg = _cfg("force_lowpass_sample_rate_hz", "auto") + if isinstance(rate_cfg, str): + self.lowpass_rate_auto = rate_cfg.strip().lower() in ("auto", "", "measured") + else: + try: + rate = float(rate_cfg) + except (TypeError, ValueError): + rate = 0.0 + self.lowpass_rate_auto = rate <= 0.0 + if not self.lowpass_rate_auto: + self.lowpass_sample_rate_hz = rate + try: + self.lowpass_cutoff_hz = float( + _cfg("force_lowpass_cutoff_hz", DEFAULT_FORCE_LOWPASS_HZ) + ) + except (TypeError, ValueError): + self.lowpass_cutoff_hz = 0.0 + if self.lowpass_sample_rate_hz <= 0: + self.lowpass_sample_rate_hz = self.config_fps + # An unusable cutoff is "off", never a silent jump to the default. + if not (0.0 <= self.lowpass_cutoff_hz < 0.5 * self.lowpass_sample_rate_hz): + self.lowpass_cutoff_hz = 0.0 + self.lowpass_enabled = self.lowpass_cutoff_hz > 0 + self._force_filter = LowpassFilter( + cutoff_hz=self.lowpass_cutoff_hz, + sample_rate_hz=self.lowpass_sample_rate_hz, + order=self.lowpass_order, + n_channels=3, + ) + self._force_rate = SampleRateEstimator(initial_hz=self.config_fps) + self.sample_rate_source = "config" + + try: + self.force_median_window = max( + 1, int(_cfg("force_median_window", DEFAULT_FORCE_MEDIAN_WINDOW)) + ) + except (TypeError, ValueError): + self.force_median_window = 1 + self._force_median = CausalMedianFilter( + window=self.force_median_window, n_channels=3 + ) + + self.flow_ema_alpha = 1.0 + try: + alpha = float(_cfg("flow_ema_alpha", DEFAULT_FLOW_EMA_ALPHA)) + except (TypeError, ValueError): + alpha = 1.0 + if 0.0 < alpha < 1.0: + self.flow_ema_alpha = alpha + self._ema_flow = None + if legacy_off and verbose: + print( + "Note: filters.enabled is deprecated and means 'all stages off'; " + "remove it and switch each stage by its own value " + "(flow_ema_alpha=1, force_median_window=1, force_lowpass_cutoff_hz=0)." + ) + self.any_stage_applied = bool( + self.flow_ema_alpha < 1.0 + or self.force_median_window > 1 + or self.lowpass_enabled + ) + self.filtered_raw: tuple[float, float, float] | None = None + self.raw_unfiltered: tuple[float, float, float] | None = None + self.unfiltered_newton: tuple[float, float, float] | None = None + + @property + def sample_rate_hz(self) -> float: + return float(self.lowpass_sample_rate_hz) + + def pin_file_rate(self) -> None: + """Wall-clock cadence is not signal time (video file / offline replay).""" + self.lowpass_rate_auto = False + self.sample_rate_source = "config (file)" + self.lowpass_sample_rate_hz = self.config_fps + self._force_rate.reset(self.config_fps) + + def apply_flow_ema(self, flow): + """Temporally smooth the optical-flow field before HHD. + + The frame-to-frame jitter of the estimated flow is spatially coherent + (a near-global shift: it survives ~40x less averaging than per-pixel + noise would, see the noise analysis), so it cannot be removed by + spatial smoothing, a coarser HHD grid or a different DIS setting - only + by averaging along time. Applying it here also stabilises + ``vnormal``/contact, which a force-level filter cannot reach. + + ``flow_ema_alpha >= 1`` disables the stage. + """ + alpha = self.flow_ema_alpha + if flow is None or alpha >= 1.0: + return flow + prev = self._ema_flow + if prev is None or prev.shape != flow.shape or prev.dtype != flow.dtype: + self._ema_flow = np.array(flow, copy=True) + return flow + prev += alpha * (flow - prev) + return prev + + def update_sample_rate(self) -> None: + """Track the achieved force update rate for the low-pass design.""" + if not self.lowpass_rate_auto: + return + hz = self._force_rate.tick() + if not (1.0 <= hz <= 240.0): + return + self.sample_rate_source = "measured" + used = self.lowpass_sample_rate_hz + if used > 0 and abs(hz - used) <= 0.1 * used: + return + self.lowpass_sample_rate_hz = hz + if self.lowpass_enabled: + self._force_filter.configure( + cutoff_hz=self.lowpass_cutoff_hz, + sample_rate_hz=hz, + order=self.lowpass_order, + ) + + def apply( + self, + raw, + convert: ConvertFn | None = None, + ) -> tuple[float, float, float]: + """Median → low-pass → optional convert. Returns filtered newton triple.""" + pre_filter = np.asarray(raw, dtype=np.float64) + values = pre_filter + # Median first: it deletes the heavy-tailed spikes that the flow chain + # produces, so the low-pass does not have to spread them over time. + if self._force_median.enabled: + values = self._force_median.process(values) + if self.lowpass_enabled and self.lowpass_cutoff_hz > 0: + values = self._force_filter.process(values) + self.filtered_raw = (float(values[0]), float(values[1]), float(values[2])) + fn, fx, fy = self.filtered_raw + if convert is not None: + fn, fx, fy = convert(fn, fx, fy) + fn, fx, fy = float(fn), float(fx), float(fy) + + ufn, ufx, ufy = ( + float(pre_filter[0]), + float(pre_filter[1]), + float(pre_filter[2]), + ) + self.raw_unfiltered = (ufn, ufx, ufy) + if convert is not None: + ufn, ufx, ufy = convert(ufn, ufx, ufy) + ufn, ufx, ufy = float(ufn), float(ufx), float(ufy) + self.unfiltered_newton = (ufn, ufx, ufy) + return fn, fx, fy + + 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, + ) -> bool: + """Update filter settings. Returns True when the force filter changed.""" + filter_changed = False + if lowpass_cutoff_hz is not None: + self.lowpass_cutoff_hz = float(lowpass_cutoff_hz) + filter_changed = True + if lowpass_enabled is None: + self.lowpass_enabled = self.lowpass_cutoff_hz > 0 + if lowpass_sample_rate_hz is not None: + # Accept a positive number to pin the rate, or "auto"/<=0 to track + # the measured output rate (see update_sample_rate). + auto = False + rate = 0.0 + if isinstance(lowpass_sample_rate_hz, str): + auto = lowpass_sample_rate_hz.strip().lower() in ("auto", "", "measured") + else: + try: + rate = float(lowpass_sample_rate_hz) + except (TypeError, ValueError): + auto = True + else: + auto = rate <= 0.0 + self.lowpass_rate_auto = auto + if not auto: + self.lowpass_sample_rate_hz = rate + elif self.lowpass_sample_rate_hz <= 0: + self.lowpass_sample_rate_hz = self.config_fps + self._force_rate.reset(self.config_fps) + filter_changed = True + if lowpass_order is not None: + order = int(lowpass_order) + if order < 1: + raise ValueError("lowpass_order must be >= 1") + self.lowpass_order = order + filter_changed = True + if lowpass_enabled is not None: + self.lowpass_enabled = bool(lowpass_enabled) + filter_changed = True + + 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]") + self.flow_ema_alpha = alpha + self._ema_flow = None + + if force_median_window is not None: + try: + window = max(1, int(force_median_window)) + except (TypeError, ValueError) as exc: + raise ValueError("force_median_window must be an integer >= 1") from exc + self.force_median_window = window + self._force_median.configure(window) + filter_changed = True + + if filter_changed: + cutoff = self.lowpass_cutoff_hz if self.lowpass_enabled else 0.0 + self._force_filter.configure( + cutoff_hz=cutoff, + sample_rate_hz=self.lowpass_sample_rate_hz, + order=self.lowpass_order, + ) + self.any_stage_applied = bool( + self.flow_ema_alpha < 1.0 + or self.force_median_window > 1 + or self.lowpass_enabled + ) + return filter_changed + + def reset(self) -> None: + self._force_filter.reset() + self._force_median.reset() + self._ema_flow = None + self.filtered_raw = None + self.raw_unfiltered = None + self.unfiltered_newton = None + + def state(self, *, dis_mode=None) -> dict: + """Effective filter chain, for records and audits.""" + return { + "preset": self.preset, + "measurement": { + "flow_ema_alpha": float(self.flow_ema_alpha), + "flow_ema_applied": bool(self.flow_ema_alpha < 1.0), + "dis_mode": dis_mode, + }, + "readout": { + "force_median_window": int(self.force_median_window), + "force_median_applied": bool(self._force_median.enabled), + "force_lowpass_cutoff_hz": float(self.lowpass_cutoff_hz), + "force_lowpass_order": int(self.lowpass_order), + "force_lowpass_sample_rate_hz": float(self.lowpass_sample_rate_hz), + "force_lowpass_rate_auto": bool(self.lowpass_rate_auto), + "force_lowpass_applied": bool(self.lowpass_enabled), + "sample_rate_source": str(self.sample_rate_source), + }, + "any_stage_applied": bool(self.any_stage_applied), + "unfiltered_available": True, + } diff --git a/src/orisys/sensor.py b/src/orisys/sensor.py index 5671c39..4810003 100644 --- a/src/orisys/sensor.py +++ b/src/orisys/sensor.py @@ -17,83 +17,19 @@ 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 .signal_processing import ( - DEFAULT_SAMPLE_RATE_HZ, - CausalMedianFilter, - LowpassFilter, - SampleRateEstimator, -) # from threading import Thread, Event # from queue import Queue, Empty from .Stitcher import Stitcher from .tracker import Tracker - -# Default signal-conditioning settings (config: top-level "filters" block). -# -# A key a config does not state is **off**, not "whatever we thought was -# sensible": the file has to name what runs, or a device can be filtering a -# measurement nobody asked it to filter and no reader of the file could tell. -# The shipped bundles state all three values explicitly, so they are unaffected -# by these defaults; a config with no "filters" block at all filters nothing. -# -# Which settings are worth asking for is measured, not guessed. On dd02-0703 at -# 280x300 (see the noise analysis in the perf notes) flow EMA 0.5 + median 3 -# costs ~2 frames at 30 fps and cuts steady-press force jitter by roughly half -# while also stabilising vnormal/contact (which a force-level filter cannot do); -# that combination is the "balanced" preset. The force low-pass stays off there -# too: on top of the EMA it added no measurable noise benefit at the same -# latency, so it is an opt-in stage for sites with different noise -# characteristics (the "low_noise" preset). -DEFAULT_FORCE_LOWPASS_HZ = 0.0 -DEFAULT_FORCE_MEDIAN_WINDOW = 1 -DEFAULT_FLOW_EMA_ALPHA = 1.0 - -#: Named filter presets, so a recommendation is *executable* instead of a -#: comment in a JSON file (``json.load`` cannot carry comments). A config may -#: write ``"filters": {"preset": "balanced"}`` and override any single key on -#: top of it; the resolved values are what ``Sensor.filters_state()`` reports. -#: The latency/noise trade-offs behind these numbers are measured in -#: ``docs/performance/perf-measurements/`` (moved out of the once-gitignored -#: ``orisys-sdk/_perf/`` on 2026-09-27) and tabulated in ``docs/filters.md``. -FILTER_PRESETS = { - # Everything off: the file says what runs, and the raw reading is the record. - "off": { - "flow_ema_alpha": 1.0, - "force_median_window": 1, - "force_lowpass_cutoff_hz": 0.0, - }, - # One frame of latency @30 fps: median only. The EMA changes the - # measurement, so it stays off when latency matters more than jitter. - "low_latency": { - "flow_ema_alpha": 1.0, - "force_median_window": 3, - "force_lowpass_cutoff_hz": 0.0, - }, - # Two frames @30 fps: the EMA removes the spatially-coherent flow jitter - # that no force-level filter can reach, the median removes its spikes. - "balanced": { - "flow_ema_alpha": 0.5, - "force_median_window": 3, - "force_lowpass_cutoff_hz": 0.0, - }, - # Three frames @30 fps and the quietest readout: as ``balanced`` plus the - # force low-pass on top of the median. - "low_noise": { - "flow_ema_alpha": 0.3, - "force_median_window": 3, - "force_lowpass_cutoff_hz": 10.0, - }, -} - - -def available_filter_presets() -> dict: - """The presets, for UIs that want to offer them (the studio's gear dialog).""" - return {name: dict(values) for name, values in FILTER_PRESETS.items()} - from .capture import ( CaptureConfig, CaptureSource, @@ -237,112 +173,14 @@ class Sensor: self._force_cal_attempts: list[dict] | None = None self._init_force_calibration() - # --------------------------------------------------------------- - # Signal conditioning (config: top-level "filters" block) - # --------------------------------------------------------------- - # These settings change the reported force values, so they are part of - # the measurement-pipeline identity and are recorded in the calibration - # metadata / mismatch check alongside dis_mode and hhd_downsample. - # - # Every stage is switched by its **own value**, so the config states what - # runs: ``flow_ema_alpha >= 1``, ``force_median_window <= 1`` and - # ``force_lowpass_cutoff_hz <= 0`` each mean "stage off". Values are never - # overridden: there is no master switch that could make the file say 0.5 - # while the device runs 1.0. An invalid value falls back to **off**, never - # to a default that filters. ``enabled`` is kept only as a deprecated - # alias for "turn every stage off" and is honoured when it is explicitly - # false. ``Sensor.filters_state()`` reports what actually took effect. - filters_cfg = self._config.get("filters") - if not isinstance(filters_cfg, dict): - filters_cfg = {} - legacy_off = filters_cfg.get("enabled", None) is False - preset_name = filters_cfg.get("preset", None) - preset_values = FILTER_PRESETS.get(str(preset_name), {}) if preset_name else {} - self._filter_preset = str(preset_name) if preset_name in FILTER_PRESETS else None - - def _cfg(key, default): - """The configured value: an explicit key, else the preset, else default.""" - if legacy_off: - return default - value = filters_cfg.get(key, None) - if value is None: - value = preset_values.get(key, None) - return default if value is None else value - - self._lowpass_order = 2 - self._lowpass_sample_rate_hz = float(DEFAULT_SAMPLE_RATE_HZ) - self._lowpass_cutoff_hz = 0.0 - self._lowpass_rate_auto = True - self._config_fps = 0.0 try: - self._config_fps = float(self.__config_cam.get("fps", 0.0) or 0.0) + config_fps = float(self.__config_cam.get("fps", 0.0) or 0.0) except (TypeError, ValueError): - self._config_fps = 0.0 - if self._config_fps <= 0: - self._config_fps = float(DEFAULT_SAMPLE_RATE_HZ) - self._lowpass_sample_rate_hz = self._config_fps - try: - self._lowpass_order = max(1, int(_cfg("force_lowpass_order", 2))) - except (TypeError, ValueError): - self._lowpass_order = 2 - rate_cfg = _cfg("force_lowpass_sample_rate_hz", "auto") - if isinstance(rate_cfg, str): - self._lowpass_rate_auto = rate_cfg.strip().lower() in ("auto", "", "measured") - else: - try: - rate = float(rate_cfg) - except (TypeError, ValueError): - rate = 0.0 - self._lowpass_rate_auto = rate <= 0.0 - if not self._lowpass_rate_auto: - self._lowpass_sample_rate_hz = rate - try: - self._lowpass_cutoff_hz = float( - _cfg("force_lowpass_cutoff_hz", DEFAULT_FORCE_LOWPASS_HZ) - ) - except (TypeError, ValueError): - self._lowpass_cutoff_hz = 0.0 - if self._lowpass_sample_rate_hz <= 0: - self._lowpass_sample_rate_hz = self._config_fps - # An unusable cutoff is "off", never a silent jump to the default. - if not (0.0 <= self._lowpass_cutoff_hz < 0.5 * self._lowpass_sample_rate_hz): - self._lowpass_cutoff_hz = 0.0 - self._lowpass_enabled = self._lowpass_cutoff_hz > 0 - self._force_filter = LowpassFilter( - cutoff_hz=self._lowpass_cutoff_hz, - sample_rate_hz=self._lowpass_sample_rate_hz, - order=self._lowpass_order, - n_channels=3, - ) - # Rate tracking: only meaningful while the force low-pass is in use. - self._force_rate = SampleRateEstimator(initial_hz=self._config_fps) - self._force_rate_source = "config" - - try: - self._force_median_window = max(1, int(_cfg("force_median_window", DEFAULT_FORCE_MEDIAN_WINDOW))) - except (TypeError, ValueError): - self._force_median_window = 1 - self._force_median = CausalMedianFilter(window=self._force_median_window, n_channels=3) - - self._flow_ema_alpha = 1.0 - try: - alpha = float(_cfg("flow_ema_alpha", DEFAULT_FLOW_EMA_ALPHA)) - except (TypeError, ValueError): - alpha = 1.0 - if 0.0 < alpha < 1.0: - self._flow_ema_alpha = alpha - self._ema_flow = None - if legacy_off and self.verbose: - print( - "Note: filters.enabled is deprecated and means 'all stages off'; " - "remove it and switch each stage by its own value " - "(flow_ema_alpha=1, force_median_window=1, force_lowpass_cutoff_hz=0)." - ) - # Derived, for callers that ask "is any stage filtering?". - self._filters_enabled = bool( - self._flow_ema_alpha < 1.0 - or self._force_median_window > 1 - or self._lowpass_enabled + config_fps = 0.0 + self._force_readout = ForceReadout( + self._config.get("filters"), + config_fps=config_fps, + verbose=self.verbose, ) if self.verbose: @@ -444,13 +282,10 @@ class Sensor: # 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._lowpass_rate_auto = False - self._force_rate_source = "config (file)" - self._lowpass_sample_rate_hz = self._config_fps - if getattr(self, "_force_rate", None) is not None: - self._force_rate.reset(self._config_fps) + 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']}") @@ -460,11 +295,11 @@ class Sensor: print(f" OpenCL available: {self.OPENCL_AVAILABLE}") print( " Signal filters: " - f"ema_alpha={self._flow_ema_alpha} " - f"median={self._force_median_window} " - f"lowpass={self._lowpass_cutoff_hz}Hz@" - f"{self._lowpass_sample_rate_hz:.1f}Hz " - f"({self.force_sample_rate_source})" + 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})" ) @@ -797,72 +632,19 @@ class Sensor: # flow = self.__tracker.track_raft(self.img) if hasattr(flow, 'download'): flow = flow.download() - flow = self._apply_flow_ema(flow) + flow = self._force_readout.apply_flow_ema(flow) self.flow = flow self._compute_flow_means(self.flow) - def _apply_flow_ema(self, flow): - """Temporally smooth the optical-flow field before HHD. - - The frame-to-frame jitter of the estimated flow is spatially coherent - (a near-global shift: it survives ~40x less averaging than per-pixel - noise would, see the noise analysis), so it cannot be removed by - spatial smoothing, a coarser HHD grid or a different DIS setting - only - by averaging along time. Applying it here also stabilises - ``vnormal``/contact, which a force-level filter cannot reach. - - ``flow_ema_alpha >= 1`` disables the stage. - """ - alpha = getattr(self, "_flow_ema_alpha", 1.0) - if flow is None or alpha >= 1.0: - return flow - prev = self._ema_flow - if prev is None or prev.shape != flow.shape or prev.dtype != flow.dtype: - self._ema_flow = np.array(flow, copy=True) - return flow - prev += alpha * (flow - prev) - return prev - - def _update_force_sample_rate(self): - """Track the achieved force update rate for the low-pass design. - - A Butterworth designed for 30 Hz stops meaning the same thing at 60 Hz, - so when ``force_lowpass_sample_rate_hz`` is ``"auto"`` the rate actually - achieved is used. For sources whose wall-clock cadence does not equal - signal time (video files, offline replay) ``_lowpass_rate_auto`` is - turned off by whoever knows the source - ``__init__`` for a capture the - Sensor opened itself, and ``FingerRuntime.open`` for external frames - - and the configured camera fps is used instead. - """ - if not getattr(self, "_lowpass_rate_auto", False): - return - estimator = getattr(self, "_force_rate", None) - if estimator is None: - return - hz = estimator.tick() - if not (1.0 <= hz <= 240.0): - return - self._force_rate_source = "measured" - used = self._lowpass_sample_rate_hz - if used > 0 and abs(hz - used) <= 0.1 * used: - return - self._lowpass_sample_rate_hz = hz - if self._lowpass_enabled: - self._force_filter.configure( - cutoff_hz=self._lowpass_cutoff_hz, - sample_rate_hz=hz, - order=self._lowpass_order, - ) - @property def force_sample_rate_hz(self) -> float: """Sample rate currently used to design the force low-pass (Hz).""" - return float(self._lowpass_sample_rate_hz) + return self._force_readout.sample_rate_hz @property def force_sample_rate_source(self) -> str: - """``config``, ``measured`` or ``config (replay)``.""" - return str(getattr(self, "_force_rate_source", "config")) + """``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. @@ -943,7 +725,7 @@ class Sensor: fn, fx, fy, ftor = forces self._force_raw = (float(fn), float(fx), float(fy)) self.ftor = float(ftor) - self._update_force_sample_rate() + self._force_readout.update_sample_rate() self._apply_force_filter() vnormal_ds = self.__hhd.get_normal() @@ -1084,10 +866,7 @@ class Sensor: def reset(self): """Reset optical flow tracker and force-filter state.""" self.__tracker.reset() - self._force_filter.reset() - if getattr(self, "_force_median", None) is not None: - self._force_median.reset() - self._ema_flow = None + 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 @@ -1178,66 +957,16 @@ class Sensor: ) if mismatches: raise cal_mod.CalibrationError("; ".join(mismatches)) - filter_changed = False - if lowpass_cutoff_hz is not None: - self._lowpass_cutoff_hz = float(lowpass_cutoff_hz) - filter_changed = True - if lowpass_enabled is None: - self._lowpass_enabled = self._lowpass_cutoff_hz > 0 - if lowpass_sample_rate_hz is not None: - # Accept a positive number to pin the rate, or "auto"/<=0 to track - # the measured output rate (see _update_force_sample_rate). - auto = False - rate = 0.0 - if isinstance(lowpass_sample_rate_hz, str): - auto = lowpass_sample_rate_hz.strip().lower() in ("auto", "", "measured") - else: - try: - rate = float(lowpass_sample_rate_hz) - except (TypeError, ValueError): - auto = True - else: - auto = rate <= 0.0 - self._lowpass_rate_auto = auto - if not auto: - self._lowpass_sample_rate_hz = rate - elif self._lowpass_sample_rate_hz <= 0: - self._lowpass_sample_rate_hz = self._config_fps - if getattr(self, "_force_rate", None) is not None: - self._force_rate.reset(self._config_fps) - filter_changed = True - if lowpass_order is not None: - order = int(lowpass_order) - if order < 1: - raise ValueError("lowpass_order must be >= 1") - self._lowpass_order = order - filter_changed = True - if lowpass_enabled is not None: - self._lowpass_enabled = bool(lowpass_enabled) - filter_changed = True - - if flow_ema_alpha is not None: - self._flow_ema_alpha = alpha - self._ema_flow = None - - if force_median_window is not None: - try: - window = max(1, int(force_median_window)) - except (TypeError, ValueError) as exc: - raise ValueError("force_median_window must be an integer >= 1") from exc - self._force_median_window = window - self._force_median.configure(window) - filter_changed = True - - if filter_changed: - cutoff = self._lowpass_cutoff_hz if self._lowpass_enabled else 0.0 - self._force_filter.configure( - cutoff_hz=cutoff, - sample_rate_hz=self._lowpass_sample_rate_hz, - order=self._lowpass_order, - ) - if self.compute_deformation_valid: - self._apply_force_filter() + 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 @@ -1377,7 +1106,7 @@ class Sensor: "hhd_downsample": tracker.get("hhd_downsample"), "filters": cal_mod.filters_fingerprint( self.filters_state() - if hasattr(self, "_flow_ema_alpha") + if hasattr(self, "_force_readout") else self._config.get("filters") ), } @@ -1432,45 +1161,13 @@ class Sensor: self._force_cal_attempts = None def _apply_force_filter(self): - pre_filter = np.asarray(self._force_raw, dtype=np.float64) - raw = pre_filter - # Median first: it deletes the heavy-tailed spikes that the flow chain - # produces, so the low-pass does not have to spread them over time. - median = getattr(self, "_force_median", None) - if median is not None and median.enabled: - raw = median.process(raw) - if self._lowpass_enabled and self._lowpass_cutoff_hz > 0: - raw = self._force_filter.process(raw) - # The chain's own output, before calibration: the "filtered raw counts". - # Without this the raw axis would have no filtered/unfiltered pair and - # ``force_triplets`` could only offer the device counts twice. - self._force_filtered_raw = ( - float(raw[0]), - float(raw[1]), - float(raw[2]), - ) - fn, fx, fy = self._force_filtered_raw - - # Apply loaded force_calibration.json when present; otherwise keep raw. - if self.calibration is not None: - fn, fx, fy = self.calibration.convert(fn, fx, fy) - fx, fy = float(fx), float(fy) - fn = float(fn) - + 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 - - # The same reading with no readout filtering, so a caller can keep both - # (the app stores the filtered values as the record and the unfiltered - # ones beside them). Calibrated with the same coefficients, so the two - # only differ by the filter chain. - ufn, ufx, ufy = (float(pre_filter[0]), float(pre_filter[1]), float(pre_filter[2])) - if self.calibration is not None: - ufn, ufx, ufy = self.calibration.convert(ufn, ufx, ufy) - ufx, ufy, ufn = float(ufx), float(ufy), float(ufn) - self._force_raw_unfiltered = ( - float(pre_filter[0]), float(pre_filter[1]), float(pre_filter[2]) - ) - self._force_unfiltered = (ufn, ufx, ufy) + self._force_raw_unfiltered = readout.raw_unfiltered + self._force_unfiltered = readout.unfiltered_newton # ------------------------------------------------------- filter contract def filters_state(self) -> dict: @@ -1488,29 +1185,10 @@ class Sensor: (median, low-pass): recorded, but they do not change the physical claim. """ - median_applied = bool(getattr(self._force_median, "enabled", False)) tracker_cfg = self._config.get("tracker") or {} - return { - "preset": getattr(self, "_filter_preset", None), - "measurement": { - "flow_ema_alpha": float(self._flow_ema_alpha), - "flow_ema_applied": bool(self._flow_ema_alpha < 1.0), - "dis_mode": tracker_cfg.get("dis_mode"), - }, - "readout": { - "force_median_window": int(self._force_median_window), - "force_median_applied": median_applied, - "force_lowpass_cutoff_hz": float(self._lowpass_cutoff_hz), - "force_lowpass_order": int(self._lowpass_order), - "force_lowpass_sample_rate_hz": float(self._lowpass_sample_rate_hz), - "force_lowpass_rate_auto": bool(self._lowpass_rate_auto), - "force_lowpass_applied": bool(self._lowpass_enabled), - "sample_rate_source": str(getattr(self, "_force_rate_source", "config")), - }, - "any_stage_applied": bool(self._filters_enabled), - "unfiltered_available": True, - "source": "sensor.filters_state", - } + 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.