ykcheungab
308 lines
16 KiB
Python
308 lines
16 KiB
Python
"""Validated, versioned force models; acquisition and training stay outside here."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import hashlib
|
|
import json
|
|
import math
|
|
from collections.abc import Mapping
|
|
from copy import deepcopy
|
|
from dataclasses import dataclass
|
|
from pathlib import Path, PurePosixPath, PureWindowsPath
|
|
from typing import Any
|
|
|
|
import numpy as np
|
|
from scipy.spatial import ConvexHull, QhullError
|
|
|
|
|
|
def _require(condition: bool, message: str) -> None:
|
|
if not condition:
|
|
raise ValueError(message)
|
|
|
|
|
|
def _keys(value: Any, required: set[str], optional: set[str] | None = None) -> None:
|
|
_require(isinstance(value, dict), "expected a JSON object")
|
|
_require(required <= value.keys(), f"missing fields: {required - value.keys()}")
|
|
_require(value.keys() <= required | (optional or set()), "unknown semantic fields")
|
|
|
|
|
|
def _number(value: Any) -> bool:
|
|
return isinstance(value, (int, float)) and not isinstance(value, bool) and math.isfinite(value)
|
|
|
|
|
|
def _sha(value: Any) -> bool:
|
|
return isinstance(value, str) and len(value) == 64 and all(c in "0123456789abcdef" for c in value)
|
|
|
|
|
|
def _area(polygon: np.ndarray) -> float:
|
|
if len(polygon) < 3:
|
|
return 0.0
|
|
return abs(float(np.dot(polygon[:, 0], np.roll(polygon[:, 1], 1)) -
|
|
np.dot(polygon[:, 1], np.roll(polygon[:, 0], 1)))) / 2
|
|
|
|
|
|
def _cross(a: np.ndarray, b: np.ndarray) -> float:
|
|
return float(a[0] * b[1] - a[1] * b[0])
|
|
|
|
|
|
def _intersection_area(first: np.ndarray, second: np.ndarray) -> float:
|
|
"""Clip two counterclockwise convex polygons, including shared edges."""
|
|
polygon = list(first)
|
|
for a, b in zip(second, np.roll(second, -1, axis=0)):
|
|
previous = polygon
|
|
polygon = []
|
|
if not previous:
|
|
break
|
|
for start, end in zip(previous, previous[1:] + previous[:1]):
|
|
ds, de = _cross(b - a, start - a), _cross(b - a, end - a)
|
|
if (ds >= 0) != (de >= 0):
|
|
polygon.append(start + (end - start) * ds / (ds - de))
|
|
if de >= 0:
|
|
polygon.append(end)
|
|
return _area(np.asarray(polygon))
|
|
|
|
|
|
def _mesh(params: dict[str, Any]) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
|
points, gains, triangles = (params[k] for k in ("points_uv", "gains_N_per_raw", "triangles"))
|
|
_require(isinstance(points, list) and len(points) >= 3, "spatial mesh needs at least 3 points")
|
|
_require(all(isinstance(p, list) and len(p) == 2 and all(_number(x) for x in p) for p in points),
|
|
"points_uv must be finite Nx2 numbers")
|
|
_require(len({tuple(p) for p in points}) == len(points), "duplicate spatial points")
|
|
_require(isinstance(gains, list) and len(gains) == len(points) and
|
|
all(_number(g) and g > 0 for g in gains), "gains must be finite positive numbers")
|
|
_require(isinstance(triangles, list) and bool(triangles), "triangles are required")
|
|
_require(all(isinstance(t, list) and len(t) == 3 and
|
|
all(type(i) is int and 0 <= i < len(points) for i in t) and len(set(t)) == 3
|
|
for t in triangles),
|
|
"invalid triangle indices")
|
|
vertices = np.asarray(points, dtype=float)
|
|
try:
|
|
hull_area = ConvexHull(vertices).volume
|
|
except QhullError as exc:
|
|
raise ValueError("spatial mesh points are collinear") from exc
|
|
tolerance = max(1.0, hull_area) * 1e-9
|
|
polygons = []
|
|
for indices in triangles:
|
|
polygon = vertices[indices]
|
|
signed = _cross(polygon[1] - polygon[0], polygon[2] - polygon[0])
|
|
_require(abs(signed) / 2 > tolerance, "degenerate triangle")
|
|
if signed < 0:
|
|
polygon = polygon[::-1]
|
|
_require(all(_intersection_area(polygon, other) <= tolerance for other in polygons),
|
|
"overlapping spatial triangles")
|
|
polygons.append(polygon)
|
|
_require({i for triangle in triangles for i in triangle} == set(range(len(points))),
|
|
"spatial mesh must use all support points")
|
|
_require(abs(sum(_area(p) for p in polygons) - hull_area) <= tolerance,
|
|
"triangulation must cover the complete convex hull")
|
|
return vertices, np.asarray(gains, dtype=float), np.asarray(triangles, dtype=int)
|
|
|
|
|
|
def _pipeline(data: Any, spatial: bool) -> None:
|
|
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"})
|
|
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")
|
|
for key in ("dis_mode", "hhd_downsample"):
|
|
_require(type(data[key]) is int and data[key] >= (1 if key == "hhd_downsample" else 0),
|
|
f"invalid {key}")
|
|
if "source_scale" in data:
|
|
_require(_number(data["source_scale"]) and data["source_scale"] > 0, "invalid source_scale")
|
|
_require(_sha(data["feature_signature_sha256"]), "invalid feature signature SHA-256")
|
|
_keys(data["filters"], {"measurement"}, {"readout"})
|
|
_keys(data["filters"]["measurement"], {"flow_ema_alpha"})
|
|
alpha = data["filters"]["measurement"]["flow_ema_alpha"]
|
|
_require(_number(alpha) and 0 < alpha <= 1, "invalid flow_ema_alpha")
|
|
if "geometry_sha256" in data:
|
|
_require(_sha(data["geometry_sha256"]), "invalid geometry SHA-256")
|
|
if "output_domain_sha256" in data:
|
|
_require(_sha(data["output_domain_sha256"]), "invalid output domain SHA-256")
|
|
if "coordinates" in data:
|
|
coord = data["coordinates"]
|
|
_keys(coord, {"frame", "unit", "origin_xy", "map_sha256"})
|
|
_require(coord["frame"] == "autocal_flat" and coord["unit"] == "px", "unsupported coordinates")
|
|
_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")
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class CalibrationResult:
|
|
values_N: dict[str, float | None]
|
|
status: dict[str, str]
|
|
|
|
|
|
class ModelCalibration:
|
|
"""One validated model per stream. Current gain backends are stateless."""
|
|
|
|
schema_version = 3
|
|
unit = "N"
|
|
|
|
def __init__(self, document: dict[str, Any], source_path: str | Path | None = None):
|
|
self._document = deepcopy(document)
|
|
self.source_path = Path(source_path).resolve() if source_path is not None else None
|
|
self.identity = deepcopy(document["identity"])
|
|
self.pipeline = deepcopy(document["pipeline"])
|
|
self.provenance = deepcopy(document.get("provenance", {}))
|
|
self.model_type = document["model"]["type"]
|
|
self.input_profile = document["model"]["input_profile"]
|
|
self.outputs = tuple(document["model"]["outputs"])
|
|
self.channels = {axis: "N" for axis in self.outputs}
|
|
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 = _mesh(self._params)
|
|
self._load_artifacts(document["model"].get("artifacts", []))
|
|
|
|
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",
|
|
"unsupported artifact role")
|
|
name = artifact["path"]
|
|
_require(isinstance(name, str) and bool(name) and ":" not in name and
|
|
not PureWindowsPath(name).is_absolute() and not PurePosixPath(name).is_absolute() and
|
|
".." not in PurePosixPath(name.replace("\\", "/")).parts,
|
|
"artifact path must be relative without parent traversal")
|
|
_require(self.source_path is not None, "artifact requires calibration source_path")
|
|
path = (self.source_path.parent / name).resolve()
|
|
_require(path.is_relative_to(self.source_path.parent) and path.is_file(),
|
|
"artifact missing or escapes calibration directory")
|
|
digest = hashlib.sha256(path.read_bytes()).hexdigest()
|
|
_require(_sha(artifact["sha256"]) and digest == artifact["sha256"], "artifact SHA-256 mismatch")
|
|
_require(self.pipeline["coordinates"]["map_sha256"] == digest,
|
|
"coordinate map identity does not match artifact SHA-256")
|
|
self._artifact_hashes.append(digest)
|
|
try:
|
|
with np.load(path, allow_pickle=False) as archive:
|
|
keys = set(archive.files)
|
|
_require({"uv_by_output_px", "valid_mask"} <= keys <=
|
|
{"uv_by_output_px", "valid_mask", "origin_xy", "flat_mapping_sha256"},
|
|
"invalid coordinate_map keys")
|
|
uv, mask = archive["uv_by_output_px"], archive["valid_mask"]
|
|
_require(uv.ndim == 3 and uv.shape[2] == 2 and mask.shape == uv.shape[:2],
|
|
"invalid coordinate_map shape")
|
|
_require(mask.dtype == np.bool_ and np.issubdtype(uv.dtype, np.floating),
|
|
"invalid coordinate_map dtype")
|
|
_require(np.isfinite(uv[mask]).all(), "non-finite mapped coordinates")
|
|
if "origin_xy" in keys:
|
|
_require(np.array_equal(archive["origin_xy"], self.pipeline["coordinates"]["origin_xy"]),
|
|
"coordinate_map origin mismatch")
|
|
self.coordinate_map, self.coordinate_valid_mask = uv.copy(), mask.copy()
|
|
self.coordinate_map.flags.writeable = False
|
|
self.coordinate_valid_mask.flags.writeable = False
|
|
except (OSError, TypeError) as exc:
|
|
raise ValueError("invalid coordinate_map artifact") from exc
|
|
|
|
@property
|
|
def fingerprint(self) -> str:
|
|
content = json.dumps(self._document, sort_keys=True, separators=(",", ":"), allow_nan=False)
|
|
return hashlib.sha256((content + "".join(self._artifact_hashes)).encode("utf-8")).hexdigest()
|
|
|
|
def to_dict(self) -> dict[str, Any]:
|
|
return deepcopy(self._document)
|
|
|
|
def validate_for_context(self, pipeline: Mapping[str, Any]) -> None:
|
|
for key, value in self.pipeline.items():
|
|
if key == "filters":
|
|
actual = pipeline.get(key, {})
|
|
_require(isinstance(actual, Mapping) and actual.get("measurement") == value["measurement"],
|
|
"calibration pipeline mismatch: filters.measurement")
|
|
else:
|
|
_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."""
|
|
|
|
def convert(self, fn: float, fx: float, fy: float) -> None:
|
|
"""Reject legacy scalar calls which lack frame, position and zero features."""
|
|
from .calibration import CalibrationError
|
|
|
|
raise CalibrationError("schema 3 requires frame features; use predict/apply_calibration_frame")
|
|
|
|
def predict(self, frame_id: int, timestamp_s: float | None, features: Mapping[str, Any],
|
|
reference_id: Any = None) -> CalibrationResult:
|
|
def invalid(reason: str) -> CalibrationResult:
|
|
return CalibrationResult({a: None for a in self.outputs}, {a: reason for a in self.outputs})
|
|
|
|
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 features.get("zero_valid") is not True:
|
|
return invalid("zero_unavailable")
|
|
if features.get("contact_found") is False:
|
|
return invalid("no_contact")
|
|
values = {}
|
|
if self.model_type == "spatial_gain/1":
|
|
if features.get("contact_found") is not True:
|
|
return invalid("invalid_input")
|
|
if features.get("single_contact", True) is not True or features.get("contact_count", 1) != 1:
|
|
return invalid("invalid_input")
|
|
if features.get("mapping_valid") is not True:
|
|
return invalid("coordinate_unmapped")
|
|
if not all(_number(features.get(k)) for k in ("delta_raw", "u", "v")):
|
|
return invalid("invalid_input")
|
|
query = np.asarray([features["u"], features["v"]])
|
|
for indices in self._triangles:
|
|
triangle = self._points[indices]
|
|
weights = np.linalg.solve((triangle[1:] - triangle[0]).T, query - triangle[0])
|
|
weights = np.asarray([1 - sum(weights), *weights])
|
|
if np.min(weights) >= -1e-10:
|
|
values["fn"] = float(np.dot(weights, self._gains[indices]) * features["delta_raw"])
|
|
break
|
|
if not values:
|
|
return invalid("outside_coverage")
|
|
else:
|
|
deltas = features.get("delta_raw_by_axis", features.get("force_delta_raw", {}))
|
|
if not isinstance(deltas, Mapping):
|
|
return invalid("invalid_input")
|
|
for axis in self.outputs:
|
|
delta = deltas.get(axis, features.get("delta_raw") if axis == "fn" else None)
|
|
values[axis] = delta * self._params["gains_N_per_raw"][axis] if _number(delta) else None
|
|
low, high = self._params["valid_force_range_N"]
|
|
statuses = {}
|
|
for axis, value in values.items():
|
|
if value is None or not _number(value):
|
|
values[axis], statuses[axis] = None, "invalid_input"
|
|
else:
|
|
statuses[axis] = "ok" if low <= value <= high else "out_of_range"
|
|
return CalibrationResult(values, statuses)
|
|
|
|
|
|
def parse_model(data: dict[str, Any], source_path: str | Path | None = None) -> ModelCalibration:
|
|
"""Parse schema 3 without executing document-specified code or installing backends."""
|
|
_keys(data, {"schema_version", "unit", "identity", "pipeline", "model"}, {"provenance"})
|
|
_require(type(data["schema_version"]) is int and data["schema_version"] == 3, "expected schema_version 3")
|
|
_require(data["unit"] == "N", "schema 3 unit must be N")
|
|
_keys(data["identity"], {"sensor_type", "sn"})
|
|
_require(all(isinstance(v, str) and bool(v.strip()) for v in data["identity"].values()), "invalid identity")
|
|
_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"),
|
|
"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)
|
|
params = model["params"]
|
|
_keys(params, {"gains_N_per_raw", "valid_force_range_N"} | ({"points_uv", "triangles"} if spatial else set()))
|
|
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")
|
|
return ModelCalibration(data, source_path)
|