feat(lidar): qualify Patchwork++ on GOOSE

2026-07-25 15:22:28 +03:00
parent 951b40c870
commit 60ba64004b
15 changed files with 1082 additions and 256 deletions
@@ -0,0 +1,117 @@
+"""Published, algorithm-scoped GOOSE VLS-128 ground profile."""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from typing import Final
+
+GOOSE_PATCHWORK_PROFILE_SCHEMA: Final = "missioncore.goose-patchwork-profile/v1"
+GOOSE_PAPER_URL: Final = "https://arxiv.org/pdf/2310.16788"
+GOOSE_PAPER_SHA256: Final = "cb17f38c3ae498917966a63bc0e034cc5dd3fa4ad35873b121592f99026cc77e"
+GOOSE_TF_DOT_URL: Final = "https://goose-dataset.de/docs/resources/mucar3_tf/mucar3_tf.dot"
+GOOSE_TF_DOT_SHA256: Final = "1b10e905bcff17b90305ff02734b5e6bb7f5753d9b8e74cd7c43f3d0f32ef469"
+
+
+@dataclass(frozen=True, slots=True)
+class GoosePatchworkProfile:
+    """Narrow admission profile for one native GOOSE scan.
+
+    Figure 3 of the official paper dimensions ``base_link``/INS at 0.64 m
+    above ground and the VLS-128 optical center 1.60 m above ``base_link``.
+    Their sum is the physical height Patchwork++ requires. This does not claim
+    a complete vehicle mounting transform and cannot produce a normalized scan.
+    """
+
+    profile_id: str = "goose-mucar3-vls128-patchwork/v1"
+    base_link_height_above_ground_m: float = 0.64
+    lidar_height_above_base_link_m: float = 1.60
+    patchwork_sensor_height_proxy_m: float = 2.24
+    patchwork_minimum_range_m: float = 2.7
+    patchwork_maximum_range_m: float = 80.0
+    patchwork_height_evidence: str = "published-dimensioned-schematic"
+
+    def __post_init__(self) -> None:
+        values = (
+            self.base_link_height_above_ground_m,
+            self.lidar_height_above_base_link_m,
+            self.patchwork_sensor_height_proxy_m,
+            self.patchwork_minimum_range_m,
+            self.patchwork_maximum_range_m,
+        )
+        if (
+            not self.profile_id
+            or not all(math.isfinite(value) for value in values)
+            or self.base_link_height_above_ground_m <= 0
+            or self.lidar_height_above_base_link_m <= 0
+            or not math.isclose(
+                self.base_link_height_above_ground_m + self.lidar_height_above_base_link_m,
+                self.patchwork_sensor_height_proxy_m,
+                abs_tol=1e-9,
+            )
+            or not 0 < self.patchwork_minimum_range_m < self.patchwork_maximum_range_m
+            or self.patchwork_height_evidence != "published-dimensioned-schematic"
+        ):
+            raise ValueError("GOOSE Patchwork++ profile is invalid")
+
+    def to_dict(self) -> dict[str, object]:
+        return {
+            "schema_version": GOOSE_PATCHWORK_PROFILE_SCHEMA,
+            "profile_id": self.profile_id,
+            "source_id": "goose-3d/v2025-08-22",
+            "representation": "native-scan",
+            "sensor_frame": {
+                "frame_id": "sensor/lidar/vls128_roof",
+                "handedness": "right",
+                "x": "forward",
+                "y": "left",
+                "z": "up",
+                "one_revolution": True,
+            },
+            "height": {
+                "base_link_above_ground_m": self.base_link_height_above_ground_m,
+                "lidar_above_base_link_m": self.lidar_height_above_base_link_m,
+                "sensor_above_ground_m": self.patchwork_sensor_height_proxy_m,
+                "evidence": self.patchwork_height_evidence,
+            },
+            "patchworkpp": {
+                "provider_id": "patchworkpp/v1.4.1",
+                "minimum_range_m": self.patchwork_minimum_range_m,
+                "maximum_range_m": self.patchwork_maximum_range_m,
+                "enable_rnr": True,
+                "enable_rvpf": True,
+                "enable_tgr": True,
+            },
+            "evidence": {
+                "dimensioned_schematic": {
+                    "url": GOOSE_PAPER_URL,
+                    "sha256": GOOSE_PAPER_SHA256,
+                    "paper_version": "arxiv-2310.16788v2",
+                    "figure": 3,
+                },
+                "tf_topology": {
+                    "url": GOOSE_TF_DOT_URL,
+                    "sha256": GOOSE_TF_DOT_SHA256,
+                    "numeric_transform_present": False,
+                },
+                "first_frame_cross_check": {
+                    "method": "independent-ground-near-field-plane",
+                    "expected_ground_z_m": -self.patchwork_sensor_height_proxy_m,
+                    "observed_intercept_range_m": [-2.18, -2.14],
+                    "used_for_calibration": False,
+                },
+            },
+            "scope": {
+                "patchworkpp_eligible": True,
+                "normalized_scan_produced": False,
+                "complete_vehicle_transform_known": False,
+                "deskew_claimed": False,
+            },
+            "authority": {
+                "qualification_only": True,
+                "navigation_or_safety_accepted": False,
+            },
+        }
+
+
+DEFAULT_GOOSE_PATCHWORK_PROFILE: Final = GoosePatchworkProfile()