orisys-tech/orisys-sdk
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

把 FN/FX/FY 力读出抽到 ForceReadout。

Browse Source 
滤波预设、EMA、中值、低通与标定换算离开 Sensor；对外 configure / filters_state 不变。已有 signal_processing、filter_presets、calibration 测试通过。
This commit is contained in:
ykcheungab
2026-10-08 10:08:35 +08:00
parent 68edb0dc4c
commit 0784eacf51
2 changed files with 438 additions and 366 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/orisys/sensor.py
+44 -366
View File
@@ -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.