把 FN/FX/FY 力读出抽到 ForceReadout。
滤波预设、EMA、中值、低通与标定换算离开 Sensor;对外 configure / filters_state 不变。已有 signal_processing、filter_presets、calibration 测试通过。
This commit is contained in:
@@ -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,
|
||||||
|
}
|
||||||
+44
-366
@@ -17,83 +17,19 @@ import numpy as np
|
|||||||
from . import calibration as cal_mod
|
from . import calibration as cal_mod
|
||||||
from . import configs
|
from . import configs
|
||||||
from .backend_availability import check_cupy_available, check_opencl_available
|
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 .frame_processors import create_frame_processor
|
||||||
from .get_depth import Contact3D
|
from .get_depth import Contact3D
|
||||||
from .hhd import nHHD
|
from .hhd import nHHD
|
||||||
from .signal_processing import (
|
|
||||||
DEFAULT_SAMPLE_RATE_HZ,
|
|
||||||
CausalMedianFilter,
|
|
||||||
LowpassFilter,
|
|
||||||
SampleRateEstimator,
|
|
||||||
)
|
|
||||||
|
|
||||||
# from threading import Thread, Event
|
# from threading import Thread, Event
|
||||||
# from queue import Queue, Empty
|
# from queue import Queue, Empty
|
||||||
from .Stitcher import Stitcher
|
from .Stitcher import Stitcher
|
||||||
from .tracker import Tracker
|
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 (
|
from .capture import (
|
||||||
CaptureConfig,
|
CaptureConfig,
|
||||||
CaptureSource,
|
CaptureSource,
|
||||||
@@ -237,112 +173,14 @@ class Sensor:
|
|||||||
self._force_cal_attempts: list[dict] | None = None
|
self._force_cal_attempts: list[dict] | None = None
|
||||||
self._init_force_calibration()
|
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:
|
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):
|
except (TypeError, ValueError):
|
||||||
self._config_fps = 0.0
|
config_fps = 0.0
|
||||||
if self._config_fps <= 0:
|
self._force_readout = ForceReadout(
|
||||||
self._config_fps = float(DEFAULT_SAMPLE_RATE_HZ)
|
self._config.get("filters"),
|
||||||
self._lowpass_sample_rate_hz = self._config_fps
|
config_fps=config_fps,
|
||||||
try:
|
verbose=self.verbose,
|
||||||
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
|
|
||||||
)
|
)
|
||||||
|
|
||||||
if self.verbose:
|
if self.verbose:
|
||||||
@@ -444,13 +282,10 @@ class Sensor:
|
|||||||
# A file source is replayed at whatever speed the CPU manages,
|
# A file source is replayed at whatever speed the CPU manages,
|
||||||
# so wall-clock intervals do not reflect signal time: keep the
|
# so wall-clock intervals do not reflect signal time: keep the
|
||||||
# configured camera fps for the force low-pass design.
|
# configured camera fps for the force low-pass design.
|
||||||
self._lowpass_rate_auto = False
|
self._force_readout.pin_file_rate()
|
||||||
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)
|
|
||||||
|
|
||||||
if self.verbose:
|
if self.verbose:
|
||||||
|
readout = self._force_readout
|
||||||
print(f" Video source: {vid_src}")
|
print(f" Video source: {vid_src}")
|
||||||
print(f" Stitch mode: {self.stitch_valid}")
|
print(f" Stitch mode: {self.stitch_valid}")
|
||||||
print(f" Optical flow algorithm: {self.__config_tracker['algorithm']}")
|
print(f" Optical flow algorithm: {self.__config_tracker['algorithm']}")
|
||||||
@@ -460,11 +295,11 @@ class Sensor:
|
|||||||
print(f" OpenCL available: {self.OPENCL_AVAILABLE}")
|
print(f" OpenCL available: {self.OPENCL_AVAILABLE}")
|
||||||
print(
|
print(
|
||||||
" Signal filters: "
|
" Signal filters: "
|
||||||
f"ema_alpha={self._flow_ema_alpha} "
|
f"ema_alpha={readout.flow_ema_alpha} "
|
||||||
f"median={self._force_median_window} "
|
f"median={readout.force_median_window} "
|
||||||
f"lowpass={self._lowpass_cutoff_hz}Hz@"
|
f"lowpass={readout.lowpass_cutoff_hz}Hz@"
|
||||||
f"{self._lowpass_sample_rate_hz:.1f}Hz "
|
f"{readout.lowpass_sample_rate_hz:.1f}Hz "
|
||||||
f"({self.force_sample_rate_source})"
|
f"({readout.sample_rate_source})"
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
@@ -797,72 +632,19 @@ class Sensor:
|
|||||||
# flow = self.__tracker.track_raft(self.img)
|
# flow = self.__tracker.track_raft(self.img)
|
||||||
if hasattr(flow, 'download'):
|
if hasattr(flow, 'download'):
|
||||||
flow = flow.download()
|
flow = flow.download()
|
||||||
flow = self._apply_flow_ema(flow)
|
flow = self._force_readout.apply_flow_ema(flow)
|
||||||
self.flow = flow
|
self.flow = flow
|
||||||
self._compute_flow_means(self.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
|
@property
|
||||||
def force_sample_rate_hz(self) -> float:
|
def force_sample_rate_hz(self) -> float:
|
||||||
"""Sample rate currently used to design the force low-pass (Hz)."""
|
"""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
|
@property
|
||||||
def force_sample_rate_source(self) -> str:
|
def force_sample_rate_source(self) -> str:
|
||||||
"""``config``, ``measured`` or ``config (replay)``."""
|
"""``config``, ``measured`` or ``config (file)``."""
|
||||||
return str(getattr(self, "_force_rate_source", "config"))
|
return self._force_readout.sample_rate_source
|
||||||
|
|
||||||
def compute_hhd(self, flow=None):
|
def compute_hhd(self, flow=None):
|
||||||
"""Decompose the optical flow and update the force readout.
|
"""Decompose the optical flow and update the force readout.
|
||||||
@@ -943,7 +725,7 @@ class Sensor:
|
|||||||
fn, fx, fy, ftor = forces
|
fn, fx, fy, ftor = forces
|
||||||
self._force_raw = (float(fn), float(fx), float(fy))
|
self._force_raw = (float(fn), float(fx), float(fy))
|
||||||
self.ftor = float(ftor)
|
self.ftor = float(ftor)
|
||||||
self._update_force_sample_rate()
|
self._force_readout.update_sample_rate()
|
||||||
self._apply_force_filter()
|
self._apply_force_filter()
|
||||||
|
|
||||||
vnormal_ds = self.__hhd.get_normal()
|
vnormal_ds = self.__hhd.get_normal()
|
||||||
@@ -1084,10 +866,7 @@ class Sensor:
|
|||||||
def reset(self):
|
def reset(self):
|
||||||
"""Reset optical flow tracker and force-filter state."""
|
"""Reset optical flow tracker and force-filter state."""
|
||||||
self.__tracker.reset()
|
self.__tracker.reset()
|
||||||
self._force_filter.reset()
|
self._force_readout.reset()
|
||||||
if getattr(self, "_force_median", None) is not None:
|
|
||||||
self._force_median.reset()
|
|
||||||
self._ema_flow = None
|
|
||||||
# The unfiltered pair belongs to the frame it was computed for: a new
|
# The unfiltered pair belongs to the frame it was computed for: a new
|
||||||
# session must not report the previous one's reading.
|
# session must not report the previous one's reading.
|
||||||
self._force_unfiltered = None
|
self._force_unfiltered = None
|
||||||
@@ -1178,66 +957,16 @@ class Sensor:
|
|||||||
)
|
)
|
||||||
if mismatches:
|
if mismatches:
|
||||||
raise cal_mod.CalibrationError("; ".join(mismatches))
|
raise cal_mod.CalibrationError("; ".join(mismatches))
|
||||||
filter_changed = False
|
filter_changed = self._force_readout.configure(
|
||||||
if lowpass_cutoff_hz is not None:
|
lowpass_cutoff_hz=lowpass_cutoff_hz,
|
||||||
self._lowpass_cutoff_hz = float(lowpass_cutoff_hz)
|
lowpass_sample_rate_hz=lowpass_sample_rate_hz,
|
||||||
filter_changed = True
|
lowpass_order=lowpass_order,
|
||||||
if lowpass_enabled is None:
|
lowpass_enabled=lowpass_enabled,
|
||||||
self._lowpass_enabled = self._lowpass_cutoff_hz > 0
|
flow_ema_alpha=flow_ema_alpha,
|
||||||
if lowpass_sample_rate_hz is not None:
|
force_median_window=force_median_window,
|
||||||
# Accept a positive number to pin the rate, or "auto"/<=0 to track
|
)
|
||||||
# the measured output rate (see _update_force_sample_rate).
|
if filter_changed and self.compute_deformation_valid:
|
||||||
auto = False
|
self._apply_force_filter()
|
||||||
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()
|
|
||||||
|
|
||||||
return self
|
return self
|
||||||
|
|
||||||
@@ -1377,7 +1106,7 @@ class Sensor:
|
|||||||
"hhd_downsample": tracker.get("hhd_downsample"),
|
"hhd_downsample": tracker.get("hhd_downsample"),
|
||||||
"filters": cal_mod.filters_fingerprint(
|
"filters": cal_mod.filters_fingerprint(
|
||||||
self.filters_state()
|
self.filters_state()
|
||||||
if hasattr(self, "_flow_ema_alpha")
|
if hasattr(self, "_force_readout")
|
||||||
else self._config.get("filters")
|
else self._config.get("filters")
|
||||||
),
|
),
|
||||||
}
|
}
|
||||||
@@ -1432,45 +1161,13 @@ class Sensor:
|
|||||||
self._force_cal_attempts = None
|
self._force_cal_attempts = None
|
||||||
|
|
||||||
def _apply_force_filter(self):
|
def _apply_force_filter(self):
|
||||||
pre_filter = np.asarray(self._force_raw, dtype=np.float64)
|
convert = self.calibration.convert if self.calibration is not None else None
|
||||||
raw = pre_filter
|
fn, fx, fy = self._force_readout.apply(self._force_raw, convert)
|
||||||
# Median first: it deletes the heavy-tailed spikes that the flow chain
|
readout = self._force_readout
|
||||||
# produces, so the low-pass does not have to spread them over time.
|
self._force_filtered_raw = readout.filtered_raw
|
||||||
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)
|
|
||||||
|
|
||||||
self.fnormal, self.fshearx, self.fsheary = fn, fx, fy
|
self.fnormal, self.fshearx, self.fsheary = fn, fx, fy
|
||||||
|
self._force_raw_unfiltered = readout.raw_unfiltered
|
||||||
# The same reading with no readout filtering, so a caller can keep both
|
self._force_unfiltered = readout.unfiltered_newton
|
||||||
# (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)
|
|
||||||
|
|
||||||
# ------------------------------------------------------- filter contract
|
# ------------------------------------------------------- filter contract
|
||||||
def filters_state(self) -> dict:
|
def filters_state(self) -> dict:
|
||||||
@@ -1488,29 +1185,10 @@ class Sensor:
|
|||||||
(median, low-pass): recorded, but they do not change the physical
|
(median, low-pass): recorded, but they do not change the physical
|
||||||
claim.
|
claim.
|
||||||
"""
|
"""
|
||||||
median_applied = bool(getattr(self._force_median, "enabled", False))
|
|
||||||
tracker_cfg = self._config.get("tracker") or {}
|
tracker_cfg = self._config.get("tracker") or {}
|
||||||
return {
|
state = self._force_readout.state(dis_mode=tracker_cfg.get("dis_mode"))
|
||||||
"preset": getattr(self, "_filter_preset", None),
|
state["source"] = "sensor.filters_state"
|
||||||
"measurement": {
|
return state
|
||||||
"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",
|
|
||||||
}
|
|
||||||
|
|
||||||
def force_triplets(self) -> dict:
|
def force_triplets(self) -> dict:
|
||||||
"""The filtered and unfiltered force triples of the current frame.
|
"""The filtered and unfiltered force triples of the current frame.
|
||||||
|
|||||||
Reference in New Issue
Block a user