feat: initial commit - massive-cloud point cloud processing system
This commit is contained in:
@@ -0,0 +1,36 @@
|
||||
"""Four-hole detection on a panel point cloud.
|
||||
|
||||
Pipeline (see ``detect_four_holes``)::
|
||||
|
||||
1. plane_local.project_to_panel(...) # cloud -> 2D coords
|
||||
2. occupancy_grid.build(...) # 2D grid + connected components
|
||||
3. occupancy_grid.find_holes(...) # holes (cavities) inside main panel CC
|
||||
4. circle_fit.fit_circle(...) # boundary point fit
|
||||
5. four_hole_selector.select(...) # keep the 4 best, ordered
|
||||
"""
|
||||
|
||||
from .plane_local import LocalPanelFrame, project_to_panel, lift_back
|
||||
from .circle_fit import fit_circle, fit_circle_lsq
|
||||
from .occupancy_grid import (
|
||||
OccupancyGrid,
|
||||
HoleCandidate,
|
||||
build_grid,
|
||||
find_panel_holes,
|
||||
)
|
||||
from .four_hole_selector import select_four_holes
|
||||
from .detect_holes import detect_four_holes, FourHoleDetection
|
||||
|
||||
__all__ = [
|
||||
"LocalPanelFrame",
|
||||
"project_to_panel",
|
||||
"lift_back",
|
||||
"fit_circle",
|
||||
"fit_circle_lsq",
|
||||
"OccupancyGrid",
|
||||
"HoleCandidate",
|
||||
"build_grid",
|
||||
"find_panel_holes",
|
||||
"select_four_holes",
|
||||
"detect_four_holes",
|
||||
"FourHoleDetection",
|
||||
]
|
||||
@@ -0,0 +1,106 @@
|
||||
"""2D circle fitting.
|
||||
|
||||
Two routines:
|
||||
|
||||
- :func:`fit_circle_algebraic` — Pratt-style algebraic fit (closed-form).
|
||||
Fast, biased, but a great initial guess.
|
||||
- :func:`fit_circle_lsq` — Levenberg-Marquardt geometric fit.
|
||||
Unbiased, requires an initial guess.
|
||||
- :func:`fit_circle` — algebraic → geometric, returns the result
|
||||
along with the per-point residuals.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CircleFit:
|
||||
cx: float
|
||||
cy: float
|
||||
radius: float
|
||||
residual_rms_mm: float
|
||||
residual_max_mm: float
|
||||
n_points: int
|
||||
|
||||
|
||||
def fit_circle_algebraic(uv: np.ndarray) -> tuple[float, float, float]:
|
||||
"""Return (cx, cy, radius) by Pratt's algebraic fit."""
|
||||
if uv.shape[0] < 3:
|
||||
raise ValueError("need ≥ 3 points to fit a circle")
|
||||
x = uv[:, 0]
|
||||
y = uv[:, 1]
|
||||
A = np.column_stack([2.0 * x, 2.0 * y, np.ones_like(x)])
|
||||
b = x ** 2 + y ** 2
|
||||
sol, *_ = np.linalg.lstsq(A, b, rcond=None)
|
||||
cx, cy, c = sol
|
||||
r2 = c + cx ** 2 + cy ** 2
|
||||
if r2 <= 0:
|
||||
raise ValueError("algebraic fit produced non-positive squared radius")
|
||||
return float(cx), float(cy), float(np.sqrt(r2))
|
||||
|
||||
|
||||
def fit_circle_lsq(
|
||||
uv: np.ndarray,
|
||||
*,
|
||||
init: tuple[float, float, float] | None = None,
|
||||
max_iter: int = 30,
|
||||
tol: float = 1e-8,
|
||||
) -> tuple[float, float, float]:
|
||||
"""Geometric (orthogonal-residual) least-squares circle fit.
|
||||
|
||||
Uses Gauss–Newton on f_i(p) = sqrt((x_i-cx)² + (y_i-cy)²) - r.
|
||||
"""
|
||||
if init is None:
|
||||
cx, cy, r = fit_circle_algebraic(uv)
|
||||
else:
|
||||
cx, cy, r = init
|
||||
|
||||
x = uv[:, 0]
|
||||
y = uv[:, 1]
|
||||
for _ in range(max_iter):
|
||||
dx = x - cx
|
||||
dy = y - cy
|
||||
d = np.sqrt(dx * dx + dy * dy)
|
||||
d_safe = np.where(d > 1e-12, d, 1e-12)
|
||||
# Residual.
|
||||
f = d - r
|
||||
# Jacobian wrt (cx, cy, r).
|
||||
J = np.column_stack([-dx / d_safe, -dy / d_safe, -np.ones_like(d)])
|
||||
try:
|
||||
delta, *_ = np.linalg.lstsq(J, -f, rcond=None)
|
||||
except np.linalg.LinAlgError:
|
||||
break
|
||||
cx += delta[0]
|
||||
cy += delta[1]
|
||||
r += delta[2]
|
||||
if np.max(np.abs(delta)) < tol:
|
||||
break
|
||||
return float(cx), float(cy), float(r)
|
||||
|
||||
|
||||
def fit_circle(uv: np.ndarray, *, refine: bool = True) -> CircleFit:
|
||||
cx, cy, r = fit_circle_algebraic(uv)
|
||||
if refine:
|
||||
cx, cy, r = fit_circle_lsq(uv, init=(cx, cy, r))
|
||||
d = np.sqrt((uv[:, 0] - cx) ** 2 + (uv[:, 1] - cy) ** 2)
|
||||
res = d - r
|
||||
return CircleFit(
|
||||
cx=cx,
|
||||
cy=cy,
|
||||
radius=r,
|
||||
residual_rms_mm=float(np.sqrt(np.mean(res ** 2))),
|
||||
residual_max_mm=float(np.max(np.abs(res))),
|
||||
n_points=int(uv.shape[0]),
|
||||
)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"CircleFit",
|
||||
"fit_circle",
|
||||
"fit_circle_algebraic",
|
||||
"fit_circle_lsq",
|
||||
]
|
||||
@@ -0,0 +1,154 @@
|
||||
"""High-level orchestration: cloud → 4 ordered hole centres in 3D."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import HoleDetectionError
|
||||
from ..structures import Hole, HoleSet
|
||||
from .circle_fit import fit_circle
|
||||
from .four_hole_selector import CircleCandidate, select_four_holes
|
||||
from .occupancy_grid import build_grid, find_panel_holes
|
||||
from .plane_local import LocalPanelFrame, project_to_panel
|
||||
|
||||
|
||||
@dataclass
|
||||
class FourHoleDetection:
|
||||
holes_3d: HoleSet
|
||||
frame: LocalPanelFrame
|
||||
candidates: list[CircleCandidate]
|
||||
diagnostics: dict[str, Any]
|
||||
|
||||
|
||||
def detect_four_holes(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
detection_cfg: dict[str, Any],
|
||||
cad_cfg: dict[str, Any],
|
||||
panel_normal_seed: np.ndarray | None = None,
|
||||
) -> FourHoleDetection:
|
||||
"""Detect the four locating-hole centres in 3D from a panel point cloud."""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
if pts.shape[0] < 100:
|
||||
raise HoleDetectionError(f"too few points for hole detection: {pts.shape[0]}")
|
||||
|
||||
# 1. Project onto local panel plane.
|
||||
seed_mode = detection_cfg.get("panel_normal_seed", "from_cad")
|
||||
if panel_normal_seed is not None and seed_mode != "pca":
|
||||
seed = np.asarray(panel_normal_seed, dtype=np.float64)
|
||||
else:
|
||||
seed = "pca"
|
||||
frame, uv = project_to_panel(pts, normal_seed=seed)
|
||||
|
||||
# 2. Build 2D occupancy grid.
|
||||
pixel_mm = float(detection_cfg["grid_pixel_mm"])
|
||||
closing_iters = int(detection_cfg.get("closing_iters", 0))
|
||||
fill_bg_below = float(detection_cfg.get("fill_bg_below_mm2", 0.0))
|
||||
grid = build_grid(
|
||||
uv,
|
||||
pixel_mm=pixel_mm,
|
||||
closing_iters=closing_iters,
|
||||
fill_bg_below_mm2=fill_bg_below,
|
||||
)
|
||||
|
||||
# 3. Find panel + hole cavities.
|
||||
panel_mask, holes = find_panel_holes(
|
||||
grid,
|
||||
panel_min_area_ratio=float(detection_cfg.get("panel_min_area_ratio", 0.4)),
|
||||
hole_min_area_mm2=float(detection_cfg.get("hole_min_area_mm2", 30.0)),
|
||||
hole_max_area_mm2=float(detection_cfg.get("hole_max_area_mm2", 800.0)),
|
||||
point_uv=uv,
|
||||
)
|
||||
if len(holes) < 4:
|
||||
raise HoleDetectionError(
|
||||
f"only {len(holes)} hole-shaped cavities found in occupancy grid; "
|
||||
"consider lowering grid_pixel_mm or relaxing hole_*_area_mm2"
|
||||
)
|
||||
|
||||
# 4. Fit a circle to each cavity boundary.
|
||||
candidates: list[CircleCandidate] = []
|
||||
for h in holes:
|
||||
try:
|
||||
cf = fit_circle(h.boundary_uv)
|
||||
except Exception:
|
||||
continue
|
||||
candidates.append(
|
||||
CircleCandidate(
|
||||
fit=cf,
|
||||
centroid_uv=h.centroid_uv,
|
||||
area_mm2=h.area_mm2,
|
||||
n_boundary_points=int(h.boundary_uv.shape[0]),
|
||||
)
|
||||
)
|
||||
if len(candidates) < 4:
|
||||
raise HoleDetectionError(
|
||||
f"only {len(candidates)} circles fitted (need ≥ 4)"
|
||||
)
|
||||
|
||||
# 5. Pick the 4 most rectangle-like, equal-radius circles.
|
||||
# Detection can override the CAD-side radius range (rasterisation
|
||||
# biases the fitted radius outwards).
|
||||
r_lo, r_hi = detection_cfg.get(
|
||||
"locating_hole_radius_mm",
|
||||
cad_cfg["locating_hole_radius_mm"],
|
||||
)
|
||||
ordered, sel_idx, diag = select_four_holes(
|
||||
candidates,
|
||||
target_radius_lo_mm=float(r_lo),
|
||||
target_radius_hi_mm=float(r_hi),
|
||||
rectangle_aspect_tol=float(
|
||||
detection_cfg.get(
|
||||
"rectangle_aspect_tol",
|
||||
cad_cfg.get("rectangle_aspect_tol", 0.20),
|
||||
)
|
||||
),
|
||||
)
|
||||
|
||||
# 6. Lift the four hole centres back to 3D (signed_distance = 0).
|
||||
centers_uv = np.array([(c.fit.cx, c.fit.cy) for c in ordered])
|
||||
centers_3d = frame.from_2d(centers_uv, signed_distance=0.0)
|
||||
|
||||
# 7. Identify a central hole (radius ≥ central_hole_radius_min_mm).
|
||||
central_hole = None
|
||||
central_min = float(cad_cfg.get("central_hole_radius_min_mm", 20.0))
|
||||
for cand in candidates:
|
||||
if cand.fit.radius >= central_min:
|
||||
uv_c = np.array([[cand.fit.cx, cand.fit.cy]])
|
||||
xyz_c = frame.from_2d(uv_c, signed_distance=0.0)[0]
|
||||
central_hole = Hole(
|
||||
center=tuple(xyz_c.tolist()),
|
||||
radius_mm=float(cand.fit.radius),
|
||||
fit_residual_rms_mm=float(cand.fit.residual_rms_mm),
|
||||
role="central",
|
||||
)
|
||||
break
|
||||
|
||||
holes_3d = HoleSet(
|
||||
locating=tuple( # type: ignore[arg-type]
|
||||
Hole(
|
||||
center=tuple(centers_3d[i].tolist()),
|
||||
radius_mm=float(c.fit.radius),
|
||||
fit_residual_rms_mm=float(c.fit.residual_rms_mm),
|
||||
role="locating",
|
||||
)
|
||||
for i, c in enumerate(ordered)
|
||||
),
|
||||
central=central_hole,
|
||||
)
|
||||
|
||||
return FourHoleDetection(
|
||||
holes_3d=holes_3d,
|
||||
frame=frame,
|
||||
candidates=candidates,
|
||||
diagnostics={
|
||||
"n_cavities": len(holes),
|
||||
"n_circle_fits": len(candidates),
|
||||
"selection": diag,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["FourHoleDetection", "detect_four_holes"]
|
||||
@@ -0,0 +1,145 @@
|
||||
"""Pick four locating holes out of a list of fitted-circle candidates.
|
||||
|
||||
Given more than four candidates, choose the 4-subset that:
|
||||
|
||||
1. Has the smallest standard deviation of radius (the four locating
|
||||
holes are equal-radius by spec).
|
||||
2. Forms a near-rectangle (small `rectangle_score`, see CAD GT module).
|
||||
3. Each radius is within ``[r_lo, r_hi]``.
|
||||
|
||||
We score the cartesian product of "best radii" and "best rectangle"
|
||||
jointly, weighted.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from itertools import combinations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .circle_fit import CircleFit
|
||||
|
||||
|
||||
@dataclass
|
||||
class CircleCandidate:
|
||||
"""A candidate locating hole, in the panel's local 2D frame."""
|
||||
|
||||
fit: CircleFit
|
||||
centroid_uv: tuple[float, float] # from occupancy grid (used as initial guess)
|
||||
area_mm2: float
|
||||
n_boundary_points: int
|
||||
|
||||
|
||||
def _rectangle_score(centers_uv: np.ndarray) -> float:
|
||||
"""0 = perfect rectangle. Larger = worse."""
|
||||
if centers_uv.shape[0] != 4:
|
||||
return float("inf")
|
||||
d = sorted(
|
||||
float(np.linalg.norm(centers_uv[i] - centers_uv[j]))
|
||||
for i, j in combinations(range(4), 2)
|
||||
)
|
||||
if d[0] < 1e-6:
|
||||
return 1e6
|
||||
s1, s2 = d[0], d[1]
|
||||
l1, l2 = d[2], d[3]
|
||||
g1, g2 = d[4], d[5]
|
||||
return (
|
||||
abs(s1 - s2) / max(s1, s2)
|
||||
+ abs(l1 - l2) / max(l1, l2)
|
||||
+ abs(g1 - g2) / max(g1, g2)
|
||||
)
|
||||
|
||||
|
||||
def _radius_score(radii: np.ndarray, target_lo: float, target_hi: float) -> float:
|
||||
if np.any(radii < target_lo) or np.any(radii > target_hi):
|
||||
return float("inf")
|
||||
if radii.mean() <= 0:
|
||||
return float("inf")
|
||||
return float(radii.std() / max(radii.mean(), 1e-6))
|
||||
|
||||
|
||||
def _order_quadrants(centers_uv: np.ndarray, candidates) -> tuple[list, list[int]]:
|
||||
"""Order 4 candidates as (-u/-v, +u/-v, +u/+v, -u/+v) in the local frame."""
|
||||
cu = centers_uv[:, 0].mean()
|
||||
cv = centers_uv[:, 1].mean()
|
||||
quadrant: dict[tuple[bool, bool], int] = {}
|
||||
for idx, p in enumerate(centers_uv):
|
||||
quadrant[(p[0] >= cu, p[1] >= cv)] = idx
|
||||
order = [(False, False), (True, False), (True, True), (False, True)]
|
||||
if not all(q in quadrant for q in order):
|
||||
# Degenerate, fall back to angular sort.
|
||||
ang = np.arctan2(centers_uv[:, 1] - cv, centers_uv[:, 0] - cu)
|
||||
idxs = np.argsort(ang).tolist()
|
||||
return [candidates[i] for i in idxs], idxs
|
||||
idxs = [quadrant[q] for q in order]
|
||||
return [candidates[i] for i in idxs], idxs
|
||||
|
||||
|
||||
def select_four_holes(
|
||||
candidates: list[CircleCandidate],
|
||||
*,
|
||||
target_radius_lo_mm: float,
|
||||
target_radius_hi_mm: float,
|
||||
rectangle_aspect_tol: float = 0.20,
|
||||
radius_weight: float = 1.0,
|
||||
rectangle_weight: float = 1.0,
|
||||
) -> tuple[list[CircleCandidate], list[int], dict]:
|
||||
"""Pick the best 4-subset; return (ordered subset, indices into input, diag).
|
||||
|
||||
Raises ``RuntimeError`` if no valid 4-subset exists.
|
||||
"""
|
||||
n = len(candidates)
|
||||
if n < 4:
|
||||
raise RuntimeError(f"need ≥ 4 candidates, got {n}")
|
||||
|
||||
best_idx: tuple[int, ...] | None = None
|
||||
best_score = float("inf")
|
||||
best_diag: dict = {}
|
||||
|
||||
for combo in combinations(range(n), 4):
|
||||
radii = np.array([candidates[i].fit.radius for i in combo])
|
||||
rs = _radius_score(radii, target_radius_lo_mm, target_radius_hi_mm)
|
||||
if not np.isfinite(rs):
|
||||
continue
|
||||
centers = np.array([candidates[i].fit.cx for i in combo]), np.array(
|
||||
[candidates[i].fit.cy for i in combo]
|
||||
)
|
||||
centers_uv = np.column_stack(centers)
|
||||
rect = _rectangle_score(centers_uv)
|
||||
score = radius_weight * rs + rectangle_weight * rect
|
||||
if score < best_score:
|
||||
best_score = score
|
||||
best_idx = combo
|
||||
best_diag = {
|
||||
"radius_score": rs,
|
||||
"rectangle_score": rect,
|
||||
"score": score,
|
||||
"radii_mean": float(radii.mean()),
|
||||
"radii_std": float(radii.std()),
|
||||
}
|
||||
|
||||
if best_idx is None:
|
||||
raise RuntimeError(
|
||||
"no valid 4-subset (radius constraints excluded all combinations); "
|
||||
f"target_radius=[{target_radius_lo_mm}, {target_radius_hi_mm}], "
|
||||
f"got {sorted(round(c.fit.radius, 3) for c in candidates)}"
|
||||
)
|
||||
|
||||
subset = [candidates[i] for i in best_idx]
|
||||
centers_uv = np.array([(c.fit.cx, c.fit.cy) for c in subset])
|
||||
ordered, perm = _order_quadrants(centers_uv, subset)
|
||||
final_idx = [best_idx[p] for p in perm]
|
||||
best_diag["selected_indices"] = list(final_idx)
|
||||
best_diag["selected_radii"] = [c.fit.radius for c in ordered]
|
||||
|
||||
if best_diag["rectangle_score"] > rectangle_aspect_tol:
|
||||
best_diag.setdefault("warnings", []).append("rectangle_score_above_tol")
|
||||
|
||||
return ordered, final_idx, best_diag
|
||||
|
||||
|
||||
__all__ = [
|
||||
"CircleCandidate",
|
||||
"select_four_holes",
|
||||
]
|
||||
@@ -0,0 +1,350 @@
|
||||
"""2D occupancy grid + connected-component analysis on panel-projected points.
|
||||
|
||||
Steps used by :func:`detect_four_holes`:
|
||||
|
||||
1. ``build_grid`` — rasterise (u, v) points to a binary grid.
|
||||
2. ``find_panel_holes`` — find the largest connected component (= panel),
|
||||
then enumerate its internal cavities (= holes). Each cavity gets its
|
||||
boundary points (in the original 3D order) so we can fit a circle.
|
||||
|
||||
Implementation is pure numpy + a small flood-fill — no scikit-image
|
||||
dependency.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class OccupancyGrid:
|
||||
occupied: np.ndarray # (H, W) bool
|
||||
pixel_mm: float
|
||||
u_min: float
|
||||
v_min: float
|
||||
point_indices: np.ndarray # (H, W, K_max)? No — see point_pixel_idx instead
|
||||
point_pixel_idx: np.ndarray # (N, 2) int (row, col)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class HoleCandidate:
|
||||
"""One internal cavity of the panel CC."""
|
||||
|
||||
boundary_uv: np.ndarray # (M, 2) float — boundary pixel centres
|
||||
boundary_point_idx: np.ndarray # (M,) int — indices into the original cloud, may be empty
|
||||
centroid_uv: tuple[float, float]
|
||||
area_mm2: float
|
||||
bbox_uv: tuple[float, float, float, float] # (umin, vmin, umax, vmax)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _binary_dilate(grid: np.ndarray, iters: int = 1) -> np.ndarray:
|
||||
"""4-connected dilation, ``iters`` times. Pure numpy."""
|
||||
if iters <= 0:
|
||||
return grid.copy()
|
||||
out = grid.copy()
|
||||
for _ in range(iters):
|
||||
nxt = out.copy()
|
||||
nxt[1:, :] |= out[:-1, :]
|
||||
nxt[:-1, :] |= out[1:, :]
|
||||
nxt[:, 1:] |= out[:, :-1]
|
||||
nxt[:, :-1] |= out[:, 1:]
|
||||
out = nxt
|
||||
return out
|
||||
|
||||
|
||||
def _binary_erode(grid: np.ndarray, iters: int = 1) -> np.ndarray:
|
||||
if iters <= 0:
|
||||
return grid.copy()
|
||||
inv = ~grid
|
||||
inv = _binary_dilate(inv, iters)
|
||||
return ~inv
|
||||
|
||||
|
||||
def _binary_close(grid: np.ndarray, iters: int = 1) -> np.ndarray:
|
||||
"""Close = dilate then erode. Fills small gaps without growing the mask."""
|
||||
return _binary_erode(_binary_dilate(grid, iters), iters)
|
||||
|
||||
|
||||
def _fill_small_bg_components(grid: np.ndarray, min_keep_area_px: int) -> np.ndarray:
|
||||
"""Set every 0-component smaller than ``min_keep_area_px`` to 1.
|
||||
|
||||
Equivalent to "area opening of the background" — fills sparse-coverage
|
||||
gaps without shrinking the real cavities the caller wants to detect.
|
||||
Border-touching background components are always kept (they're outside
|
||||
the panel, not real holes).
|
||||
"""
|
||||
H, W = grid.shape
|
||||
bg_labels = _flood_fill_label(~grid)
|
||||
if bg_labels.max() == 0:
|
||||
return grid.copy()
|
||||
|
||||
# Identify border components.
|
||||
border = set()
|
||||
for j in range(W):
|
||||
if bg_labels[0, j] != 0:
|
||||
border.add(int(bg_labels[0, j]))
|
||||
if bg_labels[H - 1, j] != 0:
|
||||
border.add(int(bg_labels[H - 1, j]))
|
||||
for i in range(H):
|
||||
if bg_labels[i, 0] != 0:
|
||||
border.add(int(bg_labels[i, 0]))
|
||||
if bg_labels[i, W - 1] != 0:
|
||||
border.add(int(bg_labels[i, W - 1]))
|
||||
|
||||
counts = np.bincount(bg_labels.ravel())
|
||||
out = grid.copy()
|
||||
fill_labels = np.zeros(len(counts) + 1, dtype=bool)
|
||||
for k in range(1, len(counts)):
|
||||
if k in border:
|
||||
continue
|
||||
if counts[k] < min_keep_area_px:
|
||||
fill_labels[k] = True
|
||||
fill_mask = fill_labels[bg_labels]
|
||||
out[fill_mask] = True
|
||||
return out
|
||||
|
||||
|
||||
def build_grid(
|
||||
uv: np.ndarray,
|
||||
*,
|
||||
pixel_mm: float,
|
||||
pad_pixels: int = 4,
|
||||
closing_iters: int = 0,
|
||||
fill_bg_below_mm2: float = 0.0,
|
||||
) -> OccupancyGrid:
|
||||
"""Rasterise points to a binary grid.
|
||||
|
||||
``closing_iters`` is the number of binary-closing passes applied as a
|
||||
cheap defragmenter (also shrinks real holes by ``iters`` pixels — use
|
||||
sparingly).
|
||||
|
||||
``fill_bg_below_mm2`` is the preferred defragmenter: it fills every
|
||||
background component smaller than this area (in mm²) with foreground.
|
||||
Real cavities (holes the caller wants to detect) keep their original
|
||||
size because they exceed the threshold.
|
||||
"""
|
||||
if uv.shape[0] == 0:
|
||||
raise ValueError("empty point set")
|
||||
u_min = float(uv[:, 0].min()) - pad_pixels * pixel_mm
|
||||
v_min = float(uv[:, 1].min()) - pad_pixels * pixel_mm
|
||||
u_max = float(uv[:, 0].max()) + pad_pixels * pixel_mm
|
||||
v_max = float(uv[:, 1].max()) + pad_pixels * pixel_mm
|
||||
|
||||
W = int(np.ceil((u_max - u_min) / pixel_mm)) + 1
|
||||
H = int(np.ceil((v_max - v_min) / pixel_mm)) + 1
|
||||
|
||||
cols = np.clip(((uv[:, 0] - u_min) / pixel_mm).astype(np.int64), 0, W - 1)
|
||||
rows = np.clip(((uv[:, 1] - v_min) / pixel_mm).astype(np.int64), 0, H - 1)
|
||||
|
||||
occ = np.zeros((H, W), dtype=bool)
|
||||
occ[rows, cols] = True
|
||||
if closing_iters > 0:
|
||||
occ = _binary_close(occ, closing_iters)
|
||||
if fill_bg_below_mm2 > 0:
|
||||
min_keep_px = max(1, int(np.ceil(float(fill_bg_below_mm2) / (pixel_mm ** 2))))
|
||||
occ = _fill_small_bg_components(occ, min_keep_px)
|
||||
|
||||
return OccupancyGrid(
|
||||
occupied=occ,
|
||||
pixel_mm=float(pixel_mm),
|
||||
u_min=u_min,
|
||||
v_min=v_min,
|
||||
point_indices=np.empty(0),
|
||||
point_pixel_idx=np.column_stack([rows, cols]),
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _flood_fill_label(grid: np.ndarray) -> np.ndarray:
|
||||
"""4-connected labelling. Background (False) gets label 0."""
|
||||
H, W = grid.shape
|
||||
labels = np.zeros((H, W), dtype=np.int32)
|
||||
next_label = 0
|
||||
stack: list[tuple[int, int]] = []
|
||||
for i in range(H):
|
||||
for j in range(W):
|
||||
if grid[i, j] and labels[i, j] == 0:
|
||||
next_label += 1
|
||||
stack.append((i, j))
|
||||
while stack:
|
||||
r, c = stack.pop()
|
||||
if r < 0 or r >= H or c < 0 or c >= W:
|
||||
continue
|
||||
if not grid[r, c] or labels[r, c] != 0:
|
||||
continue
|
||||
labels[r, c] = next_label
|
||||
stack.append((r - 1, c))
|
||||
stack.append((r + 1, c))
|
||||
stack.append((r, c - 1))
|
||||
stack.append((r, c + 1))
|
||||
return labels
|
||||
|
||||
|
||||
def _largest_component(labels: np.ndarray) -> int:
|
||||
if labels.max() == 0:
|
||||
return 0
|
||||
counts = np.bincount(labels.ravel())
|
||||
counts[0] = 0
|
||||
return int(np.argmax(counts))
|
||||
|
||||
|
||||
def _holes_inside(
|
||||
labels: np.ndarray,
|
||||
panel_label: int,
|
||||
) -> list[np.ndarray]:
|
||||
"""Return masks of background components fully enclosed by ``panel_label``.
|
||||
|
||||
A 'hole' is a 0-component that is not connected to the image border
|
||||
via 4-connectivity through other 0-pixels.
|
||||
"""
|
||||
H, W = labels.shape
|
||||
bg = labels == 0
|
||||
|
||||
# Label background components.
|
||||
bg_labels = _flood_fill_label(bg)
|
||||
|
||||
# Components that touch the border.
|
||||
border_components = set()
|
||||
for j in range(W):
|
||||
if bg_labels[0, j] != 0:
|
||||
border_components.add(int(bg_labels[0, j]))
|
||||
if bg_labels[H - 1, j] != 0:
|
||||
border_components.add(int(bg_labels[H - 1, j]))
|
||||
for i in range(H):
|
||||
if bg_labels[i, 0] != 0:
|
||||
border_components.add(int(bg_labels[i, 0]))
|
||||
if bg_labels[i, W - 1] != 0:
|
||||
border_components.add(int(bg_labels[i, W - 1]))
|
||||
|
||||
holes: list[np.ndarray] = []
|
||||
n_bg = int(bg_labels.max())
|
||||
for k in range(1, n_bg + 1):
|
||||
if k in border_components:
|
||||
continue
|
||||
mask = bg_labels == k
|
||||
# Sanity: at least one neighbouring pixel must belong to ``panel_label``.
|
||||
# (Otherwise it's enclosed by a different fg component, which we ignore.)
|
||||
# Quick neighbour test:
|
||||
from numpy import roll
|
||||
nbr = (
|
||||
(roll(labels, 1, axis=0) == panel_label)
|
||||
| (roll(labels, -1, axis=0) == panel_label)
|
||||
| (roll(labels, 1, axis=1) == panel_label)
|
||||
| (roll(labels, -1, axis=1) == panel_label)
|
||||
)
|
||||
if np.any(mask & nbr):
|
||||
holes.append(mask)
|
||||
return holes
|
||||
|
||||
|
||||
def _hole_boundary_pixels(panel_mask: np.ndarray, hole_mask: np.ndarray) -> np.ndarray:
|
||||
"""Pixels in ``panel_mask`` that touch a pixel in ``hole_mask``."""
|
||||
from numpy import roll
|
||||
|
||||
nbr = (
|
||||
roll(hole_mask, 1, axis=0)
|
||||
| roll(hole_mask, -1, axis=0)
|
||||
| roll(hole_mask, 1, axis=1)
|
||||
| roll(hole_mask, -1, axis=1)
|
||||
)
|
||||
return panel_mask & nbr
|
||||
|
||||
|
||||
def find_panel_holes(
|
||||
grid: OccupancyGrid,
|
||||
*,
|
||||
panel_min_area_ratio: float = 0.4,
|
||||
hole_min_area_mm2: float = 30.0,
|
||||
hole_max_area_mm2: float = 800.0,
|
||||
point_uv: np.ndarray | None = None,
|
||||
) -> tuple[np.ndarray, list[HoleCandidate]]:
|
||||
"""Return (panel_mask, list_of_holes).
|
||||
|
||||
``point_uv`` is optional but, if provided, lets us hand back the
|
||||
*original-cloud* point indices that fall inside each hole's boundary
|
||||
pixels (= proper boundary points for a 3D circle fit).
|
||||
"""
|
||||
H, W = grid.occupied.shape
|
||||
fg_labels = _flood_fill_label(grid.occupied)
|
||||
panel_label = _largest_component(fg_labels)
|
||||
if panel_label == 0:
|
||||
raise RuntimeError("no occupied pixels")
|
||||
|
||||
panel_mask = fg_labels == panel_label
|
||||
panel_area = panel_mask.sum() * grid.pixel_mm ** 2
|
||||
bbox_area = H * W * grid.pixel_mm ** 2
|
||||
if panel_area / max(bbox_area, 1e-9) < panel_min_area_ratio:
|
||||
# The panel CC is suspiciously small. We still proceed; downstream
|
||||
# code will report a quality flag.
|
||||
pass
|
||||
|
||||
holes_masks = _holes_inside(fg_labels, panel_label)
|
||||
out: list[HoleCandidate] = []
|
||||
|
||||
# Pre-compute (row, col) → list of original-cloud indices, for boundary recovery.
|
||||
pix_rc = grid.point_pixel_idx if point_uv is not None else None
|
||||
|
||||
for mask in holes_masks:
|
||||
area = float(mask.sum() * grid.pixel_mm ** 2)
|
||||
if area < hole_min_area_mm2 or area > hole_max_area_mm2:
|
||||
continue
|
||||
|
||||
bnd_mask = _hole_boundary_pixels(panel_mask, mask)
|
||||
if not bnd_mask.any():
|
||||
continue
|
||||
|
||||
bnd_rc = np.argwhere(bnd_mask) # (M, 2): (row, col)
|
||||
bnd_uv = np.column_stack(
|
||||
[
|
||||
grid.u_min + (bnd_rc[:, 1] + 0.5) * grid.pixel_mm,
|
||||
grid.v_min + (bnd_rc[:, 0] + 0.5) * grid.pixel_mm,
|
||||
]
|
||||
)
|
||||
|
||||
rows = np.argwhere(mask)
|
||||
cu = grid.u_min + (rows[:, 1] + 0.5).mean() * grid.pixel_mm
|
||||
cv = grid.v_min + (rows[:, 0] + 0.5).mean() * grid.pixel_mm
|
||||
|
||||
bbox = (
|
||||
grid.u_min + rows[:, 1].min() * grid.pixel_mm,
|
||||
grid.v_min + rows[:, 0].min() * grid.pixel_mm,
|
||||
grid.u_min + rows[:, 1].max() * grid.pixel_mm,
|
||||
grid.v_min + rows[:, 0].max() * grid.pixel_mm,
|
||||
)
|
||||
|
||||
if pix_rc is not None:
|
||||
# Map boundary pixels back to point indices.
|
||||
row_col_set = set(map(tuple, bnd_rc.tolist()))
|
||||
point_idx = np.array(
|
||||
[i for i, (r, c) in enumerate(map(tuple, pix_rc.tolist())) if (r, c) in row_col_set],
|
||||
dtype=np.int64,
|
||||
)
|
||||
else:
|
||||
point_idx = np.empty(0, dtype=np.int64)
|
||||
|
||||
out.append(
|
||||
HoleCandidate(
|
||||
boundary_uv=bnd_uv,
|
||||
boundary_point_idx=point_idx,
|
||||
centroid_uv=(float(cu), float(cv)),
|
||||
area_mm2=area,
|
||||
bbox_uv=bbox,
|
||||
)
|
||||
)
|
||||
|
||||
return panel_mask, out
|
||||
|
||||
|
||||
__all__ = [
|
||||
"OccupancyGrid",
|
||||
"HoleCandidate",
|
||||
"build_grid",
|
||||
"find_panel_holes",
|
||||
]
|
||||
@@ -0,0 +1,127 @@
|
||||
"""Build a local 2D frame on the panel and project / lift between it and 3D.
|
||||
|
||||
The panel's normal is roughly along ±x in the CAD frame, but the point
|
||||
cloud may be misaligned. We accept a ``normal_seed`` (from the GT or
|
||||
from a robust PCA on the point cloud) and orthonormalise around it.
|
||||
|
||||
Local 2D conventions
|
||||
--------------------
|
||||
|
||||
- ``u`` axis: closest to world +y inside the panel plane
|
||||
- ``v`` axis: closest to world +z inside the panel plane
|
||||
|
||||
So a top-down picture of the panel has u → right, v → up, and matches
|
||||
how the four locating holes are arranged in the y-z plane.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class LocalPanelFrame:
|
||||
origin: np.ndarray # (3,) — point on plane (typically the centroid)
|
||||
normal: np.ndarray # (3,) — unit, n_x > 0
|
||||
u_axis: np.ndarray # (3,)
|
||||
v_axis: np.ndarray # (3,)
|
||||
|
||||
def to_2d(self, points: np.ndarray) -> np.ndarray:
|
||||
"""Project ``points`` onto the panel plane in (u, v)."""
|
||||
rel = points - self.origin
|
||||
return np.column_stack([rel @ self.u_axis, rel @ self.v_axis])
|
||||
|
||||
def signed_distance(self, points: np.ndarray) -> np.ndarray:
|
||||
return (points - self.origin) @ self.normal
|
||||
|
||||
def from_2d(self, uv: np.ndarray, *, signed_distance: float = 0.0) -> np.ndarray:
|
||||
"""Lift ``(u, v)`` coords back into 3D, optionally with a normal offset."""
|
||||
uv = np.atleast_2d(uv)
|
||||
return (
|
||||
self.origin
|
||||
+ uv[:, 0:1] * self.u_axis
|
||||
+ uv[:, 1:2] * self.v_axis
|
||||
+ float(signed_distance) * self.normal
|
||||
)
|
||||
|
||||
|
||||
def _orthonormal_basis(normal: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Return (u, v) so that {u, v, normal} is an RHC orthonormal basis,
|
||||
with ``u`` the in-plane vector closest to world +y.
|
||||
"""
|
||||
n = normal / max(np.linalg.norm(normal), 1e-12)
|
||||
y = np.array([0.0, 1.0, 0.0])
|
||||
u = y - np.dot(y, n) * n
|
||||
if np.linalg.norm(u) < 1e-6:
|
||||
# n is parallel to y; pick z instead.
|
||||
z = np.array([0.0, 0.0, 1.0])
|
||||
u = z - np.dot(z, n) * n
|
||||
u /= np.linalg.norm(u)
|
||||
v = np.cross(n, u)
|
||||
v /= np.linalg.norm(v)
|
||||
return u, v
|
||||
|
||||
|
||||
def _pca_normal(points: np.ndarray) -> np.ndarray:
|
||||
centroid = points.mean(axis=0)
|
||||
Q = points - centroid
|
||||
cov = (Q.T @ Q) / max(points.shape[0] - 1, 1)
|
||||
w, V = np.linalg.eigh(cov)
|
||||
# Smallest eigenvector ≈ panel normal.
|
||||
n = V[:, 0]
|
||||
if n[0] < 0:
|
||||
n = -n
|
||||
return n
|
||||
|
||||
|
||||
def project_to_panel(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
normal_seed: np.ndarray | str = "pca",
|
||||
origin: np.ndarray | None = None,
|
||||
pca_kth: int = 0,
|
||||
) -> tuple[LocalPanelFrame, np.ndarray]:
|
||||
"""Project ``points`` onto a local panel frame.
|
||||
|
||||
Returns
|
||||
-------
|
||||
frame : :class:`LocalPanelFrame`
|
||||
uv : ``(N, 2)`` projected coordinates
|
||||
|
||||
Parameters
|
||||
----------
|
||||
normal_seed : str or (3,)
|
||||
``"pca"`` → estimate via PCA on the cloud (smallest variance axis).
|
||||
otherwise a 3-vector.
|
||||
origin : (3,) or None
|
||||
If ``None``, use the cloud centroid.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
|
||||
if isinstance(normal_seed, str):
|
||||
if normal_seed == "pca":
|
||||
normal = _pca_normal(pts)
|
||||
else:
|
||||
raise ValueError(f"unknown normal_seed string '{normal_seed}'")
|
||||
else:
|
||||
normal = np.asarray(normal_seed, dtype=np.float64).reshape(3)
|
||||
normal = normal / max(np.linalg.norm(normal), 1e-12)
|
||||
if normal[0] < 0:
|
||||
normal = -normal
|
||||
|
||||
if origin is None:
|
||||
origin = pts.mean(axis=0)
|
||||
origin = np.asarray(origin, dtype=np.float64).reshape(3)
|
||||
|
||||
u, v = _orthonormal_basis(normal)
|
||||
frame = LocalPanelFrame(origin=origin, normal=normal, u_axis=u, v_axis=v)
|
||||
return frame, frame.to_2d(pts)
|
||||
|
||||
|
||||
def lift_back(uv: np.ndarray, frame: LocalPanelFrame) -> np.ndarray:
|
||||
return frame.from_2d(uv, signed_distance=0.0)
|
||||
|
||||
|
||||
__all__ = ["LocalPanelFrame", "project_to_panel", "lift_back"]
|
||||
Reference in New Issue
Block a user