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:
1 parent
9ae3e00f65
commit
876224a71d
2 files changed
+95
-9
No files matched your search
@@ -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
|
||||
Reference in new issue
Block a user