update filter, flow ema 0.5 + med 3
This commit is contained in:
@@ -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,
|
||||||
|
|||||||
@@ -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,
|
||||||
|
|||||||
@@ -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,
|
||||||
|
|||||||
@@ -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"):
|
||||||
|
|||||||
@@ -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,
|
||||||
|
|||||||
@@ -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:
|
||||||
|
|||||||
@@ -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
@@ -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,
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
+367
-40
@@ -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:
|
||||||
rate = float(lowpass_sample_rate_hz)
|
# Accept a positive number to pin the rate, or "auto"/<=0 to track
|
||||||
if rate <= 0:
|
# the measured output rate (see _update_force_sample_rate).
|
||||||
raise ValueError("lowpass_sample_rate_hz must be > 0")
|
auto = False
|
||||||
self._lowpass_sample_rate_hz = rate
|
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
|
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,11 +1201,14 @@ 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)
|
fn, fx, fy = float(raw[0]), float(raw[1]), float(raw[2])
|
||||||
else:
|
|
||||||
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.
|
||||||
if self.calibration is not None:
|
if self.calibration is not None:
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
+21
-4
@@ -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)
|
||||||
flow_out = flow.copy()
|
if not getattr(self, "_Tracker__adaptive", False):
|
||||||
flow_out[~mask] = 0
|
# The non-adaptive path hands back the DIS state buffer, which feeds
|
||||||
return flow_out
|
# 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):
|
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"""
|
||||||
|
|||||||
Reference in New Issue
Block a user