feat: explain local surface residuals

apps/control-station/src/core/lidar/localSurface.ts

@@ -118,6 +118,7 @@ export interface LidarLocalSurfaceFrame {
   };
   surface: {
     planeCoefficientsMap: [number, number, number, number];
+    localRadiusM: number;
     sensorHeightM: number;
     slopeDeg: number;
     roughnessM: number;
@@ -140,6 +141,19 @@ export interface LidarLocalSurfaceFrame {
     residualP50M: number;
     residualP95M: number;
     inlierFraction: number;
+    evidence: {
+      available: boolean;
+      basis: "current-lower-cell-observations";
+      coordinateFrame: "map";
+      distanceUnit: "m";
+      currentFrameExcludedFromPlane: true;
+      surfaceInlierBandM: number;
+      priorPlaneCoefficientsMap: [number, number, number, number];
+      cellPointsXyzM: Array<[number, number, number]>;
+      cellSignedResidualM: number[];
+      cellInlier: number[];
+      groundTruth: false;
+    };
   };
   temporal: {
     compared: boolean;
@@ -670,7 +684,7 @@ export function parseLidarLocalSurfaceFrame(
 ): LidarLocalSurfaceFrame {
   const source = record(value, "LiDAR local-surface frame");
   if (
-    source.schema_version !== `${LOCAL_SURFACE_SCHEMA_PREFIX}-frame/v1`
+    source.schema_version !== `${LOCAL_SURFACE_SCHEMA_PREFIX}-frame/v2`
     || source.access !== "read-only"
     || source.ground_truth !== false
     || source.coordinate_frame !== "map"
@@ -731,6 +745,7 @@ export function parseLidarLocalSurfaceFrame(
   const pose = record(source.pose, "pose");
   const surface = record(source.surface, "surface");
   const prediction = record(source.prediction, "prediction");
+  const evidence = record(prediction.evidence, "prediction.evidence");
   const temporal = record(source.temporal, "temporal");
   const counts = record(source.counts, "counts");
   const parsedCounts = {
@@ -764,6 +779,12 @@ export function parseLidarLocalSurfaceFrame(
     4,
     "surface.plane_coefficients_map",
   );
+  const localRadiusM = finite(surface.local_radius_m, "surface.local_radius_m");
+  if (localRadiusM <= 0) {
+    throw new LidarLocalSurfaceContractError(
+      "Local-surface radius несовместим",
+    );
+  }
   if (prediction.current_frame_excluded !== true) {
     throw new LidarLocalSurfaceContractError(
       "Текущий кадр попал в prediction input",
@@ -778,6 +799,82 @@ export function parseLidarLocalSurfaceFrame(
       "Prediction inlier fraction несовместим",
     );
   }
+  if (
+    evidence.basis !== "current-lower-cell-observations"
+    || evidence.coordinate_frame !== "map"
+    || evidence.distance_unit !== "m"
+    || evidence.current_frame_excluded_from_plane !== true
+    || evidence.ground_truth !== false
+  ) {
+    throw new LidarLocalSurfaceContractError(
+      "Prediction evidence несовместим",
+    );
+  }
+  const evidenceAvailable = boolean(evidence.available, "evidence.available");
+  const surfaceInlierBandM = finite(
+    evidence.surface_inlier_band_m,
+    "evidence.surface_inlier_band_m",
+  );
+  if (surfaceInlierBandM <= 0) {
+    throw new LidarLocalSurfaceContractError(
+      "Prediction evidence band несовместим",
+    );
+  }
+  const priorPlane = tuple(
+    evidence.prior_plane_coefficients_map,
+    4,
+    "evidence.prior_plane_coefficients_map",
+  );
+  const evidencePoints = array(
+    evidence.cell_points_xyz_m,
+    "evidence.cell_points_xyz_m",
+  ).map((item, index): [number, number, number] => {
+    const values = tuple(item, 3, `evidence.cell_points_xyz_m[${index}]`);
+    return [values[0], values[1], values[2]];
+  });
+  const evidenceResiduals = array(
+    evidence.cell_signed_residual_m,
+    "evidence.cell_signed_residual_m",
+  ).map((item, index) => finite(
+    item,
+    `evidence.cell_signed_residual_m[${index}]`,
+  ));
+  const evidenceInlier = array(
+    evidence.cell_inlier,
+    "evidence.cell_inlier",
+  ).map((item, index) => {
+    const parsed = integer(item, `evidence.cell_inlier[${index}]`);
+    if (parsed > 1) {
+      throw new LidarLocalSurfaceContractError(
+        "Prediction evidence mask несовместим",
+      );
+    }
+    return parsed;
+  });
+  if (
+    evidencePoints.length !== evidenceResiduals.length
+    || evidencePoints.length !== evidenceInlier.length
+    || evidenceInlier.some(
+      (item, index) =>
+        item !== (Math.abs(evidenceResiduals[index]) <= surfaceInlierBandM ? 1 : 0),
+    )
+    || (
+      evidenceAvailable
+        ? !boolean(prediction.available, "prediction.available")
+          || evidencePoints.length !== integer(
+            prediction.cell_count,
+            "prediction.cell_count",
+          )
+        : evidencePoints.length !== 0
+          || evidenceResiduals.length !== 0
+          || evidenceInlier.length !== 0
+          || priorPlane.some((value) => value !== 0)
+    )
+  ) {
+    throw new LidarLocalSurfaceContractError(
+      "Prediction evidence arrays расходятся",
+    );
+  }
   return {
     modelId: text(source.model_id, "model_id", SAFE_MODEL_ID),
     sourcePackId: text(source.source_pack_id, "source_pack_id", SAFE_PACK_ID),
@@ -808,6 +905,7 @@ export function parseLidarLocalSurfaceFrame(
     },
     surface: {
       planeCoefficientsMap: [plane[0], plane[1], plane[2], plane[3]],
+      localRadiusM,
       sensorHeightM: finite(surface.sensor_height_m, "surface.sensor_height_m"),
       slopeDeg: finite(surface.slope_deg, "surface.slope_deg"),
       roughnessM: finite(surface.roughness_m, "surface.roughness_m"),
@@ -836,6 +934,24 @@ export function parseLidarLocalSurfaceFrame(
         "prediction.residual_p95_m",
       ),
       inlierFraction: predictionInlierFraction,
+      evidence: {
+        available: evidenceAvailable,
+        basis: "current-lower-cell-observations",
+        coordinateFrame: "map",
+        distanceUnit: "m",
+        currentFrameExcludedFromPlane: true,
+        surfaceInlierBandM,
+        priorPlaneCoefficientsMap: [
+          priorPlane[0],
+          priorPlane[1],
+          priorPlane[2],
+          priorPlane[3],
+        ],
+        cellPointsXyzM: evidencePoints,
+        cellSignedResidualM: evidenceResiduals,
+        cellInlier: evidenceInlier,
+        groundTruth: false,
+      },
     },
     temporal: {
       compared: boolean(temporal.compared, "temporal.compared"),