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"],
"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": {
"min_contour_area": 3000,
"flow_threshold":5,
+9 -1
View File
@@ -22,7 +22,15 @@
"dis_preset": "medium",
"dis_mode": 1,
"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": {
"min_contour_area": 3000,
+9 -1
View File
@@ -29,10 +29,18 @@
"loop_closure_interval": 1,
"algorithm": "dis",
"dis_preset": "medium",
"dis_mode": 3,
"dis_mode": 1,
"algorithm_list": ["raft", "dis"],
"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": {
"min_contour_area": 3000,
"flow_threshold":5,
+35 -4
View File
@@ -242,7 +242,9 @@ class Calibration:
meta: dict[str, Any] = {}
meta.update(self.provenance)
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:
meta[key] = self.pipeline[key]
if self.measurements:
@@ -387,7 +389,8 @@ def check(doc: dict | Calibration, *, pipeline: dict | None = None) -> CheckResu
warns.append("empty sn on factory 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]:
warns.append(f"pipeline mismatch: {key}")
@@ -601,11 +604,14 @@ def load(
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] = {
"stitch_resolution": None,
"dis_mode": None,
"hhd_downsample": None,
"flow_ema_alpha": None,
"force_median_window": None,
"force_lowpass_cutoff_hz": None,
"config_ref": None,
}
if path is None:
@@ -622,6 +628,7 @@ def pipeline_from_sensor_json(path: str | Path | None) -> dict[str, Any]:
return pipeline
camera = data.get("camera") if isinstance(data.get("camera"), 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")
if isinstance(stitch, (list, tuple)) and len(stitch) >= 2:
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"])
except (TypeError, ValueError):
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
@@ -717,7 +747,8 @@ def build(
for k, v in extra_meta.items():
if k == "samples":
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:
pipeline[k] = v
elif k in ("sensor_type", "sn", "hardware_rev", "gel_id"):
+9
View File
@@ -293,6 +293,15 @@ class FingerRuntime:
capture = capture_source.cap
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(
domain.remap,
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
import time
import cv2
import numpy as np
from pathlib import Path
@@ -12,11 +14,24 @@ class CompoundEyeFrameProcessor:
def __init__(self, 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):
t0 = time.perf_counter()
self.stitcher.get_stitch_unit(frame)
t1 = time.perf_counter()
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:
+45
View File
@@ -73,8 +73,49 @@ class nHHD(object):
raise ValueError("nHHD Solver works only for 2 and 3 dimensions")
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)
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):
if valid_mask is None:
return None
@@ -402,6 +443,8 @@ class nHHD(object):
cross_n = numpy.where(mask, cross_n, 0)
self.Stor = numpy.sum(cross_p) + numpy.sum(cross_n)
self._apply_force_scale(shape)
# Optimized: pre-allocate output array with explicit dtype
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._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)
# 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
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):
"""Subsample boundary pixel coordinates for capacitance correction."""
if boundary_coords.size == 0:
@@ -53,6 +86,7 @@ class MaskedDomain:
"mask",
"index_map",
"factor",
"factor_dtype",
"lap",
"num_active",
"pinned",
@@ -75,11 +109,13 @@ class MaskedDomain:
pinned,
active_coords,
active_linear_indices,
factor_dtype=numpy.float32,
):
self.shape = shape
self.mask = mask
self.index_map = index_map
self.factor = factor
self.factor_dtype = numpy.dtype(factor_dtype)
self.lap = lap
self.num_active = num_active
self.pinned = pinned
@@ -156,7 +192,7 @@ class MaskedDomain:
data.append(diag)
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(
shape=active.shape,
mask=active.copy(),
@@ -167,6 +203,7 @@ class MaskedDomain:
pinned=pinned,
active_coords=active_coords,
active_linear_indices=active_linear_indices,
factor_dtype=factor_dtype,
)
def matches(self, mask):
@@ -179,13 +216,20 @@ class MaskedDomain:
f: (H, W) scalar source field
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:
return numpy.zeros(0, dtype=numpy.float64)
if self._rhs_buffer is None or self._rhs_buffer.shape != (self.num_active,):
self._rhs_buffer = numpy.empty(self.num_active, 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,)
or self._rhs_buffer.dtype != self.factor_dtype
):
self._rhs_buffer = numpy.empty(self.num_active, dtype=self.factor_dtype)
numpy.take(
numpy.asarray(f, dtype=numpy.float64).ravel(),
numpy.asarray(f, dtype=self.factor_dtype).ravel(),
self.active_linear_indices,
out=self._rhs_buffer,
)
@@ -197,11 +241,15 @@ class MaskedDomain:
def prepare_rhs_pair(self, f0, f1):
"""Extract two gauge-fixed RHS vectors into one reusable (N, 2) buffer."""
if self.num_active == 0:
return numpy.zeros((0, 2), dtype=numpy.float64)
if self._rhs_pair_buffer is None or self._rhs_pair_buffer.shape != (self.num_active, 2):
self._rhs_pair_buffer = numpy.empty((self.num_active, 2), dtype=numpy.float64)
flat0 = numpy.asarray(f0, dtype=numpy.float64).ravel()
flat1 = numpy.asarray(f1, dtype=numpy.float64).ravel()
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)
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(flat1, self.active_linear_indices, out=self._rhs_pair_buffer[:, 1])
self._rhs_pair_buffer -= self._rhs_pair_buffer.mean(axis=0, keepdims=True)
@@ -255,6 +303,7 @@ class MaskedDomain:
"mask": self.mask,
"index_map": self.index_map,
"factor": self.factor,
"factor_dtype": self.factor_dtype,
"lap": self.lap,
"num_active": self.num_active,
"pinned": self.pinned,
+5 -1
View File
@@ -33,7 +33,11 @@ class MaskedSparseSolver:
try:
sol_pair = -domain.factor(rhs_pair) # (N, 2) reduced-system potentials
except Exception:
# Some factorizations need contiguous 1-D right-hand sides.
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)
+367 -40
View File
@@ -26,8 +26,22 @@ from .backend_availability import (check_cupy_available, check_opencl_available)
from .frame_processors import create_frame_processor
from .signal_processing import (
DEFAULT_SAMPLE_RATE_HZ,
CausalMedianFilter,
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 (
CaptureConfig,
CaptureSource,
@@ -154,16 +168,92 @@ class Sensor:
self.calibration: Optional[cal_mod.Calibration] = None
self._force_cal_sensor_name: Optional[str] = None
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_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(
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"
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.calibration is not None and self.calibration.source_path is not None:
print(f"Force calibration: {self.calibration.source_path}")
@@ -182,6 +272,13 @@ class Sensor:
self.valid_mask_raw = None
self.valid_mask = 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 = {
"coverage": 0.0,
"flow_mean_valid": 0.0,
@@ -194,10 +291,16 @@ class Sensor:
self._last_capture_time = 0.0
self._last_processing_time = 0.0
self._last_stage_times = {
"extract_tiles": 0.0,
"stitch": 0.0,
"resize": 0.0,
"mirror": 0.0,
# Frame-post-processing roll-ups (seconds).
"extract_tiles": 0.0, # sensor units + tile extraction
"stitch": 0.0, # feature/feather stitch of the extracted tiles
"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,
}
@@ -218,6 +321,7 @@ class Sensor:
except (TypeError, ValueError):
self._hhd_downsample = 1
self._hhd_grid_shape = None
self._hhd_ref_shape = None
cfg_backend = self.__config_tracker.get("backend", None)
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")
@@ -244,6 +348,15 @@ class Sensor:
print("Sensor initialized in external-frame mode.")
else:
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:
print(f" Video source: {vid_src}")
@@ -253,6 +366,14 @@ class Sensor:
# print(f" CUDA acceleration: {self.cuda}")
print(f" CuPy available: {self.CUPY_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):
if flow is None:
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
if mask is None or mask.shape != mag.shape:
self.mask_stats["flow_mean_valid"] = float(mag.mean()) if mag.size else 0.0
if mask is None or mask.shape != shape:
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
return
valid = mask.astype(bool)
invalid = ~valid
self.mask_stats["flow_mean_valid"] = float(mag[valid].mean()) if np.any(valid) else 0.0
self.mask_stats["flow_mean_invalid"] = float(mag[invalid].mean()) if np.any(invalid) else 0.0
mask_u8 = self._flow_mask_u8
if mask_u8 is None or mask_u8.shape != shape or mask_u8.dtype != np.uint8:
mask_u8 = np.empty(shape, dtype=np.uint8)
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):
"""
@@ -362,6 +497,7 @@ class Sensor:
self.frame = frame if (already_processed and isinstance(frame, np.ndarray)) else (
frame.copy() if isinstance(frame, np.ndarray) else frame
)
t_after_store = time.perf_counter()
if already_processed:
img_bgr = frame
gray = None
@@ -383,8 +519,6 @@ class Sensor:
gray = None
t_after_extract = time.perf_counter()
t_after_stitch = time.perf_counter()
if gray is None:
self.img_size_raw = img_bgr.shape
self._img_bgr_raw = img_bgr
@@ -392,6 +526,8 @@ class Sensor:
else:
self.img_size_raw = gray.shape
self._img_bgr_raw = None
t_after_gray = time.perf_counter()
res = tuple(self._config_resolution_stitch)
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)
@@ -400,15 +536,22 @@ class Sensor:
if self.mirror:
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._prepare_valid_mask(valid_mask, gray.shape, self.img.shape)
t_processing_end = time.perf_counter()
self._last_processing_time = t_processing_end - t_processing_start
self._last_stage_times["extract_tiles"] = t_after_extract - t_processing_start
self._last_stage_times["stitch"] = t_after_stitch - t_after_extract
self._last_stage_times["resize"] = t_processing_end - t_after_stitch
self._record_stage_times(
t_processing_start=t_processing_start,
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
self._fps = 1.0 / (time.perf_counter() - self.last_log_time)
@@ -416,6 +559,48 @@ class Sensor:
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):
"""Produce a resized+mirrored BGR image on demand (for display)."""
if self._img_bgr_raw is None:
@@ -512,9 +697,73 @@ class Sensor:
# flow = self.__tracker.track_raft(self.img)
if hasattr(flow, 'download'):
flow = flow.download()
flow = self._apply_flow_ema(flow)
self.flow = flow
self._compute_flow_means(self.flow)
def _apply_flow_ema(self, flow):
"""Temporally smooth the optical-flow field before HHD.
The frame-to-frame jitter of the estimated flow is spatially coherent
(a near-global shift: it survives ~40x less averaging than per-pixel
noise would, see the noise analysis), so it cannot be removed by
spatial smoothing, a coarser HHD grid or a different DIS setting - only
by averaging along time. Applying it here also stabilises
``vnormal``/contact, which a force-level filter cannot reach.
``flow_ema_alpha >= 1`` disables the stage.
"""
alpha = getattr(self, "_flow_ema_alpha", 1.0)
if flow is None or alpha >= 1.0:
return flow
prev = self._ema_flow
if prev is None or prev.shape != flow.shape or prev.dtype != flow.dtype:
self._ema_flow = np.array(flow, copy=True)
return flow
prev += alpha * (flow - prev)
return prev
def _update_force_sample_rate(self):
"""Track the achieved force update rate for the low-pass design.
A Butterworth designed for 30 Hz stops meaning the same thing at 60 Hz,
so when ``force_lowpass_sample_rate_hz`` is ``"auto"`` the rate actually
achieved is used. For sources whose wall-clock cadence does not equal
signal time (video files, offline replay) ``_lowpass_rate_auto`` is
turned off by whoever knows the source - ``__init__`` for a capture the
Sensor opened itself, and ``FingerRuntime.open`` for external frames -
and the configured camera fps is used instead.
"""
if not getattr(self, "_lowpass_rate_auto", False):
return
estimator = getattr(self, "_force_rate", None)
if estimator is None:
return
hz = estimator.tick()
if not (1.0 <= hz <= 240.0):
return
self._force_rate_source = "measured"
used = self._lowpass_sample_rate_hz
if used > 0 and abs(hz - used) <= 0.1 * used:
return
self._lowpass_sample_rate_hz = hz
if self._lowpass_enabled:
self._force_filter.configure(
cutoff_hz=self._lowpass_cutoff_hz,
sample_rate_hz=hz,
order=self._lowpass_order,
)
@property
def force_sample_rate_hz(self) -> float:
"""Sample rate currently used to design the force low-pass (Hz)."""
return float(self._lowpass_sample_rate_hz)
@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):
flow = self.flow
@@ -543,7 +792,11 @@ class Sensor:
if k > 1:
ds_w = max(1, orig_w // 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 (
mask_ds.shape[1] != ds_w or mask_ds.shape[0] != ds_h
):
@@ -552,8 +805,13 @@ class Sensor:
flow_ds = flow
ds_shape = flow_ds.shape[:2]
ref_shape = (orig_h, orig_w)
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)
self.__hhd = nHHD(
cuda=self.cuda,
@@ -561,6 +819,9 @@ class Sensor:
spacings=spacings,
poisson_backend=self.poisson_backend,
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_max_boundary=self.masked_poisson_max_boundary,
masked_poisson_boundary_stride=self.masked_poisson_boundary_stride,
@@ -569,29 +830,46 @@ class Sensor:
masked_poisson_reg=self.masked_poisson_reg,
)
self._hhd_grid_shape = ds_shape
self._hhd_ref_shape = ref_shape
_, forces = self.__hhd.get_force(flow_ds, valid_mask=mask_ds)
fn, fx, fy, ftor = forces
self._force_raw = (float(fn), float(fx), float(fy))
self.ftor = float(ftor)
self._update_force_sample_rate()
self._apply_force_filter()
vnormal_ds = self.__hhd.get_normal()
vshear_ds = self.__hhd.get_shear()
if k > 1:
self.vnormal = cv2.resize(vnormal_ds, (orig_w, orig_h), interpolation=cv2.INTER_LINEAR)
self.vshear = cv2.resize(vshear_ds, (orig_w, orig_h), interpolation=cv2.INTER_LINEAR)
# Upsample into reusable buffers; the solver keeps owning its own
# 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:
self.vnormal = vnormal_ds
self.vshear = vshear_ds
self.vnormal = vnormal_ds.copy()
self.vshear = vshear_ds.copy()
if self.valid_mask is not None:
active = self.valid_mask.astype(bool)
self.vnormal = self.vnormal.copy()
self.vshear = self.vshear.copy()
self.vnormal[~active] = 0
self.vshear[~active] = 0
# In-place multiply by a 0/1 float mask instead of
# ``x.copy(); x[~active] = 0`` (copy + boolean gather/scatter).
m3 = self._hhd_mask_f32
if m3 is None or m3.shape != (orig_h, orig_w, 1) or m3.dtype != np.float32:
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
@@ -700,6 +978,9 @@ class Sensor:
"""Reset optical flow tracker and force-filter state."""
self.__tracker.reset()
self._force_filter.reset()
if getattr(self, "_force_median", None) is not None:
self._force_median.reset()
self._ema_flow = None
if self.img is not None:
self.initial_img = self.img.copy()
@@ -743,6 +1024,8 @@ class Sensor:
lowpass_sample_rate_hz=None,
lowpass_order=None,
lowpass_enabled=None,
flow_ema_alpha=None,
force_median_window=None,
hhd_downsample=None,
):
"""Update runtime-safe settings without reconnecting the camera.
@@ -754,10 +1037,18 @@ class Sensor:
value enables filtering unless ``lowpass_enabled=False``.
lowpass_sample_rate_hz : float, optional
Assumed force sample rate for the Butterworth design (default 30).
Set this to the actual output rate of the deployment.
lowpass_order : int, optional
Butterworth order (default 2).
lowpass_enabled : bool, optional
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
Integer ``>= 1``. Rebuilds the HHD solver on the next frame.
"""
@@ -768,10 +1059,26 @@ class Sensor:
if lowpass_enabled is None:
self._lowpass_enabled = self._lowpass_cutoff_hz > 0
if lowpass_sample_rate_hz is not None:
rate = float(lowpass_sample_rate_hz)
if rate <= 0:
raise ValueError("lowpass_sample_rate_hz must be > 0")
self._lowpass_sample_rate_hz = rate
# Accept a positive number to pin the rate, or "auto"/<=0 to track
# the measured output rate (see _update_force_sample_rate).
auto = False
rate = 0.0
if isinstance(lowpass_sample_rate_hz, str):
auto = lowpass_sample_rate_hz.strip().lower() in ("auto", "", "measured")
else:
try:
rate = float(lowpass_sample_rate_hz)
except (TypeError, ValueError):
auto = True
else:
auto = rate <= 0.0
self._lowpass_rate_auto = auto
if not auto:
self._lowpass_sample_rate_hz = rate
elif self._lowpass_sample_rate_hz <= 0:
self._lowpass_sample_rate_hz = self._config_fps
if getattr(self, "_force_rate", None) is not None:
self._force_rate.reset(self._config_fps)
filter_changed = True
if lowpass_order is not None:
order = int(lowpass_order)
@@ -783,6 +1090,22 @@ class Sensor:
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(
@@ -802,6 +1125,7 @@ class Sensor:
self._hhd_downsample = k
self.__hhd = None
self._hhd_grid_shape = None
self._hhd_ref_shape = None
self._update_hhd_mask_cache()
return self
@@ -877,11 +1201,14 @@ class Sensor:
def _apply_force_filter(self):
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:
filtered = 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])
raw = self._force_filter.process(raw)
fn, fx, fy = float(raw[0]), float(raw[1]), float(raw[2])
# Apply loaded force_calibration.json when present; otherwise keep raw.
if self.calibration is not None:
+114
View File
@@ -15,6 +15,8 @@ Prefer ``Sensor.calibration`` / ``read_info(FNORMAL, …)`` in new code.
from __future__ import annotations
import collections
import time
from dataclasses import dataclass
import numpy as np
@@ -312,3 +314,115 @@ class LowpassFilter:
self.reset(x)
y, self._zi = sosfilt(self._sos, x[None, :], zi=self._zi, axis=0)
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
+21 -4
View File
@@ -94,13 +94,30 @@ class Tracker:
return mask.astype(bool)
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:
return flow
mask = self._prepare_valid_mask(valid_mask, flow.shape[:2], erode_iters=erode_iters)
flow_out = flow.copy()
flow_out[~mask] = 0
return flow_out
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[~mask] = 0
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):
"""Main tracking function that dispatches to CUDA or CPU implementation"""