新增 HHD 场加权标定模型与加权空载基线
field_gain/1 用坐标增益图对无旋向量场做 L1 场加权,以连续 30 帧加权中位数替代旧 raw 零点,实时入口暂不支持。目标测试 102 项与 lint_gate 通过;契约见 orisys-sdk/docs/field-gain-model.zh.md。
This commit is contained in:
@@ -53,8 +53,13 @@ def domain_fingerprint(domain) -> str:
|
||||
|
||||
def build_context(config, filters=None, *, geometry_sha256=None, source_scale=None,
|
||||
coordinates=None, output_domain_sha256=None, processing_backend=None,
|
||||
preprocessing=None) -> dict:
|
||||
"""Build the closed v1 feature identity from effective values, excluding readout filters."""
|
||||
preprocessing=None, field=None) -> dict:
|
||||
"""Build the closed v1 feature identity from effective values, excluding readout filters.
|
||||
|
||||
``field`` carries the closed profile identity for field models (shape, dtype,
|
||||
unit, mask and lifecycle). It is omitted for scalar profiles so their
|
||||
signatures stay byte-identical to previously exported files.
|
||||
"""
|
||||
camera = config.get("camera", {})
|
||||
tracker = dict(config.get("tracker", {}))
|
||||
raw_downsample = tracker.get("hhd_downsample", 1)
|
||||
@@ -94,5 +99,8 @@ def build_context(config, filters=None, *, geometry_sha256=None, source_scale=No
|
||||
"source_scale": source_scale,
|
||||
"preprocessing": preprocessing or {},
|
||||
}
|
||||
if field is not None:
|
||||
result["field"] = deepcopy(field)
|
||||
effective["field"] = deepcopy(field)
|
||||
result["feature_signature_sha256"] = _feature_signature(effective)
|
||||
return CalibrationContext(result, effective)
|
||||
|
||||
@@ -7,13 +7,32 @@ import json
|
||||
import math
|
||||
from collections.abc import Mapping
|
||||
from copy import deepcopy
|
||||
from dataclasses import dataclass
|
||||
from dataclasses import dataclass, field as dataclass_field
|
||||
from pathlib import Path, PurePosixPath, PureWindowsPath
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial import ConvexHull, QhullError
|
||||
|
||||
from .field_calibration import (
|
||||
BASELINE_POLICY,
|
||||
FIELD_IDENTITY,
|
||||
FIELD_PROFILE,
|
||||
MAPPING_POLICY,
|
||||
ZERO_FRAME_COUNT,
|
||||
FieldGainMap,
|
||||
build_field_gain_map,
|
||||
coerce_field,
|
||||
mask_sha256,
|
||||
reduce_field,
|
||||
)
|
||||
|
||||
_PROFILE_FOR_TYPE = {
|
||||
"spatial_gain/1": "fn_delta_autocal_uv/1",
|
||||
"global_gain/1": "force_delta/1",
|
||||
"field_gain/1": FIELD_PROFILE,
|
||||
}
|
||||
|
||||
|
||||
def _require(condition: bool, message: str) -> None:
|
||||
if not condition:
|
||||
@@ -98,11 +117,15 @@ def _mesh(params: dict[str, Any]) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
return vertices, np.asarray(gains, dtype=float), np.asarray(triangles, dtype=int)
|
||||
|
||||
|
||||
def _pipeline(data: Any, spatial: bool) -> None:
|
||||
def _pipeline(data: Any, profile: str) -> None:
|
||||
spatial = profile != "force_delta/1"
|
||||
required = {"stitch_resolution", "dis_mode", "hhd_downsample",
|
||||
"filters", "feature_signature_sha256"}
|
||||
_keys(data, required | ({"geometry_sha256", "coordinates", "source_scale"} if spatial else set()),
|
||||
{"geometry_sha256", "coordinates", "output_domain_sha256", "source_scale"})
|
||||
if spatial:
|
||||
required |= {"geometry_sha256", "coordinates", "source_scale"}
|
||||
if profile == FIELD_PROFILE:
|
||||
required.add("field")
|
||||
_keys(data, required, {"geometry_sha256", "coordinates", "output_domain_sha256", "source_scale"})
|
||||
size = data["stitch_resolution"]
|
||||
_require(isinstance(size, list) and len(size) == 2 and all(type(n) is int and n > 0 for n in size),
|
||||
"invalid stitch_resolution")
|
||||
@@ -127,12 +150,17 @@ def _pipeline(data: Any, spatial: bool) -> None:
|
||||
_require(isinstance(coord["origin_xy"], list) and len(coord["origin_xy"]) == 2 and
|
||||
all(_number(x) for x in coord["origin_xy"]), "invalid coordinate origin")
|
||||
_require(_sha(coord["map_sha256"]), "invalid map SHA-256")
|
||||
if "field" in required:
|
||||
_require(data["field"] == FIELD_IDENTITY, "unsupported field identity")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CalibrationResult:
|
||||
values_N: dict[str, float | None]
|
||||
status: dict[str, str]
|
||||
#: Versioned extra evidence (field model: baseline, extrapolation fraction).
|
||||
#: Consumers must tolerate missing keys so old cached results stay readable.
|
||||
diagnostics: dict[str, Any] = dataclass_field(default_factory=dict)
|
||||
|
||||
|
||||
class ModelCalibration:
|
||||
@@ -154,16 +182,25 @@ class ModelCalibration:
|
||||
self._params = self._document["model"]["params"]
|
||||
self.coordinate_map = self.coordinate_valid_mask = None
|
||||
self._artifact_hashes: list[str] = []
|
||||
if self.model_type == "spatial_gain/1":
|
||||
self._points = self._gains = self._triangles = None
|
||||
self._field_maps: dict[tuple[tuple[int, int], str], FieldGainMap] = {}
|
||||
self._field_reference_id: str | None = None
|
||||
self._field_zero_values: list[float] = []
|
||||
self._field_baseline: float | None = None
|
||||
if self.model_type in ("spatial_gain/1", "field_gain/1"):
|
||||
self._points, self._gains, self._triangles = _mesh(self._params)
|
||||
self._load_artifacts(document["model"].get("artifacts", []))
|
||||
if self.model_type == "field_gain/1":
|
||||
_require(self.coordinate_map is not None,
|
||||
"field_gain/1 requires a coordinate_map artifact")
|
||||
|
||||
def _load_artifacts(self, artifacts: Any) -> None:
|
||||
_require(isinstance(artifacts, list), "artifacts must be an array")
|
||||
_require(len(artifacts) <= 1, "only one coordinate_map artifact is supported")
|
||||
for artifact in artifacts:
|
||||
_keys(artifact, {"role", "path", "sha256"})
|
||||
_require(self.model_type == "spatial_gain/1" and artifact["role"] == "coordinate_map",
|
||||
_require(self.model_type in ("spatial_gain/1", "field_gain/1")
|
||||
and artifact["role"] == "coordinate_map",
|
||||
"unsupported artifact role")
|
||||
name = artifact["path"]
|
||||
_require(isinstance(name, str) and bool(name) and ":" not in name and
|
||||
@@ -229,7 +266,108 @@ class ModelCalibration:
|
||||
_require(pipeline.get(key) == value, f"calibration pipeline mismatch: {key}")
|
||||
|
||||
def reset(self, reference_id: Any = None) -> None:
|
||||
"""Gain backends have no history; preserve the lifecycle for future models."""
|
||||
"""Clear per-stream state; the field model also voids its weighted baseline."""
|
||||
del reference_id
|
||||
self._field_reference_id = None
|
||||
self._field_zero_values = []
|
||||
self._field_baseline = None
|
||||
|
||||
@property
|
||||
def field_baseline_N(self) -> float | None:
|
||||
"""Current weighted unloaded baseline, or ``None`` while it is unavailable."""
|
||||
return self._field_baseline
|
||||
|
||||
def begin_reference(self, reference_id: Any) -> None:
|
||||
"""Declare a new unloaded window; the next 30 valid frames set the baseline."""
|
||||
_require(self.model_type == "field_gain/1",
|
||||
"a weighted reference requires field_gain/1")
|
||||
_require(isinstance(reference_id, str) and bool(reference_id.strip()),
|
||||
"reference_id must be a non-empty string")
|
||||
self._field_reference_id = reference_id
|
||||
self._field_zero_values = []
|
||||
self._field_baseline = None
|
||||
|
||||
def field_gain_map(self, grid_shape: tuple[int, int],
|
||||
physical_mask: np.ndarray) -> FieldGainMap:
|
||||
"""Gain/coverage map for one HHD grid and one output-resolution mask."""
|
||||
_require(self.model_type == "field_gain/1",
|
||||
"field gain maps require field_gain/1")
|
||||
mask = np.asarray(physical_mask, dtype=bool)
|
||||
_require(mask.ndim == 2 and mask.shape == self.coordinate_map.shape[:2],
|
||||
"physical mask must match the coordinate map output grid")
|
||||
return self._field_map((int(grid_shape[0]), int(grid_shape[1])), mask)
|
||||
|
||||
def _field_map(self, grid_shape: tuple[int, int], physical_mask: np.ndarray) -> FieldGainMap:
|
||||
key = (grid_shape, mask_sha256(physical_mask))
|
||||
cached = self._field_maps.get(key)
|
||||
if cached is not None:
|
||||
return cached
|
||||
gain_map = build_field_gain_map(
|
||||
self._points, self._gains, self._triangles,
|
||||
self.coordinate_map, self.coordinate_valid_mask,
|
||||
grid_shape, physical_mask,
|
||||
)
|
||||
if len(self._field_maps) >= 4:
|
||||
self._field_maps.pop(next(iter(self._field_maps)))
|
||||
self._field_maps[key] = gain_map
|
||||
return gain_map
|
||||
|
||||
def _break_field_window(self) -> None:
|
||||
"""An invalid frame breaks the consecutive declared unloaded window."""
|
||||
if self._field_baseline is None:
|
||||
self._field_zero_values = []
|
||||
|
||||
def _predict_field(self, frame_id: int, features: Mapping[str, Any]) -> CalibrationResult:
|
||||
def invalid(reason: str, **diagnostics: Any) -> CalibrationResult:
|
||||
diagnostics.setdefault("frame_id", frame_id)
|
||||
return CalibrationResult({axis: None for axis in self.outputs},
|
||||
{axis: reason for axis in self.outputs}, diagnostics)
|
||||
|
||||
try:
|
||||
field = coerce_field(features.get("field"))
|
||||
except (TypeError, ValueError) as exc:
|
||||
self._break_field_window()
|
||||
return invalid("invalid_input", error=str(exc))
|
||||
mask = features.get("output_valid_mask")
|
||||
try:
|
||||
mask = np.asarray(mask, dtype=bool)
|
||||
_require(mask.ndim == 2 and mask.shape == self.coordinate_map.shape[:2],
|
||||
"output_valid_mask must match the coordinate map output grid")
|
||||
except (TypeError, ValueError) as exc:
|
||||
self._break_field_window()
|
||||
return invalid("invalid_input", error=str(exc))
|
||||
try:
|
||||
gain_map = self._field_map(field.grid_shape, mask)
|
||||
reduction = reduce_field(field, gain_map)
|
||||
except ValueError as exc:
|
||||
self._break_field_window()
|
||||
return invalid("coordinate_unmapped", error=str(exc))
|
||||
diagnostics: dict[str, Any] = {
|
||||
"model_fingerprint": self.fingerprint,
|
||||
"mapping": dict(MAPPING_POLICY),
|
||||
"extrapolated_fraction": reduction.extrapolated_fraction,
|
||||
"coverage_fraction": reduction.coverage_fraction,
|
||||
"area_scale": field.area_scale,
|
||||
}
|
||||
if self._field_baseline is None:
|
||||
diagnostics["reference_id"] = self._field_reference_id
|
||||
if self._field_reference_id is None:
|
||||
diagnostics["pending_reference_frames"] = 0
|
||||
return invalid("zero_unavailable", **diagnostics)
|
||||
self._field_zero_values.append(reduction.weighted_N)
|
||||
if len(self._field_zero_values) < ZERO_FRAME_COUNT:
|
||||
diagnostics["pending_reference_frames"] = len(self._field_zero_values)
|
||||
return invalid("zero_unavailable", **diagnostics)
|
||||
self._field_baseline = float(np.median(self._field_zero_values))
|
||||
self._field_zero_values = []
|
||||
diagnostics["baseline_N"] = self._field_baseline
|
||||
diagnostics["pending_reference_frames"] = 0
|
||||
return invalid("zero_unavailable", **diagnostics)
|
||||
diagnostics["baseline_N"] = self._field_baseline
|
||||
value = reduction.weighted_N - self._field_baseline
|
||||
low, high = self._params["valid_force_range_N"]
|
||||
status = "ok" if low <= value <= high else "out_of_range"
|
||||
return CalibrationResult({"fn": float(value)}, {"fn": status}, diagnostics)
|
||||
|
||||
def convert(self, fn: float, fx: float, fy: float) -> None:
|
||||
"""Reject legacy scalar calls which lack frame, position and zero features."""
|
||||
@@ -245,6 +383,8 @@ class ModelCalibration:
|
||||
if (not isinstance(features, Mapping) or type(frame_id) is not int or
|
||||
(timestamp_s is not None and not _number(timestamp_s))):
|
||||
return invalid("invalid_input")
|
||||
if self.model_type == "field_gain/1":
|
||||
return self._predict_field(frame_id, features)
|
||||
if features.get("zero_valid") is not True:
|
||||
return invalid("zero_unavailable")
|
||||
if features.get("contact_found") is False:
|
||||
@@ -296,23 +436,43 @@ def parse_model(data: dict[str, Any], source_path: str | Path | None = None) ->
|
||||
_require(isinstance(data.get("provenance", {}), dict), "provenance must be an object")
|
||||
model = data["model"]
|
||||
_keys(model, {"type", "input_profile", "output_frame", "outputs", "params"}, {"artifacts"})
|
||||
_require(model["type"] in ("spatial_gain/1", "global_gain/1"), f"unsupported calibration model: {model['type']}")
|
||||
spatial = model["type"] == "spatial_gain/1"
|
||||
_require(model["input_profile"] == ("fn_delta_autocal_uv/1" if spatial else "force_delta/1"),
|
||||
_require(model["type"] in _PROFILE_FOR_TYPE,
|
||||
f"unsupported calibration model: {model['type']}")
|
||||
model_type = model["type"]
|
||||
spatial = model_type == "spatial_gain/1"
|
||||
field_weighted = model_type == "field_gain/1"
|
||||
_require(model["input_profile"] == _PROFILE_FOR_TYPE[model_type],
|
||||
"unsupported input_profile for model")
|
||||
_require(model["output_frame"] == "sdk_force/1", "unsupported output_frame")
|
||||
outputs = model["outputs"]
|
||||
_require(isinstance(outputs, list) and bool(outputs) and all(a in ("fn", "fx", "fy") for a in outputs) and
|
||||
len(set(outputs)) == len(outputs), "invalid model outputs")
|
||||
_require(not spatial or outputs == ["fn"], "spatial model supports fn only")
|
||||
_pipeline(data["pipeline"], spatial)
|
||||
_require(not (spatial or field_weighted) or outputs == ["fn"], "spatial model supports fn only")
|
||||
_pipeline(data["pipeline"], model["input_profile"])
|
||||
params = model["params"]
|
||||
_keys(params, {"gains_N_per_raw", "valid_force_range_N"} | ({"points_uv", "triangles"} if spatial else set()))
|
||||
required_params = {"gains_N_per_raw", "valid_force_range_N"}
|
||||
if spatial or field_weighted:
|
||||
required_params |= {"points_uv", "triangles"}
|
||||
if field_weighted:
|
||||
required_params |= {"mapping", "baseline"}
|
||||
_keys(params, required_params)
|
||||
force_range = params["valid_force_range_N"]
|
||||
_require(isinstance(force_range, list) and len(force_range) == 2 and
|
||||
all(_number(x) for x in force_range) and force_range[0] < force_range[1], "invalid force range")
|
||||
if not spatial:
|
||||
gains = params["gains_N_per_raw"]
|
||||
_keys(gains, set(outputs))
|
||||
_require(all(_number(g) and g > 0 for g in gains.values()), "gains must be finite positive numbers")
|
||||
if field_weighted:
|
||||
_require(isinstance(gains, list) and len(gains) == len(params["points_uv"]),
|
||||
"field gains must have one value per support point")
|
||||
else:
|
||||
_keys(gains, set(outputs))
|
||||
_require(all(_number(g) and g > 0 for g in gains.values()) if isinstance(gains, dict)
|
||||
else all(_number(g) and g > 0 for g in gains),
|
||||
"gains must be finite positive numbers")
|
||||
if field_weighted:
|
||||
_require(params["mapping"] == MAPPING_POLICY, "unsupported field mapping policy")
|
||||
_require(params["baseline"] == BASELINE_POLICY, "unsupported field baseline policy")
|
||||
artifacts = model.get("artifacts")
|
||||
_require(isinstance(artifacts, list) and len(artifacts) == 1,
|
||||
"field_gain/1 requires one coordinate_map artifact")
|
||||
return ModelCalibration(data, source_path)
|
||||
|
||||
@@ -0,0 +1,347 @@
|
||||
"""Field-weighted force estimation from a validated support mesh and HHD grid.
|
||||
|
||||
The SDK builds a gain map on the finger output grid, downsamples it by valid
|
||||
area onto the HHD computation grid, and reduces the curl-free HHD component
|
||||
with ``q = |d_x| + |d_y|``. The map is a deterministic function of the model
|
||||
mesh, the coordinate artifact and the physical valid mask, so callers cache it
|
||||
by those identities instead of rebuilding it every frame.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
from collections import Counter
|
||||
from dataclasses import dataclass
|
||||
from typing import Any, Mapping
|
||||
|
||||
import numpy as np
|
||||
|
||||
#: Closed identity of the field input profile. It is part of the exported and
|
||||
#: runtime feature signature, so changing any value is a schema/profile change.
|
||||
FIELD_PROFILE = "hhd_normal_l1_field/1"
|
||||
FIELD_IDENTITY: dict[str, Any] = {
|
||||
"grid": "hhd_native",
|
||||
"shape": [None, None, 2],
|
||||
"mask_shape": [None, None],
|
||||
"vector": "irrotational",
|
||||
"reduction": "l1",
|
||||
"dtype": "float32",
|
||||
"unit": "raw",
|
||||
"mask": "physical_valid",
|
||||
"lifecycle": "same_frame",
|
||||
"area_compensation": "reference_grid",
|
||||
}
|
||||
#: How a gain is read inside and outside the support hull. Deterministic on
|
||||
#: purpose: equal-distance points choose the lowest sorted boundary edge.
|
||||
MAPPING_POLICY: dict[str, Any] = {
|
||||
"inside": "barycentric",
|
||||
"outside": "boundary_projection",
|
||||
"tie_break": "boundary_edge_order",
|
||||
}
|
||||
#: Session baseline reduction; the caller declares the unloaded window.
|
||||
BASELINE_POLICY: dict[str, Any] = {"frames": 30, "statistic": "median"}
|
||||
ZERO_FRAME_COUNT = int(BASELINE_POLICY["frames"])
|
||||
|
||||
_INSIDE_TOLERANCE = 1e-10
|
||||
_GAIN_TOLERANCE = 1e-9
|
||||
|
||||
|
||||
def _require(condition: bool, message: str) -> None:
|
||||
if not condition:
|
||||
raise ValueError(message)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FieldFrame:
|
||||
"""One frame of the HHD curl-free field on the native computation grid."""
|
||||
|
||||
d: np.ndarray
|
||||
valid_mask: np.ndarray
|
||||
area_scale: float
|
||||
grid_shape: tuple[int, int]
|
||||
reference_shape: tuple[int, int] | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FieldGainMap:
|
||||
"""Area-averaged gains on the HHD grid, with coverage as an area fraction.
|
||||
|
||||
``coverage`` is the fraction of each cell's pixels that carry a mapped
|
||||
coordinate and a gain; ``inside`` is the fraction whose gain came from
|
||||
barycentric interpolation. ``coverage - inside`` is the boundary-projected
|
||||
(extrapolated) fraction used for the per-frame quality diagnostic.
|
||||
"""
|
||||
|
||||
grid_shape: tuple[int, int]
|
||||
gain: np.ndarray
|
||||
coverage: np.ndarray
|
||||
inside: np.ndarray
|
||||
physical_mask_sha256: str
|
||||
|
||||
def diagnostic(self) -> dict[str, float]:
|
||||
return {
|
||||
"coverage": float(self.coverage.mean()),
|
||||
"inside": float(self.inside.mean()),
|
||||
"extrapolated": float(np.clip(self.coverage - self.inside, 0.0, 1.0).mean()),
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FieldReduction:
|
||||
"""Weighted field sum and its per-frame extrapolation diagnostic."""
|
||||
|
||||
weighted_N: float
|
||||
extrapolated_fraction: float
|
||||
coverage_fraction: float
|
||||
|
||||
|
||||
def coerce_field(value: Any) -> FieldFrame:
|
||||
"""Validate the same-frame field payload published by ``hhd/api.py``."""
|
||||
_require(isinstance(value, Mapping), "field input must be a mapping")
|
||||
d = value.get("d")
|
||||
mask = value.get("valid_mask")
|
||||
area_scale = value.get("area_scale")
|
||||
grid_shape = value.get("grid_shape")
|
||||
reference_shape = value.get("reference_shape")
|
||||
_require(isinstance(d, np.ndarray) and d.ndim == 3 and d.shape[2] == 2,
|
||||
"field d must be an (H, W, 2) array")
|
||||
_require(d.dtype == np.float32, "field d dtype must be float32")
|
||||
_require(isinstance(mask, np.ndarray) and mask.dtype == np.bool_ and mask.shape == d.shape[:2],
|
||||
"field valid_mask must be an (H, W) bool array")
|
||||
_require(isinstance(area_scale, (int, float)) and not isinstance(area_scale, bool)
|
||||
and np.isfinite(area_scale) and area_scale > 0,
|
||||
"field area_scale must be finite and positive")
|
||||
_require(isinstance(grid_shape, tuple) and len(grid_shape) == 2
|
||||
and all(type(size) is int and size > 0 for size in grid_shape),
|
||||
"field grid_shape must be two positive integers")
|
||||
_require(tuple(grid_shape) == tuple(d.shape[:2]), "field grid_shape does not match d")
|
||||
if reference_shape is not None:
|
||||
_require(isinstance(reference_shape, tuple) and len(reference_shape) == 2
|
||||
and all(type(size) is int and size > 0 for size in reference_shape),
|
||||
"field reference_shape must be two positive integers")
|
||||
if not np.isfinite(d[mask]).all():
|
||||
raise ValueError("field contains non-finite active vectors")
|
||||
return FieldFrame(
|
||||
d=d,
|
||||
valid_mask=mask,
|
||||
area_scale=float(area_scale),
|
||||
grid_shape=(int(grid_shape[0]), int(grid_shape[1])),
|
||||
reference_shape=None if reference_shape is None else (int(reference_shape[0]),
|
||||
int(reference_shape[1])),
|
||||
)
|
||||
|
||||
|
||||
def mask_sha256(mask: np.ndarray) -> str:
|
||||
"""Stable identity of a boolean mask for cache keys."""
|
||||
packed = np.packbits(np.ascontiguousarray(mask, dtype=bool))
|
||||
return hashlib.sha256(packed.tobytes()).hexdigest()
|
||||
|
||||
|
||||
def _boundary_edges(triangles: np.ndarray) -> list[tuple[int, int]]:
|
||||
"""Hull edges of a validated complete triangulation, in a fixed order."""
|
||||
counts: Counter[tuple[int, int]] = Counter()
|
||||
for triangle in triangles:
|
||||
indices = [int(index) for index in triangle]
|
||||
for start, end in ((0, 1), (1, 2), (2, 0)):
|
||||
edge = tuple(sorted((indices[start], indices[end])))
|
||||
counts[edge] += 1
|
||||
boundary = [edge for edge, count in counts.items() if count == 1]
|
||||
boundary.sort()
|
||||
_require(bool(boundary), "support mesh has no boundary edges")
|
||||
return boundary
|
||||
|
||||
|
||||
def _inside_gain(points: np.ndarray, gains: np.ndarray, triangles: np.ndarray,
|
||||
uv: np.ndarray, pending: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Barycentric gain for pending pixels, first matching triangle in file order."""
|
||||
gain = np.full(len(uv), np.nan)
|
||||
inside = np.zeros(len(uv), dtype=bool)
|
||||
for triangle in triangles:
|
||||
candidate = ~inside & pending
|
||||
if not candidate.any():
|
||||
break
|
||||
p0, p1, p2 = (points[int(index)] for index in triangle)
|
||||
query = uv[candidate]
|
||||
v0, v1 = p1 - p0, p2 - p0
|
||||
det = float(v0[0] * v1[1] - v1[0] * v0[1])
|
||||
_require(abs(det) > 0, "support mesh has a degenerate triangle")
|
||||
offset = query - p0
|
||||
w1 = (offset[:, 0] * v1[1] - offset[:, 1] * v1[0]) / det
|
||||
w2 = (v0[0] * offset[:, 1] - v0[1] * offset[:, 0]) / det
|
||||
weights = np.stack((1.0 - w1 - w2, w1, w2), axis=1)
|
||||
hit = weights.min(axis=1) >= -_INSIDE_TOLERANCE
|
||||
if not hit.any():
|
||||
continue
|
||||
hit_indices = np.flatnonzero(candidate)[hit]
|
||||
gain[hit_indices] = (
|
||||
weights[hit, 0] * gains[int(triangle[0])]
|
||||
+ weights[hit, 1] * gains[int(triangle[1])]
|
||||
+ weights[hit, 2] * gains[int(triangle[2])]
|
||||
)
|
||||
inside[hit_indices] = True
|
||||
return gain, inside
|
||||
|
||||
|
||||
def _boundary_gain(points: np.ndarray, gains: np.ndarray, triangles: np.ndarray,
|
||||
uv: np.ndarray, pending: np.ndarray) -> np.ndarray:
|
||||
"""Project pending points onto the nearest hull edge and interpolate its gains.
|
||||
|
||||
Equal distances keep the first edge in sorted ``(index, index)`` order, so
|
||||
the result does not depend on triangle storage order. The projection
|
||||
parameter is clamped to the segment; gains stay inside the support range.
|
||||
"""
|
||||
gain = np.full(len(uv), np.nan)
|
||||
indices = np.flatnonzero(pending)
|
||||
if indices.size == 0:
|
||||
return gain
|
||||
query = uv[indices]
|
||||
best_distance = np.full(len(indices), np.inf)
|
||||
for start, end in _boundary_edges(triangles):
|
||||
first, second = points[start], points[end]
|
||||
direction = second - first
|
||||
denominator = float(np.dot(direction, direction))
|
||||
_require(denominator > 0, "support mesh has a zero-length boundary edge")
|
||||
parameter = np.clip(((query - first) @ direction) / denominator, 0.0, 1.0)
|
||||
projected = first + parameter[:, None] * direction
|
||||
distance = np.sum((query - projected) ** 2, axis=1)
|
||||
better = distance < best_distance - 1e-12
|
||||
best_distance[better] = distance[better]
|
||||
gain[indices[better]] = gains[start] + parameter[better] * (gains[end] - gains[start])
|
||||
_require(np.isfinite(gain[indices]).all(), "support mesh boundary projection failed")
|
||||
return gain
|
||||
|
||||
|
||||
def _block_starts(divisions: int, size: int) -> tuple[np.ndarray, np.ndarray]:
|
||||
starts = np.array([index * size // divisions for index in range(divisions)], dtype=np.intp)
|
||||
ends = np.array([(index + 1) * size // divisions for index in range(divisions)], dtype=np.intp)
|
||||
return starts, ends
|
||||
|
||||
|
||||
def build_field_gain_map(points: np.ndarray, gains: np.ndarray, triangles: np.ndarray,
|
||||
coordinate_map: np.ndarray, coordinate_valid_mask: np.ndarray,
|
||||
grid_shape: tuple[int, int],
|
||||
physical_mask: np.ndarray) -> FieldGainMap:
|
||||
"""Build the gain/coverage map for one output mask and one HHD grid shape.
|
||||
|
||||
Raises :class:`ValueError` when the physical mask reaches pixels that the
|
||||
model's coordinate artifact does not map; that is a model mapping error and
|
||||
must not silently become a zero or an extrapolated gain.
|
||||
"""
|
||||
_require(isinstance(points, np.ndarray) and points.ndim == 2 and points.shape[1] == 2,
|
||||
"support points must be an (N, 2) array")
|
||||
_require(isinstance(gains, np.ndarray) and gains.shape == (len(points),),
|
||||
"support gains must have one value per point")
|
||||
_require(isinstance(triangles, np.ndarray) and triangles.ndim == 2 and triangles.shape[1] == 3,
|
||||
"support triangles must be an (T, 3) array")
|
||||
_require(isinstance(coordinate_map, np.ndarray) and coordinate_map.ndim == 3
|
||||
and coordinate_map.shape[2] == 2, "coordinate map must be an (H, W, 2) array")
|
||||
_require(isinstance(coordinate_valid_mask, np.ndarray)
|
||||
and coordinate_valid_mask.dtype == np.bool_
|
||||
and coordinate_valid_mask.shape == coordinate_map.shape[:2],
|
||||
"coordinate valid mask must be an (H, W) bool array")
|
||||
output_shape = tuple(int(size) for size in coordinate_map.shape[:2])
|
||||
_require(isinstance(grid_shape, tuple) and len(grid_shape) == 2
|
||||
and all(type(size) is int and size > 0 for size in grid_shape),
|
||||
"grid shape must be two positive integers")
|
||||
_require(grid_shape[0] <= output_shape[0] and grid_shape[1] <= output_shape[1],
|
||||
"HHD grid cannot exceed the output grid")
|
||||
_require(isinstance(physical_mask, np.ndarray) and physical_mask.dtype == np.bool_
|
||||
and physical_mask.shape == output_shape,
|
||||
"physical mask must match the output grid")
|
||||
if np.any(physical_mask & ~coordinate_valid_mask):
|
||||
raise ValueError("model mapping error: physical mask has pixels without mapped coordinates")
|
||||
|
||||
uv = np.asarray(coordinate_map[physical_mask], dtype=np.float64).reshape(-1, 2)
|
||||
_require(np.isfinite(uv).all(), "model mapping error: mapped coordinates are not finite")
|
||||
pixels = int(uv.shape[0])
|
||||
gain_flat = np.full(pixels, np.nan)
|
||||
inside_flat = np.zeros(pixels, dtype=bool)
|
||||
pending = np.ones(pixels, dtype=bool)
|
||||
if pixels:
|
||||
inside_gain, inside_flat = _inside_gain(points, gains, triangles, uv, pending)
|
||||
gain_flat[inside_flat] = inside_gain[inside_flat]
|
||||
pending = ~inside_flat
|
||||
if pending.any():
|
||||
boundary_gain = _boundary_gain(points, gains, triangles, uv, pending)
|
||||
gain_flat[pending] = boundary_gain[pending]
|
||||
low, high = float(gains.min()), float(gains.max())
|
||||
_require(np.isfinite(gain_flat).all(), "gain map contains unmapped valid pixels")
|
||||
_require(np.all(gain_flat >= low - _GAIN_TOLERANCE)
|
||||
and np.all(gain_flat <= high + _GAIN_TOLERANCE),
|
||||
"interpolated gain outside the support range")
|
||||
|
||||
valid_flat = np.ones(pixels, dtype=np.float64)
|
||||
inside_values = inside_flat.astype(np.float64)
|
||||
gain_values = gain_flat
|
||||
|
||||
def _reduce(values: np.ndarray) -> np.ndarray:
|
||||
grid = np.zeros(output_shape, dtype=np.float64)
|
||||
grid[physical_mask] = values
|
||||
rows, _ = _block_starts(grid_shape[0], output_shape[0])
|
||||
row_blocks = np.add.reduceat(grid, rows, axis=0)
|
||||
cols, _ = _block_starts(grid_shape[1], output_shape[1])
|
||||
return np.add.reduceat(row_blocks, cols, axis=1)
|
||||
|
||||
area = _reduce(valid_flat)
|
||||
gain_sum = _reduce(gain_values)
|
||||
inside_area = _reduce(inside_values)
|
||||
row_starts, row_ends = _block_starts(grid_shape[0], output_shape[0])
|
||||
col_starts, col_ends = _block_starts(grid_shape[1], output_shape[1])
|
||||
block_area = np.outer(row_ends - row_starts, col_ends - col_starts).astype(np.float64)
|
||||
_require(np.all(block_area > 0), "HHD grid cells cannot be empty")
|
||||
coverage = area / block_area
|
||||
inside = inside_area / block_area
|
||||
gain = np.divide(gain_sum, area, out=np.zeros_like(gain_sum), where=area > 0)
|
||||
_require(np.isfinite(gain).all() and np.isfinite(coverage).all(),
|
||||
"field gain map reduction failed")
|
||||
coverage.setflags(write=False)
|
||||
inside.setflags(write=False)
|
||||
gain.setflags(write=False)
|
||||
return FieldGainMap(
|
||||
grid_shape=(int(grid_shape[0]), int(grid_shape[1])),
|
||||
gain=gain.astype(np.float64),
|
||||
coverage=coverage,
|
||||
inside=inside,
|
||||
physical_mask_sha256=mask_sha256(physical_mask),
|
||||
)
|
||||
|
||||
|
||||
def reduce_field(field: FieldFrame, gain_map: FieldGainMap) -> FieldReduction:
|
||||
"""Apply ``P = A * sum(gain * (|d_x| + |d_y|))`` over active HHD cells.
|
||||
|
||||
The HHD raw aggregate is ``A * sum(active * q)``; multiplying the same sum by
|
||||
a constant gain must therefore give exactly ``gain * raw``. ``coverage`` is
|
||||
diagnostic-only here: folding it into ``P`` would discount a partially covered
|
||||
mask-edge cell that the raw aggregate still counts in full. An active cell with
|
||||
no mapped output pixel has no valid gain, so it is reported as a model mapping
|
||||
error instead of silently contributing zero.
|
||||
|
||||
``area_scale`` comes from ``hhd/api.py``'s existing reference-grid
|
||||
compensation, so the caller must not apply another area factor.
|
||||
"""
|
||||
_require(field.grid_shape == gain_map.grid_shape,
|
||||
"field grid shape does not match the gain map; rebuild the cache")
|
||||
d = np.asarray(field.d, dtype=np.float64)
|
||||
active = np.asarray(field.valid_mask, dtype=bool)
|
||||
q = np.where(active, np.abs(d[:, :, 0]) + np.abs(d[:, :, 1]), 0.0)
|
||||
q = np.where(np.isfinite(q), q, 0.0)
|
||||
if np.any(active & (gain_map.coverage <= 0.0)):
|
||||
raise ValueError("model mapping error: active HHD cells have no mapped pixels")
|
||||
total = float(np.sum(gain_map.gain * q) * field.area_scale)
|
||||
coverage_mass = q * gain_map.coverage
|
||||
source_sum = float(np.sum(q))
|
||||
coverage_sum = float(np.sum(coverage_mass))
|
||||
if source_sum > 0.0:
|
||||
extrapolated = float(np.sum(q * np.clip(gain_map.coverage - gain_map.inside, 0.0, 1.0))
|
||||
/ source_sum)
|
||||
extrapolated = float(np.clip(extrapolated, 0.0, 1.0))
|
||||
coverage_fraction = float(np.clip(coverage_sum / source_sum, 0.0, 1.0))
|
||||
else:
|
||||
extrapolated = 0.0
|
||||
coverage_fraction = 0.0
|
||||
_require(np.isfinite(total), "weighted field sum is not finite")
|
||||
return FieldReduction(
|
||||
weighted_N=total,
|
||||
extrapolated_fraction=extrapolated,
|
||||
coverage_fraction=coverage_fraction,
|
||||
)
|
||||
@@ -14,6 +14,7 @@ from ..capture import (
|
||||
open_capture,
|
||||
)
|
||||
from ..configs import load_config
|
||||
from ..field_calibration import FIELD_IDENTITY, FIELD_PROFILE
|
||||
from ..frame_processors import FrameUnwarper
|
||||
from ..sensor import Sensor
|
||||
from ..unroll import (
|
||||
@@ -326,9 +327,10 @@ class FingerRuntime:
|
||||
|
||||
model = sensor.calibration
|
||||
uv_map = getattr(model, "coordinate_map", None)
|
||||
if model.input_profile == "fn_delta_autocal_uv/1":
|
||||
if model.input_profile in ("fn_delta_autocal_uv/1", FIELD_PROFILE):
|
||||
if uv_map is None or uv_map.shape[:2] != (geometry.out_height, geometry.out_width):
|
||||
raise ValueError("calibration coordinate map does not match finger output shape")
|
||||
field_identity = FIELD_IDENTITY if model.input_profile == FIELD_PROFILE else None
|
||||
sensor.bind_calibration_context(build_context(
|
||||
sensor._config, sensor.filters_state(),
|
||||
geometry_sha256=sha256_file(mesh_path), source_scale=geometry.source_scale,
|
||||
@@ -336,6 +338,7 @@ class FingerRuntime:
|
||||
output_domain_sha256=domain_fingerprint(domain),
|
||||
processing_backend="uv_unroll",
|
||||
preprocessing={"flip_source_vertical": flip_source_vertical, "mirror": mirror},
|
||||
field=field_identity,
|
||||
))
|
||||
|
||||
capture_config = CaptureConfig.from_camera_dict(
|
||||
@@ -399,19 +402,25 @@ class FingerRuntime:
|
||||
selected = cal_mod.validate_for_device(selected, "finger", serial)
|
||||
context = None
|
||||
if getattr(selected, "schema_version", None) == 3:
|
||||
if selected.input_profile == "fn_delta_autocal_uv/1":
|
||||
if selected.input_profile in ("fn_delta_autocal_uv/1", FIELD_PROFILE):
|
||||
uv_map = selected.coordinate_map
|
||||
if uv_map is None or uv_map.shape[:2] != (self.geometry.out_height, self.geometry.out_width):
|
||||
raise ValueError("calibration coordinate map does not match finger output shape")
|
||||
field_identity = FIELD_IDENTITY if selected.input_profile == FIELD_PROFILE else None
|
||||
context = build_context(
|
||||
self.geometry.config, sensor.filters_state(),
|
||||
geometry_sha256=sha256_file(self.geometry.export_paths.mesh),
|
||||
source_scale=self.geometry.source_scale, coordinates=selected.pipeline.get("coordinates"),
|
||||
output_domain_sha256=domain_fingerprint(self.domain), processing_backend="uv_unroll",
|
||||
preprocessing={"flip_source_vertical": self.frame_processor.flip_source_vertical,
|
||||
"mirror": sensor.mirror})
|
||||
"mirror": sensor.mirror},
|
||||
field=field_identity)
|
||||
sensor.set_calibration(selected, context=context)
|
||||
|
||||
def declare_unloaded_reference(self, reference_id):
|
||||
"""Declare the unloaded window for a field model; the SDK medians 30 weighted frames."""
|
||||
self.tactile.raw_sensor.declare_unloaded_reference(reference_id=reference_id)
|
||||
|
||||
@property
|
||||
def calibration_status(self):
|
||||
"""Copy the current per-axis calibration validity state."""
|
||||
|
||||
@@ -155,6 +155,37 @@ class nHHD:
|
||||
return self.d
|
||||
return cp.asnumpy(self._d_gpu)
|
||||
|
||||
def get_field(self, valid_mask=None):
|
||||
"""Curl-free HHD component and its reference-grid area scale for one frame.
|
||||
|
||||
The SDK calibration layer owns the field *profile* identity; this module
|
||||
only publishes the numerical payload. It belongs to the frame that was
|
||||
just decomposed: the next ``decompose`` call overwrites the solver's
|
||||
arrays, so callers copy whatever they keep.
|
||||
|
||||
Returns:
|
||||
dict with ``d`` (float32 HxWx2 copy), ``valid_mask`` (HxW bool),
|
||||
``area_scale`` (reference pixels / current pixels, the same factor
|
||||
``get_force`` already applies), ``grid_shape`` and ``reference_shape``.
|
||||
"""
|
||||
direction = getattr(self, 'd', None)
|
||||
if direction is None:
|
||||
raise ValueError("field requested before decompose")
|
||||
shape = direction.shape[:2]
|
||||
mask = self._normalize_mask(valid_mask, shape) if valid_mask is not None else None
|
||||
field = numpy.array(direction, dtype=numpy.float32, copy=True)
|
||||
if mask is not None:
|
||||
field[~mask] = 0.0
|
||||
area_scale, _ = self._force_scale(shape)
|
||||
reference = self._force_ref_shape or shape
|
||||
return {
|
||||
'd': field,
|
||||
'valid_mask': numpy.ones(shape, dtype=bool) if mask is None else mask.copy(),
|
||||
'area_scale': float(area_scale),
|
||||
'grid_shape': (int(shape[0]), int(shape[1])),
|
||||
'reference_shape': (int(reference[0]), int(reference[1])),
|
||||
}
|
||||
|
||||
def _solve_poisson_backend(self, field, verbose=False, valid_mask=None):
|
||||
"""Dispatch Poisson solve using the selected backend.
|
||||
|
||||
|
||||
+52
-9
@@ -17,12 +17,13 @@ import numpy as np
|
||||
from . import calibration as cal_mod
|
||||
from . import configs
|
||||
from .backend_availability import check_cupy_available, check_opencl_available
|
||||
from .force_readout import (
|
||||
FILTER_PRESETS,
|
||||
ForceReadout,
|
||||
available_filter_presets,
|
||||
)
|
||||
# Explicit re-exports: ``calibration.py`` resolves named presets through this
|
||||
# module and the preset tests import the table from here.
|
||||
from .force_readout import ForceReadout
|
||||
from .force_readout import FILTER_PRESETS as FILTER_PRESETS
|
||||
from .force_readout import available_filter_presets as available_filter_presets
|
||||
from .frame_processors import create_frame_processor
|
||||
from .field_calibration import FIELD_PROFILE
|
||||
from .get_depth import Contact3D
|
||||
from .hhd import nHHD
|
||||
|
||||
@@ -790,7 +791,13 @@ class Sensor:
|
||||
if self._uses_calibration_model():
|
||||
# Spatial inference must use this HHD frame's contact, even when the
|
||||
# caller did not request contact diagnostics from StreamPipeline.
|
||||
if self.calibration.input_profile == "fn_delta_autocal_uv/1":
|
||||
# The field model needs no locator: it consumes this frame's field.
|
||||
if self.calibration.input_profile == FIELD_PROFILE:
|
||||
self.apply_calibration_frame(
|
||||
frame_id=self._model_frame_id,
|
||||
field=self.__hhd.get_field(valid_mask=mask_ds),
|
||||
)
|
||||
elif self.calibration.input_profile == "fn_delta_autocal_uv/1":
|
||||
self.compute_contact()
|
||||
else:
|
||||
self.apply_calibration_frame(frame_id=self._model_frame_id)
|
||||
@@ -809,7 +816,9 @@ class Sensor:
|
||||
"""
|
||||
if self._uses_calibration_model() and not self.compute_deformation_valid:
|
||||
raise cal_mod.CalibrationError("compute current deformation before schema 3 contact")
|
||||
if (self._uses_calibration_model() and self.compute_contact_valid
|
||||
if (self._uses_calibration_model()
|
||||
and self.calibration.input_profile == "fn_delta_autocal_uv/1"
|
||||
and self.compute_contact_valid
|
||||
and self._calibration_result_frame == self._model_frame_id):
|
||||
return self._model_contact_result
|
||||
multiple_foe = False
|
||||
@@ -845,7 +854,8 @@ class Sensor:
|
||||
print("Warning: contact centroid lies outside the valid UV mask.")
|
||||
|
||||
self.compute_contact_valid = True
|
||||
if self._uses_calibration_model():
|
||||
if (self._uses_calibration_model()
|
||||
and self.calibration.input_profile == "fn_delta_autocal_uv/1"):
|
||||
self._model_contact_result = result
|
||||
uv = self._calibration_contact_uv()
|
||||
self.apply_calibration_frame(
|
||||
@@ -1290,6 +1300,12 @@ class Sensor:
|
||||
|
||||
def set_calibration_reference(self, raw, *, reference_id):
|
||||
"""Declare a measured unloaded raw reference for this stream, not the model file."""
|
||||
if (self._uses_calibration_model()
|
||||
and self.calibration.input_profile == FIELD_PROFILE):
|
||||
raise cal_mod.CalibrationError(
|
||||
"field model needs a declared weighted unloaded window, not a scalar zero; "
|
||||
"use declare_unloaded_reference()"
|
||||
)
|
||||
if not isinstance(raw, dict) or not reference_id:
|
||||
raise ValueError("raw must map axes to finite values and reference_id is required")
|
||||
values = {}
|
||||
@@ -1303,6 +1319,26 @@ class Sensor:
|
||||
if self._uses_calibration_model():
|
||||
self.calibration.reset(self._calibration_reference_id)
|
||||
|
||||
def declare_unloaded_reference(self, *, reference_id):
|
||||
"""Start the declared unloaded window for a field model.
|
||||
|
||||
The model medians the next 30 consecutive valid weighted field sums;
|
||||
until then every frame reports ``zero_unavailable``. Contact location is
|
||||
deliberately not consulted, and the scalar raw reference path stays
|
||||
rejected for this profile.
|
||||
"""
|
||||
if not self._uses_calibration_model():
|
||||
raise cal_mod.CalibrationError("unloaded reference requires a schema 3 calibration")
|
||||
if self.calibration.input_profile != FIELD_PROFILE:
|
||||
raise cal_mod.CalibrationError(
|
||||
"weighted unloaded reference requires hhd_normal_l1_field/1")
|
||||
if not isinstance(reference_id, str) or not reference_id.strip():
|
||||
raise ValueError("reference_id must be a non-empty string")
|
||||
self._reset_calibration_session()
|
||||
self._calibration_reference = {}
|
||||
self._calibration_reference_id = str(reference_id)
|
||||
self.calibration.begin_reference(str(reference_id))
|
||||
|
||||
def _calibration_contact_uv(self):
|
||||
if not self.centroid_found:
|
||||
return None
|
||||
@@ -1320,8 +1356,9 @@ class Sensor:
|
||||
def apply_calibration_frame(
|
||||
self, *, frame_id, timestamp_s=None, raw=None, uv=None,
|
||||
contact_found=None, mapping_valid=False, single_contact=True,
|
||||
field=None, output_mask=None,
|
||||
):
|
||||
"""Infer once from an unfiltered raw triple and same-frame optional contact UV."""
|
||||
"""Infer once from an unfiltered raw triple, contact UV or same-frame field."""
|
||||
if not self._uses_calibration_model():
|
||||
raise cal_mod.CalibrationError("apply_calibration_frame requires schema 3")
|
||||
if self._calibration_context is None:
|
||||
@@ -1353,6 +1390,12 @@ class Sensor:
|
||||
}
|
||||
if contact_found is not None:
|
||||
features["contact_found"] = bool(contact_found)
|
||||
if field is not None:
|
||||
features["field"] = field
|
||||
mask = self.valid_mask if output_mask is None else output_mask
|
||||
if mask is None:
|
||||
mask = np.ones(self.calibration.coordinate_map.shape[:2], dtype=bool)
|
||||
features["output_valid_mask"] = np.asarray(mask, dtype=bool)
|
||||
result = self.calibration.predict(
|
||||
frame_id=frame_id, timestamp_s=timestamp_s,
|
||||
features=features, reference_id=self._calibration_reference_id,
|
||||
|
||||
Reference in New Issue
Block a user