update to match calibration data handling

This commit is contained in:
ykcheungab
2026-09-10 21:50:36 +08:00
parent a4766abee8
commit 705eaabcca
9 changed files with 284 additions and 142 deletions
Binary file not shown.

Before

Width:  |  Height:  |  Size: 2.0 MiB

After

Width:  |  Height:  |  Size: 2.2 MiB

@@ -128,6 +128,7 @@ class CustomerMainWindow(QMainWindow):
self.setStatusBar(QStatusBar())
self.cal_panel.status_message.connect(self.statusBar().showMessage)
self.cal_panel.calibration_exported.connect(self._on_calibration_exported)
self._build_menus()
self._wire_toolbar()
self._wire_sensor_picker()
@@ -434,6 +435,13 @@ class CustomerMainWindow(QMainWindow):
self._apply_panel_visibility()
self._persist_prefs()
def _on_calibration_exported(self, path: str) -> None:
"""Apply newly written force_calibration.json to the running Sensor."""
self._ensure_workers()
for worker in (self._box_worker, self._finger_worker):
if worker is not None and hasattr(worker, "set_pending_calibration"):
worker.set_pending_calibration(path)
def _on_finger_3d_toggled(self, visible: bool) -> None:
if self.sensor_picker.sensor_type() != "finger":
return
@@ -38,6 +38,7 @@ from orisys_app_shared.paths import calibration_export_dir, sensors_dir
class CalPanel(QFrame):
status_message = pyqtSignal(str)
calibration_exported = pyqtSignal(str) # path applied to running sensor
def __init__(self, parent=None) -> None:
super().__init__(parent)
@@ -335,4 +336,7 @@ class CalPanel(QFrame):
if warnings:
msg += "\n" + t("cal.export_warnings", detail="; ".join(warnings))
self.status_message.emit(msg)
# Prefer the bundle file for live apply so the next Sensor() also sees it.
apply_path = local_path if local_path is not None else primary
self.calibration_exported.emit(str(apply_path))
QMessageBox.information(self, t("panels.cal"), msg)
@@ -9,6 +9,7 @@ from PyQt5.QtWidgets import QFrame, QHBoxLayout, QLabel, QVBoxLayout
from orisys_app_shared.acquisition.constants import FORCE_SAMPLE_RATE_HZ, LOWPASS_CUTOFF_HZ
from orisys_app_shared.force import MetricHistory
from orisys_app_shared.force.history import DEFAULT_MAX_FPS
from orisys_app_shared.i18n import t
from orisys_app_shared.theme import color
@@ -40,6 +41,7 @@ class ForcePlotsPanel(QFrame):
sample_rate_hz=FORCE_SAMPLE_RATE_HZ,
lowpass_cutoff_hz=LOWPASS_CUTOFF_HZ,
y_axis_min_span=0.5,
max_fps=DEFAULT_MAX_FPS,
)
root = QVBoxLayout(self)
@@ -73,6 +73,7 @@ class FingerAcquisitionWorker:
self._export_dir: Optional[Path] = None
self._end_reason: Optional[str] = None
self._phase: str = "idle"
self._pending_calibration: Optional[str] = None
@property
def is_running(self) -> bool:
@@ -142,6 +143,20 @@ class FingerAcquisitionWorker:
raise RuntimeError("Finger runtime is not running")
runtime.tactile.reset()
def set_pending_calibration(self, path: str | Path | None) -> None:
"""Queue a force_calibration.json path for the acquisition thread."""
with self._lock:
self._pending_calibration = str(path) if path is not None else None
def _consume_pending_calibration(self, sensor) -> None:
with self._lock:
path = self._pending_calibration
self._pending_calibration = None
if path is None or sensor is None:
return
if hasattr(sensor, "set_calibration"):
sensor.set_calibration(path)
def _set_end_reason(self, reason: str) -> None:
with self._lock:
if self._end_reason is None:
@@ -246,6 +261,7 @@ class FingerAcquisitionWorker:
none_streak_start = None
tactile = runtime.tactile
try:
self._consume_pending_calibration(tactile)
tactile.compute_deformation(decompose=True, check_motion=False)
tactile.compute_contact()
except Exception:
@@ -95,6 +95,8 @@ def _ensure_bgr(img: np.ndarray) -> np.ndarray:
def _sensor_force_calibrated(sensor, *, config_ref: str | Path | None = None) -> bool:
if getattr(sensor, "force_calibrated", False):
return True
if getattr(sensor, "_force_cal_sensor_name", None) is not None:
return True
if config_ref is None:
@@ -226,6 +228,7 @@ class SdkAcquisitionWorker:
self._video_source: Optional[str | int] = None
self._end_reason: Optional[str] = None
self._phase: str = "idle"
self._pending_calibration: Optional[str] = None
@property
def is_running(self) -> bool:
@@ -294,6 +297,20 @@ class SdkAcquisitionWorker:
raise RuntimeError("Sensor is not running")
sensor.reset()
def set_pending_calibration(self, path: str | Path | None) -> None:
"""Queue a force_calibration.json path for the acquisition thread."""
with self._lock:
self._pending_calibration = str(path) if path is not None else None
def _consume_pending_calibration(self, sensor) -> None:
with self._lock:
path = self._pending_calibration
self._pending_calibration = None
if path is None or sensor is None:
return
if hasattr(sensor, "set_calibration"):
sensor.set_calibration(path)
def _set_end_reason(self, reason: str) -> None:
with self._lock:
if self._end_reason is None:
@@ -384,6 +401,7 @@ class SdkAcquisitionWorker:
none_streak_start = None
try:
self._consume_pending_calibration(self._sensor)
self._sensor.compute_deformation(decompose=True, check_motion=False)
self._sensor.compute_contact()
except Exception:
@@ -1,28 +1,28 @@
"""Build and write schema_version-2 force_calibration.json files."""
"""Build and write schema_version-2 force_calibration.json files.
Delegates schema/parse/write to ``orisys.calibration`` so the app and SDK share
one format. ``CalSessionStore`` remains the UI sampling table.
"""
from __future__ import annotations
import json
from copy import deepcopy
from dataclasses import asdict, dataclass, field
from datetime import datetime, timezone
from datetime import datetime
from pathlib import Path
from typing import Any, Iterable, Literal
from typing import Any, Literal
from orisys.calibration import (
CalSample as SdkCalSample,
build as sdk_build,
check as sdk_check,
pipeline_from_sensor_json,
write as sdk_write,
)
ChannelKey = Literal["fn", "fx", "fy"]
CHANNEL_KEYS: tuple[ChannelKey, ...] = ("fn", "fx", "fy")
_CHANNEL_TO_JSON = {
"fn": "normal",
"fx": "shear_x",
"fy": "shear_y",
}
_JSON_RAW_KEY = {
"normal": "fnormal",
"shear_x": "fshearx",
"shear_y": "fsheary",
}
WEIGHT_EPS = 1e-6
@@ -33,6 +33,14 @@ class CalSample:
n: int
std: float = 0.0
def to_sdk(self) -> SdkCalSample:
return SdkCalSample(
weight_n=float(self.weight_n),
raw_mean=float(self.raw_mean),
n=int(self.n),
std=float(self.std),
)
@dataclass
class CalSessionStore:
@@ -78,65 +86,22 @@ class CalSessionStore:
out[key] = [asdict(s) for s in self.channel(key)]
return out
def to_sdk_samples(self) -> dict[str, list[SdkCalSample]]:
return {
key: [s.to_sdk() for s in self.channel(key)]
for key in CHANNEL_KEYS
}
def meta_from_sensor_json(path: str | Path | None) -> dict[str, Any]:
"""Pull stitch / tracker fields from a sensor.json for export meta."""
meta: dict[str, Any] = {
"stitch_resolution": None,
"dis_mode": None,
"hhd_downsample": None,
"config_ref": None,
pipeline = pipeline_from_sensor_json(path)
return {
"stitch_resolution": pipeline.get("stitch_resolution"),
"dis_mode": pipeline.get("dis_mode"),
"hhd_downsample": pipeline.get("hhd_downsample"),
"config_ref": pipeline.get("config_ref"),
}
if path is None:
return meta
p = Path(path)
meta["config_ref"] = str(p)
if not p.is_file():
return meta
try:
data = json.loads(p.read_text(encoding="utf-8"))
except Exception:
return meta
if not isinstance(data, dict):
return meta
camera = data.get("camera") if isinstance(data.get("camera"), dict) else {}
tracker = data.get("tracker") if isinstance(data.get("tracker"), dict) else {}
stitch = camera.get("resolution_stitch")
if isinstance(stitch, (list, tuple)) and len(stitch) >= 2:
meta["stitch_resolution"] = [int(stitch[0]), int(stitch[1])]
if "dis_mode" in tracker:
try:
meta["dis_mode"] = int(tracker["dis_mode"])
except (TypeError, ValueError):
meta["dis_mode"] = tracker.get("dis_mode")
if "hhd_downsample" in tracker:
try:
meta["hhd_downsample"] = int(tracker["hhd_downsample"])
except (TypeError, ValueError):
meta["hhd_downsample"] = tracker.get("hhd_downsample")
return meta
def _piecewise_channel(
*,
json_name: str,
samples: Iterable[CalSample],
signed: bool,
) -> dict[str, Any] | None:
rows = sorted(samples, key=lambda s: abs(float(s.raw_mean)))
if len(rows) < 2:
return None
points = [[abs(float(s.raw_mean)), float(s.weight_n)] for s in rows]
block: dict[str, Any] = {
"method": "piecewise_linear",
"raw_key": _JSON_RAW_KEY[json_name],
"extrapolate": True,
"clamp_output_min": 0.0,
"points": points,
}
if signed:
block["signed"] = True
return block
def build_force_calibration_v2(
@@ -145,64 +110,66 @@ def build_force_calibration_v2(
meta: dict[str, Any] | None = None,
) -> tuple[dict[str, Any], list[str]]:
"""Build schema v2 JSON. Returns (document, skipped_channel_warnings)."""
warnings: list[str] = []
channels: dict[str, Any] = {}
for key, json_name in _CHANNEL_TO_JSON.items():
signed = key in ("fx", "fy")
block = _piecewise_channel(
json_name=json_name,
samples=store.channel(key),
signed=signed,
meta = dict(meta or {})
identity = {
"sensor_type": meta.get("sensor_type", ""),
"sn": meta.get("sn", ""),
}
pipeline = {
"stitch_resolution": meta.get("stitch_resolution"),
"dis_mode": meta.get("dis_mode"),
"hhd_downsample": meta.get("hhd_downsample"),
"config_ref": meta.get("config_ref"),
}
extra = {
k: v
for k, v in meta.items()
if k
not in (
"sensor_type",
"sn",
"stitch_resolution",
"dis_mode",
"hhd_downsample",
"config_ref",
"samples",
)
if block is None:
if store.channel(key):
warnings.append(
f"{key}: need at least 2 points (have {len(store.channel(key))})"
}
try:
cal = sdk_build(
store.to_sdk_samples(),
identity=identity,
pipeline=pipeline,
origin=str(meta.get("origin") or "user"),
extra_meta=extra,
)
except Exception as exc:
# Match previous API: raise ValueError with channel warnings when empty.
raise ValueError(str(exc)) from exc
result = sdk_check(cal)
warnings = list(result.warnings)
for key in CHANNEL_KEYS:
if not cal.has(key):
rows = store.channel(key)
if rows:
msg = f"{key}: need at least 2 points (have {len(rows)})"
else:
warnings.append(f"{key}: no samples")
continue
channels[json_name] = block
base_meta = {
"date": datetime.now(timezone.utc).astimezone().isoformat(timespec="seconds"),
"sensor_type": "",
"sn": "",
"stitch_resolution": None,
"dis_mode": None,
"hhd_downsample": None,
"record_duration_s": None,
"config_ref": None,
"samples": store.to_audit_dict(),
}
if meta:
for k, v in meta.items():
if k == "samples":
continue
base_meta[k] = v
if "samples" in meta and meta["samples"] is not None:
base_meta["samples"] = meta["samples"]
doc = {
"schema_version": 2,
"unit": "N",
"channels": channels,
"meta": base_meta,
}
return doc, warnings
msg = f"{key}: no samples"
if msg not in warnings:
warnings.append(msg)
return cal.to_dict(), warnings
def write_force_calibration(
doc: dict[str, Any],
path: str | Path,
) -> Path:
path = Path(path)
path.parent.mkdir(parents=True, exist_ok=True)
path.write_text(
json.dumps(doc, indent=2, ensure_ascii=False) + "\n",
encoding="utf-8",
)
return path.resolve()
"""Write a calibration dict (already built) to disk."""
from orisys.calibration import parse
cal = parse(doc)
return sdk_write(cal, path, overwrite=True)
def export_force_calibration_v2(
+84 -20
View File
@@ -1,12 +1,20 @@
"""Rolling force history with optional display low-pass."""
"""Rolling force history with optional display low-pass.
Uses a time-windowed buffer (keep samples in ``[t_end - window_sec, t_end]``)
with a headroom cap so high FPS still covers the visible X range after scroll.
"""
from __future__ import annotations
import math
from dataclasses import dataclass
from typing import Optional
import numpy as np
DEFAULT_MAX_FPS = 90.0
HEADROOM = 1.5
@dataclass
class PlotState:
@@ -41,8 +49,8 @@ def lowpass_filter(
return y.copy()
def _npoints_for_window(window_sec: float, sample_rate_hz: float) -> int:
return max(2, int(round(float(window_sec) * float(sample_rate_hz))))
def _max_points_for_window(window_sec: float, max_fps: float) -> int:
return max(2, int(math.ceil(float(window_sec) * float(max_fps) * HEADROOM)))
class MetricHistory:
@@ -55,35 +63,79 @@ class MetricHistory:
sample_rate_hz: float | None = None,
lowpass_cutoff_hz: float = 0.0,
y_axis_min_span: float = 0.5,
max_fps: float = DEFAULT_MAX_FPS,
):
self.channels = int(channels)
self.window_sec = float(window_sec)
self.max_fps = max(1.0, float(max_fps))
self.sample_rate_hz = (
float(sample_rate_hz)
if sample_rate_hz is not None
else (30.0 if self.window_sec <= 0 else 30.0)
float(sample_rate_hz) if sample_rate_hz is not None else 30.0
)
# ``npoints`` overrides max capacity (tests / callers); else headroom cap.
if npoints is None:
self.npoints = _npoints_for_window(self.window_sec, self.sample_rate_hz)
self.max_points = _max_points_for_window(self.window_sec, self.max_fps)
else:
self.npoints = int(npoints)
self.max_points = max(2, int(npoints))
# Back-compat alias used by older callers.
self.npoints = self.max_points
self.lowpass_cutoff_hz = float(lowpass_cutoff_hz)
self.y_axis_min_span = float(y_axis_min_span)
self._capacity = self.max_points
self.reset()
def set_window_sec(self, window_sec: float) -> None:
self.window_sec = max(0.1, float(window_sec))
self.npoints = _npoints_for_window(self.window_sec, self.sample_rate_hz)
self.reset()
self.max_points = _max_points_for_window(self.window_sec, self.max_fps)
self.npoints = self.max_points
self._ensure_capacity(self.max_points)
self._prune()
self._y_range_smooth = [None for _ in range(self.channels)]
def reset(self) -> None:
self._data = np.full((self.npoints, 1 + self.channels), np.nan, dtype=np.float64)
self._capacity = max(self.max_points, 2)
self._data = np.full(
(self._capacity, 1 + self.channels), np.nan, dtype=np.float64
)
self.sample_count = 0
self._t0: Optional[float] = None
self._y_range_smooth: list[Optional[tuple[float, float]]] = [
None for _ in range(self.channels)
]
def _ensure_capacity(self, needed: int) -> None:
needed = max(needed, self.max_points, 2)
if needed <= self._capacity:
return
new_cap = max(needed, self._capacity * 2)
new_data = np.full(
(new_cap, 1 + self.channels), np.nan, dtype=np.float64
)
if self.sample_count > 0:
new_data[: self.sample_count] = self._data[: self.sample_count]
self._data = new_data
self._capacity = new_cap
def _prune(self) -> None:
"""Drop samples older than the time window, then enforce max_points."""
n = self.sample_count
if n == 0:
return
t_end = float(self._data[n - 1, 0])
cutoff = t_end - self.window_sec
times = self._data[:n, 0]
# First index with t >= cutoff
keep_from = int(np.searchsorted(times, cutoff, side="left"))
if keep_from > 0:
remaining = n - keep_from
self._data[:remaining] = self._data[keep_from:n]
self.sample_count = remaining
n = remaining
if n > self.max_points:
drop = n - self.max_points
remaining = n - drop
self._data[:remaining] = self._data[drop:n]
self.sample_count = remaining
def push(self, timestamp: float, *values: float) -> None:
if len(values) != self.channels:
raise ValueError(f"Expected {self.channels} values, got {len(values)}")
@@ -91,12 +143,11 @@ class MetricHistory:
self._t0 = float(timestamp)
t = float(timestamp) - self._t0
row = (t, *map(float, values))
if self.sample_count < self.npoints:
if self.sample_count >= self._capacity:
self._ensure_capacity(self.sample_count + 1)
self._data[self.sample_count, :] = row
self.sample_count += 1
return
self._data[:-1, :] = self._data[1:, :]
self._data[-1, :] = row
self._prune()
def _smooth_y_range(self, channel: int, ymin: float, ymax: float) -> tuple[float, float]:
if self._y_range_smooth[channel] is None:
@@ -122,6 +173,24 @@ class MetricHistory:
block = self._data[:n]
times = block[:, 0].copy()
values = block[:, 1:].copy()
t_end = float(times[-1])
if t_end >= self.window_sec:
x_range = (t_end - self.window_sec, t_end)
# Keep setData lean: only points inside the visible window.
mask = times >= x_range[0]
times = times[mask]
values = values[mask]
# Discrete samples usually start slightly after the left edge;
# prepend a hold point so the curve spans the full X range.
if times.size > 0 and float(times[0]) > x_range[0]:
times = np.concatenate(([x_range[0]], times))
values = np.vstack([values[0:1], values])
else:
x_range = (0.0, self.window_sec)
if times.size == 0:
return None
if self.lowpass_cutoff_hz > 0:
for i in range(self.channels):
values[:, i] = lowpass_filter(
@@ -129,11 +198,6 @@ class MetricHistory:
self.lowpass_cutoff_hz,
sample_rate_hz=self.sample_rate_hz,
)
t_end = float(times[-1])
if t_end >= self.window_sec:
x_range = (t_end - self.window_sec, t_end)
else:
x_range = (0.0, self.window_sec)
y_ranges = []
for i in range(self.channels):
col = values[:, i]
+63
View File
@@ -0,0 +1,63 @@
"""Tests for time-windowed MetricHistory (force plot buffer)."""
from __future__ import annotations
import pytest
from orisys_app_shared.force.history import MetricHistory
def _push_at_rate(hist: MetricHistory, *, fps: float, duration_sec: float, t0: float = 1000.0) -> None:
dt = 1.0 / fps
n = int(round(duration_sec * fps)) + 1
for i in range(n):
hist.push(t0 + i * dt, 1.0, 0.0, -1.0)
def test_fill_phase_x_range_is_fixed_window():
hist = MetricHistory(window_sec=4.0, sample_rate_hz=30.0, lowpass_cutoff_hz=0.0, max_fps=90.0)
_push_at_rate(hist, fps=30.0, duration_sec=2.0)
state = hist.state()
assert state is not None
assert state.x_range == pytest.approx((0.0, 4.0))
assert float(state.times[-1]) < 4.0
def test_high_fps_after_scroll_covers_x_range():
window = 4.0
hist = MetricHistory(window_sec=window, sample_rate_hz=30.0, lowpass_cutoff_hz=0.0, max_fps=90.0)
_push_at_rate(hist, fps=60.0, duration_sec=window + 1.0)
state = hist.state()
assert state is not None
x0, x1 = state.x_range
assert x1 - x0 == pytest.approx(window)
assert float(state.times[0]) == pytest.approx(x0)
assert float(state.times[-1]) == pytest.approx(x1)
def test_nominal_30hz_after_scroll_covers_x_range():
window = 4.0
hist = MetricHistory(window_sec=window, sample_rate_hz=30.0, lowpass_cutoff_hz=0.0, max_fps=90.0)
_push_at_rate(hist, fps=30.0, duration_sec=window + 2.0)
state = hist.state()
assert state is not None
x0, x1 = state.x_range
assert x1 - x0 == pytest.approx(window)
assert float(state.times[0]) == pytest.approx(x0)
def test_set_window_sec_prunes_older_samples():
hist = MetricHistory(window_sec=4.0, sample_rate_hz=30.0, lowpass_cutoff_hz=0.0, max_fps=90.0)
_push_at_rate(hist, fps=30.0, duration_sec=5.0)
before = hist.state()
assert before is not None
assert float(before.times[-1]) >= 4.0
hist.set_window_sec(1.0)
after = hist.state()
assert after is not None
assert after.x_range[1] - after.x_range[0] == pytest.approx(1.0)
assert float(after.times[0]) == pytest.approx(after.x_range[0])
# All retained samples must lie within the new window ending at t_end.
t_end = float(after.times[-1])
assert float(after.times[0]) >= t_end - 1.0 - 1e-9