feat(perception): partition_by_plane returns accepted + excluded sets

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed 2026-05-13 10:00:09 +02:00
1 parent 9ae3e00f65
commit 876224a71d
2 files changed
+95 -9

No files matched your search

+19 -9
View File
@@ -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