接入力标定 schema 3 与多点空间增益模型

保留旧格式和 cal_path，校验 SN、特征身份及坐标附件，按帧返回逐轴状态。相关测试 263 项通过，已核对存量预测并实测 180 帧录像。

src/orisys/sensor.py

@@ -182,6 +182,23 @@ class Sensor:
             config_fps=config_fps,
             verbose=self.verbose,
         )
+        self._model_frame_id = 0
+        self._calibration_result_frame = None
+        self._calibration_reference = {}
+        self._calibration_reference_id = None
+        self._model_readouts = {}
+        self.calibration_result = None
+        self.calibration_status = {}
+        self._calibration_context = None
+        if self._uses_calibration_model():
+            self._reset_calibration_session()
+        if (self._uses_calibration_model() and self.calibration.input_profile == "force_delta/1"
+                and not self._external_only):
+            from .calibration_context import build_context
+
+            self.bind_calibration_context(build_context(
+                self._config, self.filters_state(), preprocessing={"mirror": self.mirror},
+            ))
 
         if self.verbose:
             if self.calibration is not None and self.calibration.source_path is not None:
@@ -726,7 +743,15 @@ class Sensor:
         self._force_raw = (float(fn), float(fx), float(fy))
         self.ftor = float(ftor)
         self._force_readout.update_sample_rate()
-        self._apply_force_filter()
+        if self._uses_calibration_model():
+            self._model_frame_id += 1
+            self.compute_contact_valid = False
+            self.centroid_found = False
+            self._force_readout.apply(self._force_raw)
+            self._force_filtered_raw = self._force_readout.filtered_raw
+            self._force_raw_unfiltered = self._force_raw
+        else:
+            self._apply_force_filter()
 
         vnormal_ds = self.__hhd.get_normal()
         vshear_ds = self.__hhd.get_shear()
@@ -762,6 +787,14 @@ class Sensor:
 
         self.compute_deformation_valid = True
 
+        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":
+                self.compute_contact()
+            else:
+                self.apply_calibration_frame(frame_id=self._model_frame_id)
+
         return True
 
     def compute_contact(self, vnormal=None):
@@ -774,6 +807,11 @@ class Sensor:
         Returns:
             dict: Contact result with centroid, contour, and depth_map
         """
+        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
+                and self._calibration_result_frame == self._model_frame_id):
+            return self._model_contact_result
         multiple_foe = False
         
         # Use provided vnormal or fall back to internal state
@@ -807,6 +845,13 @@ class Sensor:
                 print("Warning: contact centroid lies outside the valid UV mask.")
 
         self.compute_contact_valid = True
+        if self._uses_calibration_model():
+            self._model_contact_result = result
+            uv = self._calibration_contact_uv()
+            self.apply_calibration_frame(
+                frame_id=self._model_frame_id, contact_found=bool(self.centroid_found),
+                uv=uv, mapping_valid=uv is not None,
+            )
         return result
     
 
@@ -871,6 +916,8 @@ class Sensor:
         # session must not report the previous one's reading.
         self._force_unfiltered = None
         self._force_raw_unfiltered = None
+        self._reset_calibration_session()
+        self.compute_deformation_valid = False
         if self.img is not None:
             self.initial_img = self.img.copy()
 
@@ -950,6 +997,10 @@ class Sensor:
             alpha = float(flow_ema_alpha)
             if not (0.0 < alpha <= 1.0):
                 raise ValueError("flow_ema_alpha must be in (0, 1]")
+            if self._uses_calibration_model() and alpha != self._force_readout.flow_ema_alpha:
+                raise cal_mod.CalibrationError(
+                    "flow_ema_alpha changes schema 3 measurement identity; load a matching pipeline"
+                )
             if self.calibration is not None:
                 mismatches, _ = cal_mod.filter_mismatches(
                     self.calibration.pipeline.get("filters"),
@@ -965,8 +1016,17 @@ class Sensor:
             flow_ema_alpha=flow_ema_alpha,
             force_median_window=force_median_window,
         )
+        for readout in getattr(self, "_model_readouts", {}).values():
+            readout.configure(
+                lowpass_cutoff_hz=lowpass_cutoff_hz,
+                lowpass_sample_rate_hz=lowpass_sample_rate_hz,
+                lowpass_order=lowpass_order,
+                lowpass_enabled=lowpass_enabled,
+                force_median_window=force_median_window,
+            )
         if filter_changed and self.compute_deformation_valid:
-            self._apply_force_filter()
+            if not self._uses_calibration_model():
+                self._apply_force_filter()
 
         return self
 
@@ -1139,6 +1199,7 @@ class Sensor:
     def set_calibration(
         self,
         cal: "cal_mod.Calibration | str | Path | None",
+        *, context=None,
     ) -> None:
         """Attach or replace the active force calibration (live swap after save)."""
         if cal is None:
@@ -1146,8 +1207,12 @@ class Sensor:
             self.device_calibration_missing = False
             self._force_cal_sensor_name = None
             self._force_cal_attempts = None
+            self._reset_calibration_session()
+            self._calibration_context = None
+            self._force_readout.reset()
+            self._apply_force_filter()
             return
-        if isinstance(cal, cal_mod.Calibration):
+        if isinstance(cal, cal_mod.Calibration) or getattr(cal, "schema_version", None) == 3:
             selected = cal
         else:
             selected = cal_mod.load(cal, strict=True)
@@ -1156,9 +1221,30 @@ class Sensor:
             selected = cal_mod.validate_for_device(
                 selected, family, self.device_id, pipeline=pipeline
             )
+        if getattr(selected, "schema_version", None) == 3:
+            context = context if context is not None else getattr(self, "_calibration_context", None)
+            if context is None and selected.input_profile == "force_delta/1":
+                from .calibration_context import build_context
+
+                context = build_context(
+                    self._config, self.filters_state(), preprocessing={"mirror": self.mirror},
+                )
+            if context is None:
+                raise cal_mod.CalibrationError("actual pipeline context required before replacing model")
+            try:
+                selected.validate_for_context(context)
+            except ValueError as exc:
+                raise cal_mod.CalibrationError(str(exc)) from exc
+        else:
+            context = None
         self.calibration = selected
+        self._calibration_context = context
         self.device_calibration_missing = False
         self._force_cal_attempts = None
+        self._reset_calibration_session()
+        if not self._uses_calibration_model():
+            self._force_readout.reset()
+            self._apply_force_filter()
 
     def _apply_force_filter(self):
         convert = self.calibration.convert if self.calibration is not None else None
@@ -1169,6 +1255,142 @@ class Sensor:
         self._force_raw_unfiltered = readout.raw_unfiltered
         self._force_unfiltered = readout.unfiltered_newton
 
+    def _uses_calibration_model(self):
+        return getattr(self.calibration, "schema_version", None) == 3
+
+    def bind_calibration_context(self, context):
+        """Verify the actual feature pipeline before allowing schema 3 inference."""
+        if not self._uses_calibration_model():
+            raise cal_mod.CalibrationError("calibration context requires schema 3")
+        try:
+            self.calibration.validate_for_context(context)
+        except ValueError as exc:
+            raise cal_mod.CalibrationError(str(exc)) from exc
+        self._calibration_context = context
+
+    def _reset_calibration_session(self):
+        self._calibration_reference = {}
+        self._calibration_reference_id = None
+        self._calibration_result_frame = None
+        self.calibration_result = None
+        self.calibration_status = {}
+        self.compute_contact_valid = False
+        self.centroid_found = False
+        self.centroid = (0, 0)
+        self._model_contact_result = None
+        for readout in getattr(self, "_model_readouts", {}).values():
+            readout.reset()
+        if self._uses_calibration_model():
+            self.calibration.reset(None)
+            self.calibration_status = {axis: "zero_unavailable" for axis in self.calibrated_axes}
+            values = [float("nan") if axis in self.calibrated_axes else raw
+                      for axis, raw in zip(cal_mod.CHANNEL_IDS, self._force_raw)]
+            self.fnormal, self.fshearx, self.fsheary = values
+            self._force_unfiltered = tuple(values)
+
+    def set_calibration_reference(self, raw, *, reference_id):
+        """Declare a measured unloaded raw reference for this stream, not the model file."""
+        if not isinstance(raw, dict) or not reference_id:
+            raise ValueError("raw must map axes to finite values and reference_id is required")
+        values = {}
+        for axis, value in raw.items():
+            if axis not in cal_mod.CHANNEL_IDS or not np.isfinite(float(value)):
+                raise ValueError("reference axes and values must be valid and finite")
+            values[axis] = float(value)
+        self._reset_calibration_session()
+        self._calibration_reference = values
+        self._calibration_reference_id = str(reference_id)
+        if self._uses_calibration_model():
+            self.calibration.reset(self._calibration_reference_id)
+
+    def _calibration_contact_uv(self):
+        if not self.centroid_found:
+            return None
+        uv_map = getattr(self.calibration, "coordinate_map", None)
+        valid = getattr(self.calibration, "coordinate_valid_mask", None)
+        if uv_map is None or valid is None:
+            return None
+        if not np.all(np.isfinite(self.centroid)):
+            return None
+        x, y = np.rint(self.centroid).astype(int)
+        if not (0 <= y < valid.shape[0] and 0 <= x < valid.shape[1] and valid[y, x]):
+            return None
+        return tuple(float(value) for value in uv_map[y, x])
+
+    def apply_calibration_frame(
+        self, *, frame_id, timestamp_s=None, raw=None, uv=None,
+        contact_found=None, mapping_valid=False, single_contact=True,
+    ):
+        """Infer once from an unfiltered raw triple and same-frame optional contact UV."""
+        if not self._uses_calibration_model():
+            raise cal_mod.CalibrationError("apply_calibration_frame requires schema 3")
+        if self._calibration_context is None:
+            raise cal_mod.CalibrationError("bind the actual calibration context before inference")
+        if type(frame_id) is not int:
+            raise ValueError("frame_id must be an integer")
+        if frame_id == self._calibration_result_frame:
+            return self.calibration_result
+        if self._calibration_result_frame is not None and frame_id < self._calibration_result_frame:
+            self._reset_calibration_session()
+        raw_values = tuple(self._force_raw if raw is None else raw)
+        if len(raw_values) != 3:
+            raise ValueError("raw must contain FN, FX and FY")
+        if raw is not None:
+            self._force_raw = tuple(float(value) for value in raw_values)
+            if np.all(np.isfinite(self._force_raw)):
+                self._force_readout.apply(self._force_raw)
+                self._force_filtered_raw = self._force_readout.filtered_raw
+        raw_by_axis = dict(zip(cal_mod.CHANNEL_IDS, raw_values))
+        reference = self._calibration_reference
+        deltas = {axis: float(value) - reference.get(axis, float("nan"))
+                  for axis, value in raw_by_axis.items()}
+        axes = self.calibrated_axes
+        features = {
+            "delta_raw": deltas["fn"], "force_delta_raw": deltas,
+            "zero_valid": all(axis in reference for axis in axes),
+            "mapping_valid": bool(mapping_valid), "single_contact": single_contact,
+            "u": None if uv is None else uv[0], "v": None if uv is None else uv[1],
+        }
+        if contact_found is not None:
+            features["contact_found"] = bool(contact_found)
+        result = self.calibration.predict(
+            frame_id=frame_id, timestamp_s=timestamp_s,
+            features=features, reference_id=self._calibration_reference_id,
+        )
+        filtered = list(self._force_readout.filtered_raw or raw_values)
+        unfiltered = list(raw_values)
+        for index, axis in enumerate(cal_mod.CHANNEL_IDS):
+            if axis not in axes:
+                continue
+            value = result.values_N.get(axis)
+            unfiltered[index] = float("nan") if value is None else float(value)
+            filtered[index] = unfiltered[index]
+            if value is not None and np.isfinite(value):
+                readout = self._model_readouts.get(axis)
+                if readout is None:
+                    readout = ForceReadout(self._config.get("filters"),
+                                           config_fps=self._force_readout.config_fps)
+                    base = self._force_readout
+                    readout.configure(
+                        lowpass_sample_rate_hz=base.lowpass_sample_rate_hz,
+                        lowpass_cutoff_hz=base.lowpass_cutoff_hz,
+                        lowpass_order=base.lowpass_order,
+                        lowpass_enabled=base.lowpass_enabled,
+                        force_median_window=base.force_median_window,
+                    )
+                    self._model_readouts[axis] = readout
+                base = self._force_readout
+                if readout.lowpass_sample_rate_hz != base.lowpass_sample_rate_hz:
+                    readout.configure(lowpass_sample_rate_hz=base.lowpass_sample_rate_hz)
+                filtered[index] = readout.apply((value, value, value))[0]
+        self.fnormal, self.fshearx, self.fsheary = filtered
+        self._force_raw_unfiltered = tuple(float(value) for value in raw_values)
+        self._force_unfiltered = tuple(unfiltered)
+        self.calibration_result = result
+        self.calibration_status = dict(result.status)
+        self._calibration_result_frame = frame_id
+        return result
+
     # ------------------------------------------------------- filter contract
     def filters_state(self) -> dict:
         """What the filter chain is **actually** doing, for records and audits.