diff --git a/src/presa_mano/perception/work_plane.py b/src/presa_mano/perception/work_plane.py index af9ca65..7e8fbbb 100644 --- a/src/presa_mano/perception/work_plane.py +++ b/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]: - """Drop points more than `offset_mm` below the plane. +) -> tuple[np.ndarray, np.ndarray, dict]: + """Split points into (accepted_above_plane, excluded_below_plane). - Returns `(filtered_pts, info)`. `info` always carries: - plane_applied (bool): true iff a plane was provided AND there were - points to filter - n_points_before_plane (int): len(pts_cv) on entry + Same threshold as `apply_below_plane_filter`: keep iff + `signed >= -offset_mm/1000`. When `wp is None` or `pts_cv` is empty, + `excluded` is empty and `plane_applied=False`. """ 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 diff --git a/tests/test_partition_plane.py b/tests/test_partition_plane.py new file mode 100644 index 0000000..998c79c --- /dev/null +++ b/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