From 0d5d8ff70dcbb6bbf67881249578729ab4317779 Mon Sep 17 00:00:00 2001 From: ykcheungab Date: Sat, 12 Sep 2026 19:36:36 +0800 Subject: [PATCH] update filter, flow ema 0.5 + med 3 --- sensors/box/sensor.json | 8 + sensors/finger/sensor.json | 10 +- sensors/mini/sensor.json | 10 +- src/orisys/calibration.py | 39 ++- src/orisys/finger/runtime.py | 9 + src/orisys/frame_processors.py | 17 +- src/orisys/hhd/api.py | 45 +++ src/orisys/hhd/domain.py | 69 +++- src/orisys/hhd/solvers/masked_sparse.py | 6 +- src/orisys/sensor.py | 407 +++++++++++++++++++++--- src/orisys/signal_processing.py | 114 +++++++ src/orisys/tracker.py | 25 +- 12 files changed, 697 insertions(+), 62 deletions(-) diff --git a/sensors/box/sensor.json b/sensors/box/sensor.json index 1bbf513..40633fd 100644 --- a/sensors/box/sensor.json +++ b/sensors/box/sensor.json @@ -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, diff --git a/sensors/finger/sensor.json b/sensors/finger/sensor.json index ebdb709..f9c3f90 100644 --- a/sensors/finger/sensor.json +++ b/sensors/finger/sensor.json @@ -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, diff --git a/sensors/mini/sensor.json b/sensors/mini/sensor.json index af88413..40633fd 100644 --- a/sensors/mini/sensor.json +++ b/sensors/mini/sensor.json @@ -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, diff --git a/src/orisys/calibration.py b/src/orisys/calibration.py index 6060d64..3c2bbc0 100644 --- a/src/orisys/calibration.py +++ b/src/orisys/calibration.py @@ -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"): diff --git a/src/orisys/finger/runtime.py b/src/orisys/finger/runtime.py index 80be9f6..e17e4cb 100644 --- a/src/orisys/finger/runtime.py +++ b/src/orisys/finger/runtime.py @@ -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, diff --git a/src/orisys/frame_processors.py b/src/orisys/frame_processors.py index 3b7e946..f432c60 100644 --- a/src/orisys/frame_processors.py +++ b/src/orisys/frame_processors.py @@ -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: diff --git a/src/orisys/hhd/api.py b/src/orisys/hhd/api.py index 4d89ccc..0c4ef9b 100644 --- a/src/orisys/hhd/api.py +++ b/src/orisys/hhd/api.py @@ -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 diff --git a/src/orisys/hhd/domain.py b/src/orisys/hhd/domain.py index dbaf415..7da6d6e 100644 --- a/src/orisys/hhd/domain.py +++ b/src/orisys/hhd/domain.py @@ -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, diff --git a/src/orisys/hhd/solvers/masked_sparse.py b/src/orisys/hhd/solvers/masked_sparse.py index e00ef0f..b14a8a4 100644 --- a/src/orisys/hhd/solvers/masked_sparse.py +++ b/src/orisys/hhd/solvers/masked_sparse.py @@ -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) diff --git a/src/orisys/sensor.py b/src/orisys/sensor.py index 7755092..0f57b5f 100644 --- a/src/orisys/sensor.py +++ b/src/orisys/sensor.py @@ -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: diff --git a/src/orisys/signal_processing.py b/src/orisys/signal_processing.py index 3df46d9..28b748f 100644 --- a/src/orisys/signal_processing.py +++ b/src/orisys/signal_processing.py @@ -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 diff --git a/src/orisys/tracker.py b/src/orisys/tracker.py index 68805c6..826e240 100644 --- a/src/orisys/tracker.py +++ b/src/orisys/tracker.py @@ -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"""