feat: add camera-first lidar geometry fusion

2026-07-26 11:49:27 +03:00
parent f791b63890
commit 677dbdb776
6 changed files with 1458 additions and 3 deletions
@@ -0,0 +1,160 @@
+from __future__ import annotations
+
+import numpy as np
+import pytest
+
+from k1link.compute.semantic_geometry_fusion import (
+    CameraGeometryFusionProfile,
+    SemanticGeometryFusionError,
+    _geometry_clusters,
+    _semantic_support,
+)
+from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
+    ProjectedPointCloud,
+)
+
+
+def _object(*, observed_geometry: bool = False) -> dict[str, object]:
+    value: dict[str, object] = {
+        "source_track_id": 42,
+        "track_id": 4200,
+        "label": "car",
+        "association_group": "vehicle",
+        "score": 0.91,
+        "bbox_xyxy": [10.0, 10.0, 90.0, 90.0],
+        "cuboid_status": "rejected-fewer-than-8-clustered-points",
+        "camera_motion_state": "static",
+        "camera_motion_confidence": 0.8,
+        "motion_state": "unknown",
+        "motion_status": "e26-insufficient-independent-evidence",
+    }
+    if observed_geometry:
+        value.update(
+            {
+                "observed_cuboid_center_map": [2.0, 0.0, 0.5],
+                "observed_cuboid_half_size": [1.0, 0.5, 0.5],
+                "observed_cuboid_quaternion_xyzw": [0.0, 0.0, 0.0, 1.0],
+            }
+        )
+    return value
+
+
+def _projection(point_count: int) -> ProjectedPointCloud:
+    return ProjectedPointCloud(
+        pixels_xy=np.column_stack(
+            (
+                np.linspace(20.0, 80.0, point_count),
+                np.linspace(20.0, 80.0, point_count),
+            )
+        ).astype(np.float64),
+        depths_m=np.linspace(2.0, 2.4, point_count).astype(np.float64),
+        source_indices=np.arange(point_count, dtype=np.int64),
+        source_point_count=point_count,
+        camera_front_point_count=point_count,
+    )
+
+
+def test_camera_semantic_and_connected_occupied_support_agree() -> None:
+    points = np.asarray(
+        [
+            [2.0, 0.0, 0.5],
+            [2.2, 0.0, 0.6],
+            [2.1, 0.2, 0.0],
+            [2.3, 0.2, -0.1],
+        ],
+        dtype=np.float64,
+    )
+    support = _semantic_support(
+        _object(),
+        projected=_projection(4),
+        frame_points_map=points,
+        point_class=np.asarray([2, 2, 1, 3], dtype=np.uint8),
+        point_height_m=np.asarray([0.5, 0.6, 0.0, -0.1], dtype=np.float32),
+        source_available=True,
+        surface_valid=True,
+        profile=CameraGeometryFusionProfile(),
+    )
+    assert support.document["geometry_status"] == "agree"
+    assert support.document["range_m"] == pytest.approx(2.0666666667)
+    assert support.document["unknown_is_occupied"] is True
+    assert support.document["navigation_or_safety_accepted"] is False
+    assert support.occupied_source_indices.tolist() == [0, 1]
+
+
+def test_camera_only_and_surface_conflict_remain_explicit() -> None:
+    points = np.column_stack(
+        (
+            np.linspace(2.0, 3.0, 6),
+            np.zeros(6),
+            np.zeros(6),
+        )
+    ).astype(np.float64)
+    camera_only = _semantic_support(
+        _object(),
+        projected=_projection(6),
+        frame_points_map=points,
+        point_class=np.asarray([2, 1, 1, 1, 1, 1], dtype=np.uint8),
+        point_height_m=np.asarray([0.5, 0, 0, 0, 0, 0], dtype=np.float32),
+        source_available=True,
+        surface_valid=True,
+        profile=CameraGeometryFusionProfile(),
+    )
+    conflict = _semantic_support(
+        _object(observed_geometry=True),
+        projected=_projection(6),
+        frame_points_map=points,
+        point_class=np.ones(6, dtype=np.uint8),
+        point_height_m=np.zeros(6, dtype=np.float32),
+        source_available=True,
+        surface_valid=True,
+        profile=CameraGeometryFusionProfile(),
+    )
+    unknown = _semantic_support(
+        _object(),
+        projected=None,
+        frame_points_map=np.empty((0, 3), dtype=np.float64),
+        point_class=np.empty(0, dtype=np.uint8),
+        point_height_m=np.empty(0, dtype=np.float32),
+        source_available=False,
+        surface_valid=False,
+        profile=CameraGeometryFusionProfile(),
+    )
+    assert camera_only.document["geometry_status"] == "single-source-camera"
+    assert conflict.document["geometry_status"] == "conflict"
+    assert unknown.document["geometry_status"] == "unknown"
+
+
+def test_unclaimed_occupied_component_is_a_separate_geometry_layer() -> None:
+    points = np.asarray(
+        [
+            [1.00, 0.00, 0.30],
+            [1.20, 0.00, 0.35],
+            [1.45, 0.00, 0.40],
+            [1.65, 0.00, 0.45],
+            [4.00, 0.00, 0.30],
+            [4.20, 0.00, 0.35],
+            [4.45, 0.00, 0.40],
+            [4.65, 0.00, 0.45],
+        ],
+        dtype=np.float64,
+    )
+    clusters = _geometry_clusters(
+        points_map=points,
+        point_class=np.full(8, 2, dtype=np.uint8),
+        point_height_m=np.linspace(0.3, 0.45, 8).astype(np.float32),
+        sensor_position_map=np.zeros(3, dtype=np.float64),
+        claimed_source_indices={0},
+        profile=CameraGeometryFusionProfile(),
+    )
+    assert len(clusters) == 1
+    assert clusters[0]["geometry_status"] == "single-source-geometry"
+    assert clusters[0]["semantic_class"] is None
+    assert clusters[0]["point_count"] == 4
+    assert clusters[0]["unknown_is_occupied"] is True
+
+
+def test_profile_rejects_free_space_shortcuts() -> None:
+    with pytest.raises(SemanticGeometryFusionError):
+        CameraGeometryFusionProfile(
+            geometry_minimum_cluster_points=0,
+        )