把 FN/FX/FY 力读出抽到 ForceReadout。
滤波预设、EMA、中值、低通与标定换算离开 Sensor；对外 configure / filters_state 不变。已有 signal_processing、filter_presets、calibration 测试通过。
This commit is contained in:
ykcheungab
@@ -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,
|
||||
}
|
||||
Reference in New Issue
Block a user