feat(perception): partition_by_plane returns accepted + excluded sets

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

src/presa_mano/perception/work_plane.py

@@ -122,25 +122,35 @@ def fit_plane_svd(points_xyz: np.ndarray
     return normal, offset, c, rms_mm
 
 
-def apply_below_plane_filter(
+def partition_by_plane(
     pts_cv: np.ndarray,
     wp: WorkPlane | None,
-) -> tuple[np.ndarray, dict]:
+) -> tuple[np.ndarray, np.ndarray, dict]:
-    """Drop points more than `offset_mm` below the plane.
+    """Split points into (accepted_above_plane, excluded_below_plane).
 
-    Returns `(filtered_pts, info)`. `info` always carries:
+    Same threshold as `apply_below_plane_filter`: keep iff
-        plane_applied (bool): true iff a plane was provided AND there were
+    `signed >= -offset_mm/1000`. When `wp is None` or `pts_cv` is empty,
-            points to filter
+    `excluded` is empty and `plane_applied=False`.
-        n_points_before_plane (int): len(pts_cv) on entry
     """
     info = {"plane_applied": False,
             "n_points_before_plane": int(len(pts_cv))}
+    empty = np.empty((0, 3), dtype=pts_cv.dtype if pts_cv.size else np.float64)
     if wp is None or len(pts_cv) == 0:
-        return pts_cv, info
+        return pts_cv, empty, info
     n_vec = np.asarray(wp.normal, dtype=np.float64)
     d_off = float(wp.offset)
     offset_m = float(wp.offset_mm) / 1000.0
     signed = pts_cv @ n_vec + d_off
     keep = signed >= -offset_m
     info["plane_applied"] = True
-    return pts_cv[keep], info
+    return pts_cv[keep], pts_cv[~keep], info
+
+
+def apply_below_plane_filter(
+    pts_cv: np.ndarray,
+    wp: WorkPlane | None,
+) -> tuple[np.ndarray, dict]:
+    """Thin wrapper around partition_by_plane that drops the excluded set.
+    Kept for backwards compatibility with existing callers."""
+    accepted, _excluded, info = partition_by_plane(pts_cv, wp)
+    return accepted, info

tests/test_partition_plane.py

@@ -0,0 +1,76 @@
+"""Unit tests for partition_by_plane."""
+from __future__ import annotations
+
+import numpy as np
+import pytest
+
+from presa_mano.perception.work_plane import WorkPlane, partition_by_plane
+
+
+def _wp(offset_mm: float = 0.0) -> WorkPlane:
+    """Work surface at z=0.5m in OpenCV camera frame (camera at origin
+    looking +Z). Normal=[0,0,-1] points toward the camera — same convention
+    as fit_plane_svd. Plane equation: -z + 0.5 = 0."""
+    return WorkPlane(
+        normal=[0.0, 0.0, -1.0],
+        offset=0.5,
+        centroid=[0.0, 0.0, 0.5],
+        n_inliers=100,
+        fit_rms_mm=0.5,
+        roi=[0, 0, 100, 100],
+        offset_mm=offset_mm,
+        created_at=0.0,
+    )
+
+
+def test_partition_no_plane_returns_all_accepted():
+    pts = np.array([[0.0, 0.0, 1.0], [1.0, 2.0, 3.0]])
+    accepted, excluded, info = partition_by_plane(pts, None)
+    assert np.array_equal(accepted, pts)
+    assert excluded.shape == (0, 3)
+    assert info == {"plane_applied": False, "n_points_before_plane": 2}
+
+
+def test_partition_empty_input():
+    pts = np.empty((0, 3), dtype=np.float64)
+    accepted, excluded, info = partition_by_plane(pts, _wp())
+    assert accepted.shape == (0, 3)
+    assert excluded.shape == (0, 3)
+    assert info["plane_applied"] is False
+    assert info["n_points_before_plane"] == 0
+
+
+def test_partition_splits_by_offset():
+    """Plane at z=0.5, normal=-Z, offset_mm=5.
+
+    Camera-side ("above") is z<0.5. Filter keeps points up to offset_mm
+    *below* the plane (i.e. z<=0.505) and excludes farther-than-offset
+    points (z>0.505)."""
+    pts = np.array([
+        [0.0, 0.0, 0.40],   # 100mm above → accepted
+        [0.0, 0.0, 0.50],   # on plane → accepted (boundary)
+        [0.0, 0.0, 0.504],  # 4mm below → accepted (within offset 5mm)
+        [0.0, 0.0, 0.510],  # 10mm below → excluded
+        [0.0, 0.0, 1.000],  # 500mm below → excluded
+    ])
+    accepted, excluded, info = partition_by_plane(pts, _wp(offset_mm=5.0))
+    assert accepted.shape[0] == 3
+    assert excluded.shape[0] == 2
+    assert info["plane_applied"] is True
+    assert info["n_points_before_plane"] == 5
+    # partition: nessuna sovrapposizione
+    union = np.vstack([accepted, excluded])
+    assert sorted(union[:, 2].tolist()) == sorted(pts[:, 2].tolist())
+
+
+def test_partition_consistent_with_apply_below_plane_filter():
+    """`accepted` di partition_by_plane deve essere bit-identico a quello
+    ritornato da apply_below_plane_filter (l'API legacy non deve cambiare)."""
+    from presa_mano.perception.work_plane import apply_below_plane_filter
+    rng = np.random.default_rng(42)
+    pts = rng.normal(scale=0.1, size=(200, 3))
+    wp = _wp(offset_mm=10.0)
+    accepted_new, _excluded, _info = partition_by_plane(pts, wp)
+    accepted_legacy, info_legacy = apply_below_plane_filter(pts, wp)
+    np.testing.assert_array_equal(accepted_new, accepted_legacy)
+    assert info_legacy["n_points_before_plane"] == 200