update filter, flow ema 0.5 + med 3

This commit is contained in:
ykcheungab
2026-09-12 19:36:36 +08:00
parent 5f78115429
commit 0d5d8ff70d
12 changed files with 697 additions and 62 deletions
+8
View File
@@ -33,6 +33,14 @@
"algorithm_list": ["raft", "dis"], "algorithm_list": ["raft", "dis"],
"hhd_downsample": 3 "hhd_downsample": 3
}, },
"filters": {
"enabled": true,
"flow_ema_alpha": 0.5,
"force_median_window": 3,
"force_lowpass_cutoff_hz": 0.0,
"force_lowpass_order": 2,
"force_lowpass_sample_rate_hz": "auto"
},
"contact": { "contact": {
"min_contour_area": 3000, "min_contour_area": 3000,
"flow_threshold":5, "flow_threshold":5,
+9 -1
View File
@@ -22,7 +22,15 @@
"dis_preset": "medium", "dis_preset": "medium",
"dis_mode": 1, "dis_mode": 1,
"algorithm_list": ["raft", "dis"], "algorithm_list": ["raft", "dis"],
"hhd_downsample": 3 "hhd_downsample": 2
},
"filters": {
"enabled": true,
"flow_ema_alpha": 0.5,
"force_median_window": 3,
"force_lowpass_cutoff_hz": 0.0,
"force_lowpass_order": 2,
"force_lowpass_sample_rate_hz": "auto"
}, },
"contact": { "contact": {
"min_contour_area": 3000, "min_contour_area": 3000,
+9 -1
View File
@@ -29,10 +29,18 @@
"loop_closure_interval": 1, "loop_closure_interval": 1,
"algorithm": "dis", "algorithm": "dis",
"dis_preset": "medium", "dis_preset": "medium",
"dis_mode": 3, "dis_mode": 1,
"algorithm_list": ["raft", "dis"], "algorithm_list": ["raft", "dis"],
"hhd_downsample": 3 "hhd_downsample": 3
}, },
"filters": {
"enabled": true,
"flow_ema_alpha": 0.5,
"force_median_window": 3,
"force_lowpass_cutoff_hz": 0.0,
"force_lowpass_order": 2,
"force_lowpass_sample_rate_hz": "auto"
},
"contact": { "contact": {
"min_contour_area": 3000, "min_contour_area": 3000,
"flow_threshold":5, "flow_threshold":5,
+35 -4
View File
@@ -242,7 +242,9 @@ class Calibration:
meta: dict[str, Any] = {} meta: dict[str, Any] = {}
meta.update(self.provenance) meta.update(self.provenance)
meta.update({k: v for k, v in self.identity.items() if k not in meta}) meta.update({k: v for k, v in self.identity.items() if k not in meta})
for key in ("stitch_resolution", "dis_mode", "hhd_downsample", "config_ref"): for key in ("stitch_resolution", "dis_mode", "hhd_downsample",
"flow_ema_alpha", "force_median_window", "force_lowpass_cutoff_hz",
"config_ref"):
if key in self.pipeline and self.pipeline[key] is not None: if key in self.pipeline and self.pipeline[key] is not None:
meta[key] = self.pipeline[key] meta[key] = self.pipeline[key]
if self.measurements: if self.measurements:
@@ -387,7 +389,8 @@ def check(doc: dict | Calibration, *, pipeline: dict | None = None) -> CheckResu
warns.append("empty sn on factory calibration") warns.append("empty sn on factory calibration")
if pipeline is not None and isinstance(doc, Calibration): if pipeline is not None and isinstance(doc, Calibration):
for key in ("stitch_resolution", "dis_mode", "hhd_downsample"): for key in ("stitch_resolution", "dis_mode", "hhd_downsample",
"flow_ema_alpha", "force_median_window", "force_lowpass_cutoff_hz"):
if key in doc.pipeline and key in pipeline and doc.pipeline[key] != pipeline[key]: if key in doc.pipeline and key in pipeline and doc.pipeline[key] != pipeline[key]:
warns.append(f"pipeline mismatch: {key}") warns.append(f"pipeline mismatch: {key}")
@@ -601,11 +604,14 @@ def load(
def pipeline_from_sensor_json(path: str | Path | None) -> dict[str, Any]: def pipeline_from_sensor_json(path: str | Path | None) -> dict[str, Any]:
"""Pull stitch / tracker fields from a sensor.json for export pipeline/meta.""" """Pull stitch / tracker / filter fields from a sensor.json for export pipeline/meta."""
pipeline: dict[str, Any] = { pipeline: dict[str, Any] = {
"stitch_resolution": None, "stitch_resolution": None,
"dis_mode": None, "dis_mode": None,
"hhd_downsample": None, "hhd_downsample": None,
"flow_ema_alpha": None,
"force_median_window": None,
"force_lowpass_cutoff_hz": None,
"config_ref": None, "config_ref": None,
} }
if path is None: if path is None:
@@ -622,6 +628,7 @@ def pipeline_from_sensor_json(path: str | Path | None) -> dict[str, Any]:
return pipeline return pipeline
camera = data.get("camera") if isinstance(data.get("camera"), dict) else {} camera = data.get("camera") if isinstance(data.get("camera"), dict) else {}
tracker = data.get("tracker") if isinstance(data.get("tracker"), dict) else {} tracker = data.get("tracker") if isinstance(data.get("tracker"), dict) else {}
filters = data.get("filters") if isinstance(data.get("filters"), dict) else {}
stitch = camera.get("resolution_stitch") stitch = camera.get("resolution_stitch")
if isinstance(stitch, (list, tuple)) and len(stitch) >= 2: if isinstance(stitch, (list, tuple)) and len(stitch) >= 2:
pipeline["stitch_resolution"] = [int(stitch[0]), int(stitch[1])] pipeline["stitch_resolution"] = [int(stitch[0]), int(stitch[1])]
@@ -635,6 +642,29 @@ def pipeline_from_sensor_json(path: str | Path | None) -> dict[str, Any]:
pipeline["hhd_downsample"] = int(tracker["hhd_downsample"]) pipeline["hhd_downsample"] = int(tracker["hhd_downsample"])
except (TypeError, ValueError): except (TypeError, ValueError):
pipeline["hhd_downsample"] = tracker.get("hhd_downsample") pipeline["hhd_downsample"] = tracker.get("hhd_downsample")
# Signal-conditioning settings change the reported force values, so they
# belong to the pipeline identity just like dis_mode / hhd_downsample.
if filters:
enabled = bool(filters.get("enabled", True))
for key in ("flow_ema_alpha", "force_lowpass_cutoff_hz"):
if key in filters:
try:
pipeline[key] = float(filters[key]) if enabled else 0.0
except (TypeError, ValueError):
pipeline[key] = filters.get(key)
elif key == "flow_ema_alpha":
pipeline[key] = 0.5 if enabled else 0.0
else:
pipeline[key] = 10.0 if enabled else 0.0
if "force_median_window" in filters:
try:
pipeline["force_median_window"] = (
int(filters["force_median_window"]) if enabled else 1
)
except (TypeError, ValueError):
pipeline["force_median_window"] = filters.get("force_median_window")
else:
pipeline["force_median_window"] = 3 if enabled else 1
return pipeline return pipeline
@@ -717,7 +747,8 @@ def build(
for k, v in extra_meta.items(): for k, v in extra_meta.items():
if k == "samples": if k == "samples":
continue continue
if k in ("stitch_resolution", "dis_mode", "hhd_downsample", "config_ref"): if k in ("stitch_resolution", "dis_mode", "hhd_downsample", "config_ref",
"flow_ema_alpha", "force_median_window", "force_lowpass_cutoff_hz"):
if k not in pipeline or pipeline[k] is None: if k not in pipeline or pipeline[k] is None:
pipeline[k] = v pipeline[k] = v
elif k in ("sensor_type", "sn", "hardware_rev", "gel_id"): elif k in ("sensor_type", "sn", "hardware_rev", "gel_id"):
+9
View File
@@ -293,6 +293,15 @@ class FingerRuntime:
capture = capture_source.cap capture = capture_source.cap
sensor = Sensor("external", config_name=config, verbose=verbose, mirror=mirror) sensor = Sensor("external", config_name=config, verbose=verbose, mirror=mirror)
if not is_live_camera(video):
# File playback: wall-clock cadence is not signal time, so pin the
# force-filter design rate to the configured camera fps.
try:
fps = float(geometry.config.get("camera", {}).get("fps", 0.0) or 0.0)
except (TypeError, ValueError):
fps = 0.0
if fps > 0:
sensor.configure(lowpass_sample_rate_hz=fps)
frame_processor = FrameUnwarper( frame_processor = FrameUnwarper(
domain.remap, domain.remap,
camera_matrix=geometry.source_K if geometry.dist_coeffs is not None else None, camera_matrix=geometry.source_K if geometry.dist_coeffs is not None else None,
+16 -1
View File
@@ -1,5 +1,7 @@
from __future__ import annotations from __future__ import annotations
import time
import cv2 import cv2
import numpy as np import numpy as np
from pathlib import Path from pathlib import Path
@@ -12,11 +14,24 @@ class CompoundEyeFrameProcessor:
def __init__(self, stitcher): def __init__(self, stitcher):
self.stitcher = stitcher self.stitcher = stitcher
# Per-call sub-stage timings (seconds), read by Sensor.get_img() for
# _last_stage_times. Keys: units, extract_tiles, stitch.
self.last_timings = {}
def process(self, frame): def process(self, frame):
t0 = time.perf_counter()
self.stitcher.get_stitch_unit(frame) self.stitcher.get_stitch_unit(frame)
t1 = time.perf_counter()
self.stitcher.extract_tiles() self.stitcher.extract_tiles()
return self.stitcher.stitch_rt_optimized() t2 = time.perf_counter()
stitched = self.stitcher.stitch_rt_optimized()
t3 = time.perf_counter()
self.last_timings = {
"units": t1 - t0,
"extract_tiles": t2 - t1,
"stitch": t3 - t2,
}
return stitched
class SingleCameraFrameProcessor: class SingleCameraFrameProcessor:
+45
View File
@@ -73,8 +73,49 @@ class nHHD(object):
raise ValueError("nHHD Solver works only for 2 and 3 dimensions") raise ValueError("nHHD Solver works only for 2 and 3 dimensions")
self._force_cpu_cache = None self._force_cpu_cache = None
self._force_ref_shape = None
ref_shape = kwargs.get('force_reference_grid')
if ref_shape is not None:
try:
ref_h, ref_w = int(ref_shape[0]), int(ref_shape[1])
except (TypeError, ValueError, IndexError):
ref_h = ref_w = 0
if ref_h > 0 and ref_w > 0:
self._force_ref_shape = (ref_h, ref_w)
self.psolver.prepare(False) self.psolver.prepare(False)
def _force_scale(self, shape):
"""Area/linear weights mapping grid sums at ``shape`` to the reference grid.
``Sn``/``Stx``/``Sty``/``Stor`` are *sums over grid pixels*, and the
torque term additionally uses pixel-index moment arms. When the flow is
downsampled by ``hhd_downsample=k`` before decomposition, the same
physical deformation therefore reports roughly ``1/k^2`` of the force
magnitude (and ``1/k^3`` of the torque) unless the sums are rescaled
back onto the reference grid.
returns: (area_scale, linear_scale); (1.0, 1.0) when no reference grid
was configured or the current grid already is the reference.
"""
ref = self._force_ref_shape
if ref is None or (int(shape[0]), int(shape[1])) == ref:
return 1.0, 1.0
cur_area = float(shape[0]) * float(shape[1])
if cur_area <= 0.0:
return 1.0, 1.0
area_scale = (float(ref[0]) * float(ref[1])) / cur_area
return area_scale, float(numpy.sqrt(area_scale))
def _apply_force_scale(self, shape):
"""Rescale aggregate force sums from a downsampled grid to the reference grid."""
area_scale, linear_scale = self._force_scale(shape)
if area_scale == 1.0 and linear_scale == 1.0:
return
self.Sn = float(self.Sn) * area_scale
self.Stx = float(self.Stx) * area_scale
self.Sty = float(self.Sty) * area_scale
self.Stor = float(self.Stor) * area_scale * linear_scale
def _normalize_mask(self, valid_mask, shape): def _normalize_mask(self, valid_mask, shape):
if valid_mask is None: if valid_mask is None:
return None return None
@@ -402,6 +443,8 @@ class nHHD(object):
cross_n = numpy.where(mask, cross_n, 0) cross_n = numpy.where(mask, cross_n, 0)
self.Stor = numpy.sum(cross_p) + numpy.sum(cross_n) self.Stor = numpy.sum(cross_p) + numpy.sum(cross_n)
self._apply_force_scale(shape)
# Optimized: pre-allocate output array with explicit dtype # Optimized: pre-allocate output array with explicit dtype
output = numpy.array([self.Sn, self.Stx, self.Sty, self.Stor], dtype=numpy.float32) output = numpy.array([self.Sn, self.Stx, self.Sty, self.Stor], dtype=numpy.float32)
@@ -515,6 +558,8 @@ class nHHD(object):
)) ))
self.Sn, self.Stx, self.Sty, self.Stor = Sn, Stx, Sty, Stor self.Sn, self.Stx, self.Sty, self.Stor = Sn, Stx, Sty, Stor
self._apply_force_scale(shape)
Sn, Stx, Sty, Stor = self.Sn, self.Stx, self.Sty, self.Stor
output = numpy.array([Sn, Stx, Sty, Stor], dtype=numpy.float32) output = numpy.array([Sn, Stx, Sty, Stor], dtype=numpy.float32)
# Keep stable CPU copies for get_normal()/get_shear() so callers get this frame's # Keep stable CPU copies for get_normal()/get_shear() so callers get this frame's
+59 -10
View File
@@ -29,6 +29,39 @@ def compute_boundary_mask(active):
return active & ~fully_surrounded return active & ~fully_surrounded
def build_masked_factor(lap):
"""Factorize the masked Laplacian for fast repeated triangular solves.
The reduced system is a symmetric M-matrix (graph Laplacian of the active
pixels with an extra Dirichlet pin), so SuperLU can skip partial pivoting
(``SymmetricMode`` + ``diag_pivot_thresh=0``), which is ~2x faster than the
default pivoting factorization. Factoring in float32 halves the memory
traffic of the triangular solves; relative error against the float64
reference stays around 1e-7 for these systems.
``rhs_dtype`` is returned because ``SuperLU.solve`` refuses to cast a
float64 right-hand side down to a float32 factor.
lap: (N, N) symmetric sparse Laplacian (any scipy dtype)
returns: (solve_callable, rhs_dtype)
"""
lap_csc = lap.tocsc()
lap32 = lap_csc.astype(numpy.float32, copy=False)
for diag_pivot_thresh in (0.0, 1.0):
try:
lu = sparse_linalg.splu(
lap32,
permc_spec="MMD_AT_PLUS_A",
diag_pivot_thresh=diag_pivot_thresh,
options=dict(SymmetricMode=True),
)
return lu.solve, numpy.float32
except Exception:
continue
# Last resort: the original pivoting LU on the float64 matrix.
return sparse_linalg.factorized(lap_csc), numpy.float64
def sample_boundary_coords(boundary_coords, config: MaskedPoissonConfig): def sample_boundary_coords(boundary_coords, config: MaskedPoissonConfig):
"""Subsample boundary pixel coordinates for capacitance correction.""" """Subsample boundary pixel coordinates for capacitance correction."""
if boundary_coords.size == 0: if boundary_coords.size == 0:
@@ -53,6 +86,7 @@ class MaskedDomain:
"mask", "mask",
"index_map", "index_map",
"factor", "factor",
"factor_dtype",
"lap", "lap",
"num_active", "num_active",
"pinned", "pinned",
@@ -75,11 +109,13 @@ class MaskedDomain:
pinned, pinned,
active_coords, active_coords,
active_linear_indices, active_linear_indices,
factor_dtype=numpy.float32,
): ):
self.shape = shape self.shape = shape
self.mask = mask self.mask = mask
self.index_map = index_map self.index_map = index_map
self.factor = factor self.factor = factor
self.factor_dtype = numpy.dtype(factor_dtype)
self.lap = lap self.lap = lap
self.num_active = num_active self.num_active = num_active
self.pinned = pinned self.pinned = pinned
@@ -156,7 +192,7 @@ class MaskedDomain:
data.append(diag) data.append(diag)
lap = sparse.csc_matrix((data, (rows, cols)), shape=(num_active, num_active), dtype=numpy.float64) lap = sparse.csc_matrix((data, (rows, cols)), shape=(num_active, num_active), dtype=numpy.float64)
factor = sparse_linalg.factorized(lap) factor, factor_dtype = build_masked_factor(lap)
return cls( return cls(
shape=active.shape, shape=active.shape,
mask=active.copy(), mask=active.copy(),
@@ -167,6 +203,7 @@ class MaskedDomain:
pinned=pinned, pinned=pinned,
active_coords=active_coords, active_coords=active_coords,
active_linear_indices=active_linear_indices, active_linear_indices=active_linear_indices,
factor_dtype=factor_dtype,
) )
def matches(self, mask): def matches(self, mask):
@@ -179,13 +216,20 @@ class MaskedDomain:
f: (H, W) scalar source field f: (H, W) scalar source field
returns: (N,) RHS on active pixels; empty if no active pixels returns: (N,) RHS on active pixels; empty if no active pixels
The returned dtype matches the factorization (see ``factor_dtype``):
``SuperLU.solve`` will not downcast a float64 RHS for a float32 factor.
""" """
if self.num_active == 0: if self.num_active == 0:
return numpy.zeros(0, dtype=numpy.float64) return numpy.zeros(0, dtype=self.factor_dtype)
if self._rhs_buffer is None or self._rhs_buffer.shape != (self.num_active,): if (
self._rhs_buffer = numpy.empty(self.num_active, dtype=numpy.float64) self._rhs_buffer is None
or self._rhs_buffer.shape != (self.num_active,)
or self._rhs_buffer.dtype != self.factor_dtype
):
self._rhs_buffer = numpy.empty(self.num_active, dtype=self.factor_dtype)
numpy.take( numpy.take(
numpy.asarray(f, dtype=numpy.float64).ravel(), numpy.asarray(f, dtype=self.factor_dtype).ravel(),
self.active_linear_indices, self.active_linear_indices,
out=self._rhs_buffer, out=self._rhs_buffer,
) )
@@ -197,11 +241,15 @@ class MaskedDomain:
def prepare_rhs_pair(self, f0, f1): def prepare_rhs_pair(self, f0, f1):
"""Extract two gauge-fixed RHS vectors into one reusable (N, 2) buffer.""" """Extract two gauge-fixed RHS vectors into one reusable (N, 2) buffer."""
if self.num_active == 0: if self.num_active == 0:
return numpy.zeros((0, 2), dtype=numpy.float64) return numpy.zeros((0, 2), dtype=self.factor_dtype)
if self._rhs_pair_buffer is None or self._rhs_pair_buffer.shape != (self.num_active, 2): if (
self._rhs_pair_buffer = numpy.empty((self.num_active, 2), dtype=numpy.float64) self._rhs_pair_buffer is None
flat0 = numpy.asarray(f0, dtype=numpy.float64).ravel() or self._rhs_pair_buffer.shape != (self.num_active, 2)
flat1 = numpy.asarray(f1, dtype=numpy.float64).ravel() or self._rhs_pair_buffer.dtype != self.factor_dtype
):
self._rhs_pair_buffer = numpy.empty((self.num_active, 2), dtype=self.factor_dtype)
flat0 = numpy.asarray(f0, dtype=self.factor_dtype).ravel()
flat1 = numpy.asarray(f1, dtype=self.factor_dtype).ravel()
numpy.take(flat0, self.active_linear_indices, out=self._rhs_pair_buffer[:, 0]) numpy.take(flat0, self.active_linear_indices, out=self._rhs_pair_buffer[:, 0])
numpy.take(flat1, self.active_linear_indices, out=self._rhs_pair_buffer[:, 1]) numpy.take(flat1, self.active_linear_indices, out=self._rhs_pair_buffer[:, 1])
self._rhs_pair_buffer -= self._rhs_pair_buffer.mean(axis=0, keepdims=True) self._rhs_pair_buffer -= self._rhs_pair_buffer.mean(axis=0, keepdims=True)
@@ -255,6 +303,7 @@ class MaskedDomain:
"mask": self.mask, "mask": self.mask,
"index_map": self.index_map, "index_map": self.index_map,
"factor": self.factor, "factor": self.factor,
"factor_dtype": self.factor_dtype,
"lap": self.lap, "lap": self.lap,
"num_active": self.num_active, "num_active": self.num_active,
"pinned": self.pinned, "pinned": self.pinned,
+5 -1
View File
@@ -33,7 +33,11 @@ class MaskedSparseSolver:
try: try:
sol_pair = -domain.factor(rhs_pair) # (N, 2) reduced-system potentials sol_pair = -domain.factor(rhs_pair) # (N, 2) reduced-system potentials
except Exception: except Exception:
# Some factorizations need contiguous 1-D right-hand sides.
sol_pair = -numpy.column_stack( sol_pair = -numpy.column_stack(
[domain.factor(rhs_pair[:, 0]), domain.factor(rhs_pair[:, 1])] [
domain.factor(numpy.ascontiguousarray(rhs_pair[:, 0])),
domain.factor(numpy.ascontiguousarray(rhs_pair[:, 1])),
]
) )
return domain.scatter_pair(sol_pair, f0) return domain.scatter_pair(sol_pair, f0)
+363 -36
View File
@@ -26,8 +26,22 @@ from .backend_availability import (check_cupy_available, check_opencl_available)
from .frame_processors import create_frame_processor from .frame_processors import create_frame_processor
from .signal_processing import ( from .signal_processing import (
DEFAULT_SAMPLE_RATE_HZ, DEFAULT_SAMPLE_RATE_HZ,
CausalMedianFilter,
LowpassFilter, LowpassFilter,
SampleRateEstimator,
) )
# Default signal-conditioning settings (config: top-level "filters" block).
# Measured 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, cuts steady-press force
# jitter by roughly half and removes ~85% of the heavy-tailed spikes, while also
# stabilising vnormal/contact (which a force-level filter cannot do).
# The force low-pass is OFF by default: on top of the EMA it added no measurable
# noise benefit at the same latency, so it is kept as an opt-in stage for sites
# with different noise characteristics.
DEFAULT_FORCE_LOWPASS_HZ = 0.0
DEFAULT_FORCE_MEDIAN_WINDOW = 3
DEFAULT_FLOW_EMA_ALPHA = 0.5
from .capture import ( from .capture import (
CaptureConfig, CaptureConfig,
CaptureSource, CaptureSource,
@@ -154,16 +168,92 @@ class Sensor:
self.calibration: Optional[cal_mod.Calibration] = None self.calibration: Optional[cal_mod.Calibration] = None
self._force_cal_sensor_name: Optional[str] = None self._force_cal_sensor_name: Optional[str] = None
self._init_force_calibration() self._init_force_calibration()
self._lowpass_enabled = False
self._lowpass_cutoff_hz = 0.0 # ---------------------------------------------------------------
self._lowpass_sample_rate_hz = float(DEFAULT_SAMPLE_RATE_HZ) # 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.
# Filtering is ON by default; set filters.enabled=false (or a per-key
# zero) to get the unfiltered chain.
filters_cfg = self._config.get("filters")
if not isinstance(filters_cfg, dict):
filters_cfg = {}
self._filters_enabled = bool(filters_cfg.get("enabled", True))
self._lowpass_order = 2 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)
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(filters_cfg.get("force_lowpass_order", 2)))
rate_cfg = filters_cfg.get("force_lowpass_sample_rate_hz", "auto")
if isinstance(rate_cfg, str):
self._lowpass_rate_auto = rate_cfg.strip().lower() in ("auto", "", "measured")
else:
rate = float(rate_cfg) if rate_cfg is not None else 0.0
self._lowpass_rate_auto = rate <= 0.0
if not self._lowpass_rate_auto:
self._lowpass_sample_rate_hz = rate
if self._filters_enabled:
self._lowpass_cutoff_hz = float(
filters_cfg.get("force_lowpass_cutoff_hz", DEFAULT_FORCE_LOWPASS_HZ)
)
except (TypeError, ValueError):
self._lowpass_order = 2
self._lowpass_sample_rate_hz = self._config_fps
self._lowpass_rate_auto = True
self._lowpass_cutoff_hz = (
DEFAULT_FORCE_LOWPASS_HZ if self._filters_enabled else 0.0
)
if self._lowpass_sample_rate_hz <= 0:
self._lowpass_sample_rate_hz = self._config_fps
if not (0.0 <= self._lowpass_cutoff_hz < 0.5 * self._lowpass_sample_rate_hz):
self._lowpass_cutoff_hz = (
DEFAULT_FORCE_LOWPASS_HZ
if (self._filters_enabled and 0.0 < DEFAULT_FORCE_LOWPASS_HZ < 0.5 * self._lowpass_sample_rate_hz)
else 0.0
)
self._lowpass_enabled = self._lowpass_cutoff_hz > 0
self._force_filter = LowpassFilter( self._force_filter = LowpassFilter(
cutoff_hz=self._lowpass_cutoff_hz, cutoff_hz=self._lowpass_cutoff_hz,
sample_rate_hz=self._lowpass_sample_rate_hz, sample_rate_hz=self._lowpass_sample_rate_hz,
order=self._lowpass_order, order=self._lowpass_order,
n_channels=3, 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"
self._force_median_window = 1
if self._filters_enabled:
try:
self._force_median_window = max(
1, int(filters_cfg.get("force_median_window", DEFAULT_FORCE_MEDIAN_WINDOW))
)
except (TypeError, ValueError):
self._force_median_window = DEFAULT_FORCE_MEDIAN_WINDOW
self._force_median = CausalMedianFilter(window=self._force_median_window, n_channels=3)
self._flow_ema_alpha = 1.0
if self._filters_enabled:
try:
alpha = float(filters_cfg.get("flow_ema_alpha", DEFAULT_FLOW_EMA_ALPHA))
except (TypeError, ValueError):
alpha = DEFAULT_FLOW_EMA_ALPHA
if 0.0 < alpha < 1.0:
self._flow_ema_alpha = alpha
self._ema_flow = None
if self.verbose: if self.verbose:
if self.calibration is not None and self.calibration.source_path is not None: if self.calibration is not None and self.calibration.source_path is not None:
print(f"Force calibration: {self.calibration.source_path}") print(f"Force calibration: {self.calibration.source_path}")
@@ -182,6 +272,13 @@ class Sensor:
self.valid_mask_raw = None self.valid_mask_raw = None
self.valid_mask = None self.valid_mask = None
self.valid_mask_hhd = None self.valid_mask_hhd = None
# Scratch buffers reused by _compute_flow_means / compute_hhd.
self._flow_mag_buf = None
self._flow_mask_u8 = None
self._hhd_vn_buf = None
self._hhd_vs_buf = None
self._hhd_flow_ds_buf = None
self._hhd_mask_f32 = None
self.mask_stats = { self.mask_stats = {
"coverage": 0.0, "coverage": 0.0,
"flow_mean_valid": 0.0, "flow_mean_valid": 0.0,
@@ -194,10 +291,16 @@ class Sensor:
self._last_capture_time = 0.0 self._last_capture_time = 0.0
self._last_processing_time = 0.0 self._last_processing_time = 0.0
self._last_stage_times = { self._last_stage_times = {
"extract_tiles": 0.0, # Frame-post-processing roll-ups (seconds).
"stitch": 0.0, "extract_tiles": 0.0, # sensor units + tile extraction
"resize": 0.0, "stitch": 0.0, # feature/feather stitch of the extracted tiles
"mirror": 0.0, "resize": 0.0, # BGR->gray + resize to resolution_stitch + mirror
# Finer breakdown.
"store": 0.0, # frame reference/copy
"units": 0.0, # compound-eye unit (sub-image) extraction
"gray": 0.0, # BGR->gray conversion
"mirror": 0.0, # horizontal flip
"mask": 0.0, # valid-mask resize/cache update
"astype": 0.0, "astype": 0.0,
} }
@@ -218,6 +321,7 @@ class Sensor:
except (TypeError, ValueError): except (TypeError, ValueError):
self._hhd_downsample = 1 self._hhd_downsample = 1
self._hhd_grid_shape = None self._hhd_grid_shape = None
self._hhd_ref_shape = None
cfg_backend = self.__config_tracker.get("backend", None) cfg_backend = self.__config_tracker.get("backend", None)
self.poisson_backend = cfg_backend if cfg_backend is not None else backend self.poisson_backend = cfg_backend if cfg_backend is not None else backend
self.masked_poisson_solver = self.__config_tracker.get("masked_poisson_solver", "sparse") self.masked_poisson_solver = self.__config_tracker.get("masked_poisson_solver", "sparse")
@@ -244,6 +348,15 @@ class Sensor:
print("Sensor initialized in external-frame mode.") print("Sensor initialized in external-frame mode.")
else: else:
self._open_video_source(override, box) self._open_video_source(override, box)
if not self._is_live_camera:
# A file source is replayed at whatever speed the CPU manages,
# so wall-clock intervals do not reflect signal time: keep the
# configured camera fps for the force low-pass design.
self._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)
if self.verbose: if self.verbose:
print(f" Video source: {vid_src}") print(f" Video source: {vid_src}")
@@ -253,6 +366,14 @@ class Sensor:
# print(f" CUDA acceleration: {self.cuda}") # print(f" CUDA acceleration: {self.cuda}")
print(f" CuPy available: {self.CUPY_AVAILABLE}") print(f" CuPy available: {self.CUPY_AVAILABLE}")
print(f" OpenCL available: {self.OPENCL_AVAILABLE}") 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})"
)
@@ -313,17 +434,31 @@ class Sensor:
def _compute_flow_means(self, flow): def _compute_flow_means(self, flow):
if flow is None: if flow is None:
return return
mag = np.linalg.norm(flow, axis=-1) shape = flow.shape[:2]
mag = self._flow_mag_buf
if mag is None or mag.shape != shape or mag.dtype != np.float32:
mag = np.empty(shape, dtype=np.float32)
self._flow_mag_buf = mag
# cv2.magnitude + cv2.mean(mask=...) avoid the (H, W) temporaries and the
# boolean fancy-index gather that np.linalg.norm + mag[mask].mean() need.
cv2.magnitude(flow[:, :, 0], flow[:, :, 1], mag)
mask = self.valid_mask mask = self.valid_mask
if mask is None or mask.shape != mag.shape: if mask is None or mask.shape != shape:
self.mask_stats["flow_mean_valid"] = float(mag.mean()) if mag.size else 0.0 self.mask_stats["flow_mean_valid"] = float(cv2.mean(mag)[0]) if mag.size else 0.0
self.mask_stats["flow_mean_invalid"] = 0.0 self.mask_stats["flow_mean_invalid"] = 0.0
return return
valid = mask.astype(bool) mask_u8 = self._flow_mask_u8
invalid = ~valid if mask_u8 is None or mask_u8.shape != shape or mask_u8.dtype != np.uint8:
self.mask_stats["flow_mean_valid"] = float(mag[valid].mean()) if np.any(valid) else 0.0 mask_u8 = np.empty(shape, dtype=np.uint8)
self.mask_stats["flow_mean_invalid"] = float(mag[invalid].mean()) if np.any(invalid) else 0.0 self._flow_mask_u8 = mask_u8
np.copyto(mask_u8, mask, casting="unsafe")
np.multiply(mask_u8, 255, out=mask_u8)
self.mask_stats["flow_mean_valid"] = float(cv2.mean(mag, mask=mask_u8)[0])
np.subtract(255, mask_u8, out=mask_u8)
self.mask_stats["flow_mean_invalid"] = float(cv2.mean(mag, mask=mask_u8)[0])
def get_img(self, frame=None, already_processed=False, valid_mask=None, already_gray=False): def get_img(self, frame=None, already_processed=False, valid_mask=None, already_gray=False):
""" """
@@ -362,6 +497,7 @@ class Sensor:
self.frame = frame if (already_processed and isinstance(frame, np.ndarray)) else ( self.frame = frame if (already_processed and isinstance(frame, np.ndarray)) else (
frame.copy() if isinstance(frame, np.ndarray) else frame frame.copy() if isinstance(frame, np.ndarray) else frame
) )
t_after_store = time.perf_counter()
if already_processed: if already_processed:
img_bgr = frame img_bgr = frame
gray = None gray = None
@@ -383,8 +519,6 @@ class Sensor:
gray = None gray = None
t_after_extract = time.perf_counter() t_after_extract = time.perf_counter()
t_after_stitch = time.perf_counter()
if gray is None: if gray is None:
self.img_size_raw = img_bgr.shape self.img_size_raw = img_bgr.shape
self._img_bgr_raw = img_bgr self._img_bgr_raw = img_bgr
@@ -392,6 +526,8 @@ class Sensor:
else: else:
self.img_size_raw = gray.shape self.img_size_raw = gray.shape
self._img_bgr_raw = None self._img_bgr_raw = None
t_after_gray = time.perf_counter()
res = tuple(self._config_resolution_stitch) res = tuple(self._config_resolution_stitch)
if self._img_gray_buf is None or self._img_gray_buf.shape != (res[1], res[0]): if self._img_gray_buf is None or self._img_gray_buf.shape != (res[1], res[0]):
self._img_gray_buf = np.empty((res[1], res[0]), dtype=np.uint8) self._img_gray_buf = np.empty((res[1], res[0]), dtype=np.uint8)
@@ -400,15 +536,22 @@ class Sensor:
if self.mirror: if self.mirror:
cv2.flip(self._img_gray_buf, 1, dst=self._img_gray_buf) cv2.flip(self._img_gray_buf, 1, dst=self._img_gray_buf)
t_after_mirror = time.perf_counter()
self.img = self._img_gray_buf self.img = self._img_gray_buf
self._prepare_valid_mask(valid_mask, gray.shape, self.img.shape) self._prepare_valid_mask(valid_mask, gray.shape, self.img.shape)
t_processing_end = time.perf_counter() t_processing_end = time.perf_counter()
self._last_processing_time = t_processing_end - t_processing_start self._last_processing_time = t_processing_end - t_processing_start
self._last_stage_times["extract_tiles"] = t_after_extract - t_processing_start self._record_stage_times(
self._last_stage_times["stitch"] = t_after_stitch - t_after_extract t_processing_start=t_processing_start,
self._last_stage_times["resize"] = t_processing_end - t_after_stitch t_after_store=t_after_store,
t_after_extract=t_after_extract,
t_after_gray=t_after_gray,
t_after_resize=t_after_resize,
t_after_mirror=t_after_mirror,
t_processing_end=t_processing_end,
)
# FPS calculation # FPS calculation
self._fps = 1.0 / (time.perf_counter() - self.last_log_time) self._fps = 1.0 / (time.perf_counter() - self.last_log_time)
@@ -416,6 +559,48 @@ class Sensor:
return self.img.copy() return self.img.copy()
def _record_stage_times(
self,
*,
t_processing_start,
t_after_store,
t_after_extract,
t_after_gray,
t_after_resize,
t_after_mirror,
t_processing_end,
):
"""Record per-stage frame-post-processing timings (seconds).
``extract_tiles``/``stitch`` come from the frame processor, which times
its own sub-steps; the remaining keys are measured here. The roll-ups
keep the historical key names while no longer collapsing ``stitch`` to
zero: previously ``t_after_stitch`` was sampled immediately after
``t_after_extract``, so ``stitch`` was always ~0 s and ``resize``
silently absorbed the whole stitch cost.
"""
stage = self._last_stage_times
processor_timings = getattr(self.frame_processor, "last_timings", None) or {}
units_s = float(processor_timings.get("units", 0.0))
extract_s = float(processor_timings.get("extract_tiles", 0.0))
stitch_s = float(processor_timings.get("stitch", 0.0))
processor_total_s = t_after_extract - t_after_store
stage["store"] = t_after_store - t_processing_start
stage["units"] = units_s
stage["gray"] = t_after_gray - t_after_extract
stage["resize"] = t_after_resize - t_after_gray
stage["mirror"] = t_after_mirror - t_after_resize
stage["mask"] = t_processing_end - t_after_mirror
if processor_timings:
stage["extract_tiles"] = units_s + extract_s
stage["stitch"] = stitch_s
else:
# Non-compound-eye processors are a single opaque step.
stage["extract_tiles"] = processor_total_s
stage["stitch"] = 0.0
def _get_img_bgr(self): def _get_img_bgr(self):
"""Produce a resized+mirrored BGR image on demand (for display).""" """Produce a resized+mirrored BGR image on demand (for display)."""
if self._img_bgr_raw is None: if self._img_bgr_raw is None:
@@ -512,9 +697,73 @@ 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)
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
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)
@property
def force_sample_rate_source(self) -> str:
"""``config``, ``measured`` or ``config (replay)``."""
return str(getattr(self, "_force_rate_source", "config"))
def compute_hhd(self): def compute_hhd(self):
flow = self.flow flow = self.flow
@@ -543,7 +792,11 @@ class Sensor:
if k > 1: if k > 1:
ds_w = max(1, orig_w // k) ds_w = max(1, orig_w // k)
ds_h = max(1, orig_h // k) ds_h = max(1, orig_h // k)
flow_ds = cv2.resize(flow, (ds_w, ds_h), interpolation=cv2.INTER_AREA) flow_ds = self._hhd_flow_ds_buf
if flow_ds is None or flow_ds.shape != (ds_h, ds_w, 2) or flow_ds.dtype != np.float32:
flow_ds = np.empty((ds_h, ds_w, 2), dtype=np.float32)
self._hhd_flow_ds_buf = flow_ds
cv2.resize(flow, (ds_w, ds_h), dst=flow_ds, interpolation=cv2.INTER_AREA)
if mask_ds is not None and ( if mask_ds is not None and (
mask_ds.shape[1] != ds_w or mask_ds.shape[0] != ds_h mask_ds.shape[1] != ds_w or mask_ds.shape[0] != ds_h
): ):
@@ -552,8 +805,13 @@ class Sensor:
flow_ds = flow flow_ds = flow
ds_shape = flow_ds.shape[:2] ds_shape = flow_ds.shape[:2]
ref_shape = (orig_h, orig_w)
self.valid_mask_hhd = mask_ds self.valid_mask_hhd = mask_ds
if self.__hhd is None or self._hhd_grid_shape != ds_shape: if (
self.__hhd is None
or self._hhd_grid_shape != ds_shape
or self._hhd_ref_shape != ref_shape
):
spacings = (18 * k, 18 * k) spacings = (18 * k, 18 * k)
self.__hhd = nHHD( self.__hhd = nHHD(
cuda=self.cuda, cuda=self.cuda,
@@ -561,6 +819,9 @@ class Sensor:
spacings=spacings, spacings=spacings,
poisson_backend=self.poisson_backend, poisson_backend=self.poisson_backend,
debug=self.debug, debug=self.debug,
# Rescale grid-summed force aggregates back onto the full-resolution
# flow grid so Sn/Stx/Sty/Stor stay comparable across hhd_downsample.
force_reference_grid=ref_shape,
masked_poisson_solver=self.masked_poisson_solver, masked_poisson_solver=self.masked_poisson_solver,
masked_poisson_max_boundary=self.masked_poisson_max_boundary, masked_poisson_max_boundary=self.masked_poisson_max_boundary,
masked_poisson_boundary_stride=self.masked_poisson_boundary_stride, masked_poisson_boundary_stride=self.masked_poisson_boundary_stride,
@@ -569,29 +830,46 @@ class Sensor:
masked_poisson_reg=self.masked_poisson_reg, masked_poisson_reg=self.masked_poisson_reg,
) )
self._hhd_grid_shape = ds_shape self._hhd_grid_shape = ds_shape
self._hhd_ref_shape = ref_shape
_, forces = self.__hhd.get_force(flow_ds, valid_mask=mask_ds) _, forces = self.__hhd.get_force(flow_ds, valid_mask=mask_ds)
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._apply_force_filter() self._apply_force_filter()
vnormal_ds = self.__hhd.get_normal() vnormal_ds = self.__hhd.get_normal()
vshear_ds = self.__hhd.get_shear() vshear_ds = self.__hhd.get_shear()
if k > 1: if k > 1:
self.vnormal = cv2.resize(vnormal_ds, (orig_w, orig_h), interpolation=cv2.INTER_LINEAR) # Upsample into reusable buffers; the solver keeps owning its own
self.vshear = cv2.resize(vshear_ds, (orig_w, orig_h), interpolation=cv2.INTER_LINEAR) # arrays, so nothing downstream can alias solver state.
vn = self._hhd_vn_buf
vs = self._hhd_vs_buf
if vn is None or vn.shape != (orig_h, orig_w, 2) or vn.dtype != np.float32:
vn = np.empty((orig_h, orig_w, 2), dtype=np.float32)
vs = np.empty((orig_h, orig_w, 2), dtype=np.float32)
self._hhd_vn_buf = vn
self._hhd_vs_buf = vs
cv2.resize(vnormal_ds, (orig_w, orig_h), dst=vn, interpolation=cv2.INTER_LINEAR)
cv2.resize(vshear_ds, (orig_w, orig_h), dst=vs, interpolation=cv2.INTER_LINEAR)
self.vnormal = vn
self.vshear = vs
else: else:
self.vnormal = vnormal_ds self.vnormal = vnormal_ds.copy()
self.vshear = vshear_ds self.vshear = vshear_ds.copy()
if self.valid_mask is not None: if self.valid_mask is not None:
active = self.valid_mask.astype(bool) # In-place multiply by a 0/1 float mask instead of
self.vnormal = self.vnormal.copy() # ``x.copy(); x[~active] = 0`` (copy + boolean gather/scatter).
self.vshear = self.vshear.copy() m3 = self._hhd_mask_f32
self.vnormal[~active] = 0 if m3 is None or m3.shape != (orig_h, orig_w, 1) or m3.dtype != np.float32:
self.vshear[~active] = 0 m3 = np.empty((orig_h, orig_w, 1), dtype=np.float32)
self._hhd_mask_f32 = m3
np.copyto(m3[:, :, 0], self.valid_mask, casting="unsafe")
np.multiply(self.vnormal, m3, out=self.vnormal)
np.multiply(self.vshear, m3, out=self.vshear)
self.compute_deformation_valid = True self.compute_deformation_valid = True
@@ -700,6 +978,9 @@ class Sensor:
"""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_filter.reset()
if getattr(self, "_force_median", None) is not None:
self._force_median.reset()
self._ema_flow = None
if self.img is not None: if self.img is not None:
self.initial_img = self.img.copy() self.initial_img = self.img.copy()
@@ -743,6 +1024,8 @@ class Sensor:
lowpass_sample_rate_hz=None, lowpass_sample_rate_hz=None,
lowpass_order=None, lowpass_order=None,
lowpass_enabled=None, lowpass_enabled=None,
flow_ema_alpha=None,
force_median_window=None,
hhd_downsample=None, hhd_downsample=None,
): ):
"""Update runtime-safe settings without reconnecting the camera. """Update runtime-safe settings without reconnecting the camera.
@@ -754,10 +1037,18 @@ class Sensor:
value enables filtering unless ``lowpass_enabled=False``. value enables filtering unless ``lowpass_enabled=False``.
lowpass_sample_rate_hz : float, optional lowpass_sample_rate_hz : float, optional
Assumed force sample rate for the Butterworth design (default 30). Assumed force sample rate for the Butterworth design (default 30).
Set this to the actual output rate of the deployment.
lowpass_order : int, optional lowpass_order : int, optional
Butterworth order (default 2). Butterworth order (default 2).
lowpass_enabled : bool, optional lowpass_enabled : bool, optional
Explicit on/off for the causal force filter. Explicit on/off for the causal force filter.
flow_ema_alpha : float, optional
EMA coefficient applied to the optical-flow field before HHD.
``1.0`` disables it; smaller values smooth more (0.5 is the default
and costs about one frame of delay at 30 fps).
force_median_window : int, optional
Causal sliding-median width applied to the raw force triple before
the low-pass. ``<= 1`` disables it; 3 is the default.
hhd_downsample : int, optional hhd_downsample : int, optional
Integer ``>= 1``. Rebuilds the HHD solver on the next frame. Integer ``>= 1``. Rebuilds the HHD solver on the next frame.
""" """
@@ -768,10 +1059,26 @@ class Sensor:
if lowpass_enabled is None: if lowpass_enabled is None:
self._lowpass_enabled = self._lowpass_cutoff_hz > 0 self._lowpass_enabled = self._lowpass_cutoff_hz > 0
if lowpass_sample_rate_hz is not None: 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) rate = float(lowpass_sample_rate_hz)
if rate <= 0: except (TypeError, ValueError):
raise ValueError("lowpass_sample_rate_hz must be > 0") auto = True
else:
auto = rate <= 0.0
self._lowpass_rate_auto = auto
if not auto:
self._lowpass_sample_rate_hz = rate 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 filter_changed = True
if lowpass_order is not None: if lowpass_order is not None:
order = int(lowpass_order) order = int(lowpass_order)
@@ -783,6 +1090,22 @@ class Sensor:
self._lowpass_enabled = bool(lowpass_enabled) self._lowpass_enabled = bool(lowpass_enabled)
filter_changed = True 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: if filter_changed:
cutoff = self._lowpass_cutoff_hz if self._lowpass_enabled else 0.0 cutoff = self._lowpass_cutoff_hz if self._lowpass_enabled else 0.0
self._force_filter.configure( self._force_filter.configure(
@@ -802,6 +1125,7 @@ class Sensor:
self._hhd_downsample = k self._hhd_downsample = k
self.__hhd = None self.__hhd = None
self._hhd_grid_shape = None self._hhd_grid_shape = None
self._hhd_ref_shape = None
self._update_hhd_mask_cache() self._update_hhd_mask_cache()
return self return self
@@ -877,10 +1201,13 @@ class Sensor:
def _apply_force_filter(self): def _apply_force_filter(self):
raw = np.asarray(self._force_raw, dtype=np.float64) raw = np.asarray(self._force_raw, dtype=np.float64)
# 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: if self._lowpass_enabled and self._lowpass_cutoff_hz > 0:
filtered = self._force_filter.process(raw) raw = self._force_filter.process(raw)
fn, fx, fy = (float(v) for v in filtered)
else:
fn, fx, fy = float(raw[0]), float(raw[1]), float(raw[2]) fn, fx, fy = float(raw[0]), float(raw[1]), float(raw[2])
# Apply loaded force_calibration.json when present; otherwise keep raw. # Apply loaded force_calibration.json when present; otherwise keep raw.
+114
View File
@@ -15,6 +15,8 @@ Prefer ``Sensor.calibration`` / ``read_info(FNORMAL, …)`` in new code.
from __future__ import annotations from __future__ import annotations
import collections
import time
from dataclasses import dataclass from dataclasses import dataclass
import numpy as np import numpy as np
@@ -312,3 +314,115 @@ class LowpassFilter:
self.reset(x) self.reset(x)
y, self._zi = sosfilt(self._sos, x[None, :], zi=self._zi, axis=0) y, self._zi = sosfilt(self._sos, x[None, :], zi=self._zi, axis=0)
return np.asarray(y[0], dtype=np.float64) return np.asarray(y[0], dtype=np.float64)
class CausalMedianFilter:
"""Causal sliding-median over the last ``window`` samples, one sample at a time.
The optical-flow force chain produces heavy-tailed jitter (rare multi-sigma
spikes). A linear low-pass spreads such a spike over several frames instead
of deleting it, whereas a median removes it outright - measured on idle
data, a 5-sample median cut the jitter 4x better than a moving average of
the same width.
``window <= 1`` disables the filter (pass-through). Until ``window`` samples
have arrived the median of the available samples is returned, so the output
is well defined from the first frame.
"""
def __init__(self, window: int = 1, n_channels: int = 3):
self.n_channels = int(n_channels)
self.window = 1
self._buf = None
self.configure(window)
@property
def enabled(self) -> bool:
return self.window > 1
def configure(self, window: int | None = None) -> None:
if window is not None:
try:
self.window = max(1, int(window))
except (TypeError, ValueError):
self.window = 1
self._buf = None
def reset(self, x0=None) -> None:
"""Drop the window history (e.g. after a tracker reset)."""
self._buf = None
def process(self, values) -> np.ndarray:
"""Filter one multi-channel sample. Returns float64 array of shape (n_channels,)."""
x = np.asarray(values, dtype=np.float64).reshape(-1)
if x.size != self.n_channels:
raise ValueError(f"expected {self.n_channels} channels, got {x.size}")
if not self.enabled:
return x.copy()
if self._buf is None or self._buf.shape[1] != x.size:
self._buf = x[None, :].copy()
return x.copy()
self._buf = np.vstack((self._buf, x[None, :]))[-self.window:]
return np.median(self._buf, axis=0)
class SampleRateEstimator:
"""Robust online estimate of the rate at which :meth:`tick` is called.
Uses the *median* of the most recent inter-call intervals, so one stall or
scheduling hiccup does not poison the estimate. :attr:`rate_hz` returns the
seed value until ``min_samples`` intervals have been observed.
This exists because a fixed floor filter designed for 30 Hz behaves
differently at 60 Hz: the cutoff stays put in Hz but the per-frame
smoothing halves. Measuring the achieved rate keeps the filter identical in
the time domain whatever the deployment runs at.
"""
def __init__(
self,
initial_hz: float = 30.0,
window: int = 45,
min_samples: int = 8,
max_interval_s: float = 1.0,
):
self.initial_hz = float(initial_hz) if initial_hz else 30.0
self.window = max(3, int(window))
self.min_samples = max(2, int(min_samples))
self.max_interval_s = float(max_interval_s)
self.reset()
def reset(self, initial_hz: float | None = None) -> None:
if initial_hz is not None:
self.initial_hz = float(initial_hz) if initial_hz else 30.0
self._dts: collections.deque = collections.deque(maxlen=self.window)
self._last = None
self._rate = self.initial_hz
self._measured = False
@property
def rate_hz(self) -> float:
return self._rate
@property
def measured(self) -> bool:
"""True once the estimate comes from real intervals."""
return self._measured
def tick(self, now: float | None = None) -> float:
"""Register one sample and return the current rate estimate."""
now = time.perf_counter() if now is None else float(now)
last, self._last = self._last, now
if last is None:
return self._rate
dt = now - last
if not (1e-5 < dt <= self.max_interval_s):
# Stalls, pauses and the first frame carry no rate information.
return self._rate
self._dts.append(dt)
if len(self._dts) >= self.min_samples:
dt_med = float(np.median(self._dts))
if dt_med > 0.0:
self._rate = 1.0 / dt_med
self._measured = True
return self._rate
+18 -1
View File
@@ -94,13 +94,30 @@ class Tracker:
return mask.astype(bool) return mask.astype(bool)
def apply_valid_mask(self, flow, valid_mask, erode_iters=1): def apply_valid_mask(self, flow, valid_mask, erode_iters=1):
"""Zero flow outside the valid UV domain.""" """Zero flow outside the valid UV domain (in place when safe).
Multiplying by a 0/1 mask is a single streaming pass, whereas
``flow.copy()`` followed by a boolean fancy-index assignment costs a
full copy plus a gather/scatter over the masked-out pixels.
Note: a masked-out *negative* component becomes ``-0.0`` rather than
``+0.0``. The two compare, sum and magnitude identically; only the raw
bit pattern of those zeros differs from the previous implementation.
"""
if flow is None or valid_mask is None: if flow is None or valid_mask is None:
return flow return flow
mask = self._prepare_valid_mask(valid_mask, flow.shape[:2], erode_iters=erode_iters) mask = self._prepare_valid_mask(valid_mask, flow.shape[:2], erode_iters=erode_iters)
if not getattr(self, "_Tracker__adaptive", False):
# The non-adaptive path hands back the DIS state buffer, which feeds
# the next frame as its initial guess; never mutate it.
flow_out = flow.copy() flow_out = flow.copy()
flow_out[~mask] = 0 flow_out[~mask] = 0
return flow_out return flow_out
if not flow.flags.writeable:
flow = flow.copy()
mask_f = mask.astype(np.float32, copy=False)[:, :, None]
np.multiply(flow, mask_f, out=flow)
return flow
def track(self, frame, valid_mask=None): def track(self, frame, valid_mask=None):
"""Main tracking function that dispatches to CUDA or CPU implementation""" """Main tracking function that dispatches to CUDA or CPU implementation"""