feat: explain local surface residuals

src/k1link/compute/lidar_local_surface.py

@@ -27,7 +27,7 @@ from .lidar_field_review import (
 
 K1_LOCAL_SURFACE_SCHEMA: Final = "missioncore.k1-local-surface/v1"
 K1_LOCAL_SURFACE_REPORT_SCHEMA: Final = "missioncore.k1-local-surface-report/v1"
-K1_LOCAL_SURFACE_FRAME_SCHEMA: Final = "missioncore.k1-local-surface-frame/v1"
+K1_LOCAL_SURFACE_FRAME_SCHEMA: Final = "missioncore.k1-local-surface-frame/v2"
 K1_LOCAL_SURFACE_TIMELINE_SCHEMA: Final = "missioncore.k1-local-surface-timeline/v1"
 K1_LOCAL_SURFACE_REVIEW_SCHEMA: Final = "missioncore.k1-local-surface-review/v1"
 K1_LOCAL_SURFACE_ARRAYS_NAME: Final = "local-surface.npz"
@@ -180,6 +180,26 @@ class K1LocalSurfaceProfile:
 DEFAULT_K1_LOCAL_SURFACE_PROFILE: Final = K1LocalSurfaceProfile()
 
 
+@dataclass(frozen=True, slots=True)
+class _PredictionEvidence:
+    """Current lower-cell evidence scored against a prior-only surface."""
+
+    prior_plane: npt.NDArray[np.float64]
+    cell_points: npt.NDArray[np.float64]
+    signed_residuals: npt.NDArray[np.float64]
+
+    @property
+    def residual_p50_m(self) -> float:
+        return float(np.percentile(np.abs(self.signed_residuals), 50))
+
+    @property
+    def residual_p95_m(self) -> float:
+        return float(np.percentile(np.abs(self.signed_residuals), 95))
+
+    def inlier_fraction(self, surface_band_m: float) -> float:
+        return float(np.mean(np.abs(self.signed_residuals) <= surface_band_m))
+
+
 class K1LocalSurfaceV1:
     """Strict reader for source-aligned, passive K1 local-surface evidence."""
 
@@ -253,10 +273,21 @@ class K1LocalSurfaceV1:
             "step_candidate_point_count",
             "point_step_candidate",
         }
+        prediction_evidence = {
+            "prediction_prior_plane_coefficients_map",
+            "prediction_cell_offsets",
+            "prediction_cell_points_map",
+            "prediction_cell_signed_residual_m",
+        }
         files = set(self.arrays.files)
-        if files not in (baseline, baseline | qualification):
+        if files not in (
+            baseline,
+            baseline | qualification,
+            baseline | qualification | prediction_evidence,
+        ):
             raise LidarGroundError("K1 local-surface arrays are incomplete")
         self.has_temporal_qualification = qualification <= files
+        self.has_prediction_evidence = prediction_evidence <= files
         vector_f64 = (
             "sensor_height_m",
             "slope_deg",
@@ -368,6 +399,63 @@ class K1LocalSurfaceV1:
                 )
             ):
                 raise LidarGroundError("K1 local-surface step candidates are invalid")
+        if self.has_prediction_evidence:
+            offsets = self.arrays["prediction_cell_offsets"]
+            points = self.arrays["prediction_cell_points_map"]
+            residuals = self.arrays["prediction_cell_signed_residual_m"]
+            planes = self.arrays["prediction_prior_plane_coefficients_map"]
+            if (
+                not self.has_temporal_qualification
+                or offsets.shape != (frame_count + 1,)
+                or offsets.dtype != np.dtype("<i8")
+                or int(offsets[0]) != 0
+                or np.any(np.diff(offsets) < 0)
+                or points.ndim != 2
+                or points.shape[1:] != (3,)
+                or points.dtype != np.dtype("<f8")
+                or residuals.shape != (points.shape[0],)
+                or residuals.dtype != np.dtype("<f8")
+                or int(offsets[-1]) != points.shape[0]
+                or planes.shape != (frame_count, 4)
+                or planes.dtype != np.dtype("<f8")
+                or not np.isfinite(points).all()
+                or not np.isfinite(residuals).all()
+                or not np.isfinite(planes).all()
+                or np.any(
+                    np.diff(offsets)
+                    != self.arrays["prediction_cell_count"]
+                )
+                or np.any(
+                    np.diff(offsets)[~self.arrays["prediction_available"]] != 0
+                )
+            ):
+                raise LidarGroundError(
+                    "K1 local-surface prediction evidence is invalid"
+                )
+            surface_band_m = self._surface_band_m()
+            for frame_index in np.flatnonzero(
+                self.arrays["prediction_available"]
+            ):
+                start = int(offsets[frame_index])
+                end = int(offsets[frame_index + 1])
+                absolute = np.abs(residuals[start:end])
+                if (
+                    not np.isclose(
+                        np.percentile(absolute, 50),
+                        self.arrays["prediction_residual_p50_m"][frame_index],
+                    )
+                    or not np.isclose(
+                        np.percentile(absolute, 95),
+                        self.arrays["prediction_residual_p95_m"][frame_index],
+                    )
+                    or not np.isclose(
+                        np.mean(absolute <= surface_band_m),
+                        self.arrays["prediction_inlier_fraction"][frame_index],
+                    )
+                ):
+                    raise LidarGroundError(
+                        "K1 local-surface prediction evidence is inconsistent"
+                    )
         valid = self.arrays["frame_valid"]
         if (
             np.any(self.arrays["frame_failure_code"][valid] != FRAME_VALID)
@@ -437,6 +525,7 @@ class K1LocalSurfaceV1:
             step_candidate = self.arrays["point_step_candidate"][start:end]
         else:
             step_candidate = np.zeros(end - start, dtype=np.uint8)
+        prediction_evidence = self._prediction_evidence_detail(frame_index)
         counts = {
             "classified": int(np.count_nonzero(point_class)),
             "surface": int(np.count_nonzero(point_class == POINT_SURFACE)),
@@ -495,6 +584,7 @@ class K1LocalSurfaceV1:
                 "plane_coefficients_map": self.arrays["plane_coefficients_map"][frame_index]
                 .astype(np.float64)
                 .tolist(),
+                "local_radius_m": self._local_radius_m(),
                 "sensor_height_m": float(self.arrays["sensor_height_m"][frame_index]),
                 "slope_deg": float(self.arrays["slope_deg"][frame_index]),
                 "roughness_m": float(self.arrays["roughness_m"][frame_index]),
@@ -535,6 +625,7 @@ class K1LocalSurfaceV1:
                     if self.has_temporal_qualification
                     else 0.0
                 ),
+                "evidence": prediction_evidence,
             },
             "temporal": {
                 "compared": (
@@ -580,6 +671,51 @@ class K1LocalSurfaceV1:
             "authority": self.report["authority"],
         }
 
+    def _prediction_evidence_detail(
+        self,
+        frame_index: int,
+    ) -> dict[str, object]:
+        surface_band_m = self._surface_band_m()
+        if not self.has_prediction_evidence:
+            return {
+                "available": False,
+                "basis": "current-lower-cell-observations",
+                "coordinate_frame": "map",
+                "distance_unit": "m",
+                "current_frame_excluded_from_plane": True,
+                "surface_inlier_band_m": surface_band_m,
+                "prior_plane_coefficients_map": [0.0, 0.0, 0.0, 0.0],
+                "cell_points_xyz_m": [],
+                "cell_signed_residual_m": [],
+                "cell_inlier": [],
+                "ground_truth": False,
+            }
+        offsets = self.arrays["prediction_cell_offsets"]
+        start = int(offsets[frame_index])
+        end = int(offsets[frame_index + 1])
+        residuals = self.arrays["prediction_cell_signed_residual_m"][start:end]
+        return {
+            "available": bool(self.arrays["prediction_available"][frame_index]),
+            "basis": "current-lower-cell-observations",
+            "coordinate_frame": "map",
+            "distance_unit": "m",
+            "current_frame_excluded_from_plane": True,
+            "surface_inlier_band_m": surface_band_m,
+            "prior_plane_coefficients_map": self.arrays[
+                "prediction_prior_plane_coefficients_map"
+            ][frame_index]
+            .astype(np.float64)
+            .tolist(),
+            "cell_points_xyz_m": self.arrays["prediction_cell_points_map"][start:end]
+            .astype(np.float64)
+            .tolist(),
+            "cell_signed_residual_m": residuals.astype(np.float64).tolist(),
+            "cell_inlier": (np.abs(residuals) <= surface_band_m)
+            .astype(np.int64)
+            .tolist(),
+            "ground_truth": False,
+        }
+
     def timeline_detail(self, source: E10LidarFieldSource) -> dict[str, object]:
         _validate_source_binding(self, source)
         frame_count = source.frame_count
@@ -825,6 +961,28 @@ class K1LocalSurfaceV1:
             "episode_max_frame_gap": 2,
         }
 
+    def _surface_band_m(self) -> float:
+        profile = _object(self.identity.get("profile"), "K1 local-surface profile")
+        classification = _object(
+            profile.get("classification"),
+            "K1 local-surface classification profile",
+        )
+        return _positive_number(
+            classification.get("surface_band_m"),
+            "K1 local-surface band",
+        )
+
+    def _local_radius_m(self) -> float:
+        profile = _object(self.identity.get("profile"), "K1 local-surface profile")
+        rolling_surface = _object(
+            profile.get("rolling_surface"),
+            "K1 local-surface rolling profile",
+        )
+        return _positive_number(
+            rolling_surface.get("local_radius_m"),
+            "K1 local-surface local radius",
+        )
+
 
 def build_k1_local_surface(
     source: E10LidarFieldSource,
@@ -850,6 +1008,12 @@ def build_k1_local_surface(
     pose_delta = np.abs(source_arrays["pose_point_delta_ms"])
     cache: dict[tuple[int, int], tuple[float, float]] = {}
     previous_surface: tuple[float, float, float, float] | None = None
+    prediction_cell_points: list[npt.NDArray[np.float64]] = [
+        np.empty((0, 3), dtype=np.float64) for _ in range(frame_count)
+    ]
+    prediction_cell_residuals: list[npt.NDArray[np.float64]] = [
+        np.empty(0, dtype=np.float64) for _ in range(frame_count)
+    ]
 
     for frame_index in range(frame_count):
         start = int(offsets[frame_index])
@@ -883,12 +1047,24 @@ def build_k1_local_surface(
             profile,
         )
         if prediction is not None:
-            residual_p50, residual_p95, inlier_fraction, prediction_cells = prediction
+            prediction_cell_points[frame_index] = prediction.cell_points
+            prediction_cell_residuals[frame_index] = prediction.signed_residuals
             arrays["prediction_available"][frame_index] = True
-            arrays["prediction_cell_count"][frame_index] = prediction_cells
-            arrays["prediction_residual_p50_m"][frame_index] = residual_p50
-            arrays["prediction_residual_p95_m"][frame_index] = residual_p95
-            arrays["prediction_inlier_fraction"][frame_index] = inlier_fraction
+            arrays["prediction_cell_count"][frame_index] = (
+                prediction.cell_points.shape[0]
+            )
+            arrays["prediction_residual_p50_m"][
+                frame_index
+            ] = prediction.residual_p50_m
+            arrays["prediction_residual_p95_m"][
+                frame_index
+            ] = prediction.residual_p95_m
+            arrays["prediction_inlier_fraction"][
+                frame_index
+            ] = prediction.inlier_fraction(profile.surface_band_m)
+            arrays["prediction_prior_plane_coefficients_map"][
+                frame_index
+            ] = prediction.prior_plane
         _update_cache(cache, local_cloud, session_seconds, profile)
         cell_keys, cell_points, cell_times = _local_cache_records(
             cache,
@@ -987,6 +1163,12 @@ def build_k1_local_surface(
             np.count_nonzero(step_candidate)
         )
 
+    arrays.update(
+        _prediction_evidence_arrays(
+            prediction_cell_points,
+            prediction_cell_residuals,
+        )
+    )
     logical_content_sha256 = _logical_sha256(arrays)
     valid = arrays["frame_valid"]
     identity = {
@@ -1145,8 +1327,8 @@ def build_k1_local_surface(
             "status": "replay-experiment-only",
             "production_promotion": False,
             "next_gate": (
-                "review the full recording, qualify local-surface stability, then run "
-                "bounded latest-wins live shadow without commands"
+                "run the accepted profile through a bounded latest-wins live-shadow "
+                "queue without commands, free-space or safety authority"
             ),
         },
         "authority": {
@@ -1230,6 +1412,10 @@ def _empty_arrays(
         "prediction_residual_p50_m": np.zeros(frame_count, dtype="<f8"),
         "prediction_residual_p95_m": np.zeros(frame_count, dtype="<f8"),
         "prediction_inlier_fraction": np.zeros(frame_count, dtype="<f8"),
+        "prediction_prior_plane_coefficients_map": np.zeros(
+            (frame_count, 4),
+            dtype="<f8",
+        ),
         "height_delta_m": np.zeros(frame_count, dtype="<f8"),
         "slope_delta_deg": np.zeros(frame_count, dtype="<f8"),
         "roughness_delta_m": np.zeros(frame_count, dtype="<f8"),
@@ -1243,6 +1429,43 @@ def _empty_arrays(
     }
 
 
+def _prediction_evidence_arrays(
+    cell_points: list[npt.NDArray[np.float64]],
+    signed_residuals: list[npt.NDArray[np.float64]],
+) -> dict[str, npt.NDArray[Any]]:
+    if len(cell_points) != len(signed_residuals):
+        raise LidarGroundError("K1 local-surface prediction evidence is unaligned")
+    offsets = np.zeros(len(cell_points) + 1, dtype="<i8")
+    for index, (points, residuals) in enumerate(
+        zip(cell_points, signed_residuals, strict=True)
+    ):
+        if (
+            points.ndim != 2
+            or points.shape[1:] != (3,)
+            or residuals.shape != (points.shape[0],)
+            or not np.isfinite(points).all()
+            or not np.isfinite(residuals).all()
+        ):
+            raise LidarGroundError(
+                "K1 local-surface prediction evidence is invalid"
+            )
+        offsets[index + 1] = offsets[index] + points.shape[0]
+    if int(offsets[-1]) == 0:
+        joined_points = np.empty((0, 3), dtype="<f8")
+        joined_residuals = np.empty(0, dtype="<f8")
+    else:
+        joined_points = np.concatenate(cell_points).astype("<f8", copy=False)
+        joined_residuals = np.concatenate(signed_residuals).astype(
+            "<f8",
+            copy=False,
+        )
+    return {
+        "prediction_cell_offsets": offsets,
+        "prediction_cell_points_map": joined_points,
+        "prediction_cell_signed_residual_m": joined_residuals,
+    }
+
+
 def _update_cache(
     cache: dict[tuple[int, int], tuple[float, float]],
     cloud: npt.NDArray[np.float64],
@@ -1411,7 +1634,7 @@ def _prediction_metrics(
     current_cell_points: npt.NDArray[np.float64],
     position: npt.NDArray[np.float64],
     profile: K1LocalSurfaceProfile,
-) -> tuple[float, float, float, int] | None:
+) -> _PredictionEvidence | None:
     """Score current lower-cell evidence against a plane built without that frame."""
 
     if (
@@ -1429,16 +1652,17 @@ def _prediction_metrics(
     evaluation = current_cell_points[:, 2] <= cutoff
     if int(np.count_nonzero(evaluation)) < profile.minimum_surface_cells:
         return None
-    residual = np.abs(
-        _height_above_plane(current_cell_points[evaluation], prior_plane)
+    evaluation_points = current_cell_points[evaluation]
+    signed_residuals = _height_above_plane(
+        evaluation_points,
+        prior_plane,
     )
-    if residual.size == 0 or not np.isfinite(residual).all():
+    if signed_residuals.size == 0 or not np.isfinite(signed_residuals).all():
         return None
-    return (
-        float(np.percentile(residual, 50)),
-        float(np.percentile(residual, 95)),
-        float(np.mean(residual <= profile.surface_band_m)),
-        int(residual.shape[0]),
+    return _PredictionEvidence(
+        prior_plane=prior_plane,
+        cell_points=evaluation_points,
+        signed_residuals=signed_residuals,
     )