feat: 算法升级、整车配准集成与性能分析文档
算法更新: delivery v1.2.1、平整度v1.5、测距v2.2、整车配准v1.3 适配器: vehicle-registration 新增、各算法 manifest 和 adapter 更新 后端: taskCadFiles/source 字段迁移、derivedPointClouds 注册、取消任务联动 前端: ElapsedTime 时长显示、两步算法流程、canRun 逻辑修复 文档: 新增5份算法性能分析报告
This commit is contained in:
@@ -0,0 +1,116 @@
|
||||
"""Coupler-panel CAD-STP to point-cloud measurement validation system.
|
||||
|
||||
The package is split into two distinct layers:
|
||||
|
||||
**1. Public measurement algorithm (integration API).**
|
||||
The single function software developers should call from their
|
||||
product code. Takes a clean point cloud, returns a
|
||||
:class:`CouplerMeasurement`. Stable schema across patch releases —
|
||||
see ``docs/USAGE.md``.
|
||||
|
||||
Entry points:
|
||||
|
||||
- :func:`compute_coupler_measurement` — pure, in-memory.
|
||||
- :class:`CouplerMeasurement`, :class:`HoleSet`, :class:`PanelPlane`,
|
||||
:class:`ReferenceFrame`, :class:`Hole`.
|
||||
- :mod:`coupler_panel.exceptions`.
|
||||
- :func:`load_point_cloud`, :func:`write_point_cloud` for IO.
|
||||
|
||||
**2. Validation / data-preparation harness (research only).**
|
||||
Used to generate synthetic point clouds from CAD, denoise them, and
|
||||
register them into the CAD frame. Not part of the integration
|
||||
surface; layout and signatures may change.
|
||||
|
||||
Entry points:
|
||||
|
||||
- :mod:`coupler_panel.data_prep` — STP → mesh → sample → noise →
|
||||
transform → denoise → register.
|
||||
- :mod:`coupler_panel.pipeline` — convenience wrappers that save
|
||||
every artifact to disk.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
# --- Stable, integration-safe public surface --------------------------------
|
||||
from ._version import ALGORITHM_VERSION, SCHEMA_VERSION, __version__
|
||||
from .exceptions import (
|
||||
CADParseError,
|
||||
CouplerError,
|
||||
HoleDetectionError,
|
||||
InsufficientPointsError,
|
||||
InvalidPointCloudError,
|
||||
PlaneFitError,
|
||||
RegistrationError,
|
||||
UnsupportedFormatError,
|
||||
)
|
||||
from .io_utils import load_point_cloud, write_point_cloud
|
||||
from .structures import (
|
||||
CouplerMeasurement,
|
||||
Hole,
|
||||
HoleSet,
|
||||
PanelPlane,
|
||||
ReferenceFrame,
|
||||
StageTiming,
|
||||
)
|
||||
|
||||
|
||||
def __getattr__(name: str):
|
||||
"""Lazy export of heavy / optional symbols.
|
||||
|
||||
Avoids importing matplotlib / open3d / OCP at ``import coupler_panel``
|
||||
time so a thin integration deployment keeps a fast, dependency-light
|
||||
startup.
|
||||
"""
|
||||
if name in ("compute_coupler_measurement", "ProgressCallback"):
|
||||
from .api import compute_coupler_measurement, ProgressCallback
|
||||
|
||||
return {"compute_coupler_measurement": compute_coupler_measurement, "ProgressCallback": ProgressCallback}[name]
|
||||
if name == "run_coupler_measurement_pipeline":
|
||||
from .pipeline import run_coupler_measurement_pipeline
|
||||
|
||||
return run_coupler_measurement_pipeline
|
||||
if name == "run_from_cad_pipeline":
|
||||
from .pipeline import run_from_cad_pipeline
|
||||
|
||||
return run_from_cad_pipeline
|
||||
if name == "prepare_cloud_from_cad":
|
||||
from .data_prep import prepare_cloud_from_cad
|
||||
|
||||
return prepare_cloud_from_cad
|
||||
if name == "prepare_cloud_from_scan":
|
||||
from .data_prep import prepare_cloud_from_scan
|
||||
|
||||
return prepare_cloud_from_scan
|
||||
raise AttributeError(name)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"__version__",
|
||||
"ALGORITHM_VERSION",
|
||||
"SCHEMA_VERSION",
|
||||
# Stable measurement API
|
||||
"compute_coupler_measurement",
|
||||
"ProgressCallback",
|
||||
"CouplerMeasurement",
|
||||
"Hole",
|
||||
"HoleSet",
|
||||
"PanelPlane",
|
||||
"ReferenceFrame",
|
||||
"StageTiming",
|
||||
"load_point_cloud",
|
||||
"write_point_cloud",
|
||||
# Exceptions
|
||||
"CouplerError",
|
||||
"InvalidPointCloudError",
|
||||
"UnsupportedFormatError",
|
||||
"InsufficientPointsError",
|
||||
"CADParseError",
|
||||
"HoleDetectionError",
|
||||
"RegistrationError",
|
||||
"PlaneFitError",
|
||||
# Research-only convenience (data prep + persisting orchestration)
|
||||
"run_coupler_measurement_pipeline",
|
||||
"run_from_cad_pipeline",
|
||||
"prepare_cloud_from_cad",
|
||||
"prepare_cloud_from_scan",
|
||||
]
|
||||
@@ -0,0 +1,19 @@
|
||||
"""Version markers for coupler_panel.
|
||||
|
||||
- ``__version__`` — package version (semver).
|
||||
- ``ALGORITHM_VERSION`` — algorithm/numerics version. Numerical results may
|
||||
change when this increments. Increment whenever any computation in
|
||||
``compute_coupler_measurement`` changes.
|
||||
- ``SCHEMA_VERSION`` — result-schema version. Only increments on a
|
||||
*breaking* change to the ``CouplerMeasurement`` dataclass or to
|
||||
``report.json`` field semantics. New optional fields do **not** bump
|
||||
this.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
__version__ = "0.1.0"
|
||||
ALGORITHM_VERSION = "0.1.0"
|
||||
SCHEMA_VERSION = "1.0"
|
||||
|
||||
__all__ = ["__version__", "ALGORITHM_VERSION", "SCHEMA_VERSION"]
|
||||
251
alg/delivery_v1.2.1/coupler_panel_release/coupler_panel/api.py
Normal file
251
alg/delivery_v1.2.1/coupler_panel_release/coupler_panel/api.py
Normal file
@@ -0,0 +1,251 @@
|
||||
"""Public Python API — **measurement algorithm only**.
|
||||
|
||||
This module is what software developers integrate against. It takes a
|
||||
*clean, already-registered* point cloud (in the CAD / world frame) of
|
||||
the coupler panel and returns a :class:`CouplerMeasurement`.
|
||||
|
||||
Responsibilities of this layer:
|
||||
|
||||
validate → detect 4 holes → build reference frame
|
||||
→ extract panel ROI → fit panel plane → compute metrics
|
||||
|
||||
Things that are **not** in this layer (they live in
|
||||
:mod:`coupler_panel.data_prep`):
|
||||
|
||||
- Reading STEP files / generating synthetic point clouds
|
||||
- Adding noise / random rigid transforms
|
||||
- Denoising (passthrough / SOR / ROR / voxel / smoothing)
|
||||
- 4-hole Kabsch registration into the CAD frame
|
||||
|
||||
The reason: those steps are *engineering* steps that vary per sensor
|
||||
setup; the production caller usually has their own scan + cleanup
|
||||
pipeline and just wants the deflection numbers out the other side.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
import tracemalloc
|
||||
from typing import Any, Callable
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ._version import ALGORITHM_VERSION, SCHEMA_VERSION
|
||||
from .config import load_config, resolve_auto
|
||||
from .detection import detect_four_holes
|
||||
from .exceptions import (
|
||||
InsufficientPointsError,
|
||||
InvalidPointCloudError,
|
||||
PlaneFitError,
|
||||
)
|
||||
from .geometry import build_reference_frame, extract_panel_roi, fit_panel_plane
|
||||
from .metrics import compute_deflection_metrics
|
||||
from .structures import (
|
||||
CouplerMeasurement,
|
||||
HoleSet,
|
||||
StageTiming,
|
||||
)
|
||||
|
||||
|
||||
ProgressCallback = Callable[[str, float], None]
|
||||
|
||||
|
||||
_STAGES = (
|
||||
"validate",
|
||||
"detect_holes",
|
||||
"build_frame",
|
||||
"extract_roi",
|
||||
"fit_plane",
|
||||
"metrics",
|
||||
)
|
||||
|
||||
|
||||
def _ping(progress: ProgressCallback | None, stage: str, frac: float) -> None:
|
||||
if progress is not None:
|
||||
try:
|
||||
progress(stage, float(frac))
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def _validate_points(points: Any) -> np.ndarray:
|
||||
try:
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
except Exception as exc:
|
||||
raise InvalidPointCloudError(f"could not coerce points to ndarray: {exc}") from exc
|
||||
if pts.ndim != 2 or pts.shape[1] != 3:
|
||||
raise InvalidPointCloudError(f"expected (N, 3) points; got shape {pts.shape}")
|
||||
if pts.shape[0] < 200:
|
||||
raise InsufficientPointsError(f"too few points: {pts.shape[0]} (need ≥ 200)")
|
||||
if not np.isfinite(pts).all():
|
||||
raise InvalidPointCloudError("input contains NaN/Inf")
|
||||
return pts
|
||||
|
||||
|
||||
def compute_coupler_measurement(
|
||||
points: Any,
|
||||
config: str | dict[str, Any] | None = None,
|
||||
profile: str = "auto",
|
||||
*,
|
||||
panel_normal_seed: np.ndarray | None = None,
|
||||
progress: ProgressCallback | None = None,
|
||||
) -> CouplerMeasurement:
|
||||
"""Run the **measurement algorithm** on an already-prepared point cloud.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
points : array-like
|
||||
``(N, 3)`` cloud in the CAD / world frame, units mm.
|
||||
The cloud is expected to be denoised and rigidly aligned to the
|
||||
CAD frame before this call. See :mod:`coupler_panel.data_prep`
|
||||
for a reference implementation of that preparation step.
|
||||
config : path / dict / None
|
||||
Optional override of the default YAML config.
|
||||
profile : str
|
||||
``"auto"`` / ``"default"`` skip profile loading. Other names are
|
||||
resolved against ``configs/profiles/<name>.yaml``.
|
||||
panel_normal_seed : (3,) or None
|
||||
Optional prior on the panel normal (e.g. the GT normal). Speeds
|
||||
up and stabilises the in-plane projection used by hole detection.
|
||||
``None`` falls back to PCA on the cloud.
|
||||
progress : callable or None
|
||||
``progress(stage_name, fraction)`` called once per stage.
|
||||
|
||||
Returns
|
||||
-------
|
||||
:class:`CouplerMeasurement`
|
||||
|
||||
Raises
|
||||
------
|
||||
InvalidPointCloudError, InsufficientPointsError, HoleDetectionError,
|
||||
PlaneFitError, CouplerError
|
||||
"""
|
||||
cfg = load_config(config, profile=profile)
|
||||
cfg = resolve_auto(cfg)
|
||||
|
||||
quality_flags: list[str] = []
|
||||
timings: list[StageTiming] = []
|
||||
|
||||
# ---- validate ----------------------------------------------------------
|
||||
pts = _validate_points(points)
|
||||
_ping(progress, "validate", 1.0 / len(_STAGES))
|
||||
|
||||
# ---- detect 4 holes ----------------------------------------------------
|
||||
t0 = time.perf_counter()
|
||||
tracemalloc.start()
|
||||
detection = detect_four_holes(
|
||||
pts,
|
||||
detection_cfg=cfg["detection"],
|
||||
cad_cfg=cfg["cad"],
|
||||
panel_normal_seed=panel_normal_seed,
|
||||
)
|
||||
_, peak = tracemalloc.get_traced_memory()
|
||||
tracemalloc.stop()
|
||||
timings.append(StageTiming("detect_holes", time.perf_counter() - t0, peak / 1024))
|
||||
_ping(progress, "detect_holes", 2.0 / len(_STAGES))
|
||||
|
||||
detected_holes = detection.holes_3d
|
||||
|
||||
# ---- reference frame ---------------------------------------------------
|
||||
t0 = time.perf_counter()
|
||||
frame = build_reference_frame(detected_holes)
|
||||
timings.append(StageTiming("build_frame", time.perf_counter() - t0, 0.0))
|
||||
_ping(progress, "build_frame", 3.0 / len(_STAGES))
|
||||
|
||||
# ---- extract panel ROI -------------------------------------------------
|
||||
t0 = time.perf_counter()
|
||||
tracemalloc.start()
|
||||
if panel_normal_seed is not None:
|
||||
seed3 = np.asarray(panel_normal_seed, dtype=np.float64)
|
||||
else:
|
||||
seed3 = np.array([1.0, 0.0, 0.0])
|
||||
roi_pts, roi_diag = extract_panel_roi(
|
||||
pts,
|
||||
holes=detected_holes,
|
||||
panel_normal_seed=seed3,
|
||||
roi_cfg=cfg["roi"],
|
||||
)
|
||||
if roi_pts.shape[0] < 100:
|
||||
raise InsufficientPointsError(
|
||||
f"only {roi_pts.shape[0]} points survived ROI extraction"
|
||||
)
|
||||
_, peak = tracemalloc.get_traced_memory()
|
||||
tracemalloc.stop()
|
||||
timings.append(StageTiming("extract_roi", time.perf_counter() - t0, peak / 1024))
|
||||
_ping(progress, "extract_roi", 4.0 / len(_STAGES))
|
||||
|
||||
# ---- fit panel plane ---------------------------------------------------
|
||||
t0 = time.perf_counter()
|
||||
tracemalloc.start()
|
||||
plane = fit_panel_plane(roi_pts, plane_fit_cfg=cfg["plane_fit"])
|
||||
if plane.inlier_ratio < float(cfg["plane_fit"].get("min_inlier_ratio", 0.6)):
|
||||
quality_flags.append("plane_inlier_ratio_low")
|
||||
_, peak = tracemalloc.get_traced_memory()
|
||||
tracemalloc.stop()
|
||||
timings.append(StageTiming("fit_plane", time.perf_counter() - t0, peak / 1024))
|
||||
_ping(progress, "fit_plane", 5.0 / len(_STAGES))
|
||||
|
||||
# ---- metrics -----------------------------------------------------------
|
||||
t0 = time.perf_counter()
|
||||
metrics_cfg = cfg.get("metrics", {})
|
||||
metrics = compute_deflection_metrics(
|
||||
normal=plane.normal,
|
||||
center_C=frame.center_C,
|
||||
panel_points=roi_pts,
|
||||
right_axis_sign=int(metrics_cfg.get("right_axis_sign", 1)),
|
||||
angle_clamp_deg=float(metrics_cfg.get("angle_clamp_deg", 0.0)),
|
||||
)
|
||||
timings.append(StageTiming("metrics", time.perf_counter() - t0, 0.0))
|
||||
_ping(progress, "metrics", 1.0)
|
||||
|
||||
report = {
|
||||
"schema_version": SCHEMA_VERSION,
|
||||
"algorithm_version": ALGORITHM_VERSION,
|
||||
"config": cfg,
|
||||
"n_points_in": int(pts.shape[0]),
|
||||
"detection": detection.diagnostics,
|
||||
"roi": roi_diag,
|
||||
"plane": {
|
||||
"normal": list(plane.normal),
|
||||
"point_on_plane": list(plane.point_on_plane),
|
||||
"d_offset": plane.d_offset,
|
||||
"inlier_ratio": plane.inlier_ratio,
|
||||
"inlier_residual_rms_mm": plane.inlier_residual_rms_mm,
|
||||
},
|
||||
"frame": {
|
||||
"center_C": list(frame.center_C),
|
||||
"reference_plane_z": frame.reference_plane_z,
|
||||
"longitudinal_mid_y": frame.longitudinal_mid_y,
|
||||
},
|
||||
"metrics": metrics.to_dict(),
|
||||
"quality_flags": quality_flags,
|
||||
"timings": [
|
||||
{"stage": t.stage, "seconds": t.seconds, "peak_kib": t.peak_kib}
|
||||
for t in timings
|
||||
],
|
||||
}
|
||||
|
||||
return CouplerMeasurement(
|
||||
schema_version=SCHEMA_VERSION,
|
||||
algorithm_version=ALGORITHM_VERSION,
|
||||
theta_right_deg=metrics.theta_right_deg,
|
||||
theta_ref_deg=metrics.theta_ref_deg,
|
||||
D_y_max_mm=metrics.D_y_max_mm,
|
||||
D_z_max_mm=metrics.D_z_max_mm,
|
||||
dx_y_max_mm=metrics.dx_y_max_mm,
|
||||
dx_z_max_mm=metrics.dx_z_max_mm,
|
||||
center_C=frame.center_C,
|
||||
normal=plane.normal,
|
||||
holes=detected_holes,
|
||||
plane=plane,
|
||||
frame=frame,
|
||||
quality_flags=quality_flags,
|
||||
timings=timings,
|
||||
report=report,
|
||||
)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"compute_coupler_measurement",
|
||||
"ProgressCallback",
|
||||
]
|
||||
@@ -0,0 +1,21 @@
|
||||
"""CAD STP parsing + GT extraction.
|
||||
|
||||
This package depends on ``cadquery-ocp`` (OCP) for OpenCascade access. If
|
||||
OCP is not installed the modules will raise :class:`ImportError` at import
|
||||
time only when actually used; pure point-cloud workflows do not need it.
|
||||
"""
|
||||
|
||||
from .stp_parser import StepShape, load_step
|
||||
from .mesh import tessellate_to_trimesh
|
||||
from .gt_extractor import (
|
||||
GroundTruth,
|
||||
extract_ground_truth,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"StepShape",
|
||||
"load_step",
|
||||
"tessellate_to_trimesh",
|
||||
"GroundTruth",
|
||||
"extract_ground_truth",
|
||||
]
|
||||
@@ -0,0 +1,310 @@
|
||||
"""Extract Ground Truth geometry from a parsed STEP shape.
|
||||
|
||||
Strategy
|
||||
--------
|
||||
|
||||
1. Walk every circular edge in the shape (``iter_circular_edges``).
|
||||
2. Cluster by radius:
|
||||
- radii inside ``locating_hole_radius_mm`` → candidate locating holes
|
||||
- radii ≥ ``central_hole_radius_min_mm`` → candidate central hole
|
||||
3. Discard duplicates (front + back face of the same hole share centre and
|
||||
axis up to a sign).
|
||||
4. Find a set of four locating holes whose centres form a near-rectangle:
|
||||
pairwise sort by axis-distance, then verify diagonals roughly equal.
|
||||
5. Order the four locating holes consistently (anti-clockwise when viewed
|
||||
from +x looking down −x). This ordering is the canonical pairing for
|
||||
Kabsch later.
|
||||
6. Define:
|
||||
- ``C`` = mean of the four locating-hole centres (== diagonal
|
||||
intersection for a rectangle)
|
||||
- ``n`` = panel normal = unit axis of the locating circles, signed
|
||||
so that ``n_x > 0``
|
||||
- reference line, plane, longitudinal mid-plane in :class:`ReferenceFrame`
|
||||
|
||||
This GT can be used as
|
||||
- the gold standard the measurement is compared against, AND
|
||||
- a prior law that bootstraps point-cloud-side hole detection (we know the
|
||||
panel normal a priori).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import CADParseError
|
||||
from ..structures import Hole, HoleSet, PanelPlane, ReferenceFrame
|
||||
from .stp_parser import StepShape, iter_circular_edges
|
||||
|
||||
|
||||
@dataclass
|
||||
class GroundTruth:
|
||||
"""All CAD-derived reference values used as the GT for validation."""
|
||||
|
||||
holes: HoleSet
|
||||
plane: PanelPlane
|
||||
frame: ReferenceFrame
|
||||
raw_circles: list[dict[str, Any]] = field(default_factory=list)
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
return {
|
||||
"holes": {
|
||||
"locating": [
|
||||
{
|
||||
"center": list(h.center),
|
||||
"radius_mm": h.radius_mm,
|
||||
"fit_residual_rms_mm": h.fit_residual_rms_mm,
|
||||
"role": h.role,
|
||||
}
|
||||
for h in self.holes.locating
|
||||
],
|
||||
"central": (
|
||||
None
|
||||
if self.holes.central is None
|
||||
else {
|
||||
"center": list(self.holes.central.center),
|
||||
"radius_mm": self.holes.central.radius_mm,
|
||||
"fit_residual_rms_mm": self.holes.central.fit_residual_rms_mm,
|
||||
"role": self.holes.central.role,
|
||||
}
|
||||
),
|
||||
},
|
||||
"plane": {
|
||||
"normal": list(self.plane.normal),
|
||||
"point_on_plane": list(self.plane.point_on_plane),
|
||||
"d_offset": self.plane.d_offset,
|
||||
"inlier_ratio": self.plane.inlier_ratio,
|
||||
"inlier_residual_rms_mm": self.plane.inlier_residual_rms_mm,
|
||||
},
|
||||
"frame": {
|
||||
"center_C": list(self.frame.center_C),
|
||||
"line_direction": list(self.frame.line_direction),
|
||||
"reference_plane_z": self.frame.reference_plane_z,
|
||||
"longitudinal_mid_y": self.frame.longitudinal_mid_y,
|
||||
},
|
||||
"raw_circles": self.raw_circles,
|
||||
}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Public entry point
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def extract_ground_truth(
|
||||
step: StepShape,
|
||||
cad_cfg: dict[str, Any],
|
||||
) -> GroundTruth:
|
||||
"""Extract GT from a parsed STEP file using the ``cad`` config block."""
|
||||
r_lo, r_hi = cad_cfg["locating_hole_radius_mm"]
|
||||
r_central_min = float(cad_cfg["central_hole_radius_min_mm"])
|
||||
rect_aspect_tol = float(cad_cfg.get("rectangle_aspect_tol", 0.15))
|
||||
|
||||
raw: list[dict[str, Any]] = []
|
||||
locating: list[dict[str, Any]] = []
|
||||
central: list[dict[str, Any]] = []
|
||||
|
||||
for center, axis, radius, _edge in iter_circular_edges(step.shape):
|
||||
item = {
|
||||
"center": center,
|
||||
"axis": axis,
|
||||
"radius": radius,
|
||||
}
|
||||
raw.append(item)
|
||||
if r_lo <= radius <= r_hi:
|
||||
locating.append(item)
|
||||
elif radius >= r_central_min:
|
||||
central.append(item)
|
||||
|
||||
if not locating:
|
||||
raise CADParseError(
|
||||
f"no circular edge with radius in [{r_lo}, {r_hi}] mm; "
|
||||
"adjust cad.locating_hole_radius_mm to match your panel"
|
||||
)
|
||||
|
||||
locating = _dedupe_circles(locating, r_tol=0.05, c_tol=0.5)
|
||||
central = _dedupe_circles(central, r_tol=0.5, c_tol=2.0)
|
||||
|
||||
four = _select_rectangle(locating, rect_aspect_tol=rect_aspect_tol)
|
||||
four_ordered = _order_rectangle(four)
|
||||
|
||||
panel_normal = _consensus_axis(four_ordered)
|
||||
panel_normal = _enforce_nx_positive(panel_normal)
|
||||
|
||||
centers = np.asarray([c["center"] for c in four_ordered], dtype=np.float64)
|
||||
C = centers.mean(axis=0)
|
||||
|
||||
holes = HoleSet(
|
||||
locating=tuple( # type: ignore[arg-type]
|
||||
Hole(
|
||||
center=tuple(c["center"]),
|
||||
radius_mm=float(c["radius"]),
|
||||
fit_residual_rms_mm=0.0,
|
||||
role="locating",
|
||||
)
|
||||
for c in four_ordered
|
||||
),
|
||||
central=(
|
||||
Hole(
|
||||
center=tuple(central[0]["center"]),
|
||||
radius_mm=float(central[0]["radius"]),
|
||||
fit_residual_rms_mm=0.0,
|
||||
role="central",
|
||||
)
|
||||
if central
|
||||
else None
|
||||
),
|
||||
)
|
||||
|
||||
plane = PanelPlane(
|
||||
normal=tuple(panel_normal),
|
||||
point_on_plane=tuple(C),
|
||||
d_offset=float(-np.dot(panel_normal, C)),
|
||||
inlier_ratio=1.0,
|
||||
inlier_residual_rms_mm=0.0,
|
||||
)
|
||||
|
||||
frame = ReferenceFrame(
|
||||
center_C=tuple(C),
|
||||
line_direction=(1.0, 0.0, 0.0),
|
||||
reference_plane_z=float(C[2]),
|
||||
longitudinal_mid_y=float(C[1]),
|
||||
)
|
||||
|
||||
return GroundTruth(holes=holes, plane=plane, frame=frame, raw_circles=raw)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _dedupe_circles(
|
||||
items: list[dict[str, Any]],
|
||||
*,
|
||||
r_tol: float,
|
||||
c_tol: float,
|
||||
) -> list[dict[str, Any]]:
|
||||
"""Drop circles that share centre and radius (front/back face copies)."""
|
||||
out: list[dict[str, Any]] = []
|
||||
for it in items:
|
||||
c = np.asarray(it["center"])
|
||||
r = it["radius"]
|
||||
is_dup = False
|
||||
for kept in out:
|
||||
if abs(kept["radius"] - r) <= r_tol:
|
||||
if np.linalg.norm(np.asarray(kept["center"]) - c) <= c_tol:
|
||||
is_dup = True
|
||||
break
|
||||
if not is_dup:
|
||||
out.append(it)
|
||||
return out
|
||||
|
||||
|
||||
def _select_rectangle(
|
||||
items: list[dict[str, Any]],
|
||||
*,
|
||||
rect_aspect_tol: float,
|
||||
) -> list[dict[str, Any]]:
|
||||
"""Pick four items whose centres look most like a rectangle.
|
||||
|
||||
For tractability we (a) require ≥ 4 candidates, (b) if exactly 4, return
|
||||
them, (c) otherwise enumerate all 4-subsets and pick the one with the
|
||||
smallest "rectangleness" residual.
|
||||
"""
|
||||
if len(items) < 4:
|
||||
raise CADParseError(
|
||||
f"only {len(items)} locating-hole candidates found; need ≥ 4"
|
||||
)
|
||||
if len(items) == 4:
|
||||
return items
|
||||
|
||||
from itertools import combinations
|
||||
|
||||
best_score = float("inf")
|
||||
best: list[dict[str, Any]] | None = None
|
||||
for combo in combinations(items, 4):
|
||||
s = _rectangle_score(combo)
|
||||
if s < best_score:
|
||||
best_score = s
|
||||
best = list(combo)
|
||||
assert best is not None
|
||||
if best_score > rect_aspect_tol:
|
||||
# Still accept the best — the tolerance is informational; warn via
|
||||
# raw_circles in the GT report.
|
||||
pass
|
||||
return best
|
||||
|
||||
|
||||
def _rectangle_score(items) -> float:
|
||||
"""0 = perfect rectangle. Larger = worse."""
|
||||
c = np.asarray([i["center"] for i in items], dtype=np.float64)
|
||||
# Use distances between the 4 centres. For a rectangle, the 6 pairwise
|
||||
# distances are: two "long" sides equal, two "short" sides equal, two
|
||||
# diagonals equal.
|
||||
from itertools import combinations
|
||||
|
||||
d = sorted(np.linalg.norm(c[i] - c[j]) for i, j in combinations(range(4), 2))
|
||||
if d[0] < 1e-6:
|
||||
return 1e6
|
||||
s1, s2 = d[0], d[1] # short sides
|
||||
l1, l2 = d[2], d[3] # long sides
|
||||
g1, g2 = d[4], d[5] # diagonals
|
||||
score = (
|
||||
abs(s1 - s2) / max(s1, s2)
|
||||
+ abs(l1 - l2) / max(l1, l2)
|
||||
+ abs(g1 - g2) / max(g1, g2)
|
||||
)
|
||||
return score
|
||||
|
||||
|
||||
def _order_rectangle(items: list[dict[str, Any]]) -> list[dict[str, Any]]:
|
||||
"""Return four hole dicts in a canonical (-y/-z, +y/-z, +y/+z, -y/+z) order.
|
||||
|
||||
The ordering is taken in the y-z plane (panel normal is along ±x). This
|
||||
matches how a panel is normally described: bottom-left, bottom-right,
|
||||
top-right, top-left when viewed from +x looking down −x.
|
||||
"""
|
||||
c = np.asarray([i["center"] for i in items], dtype=np.float64)
|
||||
cy = c[:, 1].mean()
|
||||
cz = c[:, 2].mean()
|
||||
quadrant: dict[tuple[bool, bool], int] = {}
|
||||
for idx, p in enumerate(c):
|
||||
key = (p[1] >= cy, p[2] >= cz)
|
||||
quadrant[key] = idx
|
||||
order = [
|
||||
(False, False),
|
||||
(True, False),
|
||||
(True, True),
|
||||
(False, True),
|
||||
]
|
||||
if not all(q in quadrant for q in order):
|
||||
# Geometry is degenerate; fall back to angular sort.
|
||||
ang = np.arctan2(c[:, 2] - cz, c[:, 1] - cy)
|
||||
idxs = np.argsort(ang)
|
||||
return [items[i] for i in idxs]
|
||||
return [items[quadrant[q]] for q in order]
|
||||
|
||||
|
||||
def _consensus_axis(items: list[dict[str, Any]]) -> np.ndarray:
|
||||
"""Average and re-normalise the circle axis directions."""
|
||||
axes = np.asarray([i["axis"] for i in items], dtype=np.float64)
|
||||
# Flip any axes that disagree with the first.
|
||||
ref = axes[0]
|
||||
for k in range(1, len(axes)):
|
||||
if np.dot(axes[k], ref) < 0:
|
||||
axes[k] = -axes[k]
|
||||
n = axes.mean(axis=0)
|
||||
n /= max(np.linalg.norm(n), 1e-12)
|
||||
return n
|
||||
|
||||
|
||||
def _enforce_nx_positive(n: np.ndarray) -> np.ndarray:
|
||||
if n[0] < 0:
|
||||
return -n
|
||||
return n
|
||||
|
||||
|
||||
__all__ = ["GroundTruth", "extract_ground_truth"]
|
||||
@@ -0,0 +1,143 @@
|
||||
"""Tessellation of a TopoDS_Shape into a (V, F) triangle mesh.
|
||||
|
||||
The output is a small dataclass — we do not import ``trimesh`` until the
|
||||
caller actually asks for it, to keep the core dependency surface minimal.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .stp_parser import StepShape, _import_ocp, iter_faces
|
||||
|
||||
|
||||
@dataclass
|
||||
class TriangleMesh:
|
||||
"""Vertex / face arrays.
|
||||
|
||||
Attributes
|
||||
----------
|
||||
vertices : ``(V, 3) float64``
|
||||
faces : ``(F, 3) int64`` (vertex indices)
|
||||
face_normals : ``(F, 3) float64``
|
||||
"""
|
||||
|
||||
vertices: np.ndarray
|
||||
faces: np.ndarray
|
||||
face_normals: np.ndarray
|
||||
|
||||
@property
|
||||
def n_vertices(self) -> int:
|
||||
return int(self.vertices.shape[0])
|
||||
|
||||
@property
|
||||
def n_faces(self) -> int:
|
||||
return int(self.faces.shape[0])
|
||||
|
||||
def to_trimesh(self):
|
||||
"""Return a ``trimesh.Trimesh`` (lazy import)."""
|
||||
try:
|
||||
import trimesh
|
||||
except ImportError as exc:
|
||||
raise ImportError(
|
||||
"trimesh is required for to_trimesh(); "
|
||||
"install with: pip install 'coupler-panel-validation[cad]'"
|
||||
) from exc
|
||||
return trimesh.Trimesh(
|
||||
vertices=self.vertices,
|
||||
faces=self.faces,
|
||||
face_normals=self.face_normals,
|
||||
process=False,
|
||||
)
|
||||
|
||||
|
||||
def tessellate_to_trimesh(
|
||||
step: StepShape,
|
||||
*,
|
||||
linear_deflection_mm: float = 0.5,
|
||||
angular_deflection_rad: float = 0.1,
|
||||
) -> TriangleMesh:
|
||||
"""Tessellate a :class:`StepShape` into a single combined triangle mesh.
|
||||
|
||||
Each face is tessellated independently (with the location of the edge
|
||||
transferred onto the vertices), then concatenated. Triangle winding is
|
||||
flipped where the OCP face is reversed, so :attr:`face_normals` is the
|
||||
*outward* normal in the assembly frame.
|
||||
"""
|
||||
occ = _import_ocp()
|
||||
BRep_Tool = occ["BRep_Tool"]
|
||||
TopLoc_Location = occ["TopLoc_Location"]
|
||||
BRepMesh_IncrementalMesh = occ["BRepMesh_IncrementalMesh"]
|
||||
|
||||
BRepMesh_IncrementalMesh(
|
||||
step.shape,
|
||||
float(linear_deflection_mm),
|
||||
False,
|
||||
float(angular_deflection_rad),
|
||||
True,
|
||||
)
|
||||
|
||||
all_v: list[np.ndarray] = []
|
||||
all_f: list[np.ndarray] = []
|
||||
all_n: list[np.ndarray] = []
|
||||
v_offset = 0
|
||||
|
||||
for face in iter_faces(step.shape):
|
||||
loc = TopLoc_Location()
|
||||
triangulation = BRep_Tool.Triangulation_s(face, loc)
|
||||
if triangulation is None:
|
||||
continue
|
||||
|
||||
trsf = loc.Transformation()
|
||||
n_nodes = triangulation.NbNodes()
|
||||
n_tri = triangulation.NbTriangles()
|
||||
if n_nodes == 0 or n_tri == 0:
|
||||
continue
|
||||
|
||||
v = np.empty((n_nodes, 3), dtype=np.float64)
|
||||
for i in range(n_nodes):
|
||||
p = triangulation.Node(i + 1).Transformed(trsf)
|
||||
v[i, 0] = p.X()
|
||||
v[i, 1] = p.Y()
|
||||
v[i, 2] = p.Z()
|
||||
|
||||
f = np.empty((n_tri, 3), dtype=np.int64)
|
||||
is_reversed = face.Orientation() == 1 # TopAbs_REVERSED
|
||||
for j in range(n_tri):
|
||||
tri = triangulation.Triangle(j + 1)
|
||||
i1, i2, i3 = tri.Get()
|
||||
if is_reversed:
|
||||
f[j] = (i3 - 1, i2 - 1, i1 - 1)
|
||||
else:
|
||||
f[j] = (i1 - 1, i2 - 1, i3 - 1)
|
||||
|
||||
# Outward face normal per triangle.
|
||||
a = v[f[:, 0]]
|
||||
b = v[f[:, 1]]
|
||||
c = v[f[:, 2]]
|
||||
normals = np.cross(b - a, c - a)
|
||||
norms = np.linalg.norm(normals, axis=1, keepdims=True)
|
||||
norms[norms < 1e-12] = 1.0
|
||||
normals = normals / norms
|
||||
|
||||
all_v.append(v)
|
||||
all_f.append(f + v_offset)
|
||||
all_n.append(normals)
|
||||
v_offset += n_nodes
|
||||
|
||||
if not all_v:
|
||||
raise RuntimeError(
|
||||
"tessellation produced no triangles; "
|
||||
"try a finer linear_deflection_mm"
|
||||
)
|
||||
|
||||
vertices = np.concatenate(all_v, axis=0)
|
||||
faces = np.concatenate(all_f, axis=0)
|
||||
face_normals = np.concatenate(all_n, axis=0)
|
||||
return TriangleMesh(vertices=vertices, faces=faces, face_normals=face_normals)
|
||||
|
||||
|
||||
__all__ = ["TriangleMesh", "tessellate_to_trimesh"]
|
||||
@@ -0,0 +1,219 @@
|
||||
"""STP file loading via OCP (OpenCascade Python bindings).
|
||||
|
||||
The OCP import is deferred so the rest of the package does not require
|
||||
OpenCascade to be installed.
|
||||
|
||||
The parser exposes a thin :class:`StepShape` wrapper that gives the rest
|
||||
of the package what it needs: the underlying ``TopoDS_Shape`` for
|
||||
tessellation and topology traversal.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
from ..exceptions import CADParseError
|
||||
|
||||
|
||||
@dataclass
|
||||
class StepShape:
|
||||
"""Result of loading a STEP file.
|
||||
|
||||
Attributes
|
||||
----------
|
||||
shape : ``OCP.TopoDS.TopoDS_Shape``
|
||||
The root shape (assembly compound or single solid).
|
||||
source : ``Path``
|
||||
Source file path, for debugging.
|
||||
"""
|
||||
|
||||
shape: Any
|
||||
source: Path
|
||||
|
||||
|
||||
def _import_ocp() -> dict[str, Any]:
|
||||
"""Import the OCP modules we need. Raises a clean error if missing."""
|
||||
try:
|
||||
from OCP.STEPControl import STEPControl_Reader # type: ignore
|
||||
from OCP.IFSelect import IFSelect_RetDone # type: ignore
|
||||
from OCP.TopoDS import TopoDS, TopoDS_Shape # type: ignore
|
||||
from OCP.TopAbs import ( # type: ignore
|
||||
TopAbs_FACE,
|
||||
TopAbs_EDGE,
|
||||
TopAbs_VERTEX,
|
||||
)
|
||||
from OCP.TopExp import TopExp_Explorer # type: ignore
|
||||
from OCP.TopLoc import TopLoc_Location # type: ignore
|
||||
from OCP.BRep import BRep_Tool # type: ignore
|
||||
from OCP.BRepMesh import BRepMesh_IncrementalMesh # type: ignore
|
||||
from OCP.BRepGProp import BRepGProp # type: ignore
|
||||
from OCP.GProp import GProp_GProps # type: ignore
|
||||
from OCP.GeomAbs import GeomAbs_Circle # type: ignore
|
||||
from OCP.BRepAdaptor import BRepAdaptor_Curve, BRepAdaptor_Surface # type: ignore
|
||||
from OCP.gp import gp_Pnt, gp_Dir, gp_Vec # type: ignore
|
||||
except ImportError as exc:
|
||||
raise ImportError(
|
||||
"OCP (cadquery-ocp) is required to parse STEP files. "
|
||||
"Install with: pip install 'coupler-panel-validation[cad]'"
|
||||
) from exc
|
||||
|
||||
return {
|
||||
"STEPControl_Reader": STEPControl_Reader,
|
||||
"IFSelect_RetDone": IFSelect_RetDone,
|
||||
"TopoDS": TopoDS,
|
||||
"TopoDS_Shape": TopoDS_Shape,
|
||||
"TopAbs_FACE": TopAbs_FACE,
|
||||
"TopAbs_EDGE": TopAbs_EDGE,
|
||||
"TopAbs_VERTEX": TopAbs_VERTEX,
|
||||
"TopExp_Explorer": TopExp_Explorer,
|
||||
"TopLoc_Location": TopLoc_Location,
|
||||
"BRep_Tool": BRep_Tool,
|
||||
"BRepMesh_IncrementalMesh": BRepMesh_IncrementalMesh,
|
||||
"BRepGProp": BRepGProp,
|
||||
"GProp_GProps": GProp_GProps,
|
||||
"GeomAbs_Circle": GeomAbs_Circle,
|
||||
"BRepAdaptor_Curve": BRepAdaptor_Curve,
|
||||
"BRepAdaptor_Surface": BRepAdaptor_Surface,
|
||||
"gp_Pnt": gp_Pnt,
|
||||
"gp_Dir": gp_Dir,
|
||||
"gp_Vec": gp_Vec,
|
||||
}
|
||||
|
||||
|
||||
def load_step(path: str | Path) -> StepShape:
|
||||
"""Read a STEP file and return its root :class:`TopoDS_Shape`.
|
||||
|
||||
Raises :class:`CADParseError` on read failure.
|
||||
"""
|
||||
p = Path(path)
|
||||
if not p.exists():
|
||||
raise CADParseError(f"STEP file not found: {p}")
|
||||
|
||||
occ = _import_ocp()
|
||||
reader = occ["STEPControl_Reader"]()
|
||||
status = reader.ReadFile(str(p))
|
||||
if status != occ["IFSelect_RetDone"]:
|
||||
raise CADParseError(f"OCP failed to read {p} (status={status})")
|
||||
n_roots = reader.TransferRoots()
|
||||
if n_roots <= 0:
|
||||
raise CADParseError(f"no roots transferred from {p}")
|
||||
|
||||
shape = reader.OneShape()
|
||||
return StepShape(shape=shape, source=p)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Topology iteration helpers (used by gt_extractor)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _shape_key(shape) -> int:
|
||||
"""Stable identity key for a TopoDS_Shape across OCP versions.
|
||||
|
||||
OCP 7.8+ removed ``HashCode``. We fall back to the underlying TShape's
|
||||
Python id, which is stable per OpenCASCADE topology object.
|
||||
"""
|
||||
try:
|
||||
return shape.HashCode(2**31 - 1)
|
||||
except AttributeError:
|
||||
try:
|
||||
return id(shape.TShape())
|
||||
except Exception:
|
||||
return id(shape)
|
||||
|
||||
|
||||
def iter_faces(shape: Any):
|
||||
"""Yield every TopoDS_Face inside ``shape``.
|
||||
|
||||
Two visits of the *same underlying surface with opposite orientations*
|
||||
are treated as distinct because the orientation governs the outward
|
||||
normal of the produced triangulation.
|
||||
"""
|
||||
occ = _import_ocp()
|
||||
TopoDS = occ["TopoDS"]
|
||||
explorer = occ["TopExp_Explorer"](shape, occ["TopAbs_FACE"])
|
||||
while explorer.More():
|
||||
raw = explorer.Current()
|
||||
try:
|
||||
yield TopoDS.Face_s(raw)
|
||||
except Exception:
|
||||
pass
|
||||
explorer.Next()
|
||||
|
||||
|
||||
def iter_circular_edges(shape: Any):
|
||||
"""Yield ``(center_xyz, axis_xyz, radius_mm, edge)`` for every circular
|
||||
edge in ``shape``. Duplicates (same circle on two adjacent faces) are
|
||||
NOT removed here — downstream ``_dedupe_circles`` does that by
|
||||
geometry (centre + radius).
|
||||
"""
|
||||
occ = _import_ocp()
|
||||
explorer = occ["TopExp_Explorer"](shape, occ["TopAbs_EDGE"])
|
||||
TopoDS = occ["TopoDS"]
|
||||
BRepAdaptor_Curve = occ["BRepAdaptor_Curve"]
|
||||
GeomAbs_Circle = occ["GeomAbs_Circle"]
|
||||
while explorer.More():
|
||||
raw = explorer.Current()
|
||||
try:
|
||||
edge = TopoDS.Edge_s(raw)
|
||||
curve = BRepAdaptor_Curve(edge)
|
||||
if curve.GetType() == GeomAbs_Circle:
|
||||
circ = curve.Circle()
|
||||
loc = circ.Location()
|
||||
ax = circ.Axis().Direction()
|
||||
radius = float(circ.Radius())
|
||||
center = (float(loc.X()), float(loc.Y()), float(loc.Z()))
|
||||
axis = (float(ax.X()), float(ax.Y()), float(ax.Z()))
|
||||
yield center, axis, radius, edge
|
||||
except Exception:
|
||||
pass
|
||||
explorer.Next()
|
||||
|
||||
|
||||
def iter_cylindrical_faces(shape: Any):
|
||||
"""Yield ``(center, axis, radius, face)`` for every cylinder face.
|
||||
|
||||
Cylinder faces are typically the *walls* of through-holes; their
|
||||
radii match the circular edges that bound them.
|
||||
"""
|
||||
occ = _import_ocp()
|
||||
TopoDS = occ["TopoDS"]
|
||||
BRepAdaptor_Surface = occ["BRepAdaptor_Surface"]
|
||||
explorer = occ["TopExp_Explorer"](shape, occ["TopAbs_FACE"])
|
||||
while explorer.More():
|
||||
raw = explorer.Current()
|
||||
try:
|
||||
face = TopoDS.Face_s(raw)
|
||||
surf = BRepAdaptor_Surface(face)
|
||||
if int(surf.GetType()) == 1:
|
||||
cyl = surf.Cylinder()
|
||||
loc = cyl.Location()
|
||||
ax = cyl.Axis().Direction()
|
||||
radius = float(cyl.Radius())
|
||||
yield (
|
||||
(float(loc.X()), float(loc.Y()), float(loc.Z())),
|
||||
(float(ax.X()), float(ax.Y()), float(ax.Z())),
|
||||
radius,
|
||||
face,
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
explorer.Next()
|
||||
|
||||
|
||||
def face_area(face: Any) -> float:
|
||||
occ = _import_ocp()
|
||||
props = occ["GProp_GProps"]()
|
||||
occ["BRepGProp"].SurfaceProperties_s(face, props)
|
||||
return float(props.Mass())
|
||||
|
||||
|
||||
__all__ = [
|
||||
"StepShape",
|
||||
"load_step",
|
||||
"iter_faces",
|
||||
"iter_circular_edges",
|
||||
"face_area",
|
||||
]
|
||||
161
alg/delivery_v1.2.1/coupler_panel_release/coupler_panel/cli.py
Normal file
161
alg/delivery_v1.2.1/coupler_panel_release/coupler_panel/cli.py
Normal file
@@ -0,0 +1,161 @@
|
||||
"""``coupler-measure`` command-line entry point.
|
||||
|
||||
Three modes::
|
||||
|
||||
# measure an existing point cloud (production-like flow)
|
||||
coupler-measure --input-cloud scan.ply --out-dir runs/demo
|
||||
|
||||
# research: synthesise a cloud from a STEP file and measure it
|
||||
coupler-measure --stp panel.stp --out-dir runs/from_cad
|
||||
|
||||
# research: parameter sweep
|
||||
coupler-measure --sweep matrix.yaml --out-dir runs/sweep_xx
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import logging
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
log = logging.getLogger("coupler_panel.cli")
|
||||
|
||||
|
||||
def _build_parser() -> argparse.ArgumentParser:
|
||||
p = argparse.ArgumentParser(
|
||||
prog="coupler-measure",
|
||||
description="Coupler-panel CAD-STP to point-cloud measurement & validation.",
|
||||
)
|
||||
src = p.add_mutually_exclusive_group(required=True)
|
||||
src.add_argument("--input-cloud", type=Path, help="Path to a PLY/XYZ/CSV/PCD point cloud.")
|
||||
src.add_argument("--stp", type=Path, help="Path to a STEP file (research mode).")
|
||||
src.add_argument("--sweep", type=Path, help="Path to a sweep matrix YAML.")
|
||||
|
||||
p.add_argument("--out-dir", type=Path, required=True, help="Output directory.")
|
||||
p.add_argument("--config", type=Path, default=None, help="YAML config overrides.")
|
||||
p.add_argument(
|
||||
"--profile",
|
||||
type=str,
|
||||
default="auto",
|
||||
help="auto | default | high_density | low_density | custom",
|
||||
)
|
||||
p.add_argument(
|
||||
"--target-holes",
|
||||
type=Path,
|
||||
default=None,
|
||||
help="DEPRECATED — the main pipeline no longer performs 4-hole Kabsch "
|
||||
"registration (it cancels the panel deflection signal). The flag "
|
||||
"is accepted for backward compatibility but is ignored.",
|
||||
)
|
||||
p.add_argument(
|
||||
"--render",
|
||||
action="store_true",
|
||||
help="After measurement, render PNG diagnostics into <out-dir>/images/.",
|
||||
)
|
||||
p.add_argument("-v", "--verbose", action="count", default=0, help="-v: INFO, -vv: DEBUG")
|
||||
return p
|
||||
|
||||
|
||||
def _setup_logging(verbose: int) -> None:
|
||||
level = logging.WARNING
|
||||
if verbose == 1:
|
||||
level = logging.INFO
|
||||
elif verbose >= 2:
|
||||
level = logging.DEBUG
|
||||
logging.basicConfig(
|
||||
level=level,
|
||||
format="%(asctime)s [%(levelname)s] %(name)s: %(message)s",
|
||||
)
|
||||
|
||||
|
||||
def _on_progress(stage: str, fraction: float) -> None:
|
||||
log.info("[%5.1f%%] %s", fraction * 100, stage)
|
||||
|
||||
|
||||
def _maybe_render(out_dir: Path) -> None:
|
||||
try:
|
||||
from .viz import render_all
|
||||
|
||||
render_all(out_dir)
|
||||
except ImportError:
|
||||
log.warning("matplotlib not installed; skip --render")
|
||||
except Exception as exc:
|
||||
log.warning("render_all failed: %s", exc)
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
args = _build_parser().parse_args(argv)
|
||||
_setup_logging(args.verbose)
|
||||
|
||||
try:
|
||||
if args.input_cloud is not None:
|
||||
from .pipeline import run_coupler_measurement_pipeline
|
||||
|
||||
if args.target_holes is not None:
|
||||
log.warning(
|
||||
"--target-holes is deprecated and ignored; the main pipeline "
|
||||
"no longer performs Kabsch registration."
|
||||
)
|
||||
result = run_coupler_measurement_pipeline(
|
||||
input_cloud=args.input_cloud,
|
||||
out_dir=args.out_dir,
|
||||
config_path=args.config,
|
||||
profile=args.profile,
|
||||
progress=_on_progress,
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"theta_right_deg": result.theta_right_deg,
|
||||
"theta_ref_deg": result.theta_ref_deg,
|
||||
"dx_y_max_mm": result.dx_y_max_mm,
|
||||
"dx_z_max_mm": result.dx_z_max_mm,
|
||||
"quality_flags": result.quality_flags,
|
||||
},
|
||||
indent=2,
|
||||
ensure_ascii=False,
|
||||
)
|
||||
)
|
||||
|
||||
elif args.stp is not None:
|
||||
from .pipeline import run_from_cad_pipeline
|
||||
|
||||
summary = run_from_cad_pipeline(
|
||||
stp_path=args.stp,
|
||||
out_dir=args.out_dir,
|
||||
config_path=args.config,
|
||||
profile=args.profile,
|
||||
progress=_on_progress,
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"diff": summary["diff"],
|
||||
"quality_flags": summary["measured"]["quality_flags"],
|
||||
},
|
||||
indent=2,
|
||||
ensure_ascii=False,
|
||||
)
|
||||
)
|
||||
|
||||
elif args.sweep is not None:
|
||||
from importlib import import_module
|
||||
|
||||
mod = import_module("experiments.sweep_runner")
|
||||
mod.main(matrix_path=args.sweep, out_dir=args.out_dir, config_path=args.config)
|
||||
|
||||
if args.render:
|
||||
_maybe_render(args.out_dir)
|
||||
|
||||
except Exception as exc:
|
||||
log.exception("pipeline failed: %s", exc)
|
||||
return 2
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,209 @@
|
||||
"""Configuration loading and merging.
|
||||
|
||||
Configs are nested dicts. The single source of truth for defaults is
|
||||
``configs/default.yaml`` (shipped alongside the package); ``DEFAULT_CONFIG``
|
||||
mirrors its structure for in-memory programmatic overrides.
|
||||
|
||||
Public functions:
|
||||
|
||||
- :func:`load_config` — load a YAML file or accept an already-built dict.
|
||||
- :func:`load_profile` — load ``configs/profiles/<name>.yaml`` and merge
|
||||
on top of the defaults.
|
||||
- :func:`deep_merge` — recursive dict merge (override wins).
|
||||
- :func:`apply_dotted` — set a value via ``"sampling.spacing_mm"``.
|
||||
- :func:`resolve_auto` — fill in fields whose value is the literal
|
||||
string ``"auto"`` (currently: ``plane_fit.ransac_inlier_thresh_mm``).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import copy
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import yaml
|
||||
|
||||
|
||||
_PKG_ROOT = Path(__file__).resolve().parent
|
||||
_DEFAULT_YAML = _PKG_ROOT.parent / "configs" / "default.yaml"
|
||||
|
||||
|
||||
def _load_yaml(path: Path) -> dict[str, Any]:
|
||||
with path.open("r", encoding="utf-8") as fh:
|
||||
data = yaml.safe_load(fh)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError(f"{path}: expected a mapping at the top level")
|
||||
return data
|
||||
|
||||
|
||||
def _load_default() -> dict[str, Any]:
|
||||
if _DEFAULT_YAML.exists():
|
||||
return _load_yaml(_DEFAULT_YAML)
|
||||
# Package may have been installed without configs/. Fall back to a minimal
|
||||
# in-code default so library use without the YAML still works.
|
||||
return _IN_CODE_FALLBACK
|
||||
|
||||
|
||||
# Loaded lazily; recomputed on each call to keep tests deterministic.
|
||||
def default_config() -> dict[str, Any]:
|
||||
"""Return a fresh copy of the default configuration."""
|
||||
return copy.deepcopy(_load_default())
|
||||
|
||||
|
||||
DEFAULT_CONFIG: dict[str, Any] # populated below for convenience
|
||||
|
||||
|
||||
def load_config(
|
||||
source: str | Path | dict[str, Any] | None,
|
||||
*,
|
||||
profile: str = "auto",
|
||||
) -> dict[str, Any]:
|
||||
"""Resolve the final configuration to use.
|
||||
|
||||
The merge order is::
|
||||
|
||||
default.yaml < profile (if any) < source (path or dict)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
source : path, dict, or None
|
||||
If ``None``, only defaults (and the profile) are used.
|
||||
profile : str
|
||||
``"auto"`` / ``"default"`` skip the profile step. Any other name
|
||||
is looked up at ``configs/profiles/<name>.yaml``.
|
||||
"""
|
||||
cfg = default_config()
|
||||
|
||||
if profile not in (None, "auto", "default", "custom"):
|
||||
prof_path = _PKG_ROOT.parent / "configs" / "profiles" / f"{profile}.yaml"
|
||||
if not prof_path.exists():
|
||||
raise FileNotFoundError(f"profile '{profile}' not found at {prof_path}")
|
||||
cfg = deep_merge(cfg, _load_yaml(prof_path))
|
||||
|
||||
if source is None:
|
||||
pass
|
||||
elif isinstance(source, dict):
|
||||
cfg = deep_merge(cfg, source)
|
||||
else:
|
||||
cfg = deep_merge(cfg, _load_yaml(Path(source)))
|
||||
|
||||
return cfg
|
||||
|
||||
|
||||
def load_profile(name: str) -> dict[str, Any]:
|
||||
"""Load a profile YAML directly; useful for tests."""
|
||||
path = _PKG_ROOT.parent / "configs" / "profiles" / f"{name}.yaml"
|
||||
return _load_yaml(path)
|
||||
|
||||
|
||||
def deep_merge(base: dict[str, Any], override: dict[str, Any]) -> dict[str, Any]:
|
||||
"""Recursive dict merge. ``override`` wins on leaf collisions."""
|
||||
out = copy.deepcopy(base)
|
||||
for key, val in override.items():
|
||||
if (
|
||||
key in out
|
||||
and isinstance(out[key], dict)
|
||||
and isinstance(val, dict)
|
||||
):
|
||||
out[key] = deep_merge(out[key], val)
|
||||
else:
|
||||
out[key] = copy.deepcopy(val)
|
||||
return out
|
||||
|
||||
|
||||
def apply_dotted(cfg: dict[str, Any], dotted_key: str, value: Any) -> dict[str, Any]:
|
||||
"""Set ``cfg["a"]["b"]["c"] = value`` from ``"a.b.c"``. Returns ``cfg``."""
|
||||
parts = dotted_key.split(".")
|
||||
node = cfg
|
||||
for p in parts[:-1]:
|
||||
node = node.setdefault(p, {})
|
||||
if not isinstance(node, dict):
|
||||
raise TypeError(f"cannot apply '{dotted_key}': '{p}' is not a dict")
|
||||
node[parts[-1]] = value
|
||||
return cfg
|
||||
|
||||
|
||||
def resolve_auto(cfg: dict[str, Any]) -> dict[str, Any]:
|
||||
"""Replace literal ``"auto"`` markers with concrete values.
|
||||
|
||||
Currently:
|
||||
|
||||
- ``plane_fit.ransac_inlier_thresh_mm == "auto"`` → ``3 * noise.sigma_mm``
|
||||
(or 0.5 mm if ``noise.sigma_mm == 0``).
|
||||
"""
|
||||
out = copy.deepcopy(cfg)
|
||||
pf = out.get("plane_fit", {})
|
||||
if pf.get("ransac_inlier_thresh_mm", None) == "auto":
|
||||
sigma = float(out.get("noise", {}).get("sigma_mm", 0.0) or 0.0)
|
||||
pf["ransac_inlier_thresh_mm"] = max(3.0 * sigma, 0.5)
|
||||
out["plane_fit"] = pf
|
||||
return out
|
||||
|
||||
|
||||
# Minimal in-code fallback used only when the YAML is missing.
|
||||
_IN_CODE_FALLBACK: dict[str, Any] = {
|
||||
"schema_version": "1.0",
|
||||
"cad": {
|
||||
"linear_deflection_mm": 0.5,
|
||||
"angular_deflection_rad": 0.1,
|
||||
"locating_hole_radius_mm": [4.0, 12.0],
|
||||
"central_hole_radius_min_mm": 20.0,
|
||||
"rectangle_aspect_tol": 0.15,
|
||||
},
|
||||
"sampling": {"method": "poisson_disk", "spacing_mm": 1.0, "num_points": 100000, "seed": 42},
|
||||
"noise": {"enabled": True, "sigma_mm": 0.5, "mode": "isotropic", "seed": 43},
|
||||
"transform": {
|
||||
"enabled": True,
|
||||
"rotation_deg": 3.0,
|
||||
"rotation_axis": "random",
|
||||
"translation_mm": 5.0,
|
||||
"translation_dir": "random",
|
||||
"seed": 44,
|
||||
},
|
||||
"denoise": {"steps": []},
|
||||
"registration": {"method": "kabsch_four_holes", "pre_align": "pca_centroid"},
|
||||
"detection": {
|
||||
"panel_normal_seed": "from_cad",
|
||||
"grid_pixel_mm": 0.5,
|
||||
"panel_min_area_ratio": 0.4,
|
||||
"hole_min_area_mm2": 30.0,
|
||||
"hole_max_area_mm2": 800.0,
|
||||
"circle_fit": {"method": "algebraic_then_lsq", "inlier_residual_mm": 0.5},
|
||||
"select_top_k_by_radius_match": 4,
|
||||
"rectangle_aspect_tol": 0.20,
|
||||
},
|
||||
"roi": {
|
||||
"hole_dilate_mm": 3.0,
|
||||
"central_hole_dilate_mm": 5.0,
|
||||
"edge_buffer_mm": 5.0,
|
||||
"chamfer_normal_dot_min": 0.85,
|
||||
"outlier_z_score": 3.0,
|
||||
},
|
||||
"plane_fit": {
|
||||
"method": "ransac_svd",
|
||||
"ransac_iterations": 200,
|
||||
"ransac_inlier_thresh_mm": "auto",
|
||||
"min_inlier_ratio": 0.6,
|
||||
"enforce_nx_positive": True,
|
||||
},
|
||||
"metrics": {"right_axis_sign": 1, "angle_clamp_deg": 0.0},
|
||||
"artifacts": {
|
||||
"save_intermediate": True,
|
||||
"ply_format": "binary",
|
||||
"image_dpi": 150,
|
||||
"heatmap_colormap": "viridis",
|
||||
},
|
||||
}
|
||||
|
||||
DEFAULT_CONFIG = copy.deepcopy(_IN_CODE_FALLBACK)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"DEFAULT_CONFIG",
|
||||
"default_config",
|
||||
"load_config",
|
||||
"load_profile",
|
||||
"deep_merge",
|
||||
"apply_dotted",
|
||||
"resolve_auto",
|
||||
]
|
||||
@@ -0,0 +1,528 @@
|
||||
"""Data-preparation pipeline (research / validation use only).
|
||||
|
||||
This module is **not** part of the integration API. It exists so that
|
||||
researchers can take a CAD STP file, synthesise scan-like point clouds,
|
||||
denoise them, and align them to the CAD frame — i.e. it produces an
|
||||
input that :func:`coupler_panel.api.compute_coupler_measurement` can
|
||||
then run on.
|
||||
|
||||
Stages performed (each one is independently re-runnable so the experiment
|
||||
sweep can iterate over individual knobs)::
|
||||
|
||||
parse_cad STP → mesh + GT
|
||||
synthesize_cloud mesh → sampled (multi-strategy) → noisy → transformed
|
||||
denoise_cloud cloud → cloud (configurable pipeline)
|
||||
register_cloud cloud → cloud (4-hole Kabsch into CAD frame)
|
||||
|
||||
The end-to-end convenience wrapper :func:`prepare_cloud_from_cad` glues
|
||||
the four stages together and returns a :class:`PreparedCloud` ready to
|
||||
hand to the measurement API.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .config import load_config, resolve_auto
|
||||
from .denoise import run_pipeline as run_denoise
|
||||
from .detection import detect_four_holes
|
||||
from .exceptions import RegistrationError
|
||||
from .registration import register_four_holes
|
||||
from .structures import Hole, HoleSet
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Result types
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
@dataclass
|
||||
class CADParsed:
|
||||
"""Output of :func:`parse_cad`."""
|
||||
|
||||
mesh: Any # coupler_panel.cad.mesh.TriangleMesh
|
||||
gt: Any # coupler_panel.cad.gt_extractor.GroundTruth
|
||||
|
||||
|
||||
@dataclass
|
||||
class SynthesizedCloud:
|
||||
"""Output of :func:`synthesize_cloud`."""
|
||||
|
||||
sampled: np.ndarray # (Ns, 3) fresh from the mesh, optional HPR applied
|
||||
deflected: np.ndarray # (Ns, 3) + panel-deflection injection (known GT)
|
||||
noisy: np.ndarray # (Ns, 3) + Gaussian noise
|
||||
transformed: np.ndarray # (Ns, 3) + scanner-pose rigid transform
|
||||
gt_transform_4x4: np.ndarray # (4, 4) scanner-pose transform applied at end
|
||||
gt_holes_in_scan_frame: HoleSet # deflected GT holes after T_jitter — Kabsch target
|
||||
# ---- Panel-deflection ground truth ----------------------------------
|
||||
panel_deflection_R: np.ndarray # (3, 3) rotation that tilts the panel
|
||||
panel_deflection_theta_right_deg: float
|
||||
panel_deflection_theta_ref_deg: float
|
||||
gt_holes_after_deflection: HoleSet # in CAD frame, post-deflection
|
||||
gt_normal_after_deflection: np.ndarray # (3,) deflected panel normal in CAD frame
|
||||
info: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
|
||||
@dataclass
|
||||
class DenoisedCloud:
|
||||
"""Output of :func:`denoise_cloud`."""
|
||||
|
||||
points: np.ndarray
|
||||
log: list[dict[str, Any]]
|
||||
|
||||
|
||||
@dataclass
|
||||
class RegisteredCloud:
|
||||
"""Output of :func:`register_cloud`."""
|
||||
|
||||
points: np.ndarray # cloud aligned into the CAD frame
|
||||
transform_4x4: np.ndarray # the rigid alignment (scan → CAD)
|
||||
rmse_mm: float
|
||||
max_residual_mm: float
|
||||
detected_holes_in_cad_frame: HoleSet
|
||||
|
||||
|
||||
@dataclass
|
||||
class PreparedCloud:
|
||||
"""End-to-end output, ready to feed into the public measurement API.
|
||||
|
||||
``points`` is in the CAD frame, denoised, with all the meta-data the
|
||||
experiment / validation harness needs to score the measurement.
|
||||
"""
|
||||
|
||||
points: np.ndarray
|
||||
gt: Any # GroundTruth
|
||||
gt_transform_4x4: np.ndarray | None
|
||||
registration_transform_4x4: np.ndarray | None
|
||||
registration_rmse_mm: float | None
|
||||
detected_holes_in_cad_frame: HoleSet | None
|
||||
intermediates: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Stage 1 — STP → mesh + GT
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def parse_cad(stp_path: str | Path, cad_cfg: dict[str, Any]) -> CADParsed:
|
||||
from .cad import extract_ground_truth, load_step, tessellate_to_trimesh
|
||||
|
||||
step = load_step(stp_path)
|
||||
mesh = tessellate_to_trimesh(
|
||||
step,
|
||||
linear_deflection_mm=float(cad_cfg["linear_deflection_mm"]),
|
||||
angular_deflection_rad=float(cad_cfg["angular_deflection_rad"]),
|
||||
)
|
||||
gt = extract_ground_truth(step, cad_cfg)
|
||||
return CADParsed(mesh=mesh, gt=gt)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Stage 2 — mesh → sampled → noisy → transformed
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _build_panel_deflection_R(theta_right_deg: float, theta_ref_deg: float) -> np.ndarray:
|
||||
"""Build the 3×3 rotation that tilts a panel whose normal is (1,0,0).
|
||||
|
||||
The post-rotation normal is::
|
||||
|
||||
n = (cos(th_ref)·cos(th_right), cos(th_ref)·sin(th_right), sin(th_ref))
|
||||
|
||||
so that ``atan2(n_y, n_x) = th_right`` and ``atan2(n_z, n_x) = th_ref``
|
||||
exactly.
|
||||
|
||||
R = R_z(th_right) · R_y(-th_ref)
|
||||
"""
|
||||
a = np.deg2rad(float(theta_right_deg))
|
||||
b = np.deg2rad(float(theta_ref_deg))
|
||||
Rz = np.array(
|
||||
[
|
||||
[np.cos(a), -np.sin(a), 0.0],
|
||||
[np.sin(a), np.cos(a), 0.0],
|
||||
[0.0, 0.0, 1.0],
|
||||
]
|
||||
)
|
||||
Ry_neg = np.array(
|
||||
[
|
||||
[np.cos(b), 0.0, -np.sin(b)],
|
||||
[0.0, 1.0, 0.0],
|
||||
[np.sin(b), 0.0, np.cos(b)],
|
||||
]
|
||||
)
|
||||
return Rz @ Ry_neg
|
||||
|
||||
|
||||
def _rotate_holeset(holes: HoleSet, R: np.ndarray) -> HoleSet:
|
||||
locating = tuple(
|
||||
Hole(
|
||||
center=tuple((R @ np.asarray(h.center)).tolist()),
|
||||
radius_mm=h.radius_mm,
|
||||
fit_residual_rms_mm=h.fit_residual_rms_mm,
|
||||
role=h.role,
|
||||
)
|
||||
for h in holes.locating
|
||||
)
|
||||
central = (
|
||||
None
|
||||
if holes.central is None
|
||||
else Hole(
|
||||
center=tuple((R @ np.asarray(holes.central.center)).tolist()),
|
||||
radius_mm=holes.central.radius_mm,
|
||||
fit_residual_rms_mm=holes.central.fit_residual_rms_mm,
|
||||
role="central",
|
||||
)
|
||||
)
|
||||
return HoleSet(locating=locating, central=central)
|
||||
|
||||
|
||||
def synthesize_cloud(
|
||||
parsed: CADParsed,
|
||||
*,
|
||||
sampling_cfg: dict[str, Any],
|
||||
noise_cfg: dict[str, Any],
|
||||
transform_cfg: dict[str, Any],
|
||||
scan_cfg: dict[str, Any] | None = None,
|
||||
deflection_cfg: dict[str, Any] | None = None,
|
||||
) -> SynthesizedCloud:
|
||||
from .sampling import resolve_num_points, sample_mesh
|
||||
from .simulation import (
|
||||
add_gaussian_noise,
|
||||
apply_rigid,
|
||||
sample_random_rigid,
|
||||
simulate_scan_view,
|
||||
)
|
||||
|
||||
n = resolve_num_points(
|
||||
parsed.mesh,
|
||||
num_points=sampling_cfg.get("num_points"),
|
||||
spacing_mm=sampling_cfg.get("spacing_mm"),
|
||||
)
|
||||
sampled = sample_mesh(
|
||||
parsed.mesh,
|
||||
method=sampling_cfg["method"],
|
||||
num_points=n,
|
||||
spacing_mm=sampling_cfg.get("spacing_mm"),
|
||||
seed=int(sampling_cfg.get("seed", 42)),
|
||||
curvature_weight=float(sampling_cfg.get("adaptive", {}).get("curvature_weight", 1.0)),
|
||||
)
|
||||
|
||||
# Optional: simulate a single-view scan via hidden-point removal.
|
||||
# Without this, the sampler returns points from BOTH sides of the panel
|
||||
# (front face, back face, cylinder hole walls, …); the projected
|
||||
# occupancy grid then has no cavities because the back-face points fill
|
||||
# the hole region. Enable this for any "facing-scan" simulation.
|
||||
if scan_cfg and bool(scan_cfg.get("enabled", False)):
|
||||
view = scan_cfg.get("view_origin", [10000.0, 0.0, 0.0])
|
||||
sampled = simulate_scan_view(
|
||||
sampled,
|
||||
view_origin=view,
|
||||
hpr_radius_mm=float(scan_cfg.get("hpr_radius_mm", 50000.0)),
|
||||
)
|
||||
|
||||
# ---- Panel deflection injection ---------------------------------------
|
||||
# This is the *signal* the algorithm should later recover. We rotate
|
||||
# the entire panel (and its 4 locating holes + central hole) by a known
|
||||
# small angle so that the GT panel normal becomes
|
||||
# n_new = R · (1, 0, 0)
|
||||
# with atan2(n_new.y, n_new.x) = theta_right_deg (rotation around z)
|
||||
# and atan2(n_new.z, n_new.x) = theta_ref_deg (rotation around -y).
|
||||
# The 4 hole centres rotate with the panel, so 4-hole Kabsch (which
|
||||
# targets the *deflected* GT holes) does NOT undo the deflection — the
|
||||
# panel orientation in the registered cloud still carries the signal.
|
||||
th_right = 0.0
|
||||
th_ref = 0.0
|
||||
if deflection_cfg and bool(deflection_cfg.get("enabled", False)):
|
||||
th_right = float(deflection_cfg.get("theta_right_deg", 0.0))
|
||||
th_ref = float(deflection_cfg.get("theta_ref_deg", 0.0))
|
||||
R_panel = _build_panel_deflection_R(th_right, th_ref)
|
||||
deflected = sampled @ R_panel.T
|
||||
gt_holes_after = _rotate_holeset(parsed.gt.holes, R_panel)
|
||||
gt_normal_after = R_panel @ np.asarray(parsed.gt.plane.normal, dtype=np.float64)
|
||||
sampled = deflected # downstream uses the deflected cloud as the new ground
|
||||
|
||||
if bool(noise_cfg.get("enabled", True)) and float(noise_cfg.get("sigma_mm", 0.0)) > 0:
|
||||
noisy = add_gaussian_noise(
|
||||
sampled,
|
||||
sigma_mm=float(noise_cfg["sigma_mm"]),
|
||||
mode=noise_cfg.get("mode", "isotropic"),
|
||||
seed=int(noise_cfg.get("seed", 43)),
|
||||
)
|
||||
else:
|
||||
noisy = sampled.copy()
|
||||
|
||||
# The "GT-side" scan-pose perturbation: shifts and rotates the cloud as
|
||||
# a whole to mimic mounting / scanner-stage repeatability error. It is
|
||||
# NOT the deflection we want to measure; 4-hole Kabsch should cancel it.
|
||||
if bool(transform_cfg.get("enabled", True)):
|
||||
T = sample_random_rigid(
|
||||
rotation_deg=float(transform_cfg.get("rotation_deg", 0.0)),
|
||||
translation_mm=float(transform_cfg.get("translation_mm", 0.0)),
|
||||
rotation_axis=transform_cfg.get("rotation_axis", "random"),
|
||||
translation_dir=transform_cfg.get("translation_dir", "random"),
|
||||
seed=int(transform_cfg.get("seed", 44)),
|
||||
)
|
||||
transformed = apply_rigid(noisy, T.matrix)
|
||||
Mat = T.matrix
|
||||
R = Mat[:3, :3]
|
||||
t = Mat[:3, 3]
|
||||
# Deflected GT holes, then jitter — same composition the cloud went through.
|
||||
gt_holes_scan = HoleSet(
|
||||
locating=tuple( # type: ignore[arg-type]
|
||||
Hole(
|
||||
center=tuple((R @ np.asarray(h.center) + t).tolist()),
|
||||
radius_mm=h.radius_mm,
|
||||
fit_residual_rms_mm=0.0,
|
||||
role=h.role,
|
||||
)
|
||||
for h in gt_holes_after.locating
|
||||
),
|
||||
central=(
|
||||
None
|
||||
if gt_holes_after.central is None
|
||||
else Hole(
|
||||
center=tuple(
|
||||
(R @ np.asarray(gt_holes_after.central.center) + t).tolist()
|
||||
),
|
||||
radius_mm=gt_holes_after.central.radius_mm,
|
||||
fit_residual_rms_mm=0.0,
|
||||
role="central",
|
||||
)
|
||||
),
|
||||
)
|
||||
info = {"transform": T.to_dict()}
|
||||
gt_T = T.matrix
|
||||
else:
|
||||
transformed = noisy.copy()
|
||||
gt_holes_scan = gt_holes_after
|
||||
info = {"transform": {"enabled": False}}
|
||||
gt_T = np.eye(4)
|
||||
|
||||
return SynthesizedCloud(
|
||||
sampled=sampled,
|
||||
deflected=deflected,
|
||||
noisy=noisy,
|
||||
transformed=transformed,
|
||||
gt_transform_4x4=gt_T,
|
||||
gt_holes_in_scan_frame=gt_holes_scan,
|
||||
panel_deflection_R=R_panel,
|
||||
panel_deflection_theta_right_deg=th_right,
|
||||
panel_deflection_theta_ref_deg=th_ref,
|
||||
gt_holes_after_deflection=gt_holes_after,
|
||||
gt_normal_after_deflection=gt_normal_after,
|
||||
info=info,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Stage 3 — denoise
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def denoise_cloud(
|
||||
points: np.ndarray,
|
||||
denoise_cfg: dict[str, Any],
|
||||
) -> DenoisedCloud:
|
||||
pts, log = run_denoise(points, denoise_cfg.get("steps", []))
|
||||
return DenoisedCloud(points=pts, log=log)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Stage 4 — register (4-hole Kabsch)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def register_cloud(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
target_holes: HoleSet,
|
||||
detection_cfg: dict[str, Any],
|
||||
cad_cfg: dict[str, Any],
|
||||
registration_cfg: dict[str, Any],
|
||||
panel_normal_seed: np.ndarray | None = None,
|
||||
) -> RegisteredCloud:
|
||||
"""Detect 4 holes in ``points`` and Kabsch-align them to ``target_holes``.
|
||||
|
||||
``target_holes`` is the GT (CAD-frame) hole set. The output cloud is
|
||||
in the CAD frame. The 4-hole pairing is canonical-quadrant-ordered
|
||||
on both sides, so no greedy NN matching is needed.
|
||||
"""
|
||||
detection = detect_four_holes(
|
||||
points,
|
||||
detection_cfg=detection_cfg,
|
||||
cad_cfg=cad_cfg,
|
||||
panel_normal_seed=panel_normal_seed,
|
||||
)
|
||||
|
||||
src = detection.holes_3d.locating_centers()
|
||||
tgt = target_holes.locating_centers()
|
||||
kabsch = register_four_holes(
|
||||
src,
|
||||
tgt,
|
||||
refuse_above_mm=registration_cfg.get("refuse_if_residual_above_mm"),
|
||||
)
|
||||
|
||||
R = kabsch.matrix[:3, :3]
|
||||
t = kabsch.matrix[:3, 3]
|
||||
aligned = points @ R.T + t
|
||||
|
||||
detected_in_cad = HoleSet(
|
||||
locating=tuple( # type: ignore[arg-type]
|
||||
Hole(
|
||||
center=tuple((R @ np.asarray(h.center) + t).tolist()),
|
||||
radius_mm=h.radius_mm,
|
||||
fit_residual_rms_mm=h.fit_residual_rms_mm,
|
||||
role=h.role,
|
||||
)
|
||||
for h in detection.holes_3d.locating
|
||||
),
|
||||
central=(
|
||||
None
|
||||
if detection.holes_3d.central is None
|
||||
else Hole(
|
||||
center=tuple(
|
||||
(R @ np.asarray(detection.holes_3d.central.center) + t).tolist()
|
||||
),
|
||||
radius_mm=detection.holes_3d.central.radius_mm,
|
||||
fit_residual_rms_mm=detection.holes_3d.central.fit_residual_rms_mm,
|
||||
role="central",
|
||||
)
|
||||
),
|
||||
)
|
||||
|
||||
return RegisteredCloud(
|
||||
points=aligned,
|
||||
transform_4x4=kabsch.matrix,
|
||||
rmse_mm=kabsch.rmse_mm,
|
||||
max_residual_mm=kabsch.max_residual_mm,
|
||||
detected_holes_in_cad_frame=detected_in_cad,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# End-to-end convenience: STP → PreparedCloud
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def prepare_cloud_from_cad(
|
||||
stp_path: str | Path,
|
||||
config: str | Path | dict[str, Any] | None = None,
|
||||
profile: str = "auto",
|
||||
) -> PreparedCloud:
|
||||
"""Run all four stages and return a :class:`PreparedCloud`.
|
||||
|
||||
The returned cloud is denoised and aligned into the CAD frame; pass
|
||||
``prepared.points`` to
|
||||
:func:`coupler_panel.api.compute_coupler_measurement`.
|
||||
"""
|
||||
cfg = load_config(config, profile=profile)
|
||||
cfg = resolve_auto(cfg)
|
||||
|
||||
parsed = parse_cad(stp_path, cfg["cad"])
|
||||
synth = synthesize_cloud(
|
||||
parsed,
|
||||
sampling_cfg=cfg["sampling"],
|
||||
noise_cfg=cfg.get("noise", {}),
|
||||
transform_cfg=cfg.get("transform", {}),
|
||||
scan_cfg=cfg.get("scan_simulator", {}),
|
||||
deflection_cfg=cfg.get("panel_deflection", {}),
|
||||
)
|
||||
denoised = denoise_cloud(synth.transformed, cfg.get("denoise", {}))
|
||||
# Important: register against the *deflected* GT — the holes have moved
|
||||
# with the panel, so Kabsch only undoes the scan-pose jitter, NOT the
|
||||
# injected deflection. The plane fit downstream then sees the tilted
|
||||
# panel and can recover theta_right / theta_ref.
|
||||
registered = register_cloud(
|
||||
denoised.points,
|
||||
target_holes=synth.gt_holes_after_deflection,
|
||||
detection_cfg=cfg["detection"],
|
||||
cad_cfg=cfg["cad"],
|
||||
registration_cfg=cfg["registration"],
|
||||
)
|
||||
|
||||
return PreparedCloud(
|
||||
points=registered.points,
|
||||
gt=parsed.gt,
|
||||
gt_transform_4x4=synth.gt_transform_4x4,
|
||||
registration_transform_4x4=registered.transform_4x4,
|
||||
registration_rmse_mm=registered.rmse_mm,
|
||||
detected_holes_in_cad_frame=registered.detected_holes_in_cad_frame,
|
||||
intermediates={
|
||||
"config": cfg,
|
||||
"sampled": synth.sampled,
|
||||
"noisy": synth.noisy,
|
||||
"transformed": synth.transformed,
|
||||
"denoised": denoised.points,
|
||||
"denoise_log": denoised.log,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def prepare_cloud_from_scan(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
config: str | Path | dict[str, Any] | None = None,
|
||||
profile: str = "auto",
|
||||
target_holes: HoleSet | None = None,
|
||||
) -> PreparedCloud:
|
||||
"""Same as :func:`prepare_cloud_from_cad` but starting from a real scan.
|
||||
|
||||
Skips parsing / synthesis. If ``target_holes`` is given, runs the
|
||||
4-hole Kabsch registration; otherwise assumes the cloud is already
|
||||
in the CAD frame.
|
||||
"""
|
||||
cfg = load_config(config, profile=profile)
|
||||
cfg = resolve_auto(cfg)
|
||||
|
||||
denoised = denoise_cloud(np.asarray(points, dtype=np.float64), cfg.get("denoise", {}))
|
||||
|
||||
if target_holes is None:
|
||||
return PreparedCloud(
|
||||
points=denoised.points,
|
||||
gt=None,
|
||||
gt_transform_4x4=None,
|
||||
registration_transform_4x4=None,
|
||||
registration_rmse_mm=None,
|
||||
detected_holes_in_cad_frame=None,
|
||||
intermediates={"config": cfg, "denoise_log": denoised.log},
|
||||
)
|
||||
|
||||
registered = register_cloud(
|
||||
denoised.points,
|
||||
target_holes=target_holes,
|
||||
detection_cfg=cfg["detection"],
|
||||
cad_cfg=cfg["cad"],
|
||||
registration_cfg=cfg["registration"],
|
||||
)
|
||||
return PreparedCloud(
|
||||
points=registered.points,
|
||||
gt=None,
|
||||
gt_transform_4x4=None,
|
||||
registration_transform_4x4=registered.transform_4x4,
|
||||
registration_rmse_mm=registered.rmse_mm,
|
||||
detected_holes_in_cad_frame=registered.detected_holes_in_cad_frame,
|
||||
intermediates={
|
||||
"config": cfg,
|
||||
"denoised": denoised.points,
|
||||
"denoise_log": denoised.log,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"CADParsed",
|
||||
"SynthesizedCloud",
|
||||
"DenoisedCloud",
|
||||
"RegisteredCloud",
|
||||
"PreparedCloud",
|
||||
"parse_cad",
|
||||
"synthesize_cloud",
|
||||
"denoise_cloud",
|
||||
"register_cloud",
|
||||
"prepare_cloud_from_cad",
|
||||
"prepare_cloud_from_scan",
|
||||
]
|
||||
@@ -0,0 +1,29 @@
|
||||
"""Composable point cloud denoising pipeline.
|
||||
|
||||
Each step is a pure function ``points -> points``. Steps are configured
|
||||
via YAML and chained by :func:`run_pipeline`.
|
||||
|
||||
Available steps (matching ``denoise.steps[].type``):
|
||||
|
||||
- ``passthrough`` — keep points inside an axis-aligned bounding box
|
||||
- ``voxel`` — voxel-grid downsample
|
||||
- ``statistical`` — statistical outlier removal (SOR)
|
||||
- ``radius`` — radius outlier removal (ROR)
|
||||
- ``smoothing`` — MLS smoothing (only when explicitly enabled)
|
||||
"""
|
||||
|
||||
from .passthrough import passthrough_filter
|
||||
from .voxel import voxel_downsample
|
||||
from .statistical import statistical_outlier_removal
|
||||
from .radius import radius_outlier_removal
|
||||
from .smoothing import mls_smoothing
|
||||
from .pipeline import run_pipeline
|
||||
|
||||
__all__ = [
|
||||
"passthrough_filter",
|
||||
"voxel_downsample",
|
||||
"statistical_outlier_removal",
|
||||
"radius_outlier_removal",
|
||||
"mls_smoothing",
|
||||
"run_pipeline",
|
||||
]
|
||||
@@ -0,0 +1,41 @@
|
||||
"""Passthrough (axis-aligned bounding-box) filter."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def passthrough_filter(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
bbox: tuple[tuple[float, float], tuple[float, float], tuple[float, float]] | None = None,
|
||||
bbox_padding_mm: float | None = None,
|
||||
) -> np.ndarray:
|
||||
"""Keep only points inside ``bbox``.
|
||||
|
||||
``bbox`` is ``((xmin, xmax), (ymin, ymax), (zmin, zmax))``.
|
||||
|
||||
If ``bbox`` is ``None`` and ``bbox_padding_mm`` is given, the bbox is
|
||||
derived from the cloud itself, padded by ``bbox_padding_mm`` on each
|
||||
side — which is a useful no-op default that nonetheless drops far-out
|
||||
NaNs / spurious points.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
if bbox is None:
|
||||
if bbox_padding_mm is None:
|
||||
return pts.copy()
|
||||
pad = float(bbox_padding_mm)
|
||||
lo = pts.min(axis=0) - pad
|
||||
hi = pts.max(axis=0) + pad
|
||||
bbox = ((lo[0], hi[0]), (lo[1], hi[1]), (lo[2], hi[2]))
|
||||
|
||||
(xlo, xhi), (ylo, yhi), (zlo, zhi) = bbox
|
||||
mask = (
|
||||
(pts[:, 0] >= xlo) & (pts[:, 0] <= xhi)
|
||||
& (pts[:, 1] >= ylo) & (pts[:, 1] <= yhi)
|
||||
& (pts[:, 2] >= zlo) & (pts[:, 2] <= zhi)
|
||||
)
|
||||
return pts[mask]
|
||||
|
||||
|
||||
__all__ = ["passthrough_filter"]
|
||||
@@ -0,0 +1,71 @@
|
||||
"""Compose denoising steps from a YAML-style step list.
|
||||
|
||||
Example config::
|
||||
|
||||
denoise:
|
||||
steps:
|
||||
- type: passthrough
|
||||
bbox_padding_mm: 50.0
|
||||
- type: voxel
|
||||
voxel_mm: 0.5
|
||||
- type: statistical
|
||||
nb_neighbors: 30
|
||||
std_ratio: 2.0
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Callable
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .passthrough import passthrough_filter
|
||||
from .radius import radius_outlier_removal
|
||||
from .smoothing import mls_smoothing
|
||||
from .statistical import statistical_outlier_removal
|
||||
from .voxel import voxel_downsample
|
||||
|
||||
|
||||
_STEP_FNS: dict[str, Callable[..., np.ndarray]] = {
|
||||
"passthrough": passthrough_filter,
|
||||
"voxel": voxel_downsample,
|
||||
"statistical": statistical_outlier_removal,
|
||||
"radius": radius_outlier_removal,
|
||||
"smoothing": mls_smoothing,
|
||||
}
|
||||
|
||||
|
||||
def run_pipeline(
|
||||
points: np.ndarray,
|
||||
steps: list[dict[str, Any]] | None,
|
||||
*,
|
||||
on_step: Callable[[str, np.ndarray], None] | None = None,
|
||||
) -> tuple[np.ndarray, list[dict[str, Any]]]:
|
||||
"""Apply each step in order and return the final cloud + a per-step log.
|
||||
|
||||
Each log entry is::
|
||||
|
||||
{"type": ..., "params": {...}, "n_in": int, "n_out": int}
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
log: list[dict[str, Any]] = []
|
||||
if not steps:
|
||||
return pts.copy(), log
|
||||
|
||||
for raw in steps:
|
||||
cfg = dict(raw)
|
||||
kind = cfg.pop("type")
|
||||
if kind not in _STEP_FNS:
|
||||
raise ValueError(f"unknown denoise step '{kind}'; valid: {sorted(_STEP_FNS)}")
|
||||
fn = _STEP_FNS[kind]
|
||||
n_in = pts.shape[0]
|
||||
pts = fn(pts, **cfg)
|
||||
log.append(
|
||||
{"type": kind, "params": cfg, "n_in": n_in, "n_out": int(pts.shape[0])}
|
||||
)
|
||||
if on_step is not None:
|
||||
on_step(kind, pts)
|
||||
return pts, log
|
||||
|
||||
|
||||
__all__ = ["run_pipeline"]
|
||||
@@ -0,0 +1,38 @@
|
||||
"""Radius outlier removal (ROR).
|
||||
|
||||
A point is kept iff at least ``nb_points`` other points lie within
|
||||
``radius_mm``. Useful for getting rid of spray noise in scanner data.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def radius_outlier_removal(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
nb_points: int = 8,
|
||||
radius_mm: float = 2.0,
|
||||
) -> np.ndarray:
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n = pts.shape[0]
|
||||
if n == 0:
|
||||
return pts.copy()
|
||||
|
||||
try:
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
tree = cKDTree(pts)
|
||||
counts = tree.query_ball_point(pts, r=float(radius_mm), return_length=True)
|
||||
# `counts` includes the point itself; threshold accordingly.
|
||||
keep = counts >= int(nb_points) + 1
|
||||
except ImportError:
|
||||
diff = pts[:, None, :] - pts[None, :, :]
|
||||
d = np.sqrt(np.sum(diff * diff, axis=2))
|
||||
keep = (d <= float(radius_mm)).sum(axis=1) >= int(nb_points) + 1
|
||||
|
||||
return pts[keep]
|
||||
|
||||
|
||||
__all__ = ["radius_outlier_removal"]
|
||||
@@ -0,0 +1,84 @@
|
||||
"""Optional moving-least-squares (MLS) smoothing.
|
||||
|
||||
For each point, fit a local polynomial in the neighbour set's tangent
|
||||
plane and project the point onto it. Implementation here is a degree-2
|
||||
local fit using only numpy + scipy.cKDTree. For most coupler-panel work
|
||||
this stage is *off* by default — leave it disabled unless you actually
|
||||
need smoothing in addition to SOR/ROR.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def mls_smoothing(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
search_radius_mm: float = 2.0,
|
||||
polynomial_order: int = 2,
|
||||
min_neighbors: int = 8,
|
||||
) -> np.ndarray:
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n = pts.shape[0]
|
||||
if n == 0:
|
||||
return pts.copy()
|
||||
|
||||
try:
|
||||
from scipy.spatial import cKDTree
|
||||
except ImportError:
|
||||
# Without a tree this is hopelessly slow; skip safely.
|
||||
return pts.copy()
|
||||
|
||||
tree = cKDTree(pts)
|
||||
out = pts.copy()
|
||||
|
||||
for i in range(n):
|
||||
idx = tree.query_ball_point(pts[i], r=float(search_radius_mm))
|
||||
if len(idx) < min_neighbors:
|
||||
continue
|
||||
|
||||
nbh = pts[idx]
|
||||
center = nbh.mean(axis=0)
|
||||
# Local frame from PCA on the neighbourhood.
|
||||
Q = nbh - center
|
||||
cov = (Q.T @ Q) / max(len(idx) - 1, 1)
|
||||
w, V = np.linalg.eigh(cov)
|
||||
# Smallest eigenvector ≈ surface normal; the other two span the tangent.
|
||||
normal = V[:, 0]
|
||||
tangents = V[:, 1:]
|
||||
|
||||
local_xy = Q @ tangents # (k, 2)
|
||||
local_z = Q @ normal # (k,)
|
||||
|
||||
if polynomial_order == 1:
|
||||
A = np.column_stack([np.ones(len(idx)), local_xy])
|
||||
else:
|
||||
x, y = local_xy[:, 0], local_xy[:, 1]
|
||||
A = np.column_stack([np.ones(len(idx)), x, y, x * x, x * y, y * y])
|
||||
|
||||
try:
|
||||
coef, *_ = np.linalg.lstsq(A, local_z, rcond=None)
|
||||
except np.linalg.LinAlgError:
|
||||
continue
|
||||
|
||||
# Project pts[i] onto the surface in local frame.
|
||||
rel = pts[i] - center
|
||||
xy_i = np.array([rel @ tangents[:, 0], rel @ tangents[:, 1]])
|
||||
if polynomial_order == 1:
|
||||
z_i = coef[0] + coef[1] * xy_i[0] + coef[2] * xy_i[1]
|
||||
else:
|
||||
z_i = (
|
||||
coef[0]
|
||||
+ coef[1] * xy_i[0]
|
||||
+ coef[2] * xy_i[1]
|
||||
+ coef[3] * xy_i[0] ** 2
|
||||
+ coef[4] * xy_i[0] * xy_i[1]
|
||||
+ coef[5] * xy_i[1] ** 2
|
||||
)
|
||||
out[i] = center + tangents[:, 0] * xy_i[0] + tangents[:, 1] * xy_i[1] + normal * z_i
|
||||
|
||||
return out
|
||||
|
||||
|
||||
__all__ = ["mls_smoothing"]
|
||||
@@ -0,0 +1,44 @@
|
||||
"""Statistical outlier removal (SOR).
|
||||
|
||||
For each point, compute the mean distance to its ``k`` nearest neighbours.
|
||||
A point whose mean distance exceeds the global mean by ``std_ratio`` ×
|
||||
the global std is dropped.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def statistical_outlier_removal(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
nb_neighbors: int = 30,
|
||||
std_ratio: float = 2.0,
|
||||
) -> np.ndarray:
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n = pts.shape[0]
|
||||
if n <= nb_neighbors + 1:
|
||||
return pts.copy()
|
||||
|
||||
try:
|
||||
from scipy.spatial import cKDTree
|
||||
|
||||
tree = cKDTree(pts)
|
||||
dist, _ = tree.query(pts, k=nb_neighbors + 1)
|
||||
# First column is the point itself (distance 0).
|
||||
mean_d = dist[:, 1:].mean(axis=1)
|
||||
except ImportError:
|
||||
# Plain numpy fallback (O(N²); fine for N up to a few thousand).
|
||||
diff = pts[:, None, :] - pts[None, :, :]
|
||||
d = np.sqrt(np.sum(diff * diff, axis=2))
|
||||
d.sort(axis=1)
|
||||
mean_d = d[:, 1 : nb_neighbors + 1].mean(axis=1)
|
||||
|
||||
mu = mean_d.mean()
|
||||
sigma = mean_d.std()
|
||||
keep = mean_d <= mu + float(std_ratio) * sigma
|
||||
return pts[keep]
|
||||
|
||||
|
||||
__all__ = ["statistical_outlier_removal"]
|
||||
@@ -0,0 +1,37 @@
|
||||
"""Voxel-grid downsampling."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def voxel_downsample(points: np.ndarray, *, voxel_mm: float) -> np.ndarray:
|
||||
"""Replace each occupied voxel with the centroid of its points."""
|
||||
if voxel_mm <= 0:
|
||||
raise ValueError("voxel_mm must be > 0")
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
if pts.shape[0] == 0:
|
||||
return pts.copy()
|
||||
|
||||
keys = np.floor(pts / float(voxel_mm)).astype(np.int64)
|
||||
# Compose a single 1-D key for fast unique() grouping.
|
||||
kmin = keys.min(axis=0)
|
||||
keys = keys - kmin
|
||||
span = keys.max(axis=0) + 1
|
||||
flat = keys[:, 0] + span[0] * (keys[:, 1] + span[1] * keys[:, 2])
|
||||
|
||||
order = np.argsort(flat, kind="stable")
|
||||
flat_sorted = flat[order]
|
||||
pts_sorted = pts[order]
|
||||
|
||||
# Group by run-length on the sorted flat keys.
|
||||
boundaries = np.concatenate(([0], np.where(np.diff(flat_sorted) != 0)[0] + 1, [pts.shape[0]]))
|
||||
n_groups = len(boundaries) - 1
|
||||
out = np.empty((n_groups, 3), dtype=np.float64)
|
||||
for i in range(n_groups):
|
||||
s, e = boundaries[i], boundaries[i + 1]
|
||||
out[i] = pts_sorted[s:e].mean(axis=0)
|
||||
return out
|
||||
|
||||
|
||||
__all__ = ["voxel_downsample"]
|
||||
@@ -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,114 @@
|
||||
"""Derive scale/density-dependent config from CAD geometry + cloud spacing.
|
||||
|
||||
This is a **configuration helper**, not part of the measurement algorithm.
|
||||
The denoise, detection, ROI and plane-fit algorithms are all unchanged;
|
||||
this module only fills in the knobs whose *correct value depends on the
|
||||
panel size and the point spacing*, so that the "CAD STP + cloud + ROI"
|
||||
input contract is self-sufficient and does not need a hand-tuned profile
|
||||
per panel.
|
||||
|
||||
Why it is needed
|
||||
----------------
|
||||
|
||||
The shipped ``default.yaml`` is tuned for a small, densely-sampled panel.
|
||||
Two groups of knobs break on a large, sparsely-scanned panel (e.g. the
|
||||
lu1 coupler panel: r≈16 mm locating holes, ~3-4 mm point spacing over a
|
||||
~1.5 m face):
|
||||
|
||||
1. **Denoise radius filter** ``{type: radius, nb_points: 8, radius_mm: 2}``
|
||||
demands 8 neighbours within 2 mm. At 3.7 mm spacing *every* point fails
|
||||
that test, so the whole cloud is deleted → ``too few points: 0``.
|
||||
|
||||
2. **Detection grid / area band** (``grid_pixel_mm=0.5``,
|
||||
``hole_*_area_mm2=[30,800]``, ``locating_hole_radius_mm=[4,12]``) make
|
||||
the occupancy grid too fine and the cavity-area window too small, so the
|
||||
locating holes never form detectable cavities →
|
||||
``only N hole-shaped cavities found``.
|
||||
|
||||
Two-stage application (around denoise)
|
||||
--------------------------------------
|
||||
|
||||
The two groups must be tuned at *different* moments because the point
|
||||
spacing changes across denoising:
|
||||
|
||||
- :func:`relax_denoise_steps` runs **before** denoising. It drops the
|
||||
density-fragile radius filter (the statistical filter still removes
|
||||
outliers), so a sparse cloud survives.
|
||||
- :func:`detection_overlay` runs **after** denoising, measuring the point
|
||||
spacing on the *denoised* cloud — the same cloud detection will see —
|
||||
and sizes the grid / fill / area band from the CAD locating-hole radius
|
||||
plus that spacing.
|
||||
|
||||
Together they reproduce, to within a few percent, the values previously
|
||||
hand-tuned in ``configs/profiles/lu1_af4.yaml``.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import copy
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def median_nn_spacing(points: np.ndarray, *, max_samples: int = 5000) -> float:
|
||||
"""Median nearest-neighbour distance, sub-sampled for speed (deterministic)."""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n = pts.shape[0]
|
||||
if n < 2:
|
||||
return 1.0
|
||||
try:
|
||||
from scipy.spatial import cKDTree
|
||||
except ImportError:
|
||||
span = float(np.linalg.norm(pts.max(axis=0) - pts.min(axis=0)))
|
||||
return max(span / max(n ** (1.0 / 3.0), 1.0), 1e-6)
|
||||
|
||||
step = max(1, n // max_samples)
|
||||
probe = pts[::step]
|
||||
tree = cKDTree(pts)
|
||||
d, _ = tree.query(probe, k=2)
|
||||
return float(np.median(d[:, 1]))
|
||||
|
||||
|
||||
def relax_denoise_steps(denoise_cfg: dict[str, Any]) -> dict[str, Any]:
|
||||
"""Return a denoise cfg with the density-fragile ``radius`` filter removed.
|
||||
|
||||
The radius outlier filter deletes everything on a cloud sparser than its
|
||||
``radius_mm``; the statistical filter already handles outliers, so for an
|
||||
auto-tuned run we simply drop the radius step (this matches the proven
|
||||
lu1_af4 denoise recipe). Other steps are kept as-is.
|
||||
"""
|
||||
out = copy.deepcopy(denoise_cfg) if denoise_cfg else {}
|
||||
steps = out.get("steps", [])
|
||||
out["steps"] = [s for s in steps if s.get("type") != "radius"]
|
||||
return out
|
||||
|
||||
|
||||
def detection_overlay(locating_hole_radius_mm: float, points: np.ndarray) -> dict[str, Any]:
|
||||
"""Detection sub-config derived from locating-hole radius + DENOISED spacing.
|
||||
|
||||
``points`` must be the cloud detection will actually run on (i.e. the
|
||||
denoised cloud), because the fill-gap threshold is sized from its point
|
||||
spacing.
|
||||
"""
|
||||
r = float(locating_hole_radius_mm)
|
||||
nn = median_nn_spacing(points)
|
||||
# Grid fine enough to resolve a locating hole (~16 px across its
|
||||
# diameter), but never finer than 0.5 mm.
|
||||
pixel = round(max(r / 8.0, 0.5), 2)
|
||||
# Fill background gaps up to a few point-spacings so sub-sampled panel
|
||||
# regions read as solid rather than as spurious cavities.
|
||||
fill_bg = round((2.5 * nn) ** 2, 1)
|
||||
a_loc = np.pi * r * r
|
||||
return {
|
||||
"grid_pixel_mm": pixel,
|
||||
"fill_bg_below_mm2": fill_bg,
|
||||
"panel_min_area_ratio": 0.15,
|
||||
"hole_min_area_mm2": round(0.30 * a_loc, 1),
|
||||
"hole_max_area_mm2": round(2.2 * a_loc, 1),
|
||||
"locating_hole_radius_mm": [round(r * 0.80, 2), round(r * 1.45, 2)],
|
||||
"panel_normal_seed": "pca",
|
||||
}
|
||||
|
||||
|
||||
__all__ = ["median_nn_spacing", "relax_denoise_steps", "detection_overlay"]
|
||||
@@ -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,129 @@
|
||||
"""Isolate a single panel layer from a multi-layer ROI cloud.
|
||||
|
||||
A ROI box cut out of a real vehicle scan can slice through several
|
||||
structures stacked along the panel normal — e.g. the big base plate, a
|
||||
raised central mounting plate, and a roof truss behind them. The 4-hole
|
||||
detector and the plane fit both assume a *single* thin plate; feeding
|
||||
them the multi-layer cloud makes holes fill in (back-layer points project
|
||||
into the front holes) and pollutes the plane fit.
|
||||
|
||||
This module keeps only the points belonging to one layer along the panel
|
||||
normal, selected by a 1-D histogram of signed distance to a robust plane
|
||||
seed (PCA normal by default, or a supplied CAD normal).
|
||||
|
||||
Pure geometry — it does not touch the detection / plane-fit algorithms.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class LayerResult:
|
||||
points: np.ndarray # kept layer points
|
||||
normal: np.ndarray # (3,) unit normal used, n_x > 0
|
||||
center_signed: float # signed-distance center of the kept layer
|
||||
half_width_mm: float
|
||||
diagnostics: dict[str, Any]
|
||||
|
||||
|
||||
def _pca_normal(points: np.ndarray) -> np.ndarray:
|
||||
c = points - points.mean(axis=0)
|
||||
cov = (c.T @ c) / max(points.shape[0] - 1, 1)
|
||||
_, V = np.linalg.eigh(cov)
|
||||
n = V[:, 0]
|
||||
if n[0] < 0:
|
||||
n = -n
|
||||
return n / max(np.linalg.norm(n), 1e-12)
|
||||
|
||||
|
||||
def select_panel_layer(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
normal_seed: np.ndarray | str = "pca",
|
||||
half_width_mm: float = 6.0,
|
||||
bins: int = 240,
|
||||
pick: str = "frontmost",
|
||||
) -> LayerResult:
|
||||
"""Keep one layer of ``points`` along the panel normal.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
normal_seed : (3,) or "pca"
|
||||
Plane normal used to compute signed distances. ``"pca"`` estimates
|
||||
it from the cloud (smallest-variance axis).
|
||||
half_width_mm : float
|
||||
Half-thickness of the kept slab around the chosen layer center.
|
||||
bins : int
|
||||
Histogram resolution for finding layer peaks.
|
||||
pick : "frontmost" | "densest"
|
||||
``"frontmost"`` keeps the layer with the largest signed distance
|
||||
(the face toward the scanner, +normal side) among the dominant
|
||||
peaks — this is the raised plate the user wants. ``"densest"``
|
||||
keeps the single most-populated layer.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
if pts.shape[0] == 0:
|
||||
raise ValueError("empty cloud")
|
||||
|
||||
if isinstance(normal_seed, str):
|
||||
if normal_seed != "pca":
|
||||
raise ValueError(f"unknown normal_seed '{normal_seed}'")
|
||||
normal = _pca_normal(pts)
|
||||
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
|
||||
|
||||
sd = pts @ normal
|
||||
hist, edges = np.histogram(sd, bins=bins)
|
||||
centers = 0.5 * (edges[:-1] + edges[1:])
|
||||
|
||||
# Candidate layers = histogram peaks holding a meaningful share of points.
|
||||
thresh = 0.25 * hist.max()
|
||||
peak_idx = [i for i in range(len(hist)) if hist[i] >= thresh]
|
||||
# group contiguous bins into peaks; take each group's argmax center
|
||||
groups: list[list[int]] = []
|
||||
for i in peak_idx:
|
||||
if groups and i == groups[-1][-1] + 1:
|
||||
groups[-1].append(i)
|
||||
else:
|
||||
groups.append([i])
|
||||
peak_centers = []
|
||||
for g in groups:
|
||||
gi = max(g, key=lambda i: hist[i])
|
||||
peak_centers.append((centers[gi], int(hist[gi])))
|
||||
|
||||
if not peak_centers:
|
||||
peak_centers = [(centers[int(np.argmax(hist))], int(hist.max()))]
|
||||
|
||||
if pick == "densest":
|
||||
chosen = max(peak_centers, key=lambda c: c[1])[0]
|
||||
else: # frontmost = max signed distance (toward +normal / scanner)
|
||||
chosen = max(peak_centers, key=lambda c: c[0])[0]
|
||||
|
||||
mask = np.abs(sd - chosen) <= half_width_mm
|
||||
kept = pts[mask]
|
||||
|
||||
return LayerResult(
|
||||
points=kept,
|
||||
normal=normal,
|
||||
center_signed=float(chosen),
|
||||
half_width_mm=float(half_width_mm),
|
||||
diagnostics={
|
||||
"n_in": int(pts.shape[0]),
|
||||
"n_out": int(kept.shape[0]),
|
||||
"normal": [float(v) for v in normal],
|
||||
"layer_center_signed": float(chosen),
|
||||
"peak_centers": [[float(c), int(n)] for c, n in peak_centers],
|
||||
"pick": pick,
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["LayerResult", "select_panel_layer"]
|
||||
@@ -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"]
|
||||
@@ -0,0 +1,55 @@
|
||||
"""Public exception hierarchy for coupler_panel.
|
||||
|
||||
Catch :class:`CouplerError` for any algorithm failure that should be
|
||||
surfaced to the user. Subclasses indicate what specifically went wrong so
|
||||
callers can render targeted error messages.
|
||||
|
||||
Non-fatal data-quality issues are **not** raised; they are reported via
|
||||
``CouplerMeasurement.quality_flags``.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
|
||||
class CouplerError(Exception):
|
||||
"""Base class for all errors raised by the coupler-panel pipeline."""
|
||||
|
||||
|
||||
class InvalidPointCloudError(CouplerError):
|
||||
"""Raised when the input point cloud is malformed or unreadable."""
|
||||
|
||||
|
||||
class UnsupportedFormatError(InvalidPointCloudError):
|
||||
"""Raised when the input file extension is not supported."""
|
||||
|
||||
|
||||
class InsufficientPointsError(CouplerError):
|
||||
"""Raised when the input has too few points to run the pipeline."""
|
||||
|
||||
|
||||
class CADParseError(CouplerError):
|
||||
"""Raised when the STP file cannot be parsed or has unexpected topology."""
|
||||
|
||||
|
||||
class HoleDetectionError(CouplerError):
|
||||
"""Raised when the four locating holes cannot be reliably detected."""
|
||||
|
||||
|
||||
class RegistrationError(CouplerError):
|
||||
"""Raised when 4-hole Kabsch registration fails (e.g. degenerate pairing)."""
|
||||
|
||||
|
||||
class PlaneFitError(CouplerError):
|
||||
"""Raised when the panel plane cannot be fitted (too few inliers, etc.)."""
|
||||
|
||||
|
||||
__all__ = [
|
||||
"CouplerError",
|
||||
"InvalidPointCloudError",
|
||||
"UnsupportedFormatError",
|
||||
"InsufficientPointsError",
|
||||
"CADParseError",
|
||||
"HoleDetectionError",
|
||||
"RegistrationError",
|
||||
"PlaneFitError",
|
||||
]
|
||||
@@ -0,0 +1,11 @@
|
||||
"""Geometry construction & plane fitting for the coupler panel."""
|
||||
|
||||
from .reference import build_reference_frame
|
||||
from .roi import extract_panel_roi
|
||||
from .plane_fit import fit_panel_plane
|
||||
|
||||
__all__ = [
|
||||
"build_reference_frame",
|
||||
"extract_panel_roi",
|
||||
"fit_panel_plane",
|
||||
]
|
||||
@@ -0,0 +1,113 @@
|
||||
"""Robust panel-plane fitting.
|
||||
|
||||
Pipeline:
|
||||
|
||||
1. RANSAC: pick 3 random points, build a plane, count inliers within
|
||||
``inlier_thresh_mm``. Repeat ``ransac_iterations`` times; keep the
|
||||
best inlier set.
|
||||
2. SVD: refit the plane on the inliers (eigenvector with the smallest
|
||||
eigenvalue of the centred covariance).
|
||||
3. Sign convention: enforce ``n_x > 0``.
|
||||
|
||||
Returns a :class:`PanelPlane`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import PlaneFitError
|
||||
from ..structures import PanelPlane
|
||||
|
||||
|
||||
def _plane_from_three(p1, p2, p3) -> tuple[np.ndarray, float] | None:
|
||||
n = np.cross(p2 - p1, p3 - p1)
|
||||
norm = np.linalg.norm(n)
|
||||
if norm < 1e-9:
|
||||
return None
|
||||
n = n / norm
|
||||
d = -float(np.dot(n, p1))
|
||||
return n, d
|
||||
|
||||
|
||||
def _svd_plane(points: np.ndarray) -> tuple[np.ndarray, float]:
|
||||
centroid = points.mean(axis=0)
|
||||
Q = points - centroid
|
||||
cov = (Q.T @ Q) / max(points.shape[0] - 1, 1)
|
||||
w, V = np.linalg.eigh(cov)
|
||||
n = V[:, 0]
|
||||
n = n / max(np.linalg.norm(n), 1e-12)
|
||||
d = -float(np.dot(n, centroid))
|
||||
return n, d
|
||||
|
||||
|
||||
def fit_panel_plane(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
plane_fit_cfg: dict[str, Any],
|
||||
seed: int | None = 7,
|
||||
) -> PanelPlane:
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n_pts = pts.shape[0]
|
||||
if n_pts < 3:
|
||||
raise PlaneFitError(f"need ≥ 3 points, got {n_pts}")
|
||||
|
||||
iterations = int(plane_fit_cfg.get("ransac_iterations", 200))
|
||||
thresh = plane_fit_cfg.get("ransac_inlier_thresh_mm", 1.5)
|
||||
if thresh in (None, "auto"):
|
||||
thresh = 1.5
|
||||
thresh = float(thresh)
|
||||
min_inlier_ratio = float(plane_fit_cfg.get("min_inlier_ratio", 0.6))
|
||||
enforce_nx_positive = bool(plane_fit_cfg.get("enforce_nx_positive", True))
|
||||
|
||||
rng = np.random.default_rng(seed)
|
||||
|
||||
best_inliers: np.ndarray | None = None
|
||||
best_count = -1
|
||||
|
||||
for _ in range(iterations):
|
||||
sample = rng.choice(n_pts, size=3, replace=False)
|
||||
plane = _plane_from_three(pts[sample[0]], pts[sample[1]], pts[sample[2]])
|
||||
if plane is None:
|
||||
continue
|
||||
n, d = plane
|
||||
dist = np.abs(pts @ n + d)
|
||||
inliers = dist <= thresh
|
||||
c = int(inliers.sum())
|
||||
if c > best_count:
|
||||
best_count = c
|
||||
best_inliers = inliers
|
||||
|
||||
if best_inliers is None or best_count < 3:
|
||||
raise PlaneFitError(
|
||||
f"RANSAC found {best_count} inliers in {iterations} iters at "
|
||||
f"thresh={thresh} mm; check noise level / threshold"
|
||||
)
|
||||
|
||||
inlier_ratio = best_count / n_pts
|
||||
inlier_pts = pts[best_inliers]
|
||||
n, d = _svd_plane(inlier_pts)
|
||||
if enforce_nx_positive and n[0] < 0:
|
||||
n = -n
|
||||
d = -d
|
||||
|
||||
residuals = inlier_pts @ n + d
|
||||
rms = float(np.sqrt(np.mean(residuals ** 2)))
|
||||
|
||||
if inlier_ratio < min_inlier_ratio:
|
||||
# Caller will log a quality flag; we still return the result.
|
||||
pass
|
||||
|
||||
centroid = inlier_pts.mean(axis=0)
|
||||
return PanelPlane(
|
||||
normal=tuple(n.tolist()),
|
||||
point_on_plane=tuple(centroid.tolist()),
|
||||
d_offset=float(d),
|
||||
inlier_ratio=float(inlier_ratio),
|
||||
inlier_residual_rms_mm=rms,
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["fit_panel_plane"]
|
||||
@@ -0,0 +1,32 @@
|
||||
"""Construct the reference frame from four hole centres.
|
||||
|
||||
Given the four locating-hole centres :math:`H_1 .. H_4`:
|
||||
|
||||
- Panel centre :math:`C` = mean of the four centres (= diagonal
|
||||
intersection for a rectangle).
|
||||
- Reference line :math:`L = C + \\lambda \\hat e_x`.
|
||||
- Reference plane: :math:`z = C_z`.
|
||||
- Longitudinal mid-plane: :math:`y = C_y`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..structures import HoleSet, ReferenceFrame
|
||||
|
||||
|
||||
def build_reference_frame(holes: HoleSet) -> ReferenceFrame:
|
||||
centers = holes.locating_centers()
|
||||
if centers.shape != (4, 3):
|
||||
raise ValueError(f"need 4 locating-hole centres, got shape {centers.shape}")
|
||||
C = centers.mean(axis=0)
|
||||
return ReferenceFrame(
|
||||
center_C=tuple(C.tolist()),
|
||||
line_direction=(1.0, 0.0, 0.0),
|
||||
reference_plane_z=float(C[2]),
|
||||
longitudinal_mid_y=float(C[1]),
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["build_reference_frame"]
|
||||
@@ -0,0 +1,159 @@
|
||||
"""Extract the *clean* panel ROI from the registered point cloud.
|
||||
|
||||
Strategy:
|
||||
|
||||
1. Take points whose 2D projection falls inside the four-hole rectangle
|
||||
(shrunk inwards by ``edge_buffer_mm``).
|
||||
2. Drop points within ``hole_dilate_mm`` of any locating-hole centre
|
||||
(radius + dilation).
|
||||
3. Drop points within ``central_hole_dilate_mm`` of the central hole.
|
||||
4. Drop chamfer points (per-point normal deviates from panel normal by
|
||||
more than ``acos(chamfer_normal_dot_min)``). Normals are estimated
|
||||
on the fly via PCA on each point's k-neighbourhood.
|
||||
5. Drop z-score outliers along the panel normal.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..structures import HoleSet
|
||||
|
||||
|
||||
def _kth_normals(points: np.ndarray, k: int = 20) -> np.ndarray:
|
||||
"""Per-point normal via PCA on the k-NN. Returns ``(N, 3)``."""
|
||||
n = points.shape[0]
|
||||
if n < k + 1:
|
||||
return np.tile(np.array([1.0, 0.0, 0.0]), (n, 1))
|
||||
try:
|
||||
from scipy.spatial import cKDTree
|
||||
except ImportError:
|
||||
return np.tile(np.array([1.0, 0.0, 0.0]), (n, 1))
|
||||
|
||||
tree = cKDTree(points)
|
||||
_, idx = tree.query(points, k=k + 1)
|
||||
normals = np.empty((n, 3), dtype=np.float64)
|
||||
for i in range(n):
|
||||
nbh = points[idx[i, 1:]]
|
||||
c = nbh.mean(axis=0)
|
||||
Q = nbh - c
|
||||
cov = (Q.T @ Q) / max(k - 1, 1)
|
||||
try:
|
||||
w, V = np.linalg.eigh(cov)
|
||||
normals[i] = V[:, 0]
|
||||
except np.linalg.LinAlgError:
|
||||
normals[i] = np.array([1.0, 0.0, 0.0])
|
||||
return normals
|
||||
|
||||
|
||||
def _signed_to_plane(points: np.ndarray, normal: np.ndarray, offset: float) -> np.ndarray:
|
||||
"""Signed distance from each point to plane n·x + d = 0."""
|
||||
return points @ normal + offset
|
||||
|
||||
|
||||
def extract_panel_roi(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
holes: HoleSet,
|
||||
panel_normal_seed: np.ndarray,
|
||||
roi_cfg: dict[str, Any],
|
||||
) -> tuple[np.ndarray, dict[str, Any]]:
|
||||
"""Return (ROI points, diagnostics dict).
|
||||
|
||||
``holes`` and ``panel_normal_seed`` are expected in the *same* frame
|
||||
as ``points`` — registration must already have happened before this
|
||||
is called.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
if pts.shape[0] == 0:
|
||||
return pts.copy(), {"n_in": 0, "n_out": 0, "stages": []}
|
||||
|
||||
n = np.asarray(panel_normal_seed, dtype=np.float64)
|
||||
n = n / max(np.linalg.norm(n), 1e-12)
|
||||
if n[0] < 0:
|
||||
n = -n
|
||||
|
||||
centers_3d = holes.locating_centers() # (4, 3)
|
||||
C = centers_3d.mean(axis=0)
|
||||
|
||||
# Build local 2D frame to test "inside the rectangle".
|
||||
y_world = np.array([0.0, 1.0, 0.0])
|
||||
u = y_world - np.dot(y_world, n) * n
|
||||
if np.linalg.norm(u) < 1e-6:
|
||||
u = np.array([0.0, 0.0, 1.0]) - np.dot(np.array([0.0, 0.0, 1.0]), n) * n
|
||||
u /= np.linalg.norm(u)
|
||||
v = np.cross(n, u)
|
||||
v /= np.linalg.norm(v)
|
||||
|
||||
rel = pts - C
|
||||
uv = np.column_stack([rel @ u, rel @ v])
|
||||
centers_uv = np.column_stack([(centers_3d - C) @ u, (centers_3d - C) @ v])
|
||||
|
||||
edge_buf = float(roi_cfg.get("edge_buffer_mm", 5.0))
|
||||
u_lo = centers_uv[:, 0].min() + edge_buf
|
||||
u_hi = centers_uv[:, 0].max() - edge_buf
|
||||
v_lo = centers_uv[:, 1].min() + edge_buf
|
||||
v_hi = centers_uv[:, 1].max() - edge_buf
|
||||
|
||||
stages: list[dict[str, Any]] = []
|
||||
n_in = pts.shape[0]
|
||||
|
||||
mask = (uv[:, 0] >= u_lo) & (uv[:, 0] <= u_hi) & (uv[:, 1] >= v_lo) & (uv[:, 1] <= v_hi)
|
||||
stages.append({"name": "rectangle", "n_kept": int(mask.sum())})
|
||||
|
||||
# 2. Locating hole exclusion (use 3D distance to each hole's centre,
|
||||
# with the hole's own radius + dilation).
|
||||
hole_dilate = float(roi_cfg.get("hole_dilate_mm", 3.0))
|
||||
for h in holes.locating:
|
||||
center = np.asarray(h.center)
|
||||
r = float(h.radius_mm) + hole_dilate
|
||||
d2 = np.sum((pts - center) ** 2, axis=1)
|
||||
mask &= d2 > r * r
|
||||
stages.append({"name": "locating_holes", "n_kept": int(mask.sum())})
|
||||
|
||||
# 3. Central hole exclusion.
|
||||
if holes.central is not None:
|
||||
center = np.asarray(holes.central.center)
|
||||
r = float(holes.central.radius_mm) + float(roi_cfg.get("central_hole_dilate_mm", 5.0))
|
||||
d2 = np.sum((pts - center) ** 2, axis=1)
|
||||
mask &= d2 > r * r
|
||||
stages.append({"name": "central_hole", "n_kept": int(mask.sum())})
|
||||
|
||||
# 4. Chamfer/normal filter — only run if we still have enough points to PCA.
|
||||
chamfer_dot_min = float(roi_cfg.get("chamfer_normal_dot_min", 0.85))
|
||||
if mask.sum() > 50 and chamfer_dot_min > 0:
|
||||
kept = pts[mask]
|
||||
normals = _kth_normals(kept, k=20)
|
||||
# Align all normals so they point along +n_panel (sign-invariant).
|
||||
flips = (normals @ n) < 0
|
||||
normals[flips] = -normals[flips]
|
||||
keep_local = (normals @ n) >= chamfer_dot_min
|
||||
# Map back into the full mask.
|
||||
idxs = np.where(mask)[0]
|
||||
new_mask = np.zeros_like(mask)
|
||||
new_mask[idxs[keep_local]] = True
|
||||
mask = new_mask
|
||||
stages.append({"name": "chamfer_normal", "n_kept": int(mask.sum())})
|
||||
|
||||
# 5. Z-score outlier rejection along panel normal.
|
||||
z_score = float(roi_cfg.get("outlier_z_score", 3.0))
|
||||
if mask.sum() > 50 and z_score > 0:
|
||||
kept = pts[mask]
|
||||
d = kept @ n
|
||||
mu = d.mean()
|
||||
sigma = d.std()
|
||||
if sigma > 1e-12:
|
||||
keep_local = np.abs(d - mu) <= z_score * sigma
|
||||
idxs = np.where(mask)[0]
|
||||
new_mask = np.zeros_like(mask)
|
||||
new_mask[idxs[keep_local]] = True
|
||||
mask = new_mask
|
||||
stages.append({"name": "z_score", "n_kept": int(mask.sum())})
|
||||
|
||||
out = pts[mask]
|
||||
return out, {"n_in": int(n_in), "n_out": int(out.shape[0]), "stages": stages}
|
||||
|
||||
|
||||
__all__ = ["extract_panel_roi"]
|
||||
@@ -0,0 +1,27 @@
|
||||
"""Point-cloud / artifact IO utilities for coupler_panel."""
|
||||
|
||||
from .pointcloud_io import (
|
||||
load_point_cloud,
|
||||
write_point_cloud,
|
||||
write_metadata_json,
|
||||
)
|
||||
from .ply_stream import crop_ply_bbox
|
||||
from .artifacts import (
|
||||
ensure_dir,
|
||||
write_json,
|
||||
write_yaml,
|
||||
write_csv_rows,
|
||||
Artifacts,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"load_point_cloud",
|
||||
"write_point_cloud",
|
||||
"write_metadata_json",
|
||||
"crop_ply_bbox",
|
||||
"ensure_dir",
|
||||
"write_json",
|
||||
"write_yaml",
|
||||
"write_csv_rows",
|
||||
"Artifacts",
|
||||
]
|
||||
@@ -0,0 +1,168 @@
|
||||
"""Artifact directory layout for one pipeline run.
|
||||
|
||||
A run directory looks like::
|
||||
|
||||
runs/<ts>/
|
||||
├── 00_cad_mesh.ply
|
||||
├── 01_gt.json
|
||||
├── 02_sampled.ply
|
||||
├── 03_noisy.ply
|
||||
├── 04_transformed.ply
|
||||
├── 05_denoised.ply
|
||||
├── 06_registered.ply
|
||||
├── 07_holes.json
|
||||
├── 08_panel_roi.ply
|
||||
├── 09_plane.json
|
||||
├── 10_metrics.json
|
||||
├── images/
|
||||
├── config.resolved.yaml
|
||||
└── report.json
|
||||
|
||||
Most modules write through the :class:`Artifacts` helper so naming stays
|
||||
consistent.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Iterable, Sequence
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Generic helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def ensure_dir(path: str | Path) -> Path:
|
||||
p = Path(path)
|
||||
p.mkdir(parents=True, exist_ok=True)
|
||||
return p
|
||||
|
||||
|
||||
def write_json(data: Any, path: str | Path) -> Path:
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", encoding="utf-8") as fh:
|
||||
json.dump(data, fh, ensure_ascii=False, indent=2, default=_json_default)
|
||||
return path
|
||||
|
||||
|
||||
def write_yaml(data: Any, path: str | Path) -> Path:
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", encoding="utf-8") as fh:
|
||||
yaml.safe_dump(_yaml_default(data), fh, allow_unicode=True, sort_keys=False)
|
||||
return path
|
||||
|
||||
|
||||
def write_csv_rows(
|
||||
rows: Iterable[Sequence[Any]],
|
||||
path: str | Path,
|
||||
*,
|
||||
header: Sequence[str] | None = None,
|
||||
) -> Path:
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", encoding="utf-8", newline="") as fh:
|
||||
w = csv.writer(fh)
|
||||
if header is not None:
|
||||
w.writerow(header)
|
||||
for row in rows:
|
||||
w.writerow(row)
|
||||
return path
|
||||
|
||||
|
||||
def _json_default(obj: Any) -> Any:
|
||||
if isinstance(obj, np.ndarray):
|
||||
return obj.tolist()
|
||||
if isinstance(obj, (np.floating, np.integer)):
|
||||
return obj.item()
|
||||
if isinstance(obj, Path):
|
||||
return str(obj)
|
||||
raise TypeError(f"not JSON serialisable: {type(obj).__name__}")
|
||||
|
||||
|
||||
def _yaml_default(obj: Any) -> Any:
|
||||
if isinstance(obj, dict):
|
||||
return {k: _yaml_default(v) for k, v in obj.items()}
|
||||
if isinstance(obj, (list, tuple)):
|
||||
return [_yaml_default(v) for v in obj]
|
||||
if isinstance(obj, np.ndarray):
|
||||
return obj.tolist()
|
||||
if isinstance(obj, (np.floating, np.integer)):
|
||||
return obj.item()
|
||||
return obj
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Stage-naming helper
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
@dataclass
|
||||
class Artifacts:
|
||||
"""Resolves stage filenames inside a run directory.
|
||||
|
||||
Use :meth:`path` to get the canonical name for a given stage:
|
||||
|
||||
>>> art = Artifacts("runs/demo")
|
||||
>>> art.path("sampled", ext="ply")
|
||||
PosixPath('runs/demo/02_sampled.ply')
|
||||
"""
|
||||
|
||||
root: Path
|
||||
|
||||
_STAGE_INDEX: dict[str, int] = None # type: ignore[assignment]
|
||||
|
||||
# Canonical stage ordering. Keys here are referenced from pipeline.py.
|
||||
STAGE_ORDER = (
|
||||
"cad_mesh", # 00
|
||||
"gt", # 01
|
||||
"sampled", # 02
|
||||
"noisy", # 03
|
||||
"transformed", # 04
|
||||
"denoised", # 05
|
||||
"registered", # 06
|
||||
"holes", # 07
|
||||
"panel_roi", # 08
|
||||
"plane", # 09
|
||||
"metrics", # 10
|
||||
)
|
||||
|
||||
def __init__(self, root: str | Path) -> None:
|
||||
self.root = ensure_dir(root)
|
||||
ensure_dir(self.root / "images")
|
||||
self._STAGE_INDEX = {name: i for i, name in enumerate(self.STAGE_ORDER)}
|
||||
|
||||
def path(self, stage: str, *, ext: str) -> Path:
|
||||
idx = self._STAGE_INDEX.get(stage)
|
||||
if idx is None:
|
||||
raise KeyError(f"unknown stage '{stage}'; valid: {list(self._STAGE_INDEX)}")
|
||||
ext = ext.lstrip(".")
|
||||
return self.root / f"{idx:02d}_{stage}.{ext}"
|
||||
|
||||
def image(self, name: str) -> Path:
|
||||
return self.root / "images" / name
|
||||
|
||||
@property
|
||||
def report_json(self) -> Path:
|
||||
return self.root / "report.json"
|
||||
|
||||
@property
|
||||
def resolved_config(self) -> Path:
|
||||
return self.root / "config.resolved.yaml"
|
||||
|
||||
|
||||
__all__ = [
|
||||
"ensure_dir",
|
||||
"write_json",
|
||||
"write_yaml",
|
||||
"write_csv_rows",
|
||||
"Artifacts",
|
||||
]
|
||||
@@ -0,0 +1,165 @@
|
||||
"""Streaming AABB crop for large binary PLY point clouds.
|
||||
|
||||
A full-vehicle scan PLY can be 1-12 GB. Loading it whole to apply a
|
||||
region-of-interest box would blow up memory, so this module parses the
|
||||
PLY header, memory-maps the vertex block, and filters it in chunks,
|
||||
keeping only points inside the ROI bounding box.
|
||||
|
||||
Supports binary little-/big-endian PLY with a single ``element vertex``
|
||||
block whose first three float properties are x, y, z (extra properties
|
||||
such as colours or normals are tolerated — the per-vertex stride is read
|
||||
from the header). ASCII PLY and other layouts fall back to the normal
|
||||
loader in :mod:`coupler_panel.io_utils.pointcloud_io`.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import InvalidPointCloudError
|
||||
|
||||
|
||||
# PLY property type -> (numpy dtype char, byte size)
|
||||
_PLY_TYPES = {
|
||||
"char": ("i1", 1), "int8": ("i1", 1),
|
||||
"uchar": ("u1", 1), "uint8": ("u1", 1),
|
||||
"short": ("i2", 2), "int16": ("i2", 2),
|
||||
"ushort": ("u2", 2), "uint16": ("u2", 2),
|
||||
"int": ("i4", 4), "int32": ("i4", 4),
|
||||
"uint": ("u4", 4), "uint32": ("u4", 4),
|
||||
"float": ("f4", 4), "float32": ("f4", 4),
|
||||
"double": ("f8", 8), "float64": ("f8", 8),
|
||||
}
|
||||
|
||||
|
||||
@dataclass
|
||||
class _PlyHeader:
|
||||
fmt: str # "binary_little_endian" | "binary_big_endian" | "ascii"
|
||||
n_vertices: int
|
||||
data_offset: int # byte offset where vertex data starts
|
||||
props: list[tuple[str, str]] # ordered (name, type) for the vertex element
|
||||
|
||||
|
||||
def _parse_header(path: Path) -> _PlyHeader:
|
||||
fmt = None
|
||||
n_vertices = 0
|
||||
props: list[tuple[str, str]] = []
|
||||
in_vertex = False
|
||||
# Read a generous prefix; PLY headers are tiny (well under 64 KB).
|
||||
with path.open("rb") as f:
|
||||
prefix = f.read(65536)
|
||||
marker = b"end_header"
|
||||
mpos = prefix.find(marker)
|
||||
if mpos < 0:
|
||||
raise InvalidPointCloudError(f"{path}: no end_header in first 64KB")
|
||||
# data starts right after the newline(s) following 'end_header'
|
||||
after = prefix[mpos + len(marker):]
|
||||
if after.startswith(b"\r\n"):
|
||||
nl_len = 2
|
||||
elif after.startswith(b"\n") or after.startswith(b"\r"):
|
||||
nl_len = 1
|
||||
else:
|
||||
nl_len = 0
|
||||
data_offset = mpos + len(marker) + nl_len
|
||||
|
||||
text = prefix[:mpos].decode("ascii", errors="replace")
|
||||
lines = text.splitlines()
|
||||
for ln in lines:
|
||||
parts = ln.split()
|
||||
if not parts:
|
||||
continue
|
||||
if parts[0] == "format":
|
||||
fmt = parts[1]
|
||||
elif parts[0] == "element":
|
||||
in_vertex = parts[1] == "vertex"
|
||||
if in_vertex:
|
||||
n_vertices = int(parts[2])
|
||||
elif parts[0] == "property" and in_vertex:
|
||||
# property <type> <name> (lists not supported for vertex)
|
||||
if parts[1] == "list":
|
||||
raise InvalidPointCloudError(f"{path}: list property in vertex unsupported")
|
||||
props.append((parts[-1], parts[1]))
|
||||
elif parts[0] == "element" and not in_vertex:
|
||||
in_vertex = False
|
||||
|
||||
if fmt is None or n_vertices <= 0 or not props:
|
||||
raise InvalidPointCloudError(f"{path}: malformed PLY header")
|
||||
return _PlyHeader(fmt=fmt, n_vertices=n_vertices, data_offset=data_offset, props=props)
|
||||
|
||||
|
||||
def crop_ply_bbox(
|
||||
path: str | Path,
|
||||
*,
|
||||
x_min: float, x_max: float,
|
||||
y_min: float, y_max: float,
|
||||
z_min: float, z_max: float,
|
||||
chunk: int = 10_000_000,
|
||||
) -> tuple[np.ndarray, dict[str, Any]]:
|
||||
"""Stream-crop a binary PLY to an AABB. Returns ``(points (M,3), diag)``.
|
||||
|
||||
Falls back to a full load (via the standard reader) for ASCII PLY or
|
||||
headers this fast path can't handle.
|
||||
"""
|
||||
path = Path(path)
|
||||
if not path.exists():
|
||||
raise InvalidPointCloudError(f"point cloud not found: {path}")
|
||||
|
||||
try:
|
||||
hdr = _parse_header(path)
|
||||
except InvalidPointCloudError:
|
||||
hdr = None
|
||||
|
||||
if hdr is None or not hdr.fmt.startswith("binary"):
|
||||
# Fallback: load whole then filter (ASCII / unknown layout).
|
||||
from .pointcloud_io import load_point_cloud
|
||||
pts = load_point_cloud(path)
|
||||
m = ((pts[:, 0] >= x_min) & (pts[:, 0] <= x_max)
|
||||
& (pts[:, 1] >= y_min) & (pts[:, 1] <= y_max)
|
||||
& (pts[:, 2] >= z_min) & (pts[:, 2] <= z_max))
|
||||
out = pts[m]
|
||||
return out, {"mode": "fallback_full_load", "n_in": int(pts.shape[0]),
|
||||
"n_out": int(out.shape[0])}
|
||||
|
||||
# Build a structured dtype matching the vertex layout.
|
||||
byteorder = "<" if "little" in hdr.fmt else ">"
|
||||
names = [p[0] for p in hdr.props]
|
||||
formats = []
|
||||
for _, t in hdr.props:
|
||||
if t not in _PLY_TYPES:
|
||||
raise InvalidPointCloudError(f"{path}: unsupported property type {t}")
|
||||
formats.append(byteorder + _PLY_TYPES[t][0])
|
||||
dt = np.dtype({"names": names, "formats": formats})
|
||||
if not all(c in names for c in ("x", "y", "z")):
|
||||
raise InvalidPointCloudError(f"{path}: vertex has no x/y/z properties")
|
||||
|
||||
mm = np.memmap(path, dtype=dt, mode="r", offset=hdr.data_offset, shape=(hdr.n_vertices,))
|
||||
|
||||
kept = []
|
||||
n_in = hdr.n_vertices
|
||||
for start in range(0, n_in, chunk):
|
||||
end = min(start + chunk, n_in)
|
||||
blk = mm[start:end]
|
||||
x = np.asarray(blk["x"], dtype=np.float64)
|
||||
y = np.asarray(blk["y"], dtype=np.float64)
|
||||
z = np.asarray(blk["z"], dtype=np.float64)
|
||||
m = ((x >= x_min) & (x <= x_max)
|
||||
& (y >= y_min) & (y <= y_max)
|
||||
& (z >= z_min) & (z <= z_max))
|
||||
if m.any():
|
||||
kept.append(np.column_stack([x[m], y[m], z[m]]))
|
||||
out = np.concatenate(kept, axis=0) if kept else np.zeros((0, 3))
|
||||
return out, {
|
||||
"mode": "streaming",
|
||||
"format": hdr.fmt,
|
||||
"n_in": int(n_in),
|
||||
"n_out": int(out.shape[0]),
|
||||
"bbox": {"x_min": x_min, "x_max": x_max, "y_min": y_min,
|
||||
"y_max": y_max, "z_min": z_min, "z_max": z_max},
|
||||
}
|
||||
|
||||
|
||||
__all__ = ["crop_ply_bbox"]
|
||||
@@ -0,0 +1,238 @@
|
||||
"""Point cloud read / write.
|
||||
|
||||
Supported formats:
|
||||
- ``.ply`` binary or ASCII (open3d if available, else minimal builtin reader)
|
||||
- ``.xyz`` whitespace-separated XYZ[ R G B][ NX NY NZ]
|
||||
- ``.txt`` alias for ``.xyz``
|
||||
- ``.csv`` comma-separated, first three columns must be X, Y, Z
|
||||
- ``.pcd`` open3d only
|
||||
|
||||
The XYZ reader also serves as a fallback for ``.ply`` if open3d is
|
||||
unavailable and the PLY is ASCII.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import InvalidPointCloudError, UnsupportedFormatError
|
||||
|
||||
|
||||
_TEXT_SUFFIXES = (".xyz", ".txt", ".csv")
|
||||
_OPEN3D_SUFFIXES = (".ply", ".pcd")
|
||||
|
||||
|
||||
def _try_import_open3d():
|
||||
try:
|
||||
import open3d as o3d # type: ignore
|
||||
|
||||
return o3d
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Loading
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def load_point_cloud(path: str | Path) -> np.ndarray:
|
||||
"""Load a point cloud as a float64 ``(N, 3)`` ndarray.
|
||||
|
||||
Raises
|
||||
------
|
||||
UnsupportedFormatError
|
||||
If the file extension is not supported.
|
||||
InvalidPointCloudError
|
||||
If the file is missing, empty, or fails to parse.
|
||||
"""
|
||||
path = Path(path)
|
||||
if not path.exists():
|
||||
raise InvalidPointCloudError(f"point cloud not found: {path}")
|
||||
|
||||
suffix = path.suffix.lower()
|
||||
if suffix in _TEXT_SUFFIXES:
|
||||
return _load_text(path, sep="," if suffix == ".csv" else None)
|
||||
|
||||
if suffix in _OPEN3D_SUFFIXES:
|
||||
o3d = _try_import_open3d()
|
||||
if o3d is not None:
|
||||
pcd = o3d.io.read_point_cloud(str(path))
|
||||
arr = np.asarray(pcd.points, dtype=np.float64)
|
||||
if arr.size == 0:
|
||||
# PLY may also be ASCII XYZ-style; fall back.
|
||||
if suffix == ".ply":
|
||||
return _load_ascii_ply_fallback(path)
|
||||
raise InvalidPointCloudError(f"empty point cloud: {path}")
|
||||
return arr
|
||||
if suffix == ".ply":
|
||||
return _load_ascii_ply_fallback(path)
|
||||
raise InvalidPointCloudError(
|
||||
f"open3d is required to read {suffix}; install with `pip install open3d`"
|
||||
)
|
||||
|
||||
raise UnsupportedFormatError(f"unsupported extension: {suffix}")
|
||||
|
||||
|
||||
def _load_text(path: Path, sep: str | None) -> np.ndarray:
|
||||
try:
|
||||
if sep is None:
|
||||
arr = np.loadtxt(path, comments=("#", "//"))
|
||||
else:
|
||||
arr = np.loadtxt(path, comments=("#", "//"), delimiter=sep)
|
||||
except Exception as exc:
|
||||
raise InvalidPointCloudError(f"failed to parse {path}: {exc}") from exc
|
||||
if arr.ndim == 1:
|
||||
arr = arr.reshape(1, -1)
|
||||
if arr.shape[1] < 3:
|
||||
raise InvalidPointCloudError(
|
||||
f"{path} has fewer than 3 columns (got {arr.shape[1]})"
|
||||
)
|
||||
return arr[:, :3].astype(np.float64, copy=False)
|
||||
|
||||
|
||||
def _load_ascii_ply_fallback(path: Path) -> np.ndarray:
|
||||
"""Minimal ASCII-PLY reader for the 'just XYZ' case."""
|
||||
with path.open("r", encoding="utf-8", errors="replace") as fh:
|
||||
header_done = False
|
||||
n_vertices = 0
|
||||
x_idx = y_idx = z_idx = -1
|
||||
prop_idx = 0
|
||||
format_line: str | None = None
|
||||
for line in fh:
|
||||
line = line.strip()
|
||||
if line == "end_header":
|
||||
header_done = True
|
||||
break
|
||||
if line.startswith("format"):
|
||||
format_line = line
|
||||
elif line.startswith("element vertex"):
|
||||
n_vertices = int(line.split()[-1])
|
||||
prop_idx = 0
|
||||
elif line.startswith("property"):
|
||||
name = line.split()[-1]
|
||||
if name == "x":
|
||||
x_idx = prop_idx
|
||||
elif name == "y":
|
||||
y_idx = prop_idx
|
||||
elif name == "z":
|
||||
z_idx = prop_idx
|
||||
prop_idx += 1
|
||||
if not header_done or n_vertices <= 0 or -1 in (x_idx, y_idx, z_idx):
|
||||
raise InvalidPointCloudError(
|
||||
f"unsupported PLY header in {path}; install open3d for full PLY support"
|
||||
)
|
||||
if format_line and "binary" in format_line:
|
||||
raise InvalidPointCloudError(
|
||||
f"{path} is binary PLY; install open3d to read it"
|
||||
)
|
||||
out = np.empty((n_vertices, 3), dtype=np.float64)
|
||||
for i in range(n_vertices):
|
||||
parts = fh.readline().split()
|
||||
out[i, 0] = float(parts[x_idx])
|
||||
out[i, 1] = float(parts[y_idx])
|
||||
out[i, 2] = float(parts[z_idx])
|
||||
return out
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Writing
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def write_point_cloud(
|
||||
points: np.ndarray,
|
||||
path: str | Path,
|
||||
*,
|
||||
colors: np.ndarray | None = None,
|
||||
binary: bool = True,
|
||||
) -> Path:
|
||||
"""Write a point cloud. Format is inferred from the extension.
|
||||
|
||||
For ``.ply``/``.pcd`` open3d is preferred; if open3d is missing and the
|
||||
target is ``.ply``, we fall back to writing ASCII PLY ourselves.
|
||||
"""
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
points = np.ascontiguousarray(points, dtype=np.float64)
|
||||
if points.ndim != 2 or points.shape[1] != 3:
|
||||
raise InvalidPointCloudError(
|
||||
f"expected (N,3) points, got shape {points.shape}"
|
||||
)
|
||||
|
||||
suffix = path.suffix.lower()
|
||||
if suffix in (".xyz", ".txt"):
|
||||
np.savetxt(path, points, fmt="%.6f")
|
||||
return path
|
||||
if suffix == ".csv":
|
||||
np.savetxt(path, points, fmt="%.6f", delimiter=",")
|
||||
return path
|
||||
|
||||
if suffix in _OPEN3D_SUFFIXES:
|
||||
o3d = _try_import_open3d()
|
||||
if o3d is not None:
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(points)
|
||||
if colors is not None:
|
||||
pcd.colors = o3d.utility.Vector3dVector(np.asarray(colors)[:, :3])
|
||||
o3d.io.write_point_cloud(str(path), pcd, write_ascii=not binary)
|
||||
return path
|
||||
if suffix == ".ply":
|
||||
_write_ascii_ply(points, path, colors=colors)
|
||||
return path
|
||||
raise InvalidPointCloudError(
|
||||
f"open3d is required to write {suffix}; install with `pip install open3d`"
|
||||
)
|
||||
|
||||
raise UnsupportedFormatError(f"unsupported extension: {suffix}")
|
||||
|
||||
|
||||
def _write_ascii_ply(
|
||||
points: np.ndarray,
|
||||
path: Path,
|
||||
colors: np.ndarray | None = None,
|
||||
) -> None:
|
||||
have_color = colors is not None
|
||||
n = points.shape[0]
|
||||
with path.open("w", encoding="ascii") as fh:
|
||||
fh.write("ply\nformat ascii 1.0\n")
|
||||
fh.write(f"element vertex {n}\n")
|
||||
fh.write("property float x\nproperty float y\nproperty float z\n")
|
||||
if have_color:
|
||||
fh.write("property uchar red\nproperty uchar green\nproperty uchar blue\n")
|
||||
fh.write("end_header\n")
|
||||
if have_color:
|
||||
cols = np.clip(np.asarray(colors), 0.0, 1.0)
|
||||
cols = (cols * 255).astype(np.uint8)
|
||||
for p, c in zip(points, cols):
|
||||
fh.write(f"{p[0]:.6f} {p[1]:.6f} {p[2]:.6f} {int(c[0])} {int(c[1])} {int(c[2])}\n")
|
||||
else:
|
||||
for p in points:
|
||||
fh.write(f"{p[0]:.6f} {p[1]:.6f} {p[2]:.6f}\n")
|
||||
|
||||
|
||||
def write_metadata_json(meta: dict[str, Any], path: str | Path) -> Path:
|
||||
path = Path(path)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", encoding="utf-8") as fh:
|
||||
json.dump(meta, fh, ensure_ascii=False, indent=2, default=_json_default)
|
||||
return path
|
||||
|
||||
|
||||
def _json_default(obj: Any) -> Any:
|
||||
if isinstance(obj, np.ndarray):
|
||||
return obj.tolist()
|
||||
if isinstance(obj, (np.floating, np.integer)):
|
||||
return obj.item()
|
||||
raise TypeError(f"not JSON serialisable: {type(obj).__name__}")
|
||||
|
||||
|
||||
__all__ = [
|
||||
"load_point_cloud",
|
||||
"write_point_cloud",
|
||||
"write_metadata_json",
|
||||
]
|
||||
@@ -0,0 +1,14 @@
|
||||
"""Deflection-angle and maximum-deviation computations + GT comparison."""
|
||||
|
||||
from .deflection import (
|
||||
DeflectionMetrics,
|
||||
compute_deflection_metrics,
|
||||
)
|
||||
from .compare import compare_to_gt, GTComparison
|
||||
|
||||
__all__ = [
|
||||
"DeflectionMetrics",
|
||||
"compute_deflection_metrics",
|
||||
"compare_to_gt",
|
||||
"GTComparison",
|
||||
]
|
||||
@@ -0,0 +1,60 @@
|
||||
"""Compare measured deflection against the GT (CAD-derived) values."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
from .deflection import DeflectionMetrics
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GTComparison:
|
||||
measured: DeflectionMetrics
|
||||
gt: DeflectionMetrics
|
||||
|
||||
@property
|
||||
def d_theta_right_deg(self) -> float:
|
||||
return self.measured.theta_right_deg - self.gt.theta_right_deg
|
||||
|
||||
@property
|
||||
def d_theta_ref_deg(self) -> float:
|
||||
return self.measured.theta_ref_deg - self.gt.theta_ref_deg
|
||||
|
||||
@property
|
||||
def d_D_y_max_mm(self) -> float:
|
||||
return self.measured.D_y_max_mm - self.gt.D_y_max_mm
|
||||
|
||||
@property
|
||||
def d_D_z_max_mm(self) -> float:
|
||||
return self.measured.D_z_max_mm - self.gt.D_z_max_mm
|
||||
|
||||
@property
|
||||
def d_dx_y_max_mm(self) -> float:
|
||||
return self.measured.dx_y_max_mm - self.gt.dx_y_max_mm
|
||||
|
||||
@property
|
||||
def d_dx_z_max_mm(self) -> float:
|
||||
return self.measured.dx_z_max_mm - self.gt.dx_z_max_mm
|
||||
|
||||
def to_dict(self) -> dict[str, float | dict]:
|
||||
return {
|
||||
"measured": self.measured.to_dict(),
|
||||
"gt": self.gt.to_dict(),
|
||||
"diff": {
|
||||
"theta_right_deg": self.d_theta_right_deg,
|
||||
"theta_ref_deg": self.d_theta_ref_deg,
|
||||
"D_y_max_mm": self.d_D_y_max_mm,
|
||||
"D_z_max_mm": self.d_D_z_max_mm,
|
||||
"dx_y_max_mm": self.d_dx_y_max_mm,
|
||||
"dx_z_max_mm": self.d_dx_z_max_mm,
|
||||
"abs_theta_right_deg": abs(self.d_theta_right_deg),
|
||||
"abs_theta_ref_deg": abs(self.d_theta_ref_deg),
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def compare_to_gt(measured: DeflectionMetrics, gt: DeflectionMetrics) -> GTComparison:
|
||||
return GTComparison(measured=measured, gt=gt)
|
||||
|
||||
|
||||
__all__ = ["GTComparison", "compare_to_gt"]
|
||||
@@ -0,0 +1,103 @@
|
||||
"""Deflection-angle and maximum-deviation formulas.
|
||||
|
||||
For a fitted panel-plane normal :math:`(n_x, n_y, n_z)` with
|
||||
:math:`n_x > 0`, the spec is::
|
||||
|
||||
theta_right_deg = atan2(n_y, n_x) * 180/pi (right-axis sign applied)
|
||||
theta_ref_deg = atan2(n_z, n_x) * 180/pi
|
||||
A = -n_y / n_x
|
||||
B = -n_z / n_x
|
||||
D_y_max = max |y_i - C_y|
|
||||
D_z_max = max |z_i - C_z|
|
||||
dx_y_max = |A| * D_y_max
|
||||
dx_z_max = |B| * D_z_max
|
||||
|
||||
``points`` should already be in the *world / CAD* frame, i.e. after the
|
||||
4-hole Kabsch registration, and clipped to the panel ROI so the maxima
|
||||
do not pick up edge or chamfer pixels.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DeflectionMetrics:
|
||||
theta_right_deg: float
|
||||
theta_ref_deg: float
|
||||
D_y_max_mm: float
|
||||
D_z_max_mm: float
|
||||
dx_y_max_mm: float
|
||||
dx_z_max_mm: float
|
||||
A: float # -n_y / n_x
|
||||
B: float # -n_z / n_x
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
return {
|
||||
"theta_right_deg": self.theta_right_deg,
|
||||
"theta_ref_deg": self.theta_ref_deg,
|
||||
"D_y_max_mm": self.D_y_max_mm,
|
||||
"D_z_max_mm": self.D_z_max_mm,
|
||||
"dx_y_max_mm": self.dx_y_max_mm,
|
||||
"dx_z_max_mm": self.dx_z_max_mm,
|
||||
"A": self.A,
|
||||
"B": self.B,
|
||||
}
|
||||
|
||||
|
||||
def compute_deflection_metrics(
|
||||
*,
|
||||
normal: tuple[float, float, float] | np.ndarray,
|
||||
center_C: tuple[float, float, float] | np.ndarray,
|
||||
panel_points: np.ndarray,
|
||||
right_axis_sign: int = 1,
|
||||
angle_clamp_deg: float = 0.0,
|
||||
) -> DeflectionMetrics:
|
||||
"""Compute the six measurands."""
|
||||
n = np.asarray(normal, dtype=np.float64).reshape(3)
|
||||
if n[0] <= 0:
|
||||
# Caller is expected to enforce n_x > 0 before this; we hard-fail
|
||||
# because every formula divides by n_x.
|
||||
raise ValueError(
|
||||
f"compute_deflection_metrics requires n_x > 0; got n = {tuple(n)}"
|
||||
)
|
||||
C = np.asarray(center_C, dtype=np.float64).reshape(3)
|
||||
pts = np.asarray(panel_points, dtype=np.float64)
|
||||
if pts.ndim != 2 or pts.shape[1] != 3:
|
||||
raise ValueError(f"panel_points must be (N,3); got {pts.shape}")
|
||||
if pts.shape[0] == 0:
|
||||
raise ValueError("panel_points is empty")
|
||||
|
||||
sign = 1.0 if int(right_axis_sign) >= 0 else -1.0
|
||||
|
||||
theta_right = sign * float(np.degrees(np.arctan2(n[1], n[0])))
|
||||
theta_ref = float(np.degrees(np.arctan2(n[2], n[0])))
|
||||
|
||||
if abs(theta_right) < float(angle_clamp_deg):
|
||||
theta_right = 0.0
|
||||
if abs(theta_ref) < float(angle_clamp_deg):
|
||||
theta_ref = 0.0
|
||||
|
||||
A = float(-n[1] / n[0])
|
||||
B = float(-n[2] / n[0])
|
||||
|
||||
D_y_max = float(np.max(np.abs(pts[:, 1] - C[1])))
|
||||
D_z_max = float(np.max(np.abs(pts[:, 2] - C[2])))
|
||||
|
||||
return DeflectionMetrics(
|
||||
theta_right_deg=theta_right,
|
||||
theta_ref_deg=theta_ref,
|
||||
D_y_max_mm=D_y_max,
|
||||
D_z_max_mm=D_z_max,
|
||||
dx_y_max_mm=abs(A) * D_y_max,
|
||||
dx_z_max_mm=abs(B) * D_z_max,
|
||||
A=A,
|
||||
B=B,
|
||||
)
|
||||
|
||||
|
||||
__all__ = ["DeflectionMetrics", "compute_deflection_metrics"]
|
||||
@@ -0,0 +1,364 @@
|
||||
"""End-to-end orchestration for *research / validation* runs.
|
||||
|
||||
Two entry points:
|
||||
|
||||
- :func:`run_coupler_measurement_pipeline` — the integration path:
|
||||
cloud (in CAD frame) + optional CAD STP + ROI → ROI clip → denoise →
|
||||
measure. **No registration.**
|
||||
- :func:`run_from_cad_pipeline` — the research path: STP → synthesise →
|
||||
denoise → 4-hole Kabsch (against oracle deflected GT) → measure → diff
|
||||
GT. Saves every intermediate artifact under a structured run directory.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .api import compute_coupler_measurement, ProgressCallback
|
||||
from .config import load_config, resolve_auto
|
||||
from .data_prep import (
|
||||
PreparedCloud,
|
||||
denoise_cloud,
|
||||
parse_cad,
|
||||
prepare_cloud_from_cad,
|
||||
prepare_cloud_from_scan,
|
||||
register_cloud,
|
||||
synthesize_cloud,
|
||||
)
|
||||
from .exceptions import InsufficientPointsError
|
||||
from .io_utils import (
|
||||
Artifacts,
|
||||
crop_ply_bbox,
|
||||
load_point_cloud,
|
||||
write_json,
|
||||
write_point_cloud,
|
||||
write_yaml,
|
||||
)
|
||||
from .metrics import compute_deflection_metrics
|
||||
from .detection.autotune import detection_overlay, relax_denoise_steps
|
||||
from .detection.layer_select import select_panel_layer
|
||||
from .roi_select import ROISpec, apply_roi, default_roi_spec, load_roi_json
|
||||
from .structures import CouplerMeasurement
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Integration entry point
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def run_coupler_measurement_pipeline(
|
||||
input_cloud: str | Path,
|
||||
out_dir: str | Path,
|
||||
*,
|
||||
config_path: str | Path | None = None,
|
||||
profile: str = "auto",
|
||||
cad_stp_path: str | Path | None = None,
|
||||
roi_spec: ROISpec | None = None,
|
||||
roi_json: str | Path | None = None,
|
||||
progress: ProgressCallback | None = None,
|
||||
) -> CouplerMeasurement:
|
||||
"""Main measurement entry: cloud (in CAD frame) → ROI → denoise → measure.
|
||||
|
||||
**No registration step.** The input cloud is assumed to already live
|
||||
in the CAD / world frame (scanner extrinsics calibrated upstream).
|
||||
If it doesn't, 4-hole Kabsch against nominal CAD holes would also
|
||||
cancel the panel-deflection signal we want to measure — so this
|
||||
pipeline deliberately does not register. For the research path that
|
||||
*does* register (using oracle deflected GT), see
|
||||
:func:`run_from_cad_pipeline`.
|
||||
|
||||
Inputs:
|
||||
|
||||
- ``cad_stp_path`` — optional STEP file. Parsed for the analytical
|
||||
panel-normal seed and, when no profile/config is given, to
|
||||
auto-tune the scale/density-dependent denoise + detection knobs
|
||||
(see below). The parsed GT is written as ``01_gt.json`` for audit.
|
||||
- ``roi_spec`` / ``roi_json`` — axis-aligned-bbox ROI applied to the
|
||||
cloud *before* denoising. If both omitted, the full cloud is kept.
|
||||
``roi_spec`` wins over ``roi_json``.
|
||||
|
||||
Writes every intermediate to ``out_dir``. Returns a
|
||||
:class:`CouplerMeasurement`.
|
||||
"""
|
||||
art = Artifacts(out_dir)
|
||||
cfg = load_config(config_path, profile=profile)
|
||||
cfg = resolve_auto(cfg)
|
||||
write_yaml(cfg, art.resolved_config)
|
||||
|
||||
# ---- Load + ROI clip --------------------------------------------------
|
||||
# The ROI JSON is the cropping hyper-parameter. When it is a finite
|
||||
# bounding box and the input is a (possibly multi-GB) binary PLY, crop
|
||||
# it by streaming the file so we never hold the whole vehicle cloud in
|
||||
# memory. Otherwise fall back to load-then-filter.
|
||||
if roi_spec is None:
|
||||
roi_spec = load_roi_json(roi_json) if roi_json is not None else default_roi_spec()
|
||||
|
||||
finite_bbox = all(
|
||||
math.isfinite(v) for v in (
|
||||
roi_spec.x_min, roi_spec.x_max,
|
||||
roi_spec.y_min, roi_spec.y_max,
|
||||
roi_spec.z_min, roi_spec.z_max,
|
||||
)
|
||||
)
|
||||
is_ply = str(input_cloud).lower().endswith(".ply")
|
||||
if finite_bbox and is_ply:
|
||||
pts, roi_diag = crop_ply_bbox(
|
||||
input_cloud,
|
||||
x_min=roi_spec.x_min, x_max=roi_spec.x_max,
|
||||
y_min=roi_spec.y_min, y_max=roi_spec.y_max,
|
||||
z_min=roi_spec.z_min, z_max=roi_spec.z_max,
|
||||
)
|
||||
roi_diag["kind"] = roi_spec.kind
|
||||
else:
|
||||
raw_pts = load_point_cloud(input_cloud)
|
||||
pts, roi_diag = apply_roi(raw_pts, roi_spec)
|
||||
|
||||
if pts.shape[0] == 0:
|
||||
raise InsufficientPointsError(
|
||||
f"ROI bbox kept 0 points from {input_cloud}; check the ROI JSON "
|
||||
f"bounds match the cloud's coordinate frame"
|
||||
)
|
||||
write_point_cloud(pts, art.path("sampled", ext="ply"))
|
||||
write_json({"spec": roi_spec.to_dict(), "diagnostics": roi_diag}, art.root / "roi.json")
|
||||
|
||||
# ---- Optional normal-direction layer isolation -----------------------
|
||||
# For a ROI cut from a full-vehicle scan, the box can slice through
|
||||
# several structures stacked along the panel normal (base plate +
|
||||
# raised mounting plate + roof truss). Detection and plane fit assume a
|
||||
# single thin plate, so we keep only one layer when the active config
|
||||
# turns this on (layer_select.enabled). Default config leaves it off,
|
||||
# so single-part clouds are unaffected.
|
||||
ls_cfg = cfg.get("layer_select", {})
|
||||
if ls_cfg.get("enabled", False):
|
||||
layer = select_panel_layer(
|
||||
pts,
|
||||
normal_seed=ls_cfg.get("normal_seed", "pca"),
|
||||
half_width_mm=float(ls_cfg.get("half_width_mm", 6.0)),
|
||||
bins=int(ls_cfg.get("bins", 240)),
|
||||
pick=str(ls_cfg.get("pick", "frontmost")),
|
||||
)
|
||||
pts = layer.points
|
||||
write_point_cloud(pts, art.path("transformed", ext="ply"))
|
||||
write_json(layer.diagnostics, art.root / "layer.json")
|
||||
|
||||
# Auto-tune the scale/density-dependent knobs from CAD + cloud when the
|
||||
# caller relies on the generic default config (no explicit config file
|
||||
# and no hand-tuned profile) but provides a CAD STP. This makes the
|
||||
# "CAD + cloud + ROI" contract self-sufficient for an adequately-sampled
|
||||
# scan of a panel of any scale, instead of (a) the default radius filter
|
||||
# deleting a sparse cloud or (b) 4-hole detection failing with the
|
||||
# small-panel defaults. A specific profile (e.g. lu1_af4) or an explicit
|
||||
# config is respected as-is (autotune skipped), so existing setups are
|
||||
# byte-for-byte unchanged.
|
||||
autotune = (
|
||||
cad_stp_path is not None
|
||||
and config_path is None
|
||||
and str(profile) in ("auto", "default", "none", "None")
|
||||
)
|
||||
|
||||
# ---- Optional CAD parse (panel_normal_seed + audit GT JSON) ----------
|
||||
panel_seed: np.ndarray | None = None
|
||||
parsed = None
|
||||
if cad_stp_path is not None:
|
||||
if autotune:
|
||||
# The default locating-hole band ([4,12] mm) selects the small
|
||||
# inner holes on a large panel, from which autotune would derive
|
||||
# too fine a grid. Widen it so parse_cad picks the large corner
|
||||
# locating holes (the reliably-scannable rectangle) and excludes
|
||||
# the big central pass-through hole.
|
||||
cfg["cad"] = {**cfg["cad"], "locating_hole_radius_mm": [6.0, 30.0]}
|
||||
parsed = parse_cad(cad_stp_path, cfg["cad"])
|
||||
write_json(parsed.gt.to_dict(), art.path("gt", ext="json"))
|
||||
panel_seed = np.asarray(parsed.gt.plane.normal, dtype=np.float64)
|
||||
|
||||
# ---- Denoise (no Kabsch, no prepare_cloud_from_scan) -----------------
|
||||
# Stage 1 of autotune (before denoise): drop the density-fragile radius
|
||||
# filter so a sparse cloud is not deleted.
|
||||
if autotune:
|
||||
cfg["denoise"] = relax_denoise_steps(cfg.get("denoise", {}))
|
||||
denoised = denoise_cloud(pts, cfg.get("denoise", {}))
|
||||
write_point_cloud(denoised.points, art.path("denoised", ext="ply"))
|
||||
|
||||
# Stage 2 of autotune (after denoise): size the detection grid / fill /
|
||||
# area band from the CAD locating-hole radius + the DENOISED point
|
||||
# spacing (the cloud detection actually sees).
|
||||
if autotune and parsed is not None:
|
||||
loc_r = float(np.median([h.radius_mm for h in parsed.gt.holes.locating]))
|
||||
cfg["detection"] = {
|
||||
**cfg.get("detection", {}),
|
||||
**detection_overlay(loc_r, denoised.points),
|
||||
}
|
||||
write_yaml(cfg, art.resolved_config)
|
||||
|
||||
# ---- Measurement ------------------------------------------------------
|
||||
result = compute_coupler_measurement(
|
||||
denoised.points,
|
||||
config=cfg,
|
||||
profile="custom",
|
||||
panel_normal_seed=panel_seed,
|
||||
progress=progress,
|
||||
)
|
||||
|
||||
write_json(result.to_dict(), art.report_json)
|
||||
write_json(result.to_dict()["report"], art.path("metrics", ext="json"))
|
||||
return result
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# CAD-driven research entry point
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def run_from_cad_pipeline(
|
||||
stp_path: str | Path,
|
||||
out_dir: str | Path,
|
||||
*,
|
||||
config_path: str | Path | None = None,
|
||||
profile: str = "auto",
|
||||
progress: ProgressCallback | None = None,
|
||||
) -> dict[str, Any]:
|
||||
"""End-to-end research run: STP → synthesise → prepare → measure → diff GT.
|
||||
|
||||
Returns a summary dict containing the measurement, GT metrics, and
|
||||
measured-minus-GT differences. Writes every intermediate cloud and
|
||||
JSON to ``out_dir``.
|
||||
"""
|
||||
art = Artifacts(out_dir)
|
||||
cfg = load_config(config_path, profile=profile)
|
||||
cfg = resolve_auto(cfg)
|
||||
write_yaml(cfg, art.resolved_config)
|
||||
|
||||
# ---- Phase A.1 — parse + GT -------------------------------------------
|
||||
parsed = parse_cad(stp_path, cfg["cad"])
|
||||
write_point_cloud(parsed.mesh.vertices, art.path("cad_mesh", ext="ply"))
|
||||
write_json(parsed.gt.to_dict(), art.path("gt", ext="json"))
|
||||
|
||||
# ---- Phase A.2 — synthesise --------------------------------------------
|
||||
synth = synthesize_cloud(
|
||||
parsed,
|
||||
sampling_cfg=cfg["sampling"],
|
||||
noise_cfg=cfg.get("noise", {}),
|
||||
transform_cfg=cfg.get("transform", {}),
|
||||
scan_cfg=cfg.get("scan_simulator", {}),
|
||||
deflection_cfg=cfg.get("panel_deflection", {}),
|
||||
)
|
||||
write_point_cloud(synth.sampled, art.path("sampled", ext="ply"))
|
||||
write_point_cloud(synth.noisy, art.path("noisy", ext="ply"))
|
||||
write_point_cloud(synth.transformed, art.path("transformed", ext="ply"))
|
||||
|
||||
# ---- Phase A.3 — denoise ----------------------------------------------
|
||||
denoised = denoise_cloud(synth.transformed, cfg.get("denoise", {}))
|
||||
write_point_cloud(denoised.points, art.path("denoised", ext="ply"))
|
||||
|
||||
# ---- Phase A.4 — register ---------------------------------------------
|
||||
registered = register_cloud(
|
||||
denoised.points,
|
||||
target_holes=synth.gt_holes_after_deflection,
|
||||
detection_cfg=cfg["detection"],
|
||||
cad_cfg=cfg["cad"],
|
||||
registration_cfg=cfg["registration"],
|
||||
panel_normal_seed=None,
|
||||
)
|
||||
write_point_cloud(registered.points, art.path("registered", ext="ply"))
|
||||
|
||||
# ---- Phase B — measurement algorithm ----------------------------------
|
||||
result = compute_coupler_measurement(
|
||||
registered.points,
|
||||
config=cfg,
|
||||
profile="custom",
|
||||
panel_normal_seed=np.asarray(parsed.gt.plane.normal, dtype=np.float64),
|
||||
progress=progress,
|
||||
)
|
||||
|
||||
# ---- GT metrics --------------------------------------------------------
|
||||
gt_normal = synth.gt_normal_after_deflection
|
||||
centers = synth.gt_holes_after_deflection.locating_centers()
|
||||
gt_metrics = compute_deflection_metrics(
|
||||
normal=gt_normal,
|
||||
center_C=tuple(centers.mean(axis=0).tolist()),
|
||||
panel_points=centers,
|
||||
right_axis_sign=int(cfg["metrics"].get("right_axis_sign", 1)),
|
||||
angle_clamp_deg=float(cfg["metrics"].get("angle_clamp_deg", 0.0)),
|
||||
)
|
||||
|
||||
diff = {
|
||||
"theta_right_deg": result.theta_right_deg - gt_metrics.theta_right_deg,
|
||||
"theta_ref_deg": result.theta_ref_deg - gt_metrics.theta_ref_deg,
|
||||
"D_y_max_mm": result.D_y_max_mm - gt_metrics.D_y_max_mm,
|
||||
"D_z_max_mm": result.D_z_max_mm - gt_metrics.D_z_max_mm,
|
||||
"dx_y_max_mm": result.dx_y_max_mm - gt_metrics.dx_y_max_mm,
|
||||
"dx_z_max_mm": result.dx_z_max_mm - gt_metrics.dx_z_max_mm,
|
||||
}
|
||||
|
||||
summary = {
|
||||
"stp": str(stp_path),
|
||||
"panel_deflection_injected": {
|
||||
"theta_right_deg": synth.panel_deflection_theta_right_deg,
|
||||
"theta_ref_deg": synth.panel_deflection_theta_ref_deg,
|
||||
"R": synth.panel_deflection_R.tolist(),
|
||||
},
|
||||
"scan_pose_perturbation": synth.gt_transform_4x4.tolist(),
|
||||
"registration": {
|
||||
"matrix": registered.transform_4x4.tolist(),
|
||||
"rmse_mm": registered.rmse_mm,
|
||||
"max_residual_mm": registered.max_residual_mm,
|
||||
},
|
||||
"measured": result.to_dict(),
|
||||
"gt_cad": parsed.gt.to_dict(),
|
||||
"gt_after_deflection": {
|
||||
"normal": gt_normal.tolist(),
|
||||
"locating_centers": centers.tolist(),
|
||||
},
|
||||
"gt_metrics": gt_metrics.to_dict(),
|
||||
"diff": diff,
|
||||
"config": cfg,
|
||||
}
|
||||
write_json(summary, art.path("metrics", ext="json"))
|
||||
write_json(summary, art.report_json)
|
||||
|
||||
return summary
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _load_holeset_from_json(path: str | Path):
|
||||
"""Reconstruct a HoleSet from the JSON written by ``GroundTruth.to_dict``."""
|
||||
import json
|
||||
|
||||
from .structures import Hole, HoleSet
|
||||
|
||||
with Path(path).open("r", encoding="utf-8") as fh:
|
||||
d = json.load(fh)
|
||||
holes = d["holes"]
|
||||
locating = tuple(
|
||||
Hole(
|
||||
center=tuple(h["center"]),
|
||||
radius_mm=float(h["radius_mm"]),
|
||||
fit_residual_rms_mm=float(h.get("fit_residual_rms_mm", 0.0)),
|
||||
role=h.get("role", "locating"),
|
||||
)
|
||||
for h in holes["locating"]
|
||||
)
|
||||
central = None
|
||||
if holes.get("central") is not None:
|
||||
c = holes["central"]
|
||||
central = Hole(
|
||||
center=tuple(c["center"]),
|
||||
radius_mm=float(c["radius_mm"]),
|
||||
fit_residual_rms_mm=float(c.get("fit_residual_rms_mm", 0.0)),
|
||||
role="central",
|
||||
)
|
||||
return HoleSet(locating=locating, central=central)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"run_coupler_measurement_pipeline",
|
||||
"run_from_cad_pipeline",
|
||||
]
|
||||
@@ -0,0 +1,18 @@
|
||||
"""Rigid registration via 4 hole-centre correspondence (Kabsch / Umeyama).
|
||||
|
||||
This package deliberately ships **only** the four-hole method. We do
|
||||
*not* expose a panel-plane ICP variant because that would absorb the
|
||||
deflection signal we want to measure.
|
||||
"""
|
||||
|
||||
from .kabsch import (
|
||||
KabschResult,
|
||||
kabsch_umeyama,
|
||||
register_four_holes,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"KabschResult",
|
||||
"kabsch_umeyama",
|
||||
"register_four_holes",
|
||||
]
|
||||
@@ -0,0 +1,114 @@
|
||||
"""Rigid registration from a small number of 3D point correspondences.
|
||||
|
||||
We use the closed-form Umeyama solution (Kabsch with the optional scale
|
||||
turned off — we want a rigid transform). Given 4 source points P (in the
|
||||
scan/world frame) and 4 target points Q (in the CAD/GT frame), we solve
|
||||
for the rigid T such that ``T @ P ≈ Q``.
|
||||
|
||||
The four locating holes give us four exact correspondences; their
|
||||
indexing must match (we ensure this in :mod:`coupler_panel.detection`
|
||||
via the canonical quadrant ordering).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..exceptions import RegistrationError
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class KabschResult:
|
||||
"""4×4 transform plus quality stats."""
|
||||
|
||||
matrix: np.ndarray # (4, 4) — applies to source to align with target
|
||||
rmse_mm: float
|
||||
max_residual_mm: float
|
||||
n_pairs: int
|
||||
|
||||
def apply(self, points: np.ndarray) -> np.ndarray:
|
||||
R = self.matrix[:3, :3]
|
||||
t = self.matrix[:3, 3]
|
||||
return np.asarray(points, dtype=np.float64) @ R.T + t
|
||||
|
||||
|
||||
def kabsch_umeyama(
|
||||
source: np.ndarray,
|
||||
target: np.ndarray,
|
||||
*,
|
||||
allow_reflection: bool = False,
|
||||
) -> KabschResult:
|
||||
"""Solve for rigid T such that T·source ≈ target."""
|
||||
P = np.asarray(source, dtype=np.float64)
|
||||
Q = np.asarray(target, dtype=np.float64)
|
||||
if P.shape != Q.shape or P.ndim != 2 or P.shape[1] != 3:
|
||||
raise RegistrationError(
|
||||
f"shape mismatch: source={P.shape}, target={Q.shape}; "
|
||||
"expected (N,3) with the same N"
|
||||
)
|
||||
n = P.shape[0]
|
||||
if n < 3:
|
||||
raise RegistrationError(f"need ≥ 3 correspondences, got {n}")
|
||||
|
||||
p_bar = P.mean(axis=0)
|
||||
q_bar = Q.mean(axis=0)
|
||||
Pc = P - p_bar
|
||||
Qc = Q - q_bar
|
||||
|
||||
H = Pc.T @ Qc # (3, 3)
|
||||
U, _S, Vt = np.linalg.svd(H)
|
||||
R = Vt.T @ U.T
|
||||
|
||||
if not allow_reflection and np.linalg.det(R) < 0:
|
||||
Vt[-1, :] *= -1.0
|
||||
R = Vt.T @ U.T
|
||||
|
||||
t = q_bar - R @ p_bar
|
||||
|
||||
M = np.eye(4)
|
||||
M[:3, :3] = R
|
||||
M[:3, 3] = t
|
||||
|
||||
P_aligned = P @ R.T + t
|
||||
res = np.linalg.norm(P_aligned - Q, axis=1)
|
||||
return KabschResult(
|
||||
matrix=M,
|
||||
rmse_mm=float(np.sqrt(np.mean(res ** 2))),
|
||||
max_residual_mm=float(res.max()),
|
||||
n_pairs=int(n),
|
||||
)
|
||||
|
||||
|
||||
def register_four_holes(
|
||||
source_centers: np.ndarray,
|
||||
target_centers: np.ndarray,
|
||||
*,
|
||||
refuse_above_mm: float | None = None,
|
||||
) -> KabschResult:
|
||||
"""Wrapper for the canonical 4-hole pairing case.
|
||||
|
||||
The two arrays **must** be index-aligned (same canonical quadrant
|
||||
order). The detection / GT modules in this project are responsible
|
||||
for producing them in the same order.
|
||||
"""
|
||||
P = np.asarray(source_centers, dtype=np.float64)
|
||||
Q = np.asarray(target_centers, dtype=np.float64)
|
||||
if P.shape != (4, 3) or Q.shape != (4, 3):
|
||||
raise RegistrationError(
|
||||
f"register_four_holes requires (4,3) inputs; got {P.shape} and {Q.shape}"
|
||||
)
|
||||
|
||||
result = kabsch_umeyama(P, Q)
|
||||
|
||||
if refuse_above_mm is not None and result.rmse_mm > float(refuse_above_mm):
|
||||
raise RegistrationError(
|
||||
f"4-hole Kabsch RMSE {result.rmse_mm:.3f} mm exceeds {refuse_above_mm} mm — "
|
||||
"wrong correspondences, missed hole, or detection error"
|
||||
)
|
||||
|
||||
return result
|
||||
|
||||
|
||||
__all__ = ["KabschResult", "kabsch_umeyama", "register_four_holes"]
|
||||
@@ -0,0 +1,12 @@
|
||||
"""Batch-evaluation reporting helpers (xlsx output)."""
|
||||
|
||||
from .excel import EvalRow, write_excel_report, build_row_from_summary
|
||||
from .inspection import InspectionItem, write_inspection_report
|
||||
|
||||
__all__ = [
|
||||
"EvalRow",
|
||||
"write_excel_report",
|
||||
"build_row_from_summary",
|
||||
"InspectionItem",
|
||||
"write_inspection_report",
|
||||
]
|
||||
@@ -0,0 +1,285 @@
|
||||
"""Aggregate batch-eval results into an xlsx report.
|
||||
|
||||
Each row corresponds to one input PLY × one ROI JSON × one run.
|
||||
|
||||
Columns (kept stable across releases unless the field is removed from
|
||||
the upstream report):
|
||||
|
||||
sample_id, ply_path, roi_kind,
|
||||
theta_right_gt_deg, theta_ref_gt_deg,
|
||||
theta_right_pred_deg, theta_ref_pred_deg,
|
||||
theta_right_err_deg, theta_ref_err_deg,
|
||||
D_y_max_mm, D_z_max_mm,
|
||||
dx_y_max_mm, dx_z_max_mm,
|
||||
center_C_x_mm, center_C_y_mm, center_C_z_mm,
|
||||
normal_x, normal_y, normal_z,
|
||||
plane_inlier_ratio, plane_residual_rms_mm,
|
||||
registration_rmse_mm, registration_max_residual_mm,
|
||||
n_points_in, n_points_after_roi,
|
||||
quality_flags,
|
||||
pipeline_seconds, inner_stage_seconds,
|
||||
algorithm_version, schema_version
|
||||
|
||||
``pipeline_seconds`` is the **wall-clock** time from the moment the
|
||||
inputs (cloud PLY + ROI JSON, optionally CAD STP) enter
|
||||
``run_coupler_measurement_pipeline`` to the moment the
|
||||
``CouplerMeasurement`` is returned — i.e. the algorithm's total
|
||||
processing time per sample.
|
||||
|
||||
``inner_stage_seconds`` is the older, narrower number: the sum of
|
||||
``StageTiming.seconds`` recorded inside ``compute_coupler_measurement``
|
||||
itself (validate → detect_holes → … → metrics), excluding file IO,
|
||||
cloud load, ROI apply, denoise, and any CAD STP parse. It is kept for
|
||||
per-stage debugging only.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import asdict, dataclass, field
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
try:
|
||||
from openpyxl import Workbook
|
||||
from openpyxl.styles import Alignment, Font, PatternFill
|
||||
except ImportError as exc: # pragma: no cover
|
||||
raise ImportError(
|
||||
"openpyxl is required for Excel reporting; install with `pip install openpyxl`"
|
||||
) from exc
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Row schema
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
COLUMNS: tuple[tuple[str, str], ...] = (
|
||||
("sample_id", "Sample ID"),
|
||||
("ply_path", "Input PLY"),
|
||||
("roi_kind", "ROI kind"),
|
||||
("theta_right_gt_deg", "θ_right GT (°)"),
|
||||
("theta_ref_gt_deg", "θ_ref GT (°)"),
|
||||
("theta_right_pred_deg", "θ_right Pred (°)"),
|
||||
("theta_ref_pred_deg", "θ_ref Pred (°)"),
|
||||
("theta_right_err_deg", "θ_right Err (°)"),
|
||||
("theta_ref_err_deg", "θ_ref Err (°)"),
|
||||
("D_y_max_mm", "D_y_max (mm)"),
|
||||
("D_z_max_mm", "D_z_max (mm)"),
|
||||
("dx_y_max_mm", "dx_y_max (mm)"),
|
||||
("dx_z_max_mm", "dx_z_max (mm)"),
|
||||
("center_C_x_mm", "C_x (mm)"),
|
||||
("center_C_y_mm", "C_y (mm)"),
|
||||
("center_C_z_mm", "C_z (mm)"),
|
||||
("normal_x", "n_x"),
|
||||
("normal_y", "n_y"),
|
||||
("normal_z", "n_z"),
|
||||
("plane_inlier_ratio", "plane inlier ratio"),
|
||||
("plane_residual_rms_mm", "plane res RMS (mm)"),
|
||||
("registration_rmse_mm", "Kabsch RMSE (mm)"),
|
||||
("registration_max_residual_mm", "Kabsch max res (mm)"),
|
||||
("n_points_in", "N in"),
|
||||
("n_points_after_roi", "N after ROI"),
|
||||
("quality_flags", "quality_flags"),
|
||||
("pipeline_seconds", "Algorithm runtime (s)"),
|
||||
("inner_stage_seconds", "compute_*_measurement (s)"),
|
||||
("algorithm_version", "algo ver"),
|
||||
("schema_version", "schema ver"),
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class EvalRow:
|
||||
"""One row in the Excel report (one PLY × one ROI × one config)."""
|
||||
|
||||
sample_id: str
|
||||
ply_path: str
|
||||
roi_kind: str
|
||||
theta_right_gt_deg: float | None
|
||||
theta_ref_gt_deg: float | None
|
||||
theta_right_pred_deg: float
|
||||
theta_ref_pred_deg: float
|
||||
theta_right_err_deg: float | None
|
||||
theta_ref_err_deg: float | None
|
||||
D_y_max_mm: float
|
||||
D_z_max_mm: float
|
||||
dx_y_max_mm: float
|
||||
dx_z_max_mm: float
|
||||
center_C_x_mm: float
|
||||
center_C_y_mm: float
|
||||
center_C_z_mm: float
|
||||
normal_x: float
|
||||
normal_y: float
|
||||
normal_z: float
|
||||
plane_inlier_ratio: float
|
||||
plane_residual_rms_mm: float
|
||||
registration_rmse_mm: float | None
|
||||
registration_max_residual_mm: float | None
|
||||
n_points_in: int
|
||||
n_points_after_roi: int
|
||||
quality_flags: str
|
||||
pipeline_seconds: float
|
||||
inner_stage_seconds: float
|
||||
algorithm_version: str
|
||||
schema_version: str
|
||||
extra: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
d = asdict(self)
|
||||
d.pop("extra", None)
|
||||
return d
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Row construction from pipeline outputs
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def build_row_from_summary(
|
||||
*,
|
||||
sample_id: str,
|
||||
ply_path: str | Path,
|
||||
roi_kind: str,
|
||||
measured: dict[str, Any],
|
||||
gt_angles: tuple[float, float] | None,
|
||||
registration: dict[str, Any] | None,
|
||||
n_points_in: int,
|
||||
n_points_after_roi: int,
|
||||
pipeline_seconds: float,
|
||||
) -> EvalRow:
|
||||
"""Construct an :class:`EvalRow` from the pipeline's report dict.
|
||||
|
||||
``measured`` is the ``CouplerMeasurement.to_dict()`` payload. It
|
||||
embeds ``report`` (the inner dict ``compute_coupler_measurement``
|
||||
wrote). ``gt_angles`` is the injected ``(theta_right_deg,
|
||||
theta_ref_deg)`` or ``None`` when not known.
|
||||
|
||||
``pipeline_seconds`` should be the wall-clock duration of the
|
||||
``run_coupler_measurement_pipeline`` call — i.e. the algorithm's
|
||||
end-to-end runtime per sample. Caller is responsible for timing.
|
||||
"""
|
||||
report = measured.get("report", {})
|
||||
plane = report.get("plane", {})
|
||||
timings = report.get("timings", [])
|
||||
inner_s = float(sum(t.get("seconds", 0.0) for t in timings))
|
||||
|
||||
th_r_pred = float(measured["theta_right_deg"])
|
||||
th_z_pred = float(measured["theta_ref_deg"])
|
||||
if gt_angles is None:
|
||||
th_r_gt = th_z_gt = None
|
||||
th_r_err = th_z_err = None
|
||||
else:
|
||||
th_r_gt = float(gt_angles[0])
|
||||
th_z_gt = float(gt_angles[1])
|
||||
th_r_err = th_r_pred - th_r_gt
|
||||
th_z_err = th_z_pred - th_z_gt
|
||||
|
||||
cx, cy, cz = measured["center_C"]
|
||||
nx, ny, nz = measured["normal"]
|
||||
|
||||
return EvalRow(
|
||||
sample_id=sample_id,
|
||||
ply_path=str(ply_path),
|
||||
roi_kind=roi_kind,
|
||||
theta_right_gt_deg=th_r_gt,
|
||||
theta_ref_gt_deg=th_z_gt,
|
||||
theta_right_pred_deg=th_r_pred,
|
||||
theta_ref_pred_deg=th_z_pred,
|
||||
theta_right_err_deg=th_r_err,
|
||||
theta_ref_err_deg=th_z_err,
|
||||
D_y_max_mm=float(measured["D_y_max_mm"]),
|
||||
D_z_max_mm=float(measured["D_z_max_mm"]),
|
||||
dx_y_max_mm=float(measured["dx_y_max_mm"]),
|
||||
dx_z_max_mm=float(measured["dx_z_max_mm"]),
|
||||
center_C_x_mm=float(cx),
|
||||
center_C_y_mm=float(cy),
|
||||
center_C_z_mm=float(cz),
|
||||
normal_x=float(nx),
|
||||
normal_y=float(ny),
|
||||
normal_z=float(nz),
|
||||
plane_inlier_ratio=float(plane.get("inlier_ratio", float("nan"))),
|
||||
plane_residual_rms_mm=float(plane.get("inlier_residual_rms_mm", float("nan"))),
|
||||
registration_rmse_mm=(None if registration is None else float(registration.get("rmse_mm", float("nan")))),
|
||||
registration_max_residual_mm=(
|
||||
None
|
||||
if registration is None
|
||||
else float(registration.get("max_residual_mm", float("nan")))
|
||||
),
|
||||
n_points_in=int(n_points_in),
|
||||
n_points_after_roi=int(n_points_after_roi),
|
||||
quality_flags=";".join(measured.get("quality_flags", [])) or "",
|
||||
pipeline_seconds=float(pipeline_seconds),
|
||||
inner_stage_seconds=inner_s,
|
||||
algorithm_version=str(measured.get("algorithm_version", "")),
|
||||
schema_version=str(measured.get("schema_version", "")),
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Excel writer
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def write_excel_report(rows: list[EvalRow], path: str | Path) -> Path:
|
||||
"""Write rows to an xlsx file. Returns the path."""
|
||||
out = Path(path)
|
||||
out.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
wb = Workbook()
|
||||
ws = wb.active
|
||||
ws.title = "results"
|
||||
|
||||
header_font = Font(bold=True)
|
||||
header_fill = PatternFill("solid", fgColor="DDDDDD")
|
||||
center = Alignment(horizontal="center")
|
||||
|
||||
# Header row.
|
||||
for col_idx, (_, label) in enumerate(COLUMNS, start=1):
|
||||
cell = ws.cell(row=1, column=col_idx, value=label)
|
||||
cell.font = header_font
|
||||
cell.fill = header_fill
|
||||
cell.alignment = center
|
||||
|
||||
# Data rows.
|
||||
for r_idx, row in enumerate(rows, start=2):
|
||||
d = row.to_dict()
|
||||
for c_idx, (key, _) in enumerate(COLUMNS, start=1):
|
||||
v = d.get(key)
|
||||
if isinstance(v, float) and (v != v): # NaN
|
||||
v = ""
|
||||
ws.cell(row=r_idx, column=c_idx, value=v)
|
||||
|
||||
# Number formatting for float-typed cells.
|
||||
float_keys = {
|
||||
"theta_right_gt_deg", "theta_ref_gt_deg",
|
||||
"theta_right_pred_deg", "theta_ref_pred_deg",
|
||||
"theta_right_err_deg", "theta_ref_err_deg",
|
||||
"D_y_max_mm", "D_z_max_mm", "dx_y_max_mm", "dx_z_max_mm",
|
||||
"center_C_x_mm", "center_C_y_mm", "center_C_z_mm",
|
||||
"normal_x", "normal_y", "normal_z",
|
||||
"plane_inlier_ratio", "plane_residual_rms_mm",
|
||||
"registration_rmse_mm", "registration_max_residual_mm",
|
||||
"pipeline_seconds", "inner_stage_seconds",
|
||||
}
|
||||
for c_idx, (key, _) in enumerate(COLUMNS, start=1):
|
||||
if key in float_keys:
|
||||
for r_idx in range(2, len(rows) + 2):
|
||||
ws.cell(row=r_idx, column=c_idx).number_format = "0.000000"
|
||||
|
||||
# Auto-width (cap at 32).
|
||||
for c_idx, (_, label) in enumerate(COLUMNS, start=1):
|
||||
col_letter = ws.cell(row=1, column=c_idx).column_letter
|
||||
max_len = len(str(label))
|
||||
for r_idx in range(2, len(rows) + 2):
|
||||
v = ws.cell(row=r_idx, column=c_idx).value
|
||||
if v is None:
|
||||
continue
|
||||
max_len = max(max_len, len(str(v)))
|
||||
ws.column_dimensions[col_letter].width = min(max_len + 2, 32)
|
||||
|
||||
ws.freeze_panes = "D2"
|
||||
|
||||
wb.save(out)
|
||||
return out
|
||||
|
||||
|
||||
__all__ = ["EvalRow", "COLUMNS", "build_row_from_summary", "write_excel_report"]
|
||||
@@ -0,0 +1,124 @@
|
||||
"""Inspection-item Excel reports (项点报表).
|
||||
|
||||
Each report is one measurement item with the six columns the quality
|
||||
department expects:
|
||||
|
||||
项点名称 | 理论尺寸 | 公差要求 | 测量结果 | 误差结果 | 测量结论
|
||||
|
||||
For the coupler panel, the two deflection angles are reported in a
|
||||
**slope form** ``value/L mm`` (e.g. ``0.8/375mm``) rather than degrees,
|
||||
because the tolerance is specified that way. ``value`` is the lateral
|
||||
offset over a baseline of ``L`` mm:
|
||||
|
||||
value = L * tan(theta) (theta in radians)
|
||||
|
||||
so a measured angle ``theta`` becomes ``(L*tan theta)/L mm``. The
|
||||
tolerance ``T/L mm`` is the angle whose offset over ``L`` is ``T``; the
|
||||
result passes when ``|L*tan(theta)| <= T``.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
try:
|
||||
from openpyxl import Workbook
|
||||
from openpyxl.styles import Alignment, Font, PatternFill
|
||||
except ImportError as exc: # pragma: no cover
|
||||
raise ImportError(
|
||||
"openpyxl is required for Excel reporting; install with `pip install openpyxl`"
|
||||
) from exc
|
||||
|
||||
|
||||
COLUMNS = ("项点名称", "理论尺寸", "公差要求", "测量结果", "误差结果", "测量结论")
|
||||
|
||||
|
||||
@dataclass
|
||||
class InspectionItem:
|
||||
"""One deflection-angle inspection item, reported in ``value/L mm`` form."""
|
||||
|
||||
name: str # 项点名称
|
||||
theta_deg: float # measured angle (degrees), theory = 0
|
||||
baseline_mm: float # L in the slope form
|
||||
tol_value_mm: float # tolerance numerator T (so tolerance = T/L mm)
|
||||
|
||||
# --- derived ---
|
||||
@property
|
||||
def offset_mm(self) -> float:
|
||||
"""L * tan(theta): lateral offset over the baseline, mm."""
|
||||
return self.baseline_mm * math.tan(math.radians(self.theta_deg))
|
||||
|
||||
@property
|
||||
def theoretical_str(self) -> str:
|
||||
return f"0/{self.baseline_mm:g}mm"
|
||||
|
||||
@property
|
||||
def tolerance_str(self) -> str:
|
||||
return f"{self.tol_value_mm:g}/{self.baseline_mm:g}mm"
|
||||
|
||||
@property
|
||||
def measured_str(self) -> str:
|
||||
return f"{self.offset_mm:.6f}/{self.baseline_mm:g}mm"
|
||||
|
||||
@property
|
||||
def error_str(self) -> str:
|
||||
# Error == measured result here (theory is 0).
|
||||
return self.measured_str
|
||||
|
||||
@property
|
||||
def passed(self) -> bool:
|
||||
return abs(self.offset_mm) <= self.tol_value_mm
|
||||
|
||||
@property
|
||||
def conclusion_str(self) -> str:
|
||||
return "合格" if self.passed else "不合格"
|
||||
|
||||
def row(self) -> tuple[str, str, str, str, str, str]:
|
||||
return (
|
||||
self.name,
|
||||
self.theoretical_str,
|
||||
self.tolerance_str,
|
||||
self.measured_str,
|
||||
self.error_str,
|
||||
self.conclusion_str,
|
||||
)
|
||||
|
||||
|
||||
def write_inspection_report(item: InspectionItem, path: str | Path) -> Path:
|
||||
"""Write a single-item inspection report to an xlsx file."""
|
||||
out = Path(path)
|
||||
out.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
wb = Workbook()
|
||||
ws = wb.active
|
||||
ws.title = "inspection"
|
||||
|
||||
header_font = Font(bold=True)
|
||||
header_fill = PatternFill("solid", fgColor="DDDDDD")
|
||||
center = Alignment(horizontal="center", vertical="center")
|
||||
|
||||
for c, label in enumerate(COLUMNS, start=1):
|
||||
cell = ws.cell(row=1, column=c, value=label)
|
||||
cell.font = header_font
|
||||
cell.fill = header_fill
|
||||
cell.alignment = center
|
||||
|
||||
for c, val in enumerate(item.row(), start=1):
|
||||
cell = ws.cell(row=2, column=c, value=val)
|
||||
cell.alignment = center
|
||||
|
||||
# red font for 不合格 conclusion
|
||||
if not item.passed:
|
||||
ws.cell(row=2, column=6).font = Font(color="CC0000", bold=True)
|
||||
|
||||
widths = (44, 12, 14, 16, 16, 12)
|
||||
for c, w in enumerate(widths, start=1):
|
||||
ws.column_dimensions[ws.cell(row=1, column=c).column_letter].width = w
|
||||
|
||||
wb.save(out)
|
||||
return out
|
||||
|
||||
|
||||
__all__ = ["InspectionItem", "write_inspection_report", "COLUMNS"]
|
||||
@@ -0,0 +1,25 @@
|
||||
"""Region-of-interest pre-filter for the input point cloud.
|
||||
|
||||
The main pipeline takes a *raw* point cloud (PLY) plus a ROI JSON
|
||||
descriptor (an axis-aligned bounding box in the CAD frame) that tells
|
||||
which subset of the cloud should actually feed the downstream algorithm.
|
||||
|
||||
ROI JSON layout — six flat keys, all in mm::
|
||||
|
||||
{
|
||||
"x_min": -12500.0, "x_max": 12500.0,
|
||||
"y_min": 150.0, "y_max": 4100.0,
|
||||
"z_min": -1680.0, "z_max": 200.0
|
||||
}
|
||||
"""
|
||||
|
||||
from .schema import ROISpec, default_roi_spec, load_roi_json, validate_roi_spec
|
||||
from .apply import apply_roi
|
||||
|
||||
__all__ = [
|
||||
"ROISpec",
|
||||
"default_roi_spec",
|
||||
"load_roi_json",
|
||||
"validate_roi_spec",
|
||||
"apply_roi",
|
||||
]
|
||||
@@ -0,0 +1,48 @@
|
||||
"""Apply an axis-aligned bounding-box :class:`ROISpec` to a point cloud."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .schema import ROISpec
|
||||
|
||||
|
||||
def apply_roi(points: np.ndarray, spec: ROISpec) -> tuple[np.ndarray, dict[str, Any]]:
|
||||
"""Filter ``points`` by the axis-aligned bounding box in ``spec``.
|
||||
|
||||
Returns ``(filtered_points, diagnostics)``. ``diagnostics`` is a small
|
||||
JSON-friendly dict suitable for embedding in a run report.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
n_in = int(pts.shape[0])
|
||||
|
||||
if n_in == 0:
|
||||
return pts.copy(), {
|
||||
"kind": spec.kind,
|
||||
"n_in": 0,
|
||||
"n_out": 0,
|
||||
"kept_ratio": 0.0,
|
||||
"bounds": spec.to_dict(),
|
||||
}
|
||||
|
||||
x, y, z = pts[:, 0], pts[:, 1], pts[:, 2]
|
||||
mask = (
|
||||
(x >= spec.x_min) & (x <= spec.x_max)
|
||||
& (y >= spec.y_min) & (y <= spec.y_max)
|
||||
& (z >= spec.z_min) & (z <= spec.z_max)
|
||||
)
|
||||
out = pts[mask]
|
||||
n_out = int(out.shape[0])
|
||||
|
||||
return out, {
|
||||
"kind": spec.kind,
|
||||
"n_in": n_in,
|
||||
"n_out": n_out,
|
||||
"kept_ratio": float(n_out / n_in),
|
||||
"bounds": spec.to_dict(),
|
||||
}
|
||||
|
||||
|
||||
__all__ = ["apply_roi"]
|
||||
@@ -0,0 +1,127 @@
|
||||
"""ROI JSON schema definition + loader.
|
||||
|
||||
The ROI descriptor is a **flat axis-aligned bounding box** in the CAD
|
||||
frame. Six numbers, no nesting:
|
||||
|
||||
{
|
||||
"x_min": -12500.0,
|
||||
"x_max": 12500.0,
|
||||
"y_min": 150.0,
|
||||
"y_max": 4100.0,
|
||||
"z_min": -1680.0,
|
||||
"z_max": 200.0
|
||||
}
|
||||
|
||||
Semantics
|
||||
---------
|
||||
|
||||
- All units in **mm**, all coordinates in the CAD frame (x = longitudinal,
|
||||
y = width, z = height).
|
||||
- A point ``(x, y, z)`` is kept iff::
|
||||
|
||||
x_min <= x <= x_max and y_min <= y <= y_max and z_min <= z <= z_max
|
||||
|
||||
- To pass the whole cloud through unchanged, supply bounds wider than the
|
||||
cloud's own bbox (or use :func:`default_roi_spec`, which returns
|
||||
``±∞`` on every axis).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
|
||||
@dataclass
|
||||
class ROISpec:
|
||||
"""Axis-aligned bounding box in CAD-frame mm."""
|
||||
|
||||
x_min: float
|
||||
x_max: float
|
||||
y_min: float
|
||||
y_max: float
|
||||
z_min: float
|
||||
z_max: float
|
||||
|
||||
@property
|
||||
def kind(self) -> str:
|
||||
"""Stable identifier for logging / reporting."""
|
||||
return "aabb"
|
||||
|
||||
def to_dict(self) -> dict[str, float]:
|
||||
return {
|
||||
"x_min": float(self.x_min),
|
||||
"x_max": float(self.x_max),
|
||||
"y_min": float(self.y_min),
|
||||
"y_max": float(self.y_max),
|
||||
"z_min": float(self.z_min),
|
||||
"z_max": float(self.z_max),
|
||||
}
|
||||
|
||||
|
||||
def default_roi_spec() -> ROISpec:
|
||||
"""Pass-through ROI: infinite bounds on every axis."""
|
||||
inf = math.inf
|
||||
return ROISpec(
|
||||
x_min=-inf, x_max=inf,
|
||||
y_min=-inf, y_max=inf,
|
||||
z_min=-inf, z_max=inf,
|
||||
)
|
||||
|
||||
|
||||
def validate_roi_spec(spec: ROISpec) -> None:
|
||||
"""Raise ``ValueError`` if any axis has ``min >= max``."""
|
||||
pairs = (
|
||||
("x", spec.x_min, spec.x_max),
|
||||
("y", spec.y_min, spec.y_max),
|
||||
("z", spec.z_min, spec.z_max),
|
||||
)
|
||||
for axis, lo, hi in pairs:
|
||||
if not (lo < hi):
|
||||
raise ValueError(
|
||||
f"ROI {axis}_min must be strictly less than {axis}_max "
|
||||
f"(got {axis}_min={lo}, {axis}_max={hi})"
|
||||
)
|
||||
|
||||
|
||||
def load_roi_json(path: str | Path | None) -> ROISpec:
|
||||
"""Read a ROI JSON file. ``None`` → default pass-through spec.
|
||||
|
||||
The file must contain all six keys ``x_min`` / ``x_max`` / ``y_min`` /
|
||||
``y_max`` / ``z_min`` / ``z_max``. Extra keys are ignored (so you
|
||||
can attach a ``comment`` etc. without the loader complaining).
|
||||
"""
|
||||
if path is None:
|
||||
return default_roi_spec()
|
||||
path = Path(path)
|
||||
if not path.exists():
|
||||
raise FileNotFoundError(f"roi.json not found: {path}")
|
||||
with path.open("r", encoding="utf-8") as fh:
|
||||
data: dict[str, Any] = json.load(fh)
|
||||
|
||||
required = ("x_min", "x_max", "y_min", "y_max", "z_min", "z_max")
|
||||
missing = [k for k in required if k not in data]
|
||||
if missing:
|
||||
raise ValueError(f"roi.json {path} is missing required keys: {missing}")
|
||||
|
||||
spec = ROISpec(
|
||||
x_min=float(data["x_min"]),
|
||||
x_max=float(data["x_max"]),
|
||||
y_min=float(data["y_min"]),
|
||||
y_max=float(data["y_max"]),
|
||||
z_min=float(data["z_min"]),
|
||||
z_max=float(data["z_max"]),
|
||||
)
|
||||
validate_roi_spec(spec)
|
||||
return spec
|
||||
|
||||
|
||||
__all__ = [
|
||||
"ROISpec",
|
||||
"default_roi_spec",
|
||||
"validate_roi_spec",
|
||||
"load_roi_json",
|
||||
]
|
||||
@@ -0,0 +1,61 @@
|
||||
"""Mesh-to-point-cloud sampling strategies.
|
||||
|
||||
Unified entry point::
|
||||
|
||||
from coupler_panel.sampling import sample_mesh
|
||||
pts = sample_mesh(mesh, method="poisson_disk", num_points=100_000)
|
||||
|
||||
Each strategy lives in its own module; the dispatcher just routes by name.
|
||||
``mesh`` must expose ``vertices: (V, 3)`` and ``faces: (F, 3)`` as ndarrays
|
||||
(such as our :class:`coupler_panel.cad.mesh.TriangleMesh`).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .uniform_area import sample_uniform_area
|
||||
from .poisson_disk import sample_poisson_disk
|
||||
from .adaptive import sample_adaptive
|
||||
from .grid_spacing import sample_grid_spacing, num_points_from_spacing
|
||||
|
||||
|
||||
_DISPATCH = {
|
||||
"uniform_area": sample_uniform_area,
|
||||
"poisson_disk": sample_poisson_disk,
|
||||
"adaptive": sample_adaptive,
|
||||
"grid_spacing": sample_grid_spacing,
|
||||
}
|
||||
|
||||
|
||||
def sample_mesh(mesh, method: str, **kwargs) -> np.ndarray:
|
||||
"""Dispatch to the named strategy. ``**kwargs`` are passed through."""
|
||||
if method not in _DISPATCH:
|
||||
raise ValueError(
|
||||
f"unknown sampling method '{method}'; valid: {sorted(_DISPATCH)}"
|
||||
)
|
||||
return _DISPATCH[method](mesh, **kwargs)
|
||||
|
||||
|
||||
def resolve_num_points(mesh, *, num_points: int | None, spacing_mm: float | None) -> int:
|
||||
"""If ``spacing_mm`` is given, convert it to a point count using the
|
||||
mesh surface area; otherwise return ``num_points``.
|
||||
"""
|
||||
if spacing_mm is not None and spacing_mm > 0:
|
||||
return num_points_from_spacing(mesh, spacing_mm)
|
||||
if num_points is None:
|
||||
raise ValueError("either num_points or spacing_mm must be set")
|
||||
return int(num_points)
|
||||
|
||||
|
||||
__all__ = [
|
||||
"sample_mesh",
|
||||
"sample_uniform_area",
|
||||
"sample_poisson_disk",
|
||||
"sample_adaptive",
|
||||
"sample_grid_spacing",
|
||||
"resolve_num_points",
|
||||
"num_points_from_spacing",
|
||||
]
|
||||
@@ -0,0 +1,104 @@
|
||||
"""Curvature-adaptive sampling.
|
||||
|
||||
Idea: weight every triangle by ``area * (1 + k * curvature_proxy)``, then
|
||||
do area-weighted sampling on the *weighted* areas. The curvature proxy is
|
||||
``1 - mean_dot(face_normal, neighbour_face_normals)``, normalised to
|
||||
[0, 1] across the mesh.
|
||||
|
||||
This produces denser samples on rounded edges, hole boundaries, and small
|
||||
features without needing a real curvature operator.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .uniform_area import _triangle_areas, sample_uniform_area
|
||||
|
||||
|
||||
def _face_normals(vertices: np.ndarray, faces: np.ndarray) -> np.ndarray:
|
||||
a = vertices[faces[:, 0]]
|
||||
b = vertices[faces[:, 1]]
|
||||
c = vertices[faces[:, 2]]
|
||||
n = np.cross(b - a, c - a)
|
||||
norms = np.linalg.norm(n, axis=1, keepdims=True)
|
||||
norms[norms < 1e-12] = 1.0
|
||||
return n / norms
|
||||
|
||||
|
||||
def _vertex_face_adjacency(faces: np.ndarray, n_vertices: int) -> list[list[int]]:
|
||||
adj: list[list[int]] = [[] for _ in range(n_vertices)]
|
||||
for i, f in enumerate(faces):
|
||||
adj[f[0]].append(i)
|
||||
adj[f[1]].append(i)
|
||||
adj[f[2]].append(i)
|
||||
return adj
|
||||
|
||||
|
||||
def _curvature_proxy(vertices: np.ndarray, faces: np.ndarray) -> np.ndarray:
|
||||
"""Per-face roughness in [0, 1]."""
|
||||
fn = _face_normals(vertices, faces)
|
||||
adj = _vertex_face_adjacency(faces, vertices.shape[0])
|
||||
proxy = np.zeros(faces.shape[0], dtype=np.float64)
|
||||
for i, f in enumerate(faces):
|
||||
neighbours: set[int] = set()
|
||||
for v in f:
|
||||
neighbours.update(adj[v])
|
||||
neighbours.discard(i)
|
||||
if not neighbours:
|
||||
continue
|
||||
nb = fn[list(neighbours)]
|
||||
d = np.clip(nb @ fn[i], -1.0, 1.0)
|
||||
proxy[i] = 1.0 - np.mean(d)
|
||||
if proxy.max() > 0:
|
||||
proxy = proxy / proxy.max()
|
||||
return proxy
|
||||
|
||||
|
||||
def sample_adaptive(
|
||||
mesh,
|
||||
*,
|
||||
num_points: int,
|
||||
seed: int | None = 42,
|
||||
curvature_weight: float = 1.0,
|
||||
**_: Any,
|
||||
) -> np.ndarray:
|
||||
"""Curvature-weighted sampling."""
|
||||
if num_points <= 0:
|
||||
return np.empty((0, 3), dtype=np.float64)
|
||||
|
||||
v = np.asarray(mesh.vertices, dtype=np.float64)
|
||||
f = np.asarray(mesh.faces, dtype=np.int64)
|
||||
if f.shape[0] == 0:
|
||||
return np.empty((0, 3), dtype=np.float64)
|
||||
|
||||
# On large meshes the O(F·avg_degree) curvature pass is expensive; if we
|
||||
# are above ~200k faces, fall back to plain uniform_area, which is the
|
||||
# natural definition when curvature_weight = 0.
|
||||
if f.shape[0] > 200_000:
|
||||
return sample_uniform_area(mesh, num_points=num_points, seed=seed)
|
||||
|
||||
areas = _triangle_areas(v, f)
|
||||
proxy = _curvature_proxy(v, f)
|
||||
weights = areas * (1.0 + float(curvature_weight) * proxy)
|
||||
total = weights.sum()
|
||||
if total <= 0:
|
||||
return sample_uniform_area(mesh, num_points=num_points, seed=seed)
|
||||
|
||||
rng = np.random.default_rng(seed)
|
||||
probs = weights / total
|
||||
tri_idx = rng.choice(f.shape[0], size=num_points, p=probs)
|
||||
u = rng.random(num_points)
|
||||
w = rng.random(num_points)
|
||||
flip = u + w > 1.0
|
||||
u[flip] = 1.0 - u[flip]
|
||||
w[flip] = 1.0 - w[flip]
|
||||
a = v[f[tri_idx, 0]]
|
||||
b = v[f[tri_idx, 1]]
|
||||
c = v[f[tri_idx, 2]]
|
||||
return a + (b - a) * u[:, None] + (c - a) * w[:, None]
|
||||
|
||||
|
||||
__all__ = ["sample_adaptive"]
|
||||
@@ -0,0 +1,56 @@
|
||||
"""Spacing-driven sampling.
|
||||
|
||||
Translates a target inter-point spacing in mm into a point count, then
|
||||
defers to area-weighted sampling. Useful when you want to sweep over
|
||||
``spacing_mm ∈ {0.25, 0.5, 1, 2, 5}`` rather than over absolute counts.
|
||||
|
||||
For an isotropic surface sample, the mean nearest-neighbour distance is
|
||||
roughly ``sqrt(area / n)`` (square cells). We invert that:
|
||||
|
||||
n ≈ area / spacing²
|
||||
|
||||
and add a small fudge factor of 1.15 to compensate for sample variance.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .uniform_area import _triangle_areas, sample_uniform_area
|
||||
|
||||
|
||||
def num_points_from_spacing(mesh, spacing_mm: float, *, fudge: float = 1.15) -> int:
|
||||
if spacing_mm <= 0:
|
||||
raise ValueError("spacing_mm must be > 0")
|
||||
v = np.asarray(mesh.vertices, dtype=np.float64)
|
||||
f = np.asarray(mesh.faces, dtype=np.int64)
|
||||
total_area = float(_triangle_areas(v, f).sum())
|
||||
n = int(round(fudge * total_area / (spacing_mm ** 2)))
|
||||
return max(n, 1)
|
||||
|
||||
|
||||
def sample_grid_spacing(
|
||||
mesh,
|
||||
*,
|
||||
spacing_mm: float | None = None,
|
||||
num_points: int | None = None,
|
||||
seed: int | None = 42,
|
||||
fudge: float = 1.15,
|
||||
**_: Any,
|
||||
) -> np.ndarray:
|
||||
"""If ``spacing_mm`` is given, derive ``num_points`` from it.
|
||||
|
||||
Falls through to area-weighted uniform sampling.
|
||||
"""
|
||||
if spacing_mm is not None:
|
||||
n = num_points_from_spacing(mesh, float(spacing_mm), fudge=fudge)
|
||||
elif num_points is not None:
|
||||
n = int(num_points)
|
||||
else:
|
||||
raise ValueError("either spacing_mm or num_points must be set")
|
||||
return sample_uniform_area(mesh, num_points=n, seed=seed)
|
||||
|
||||
|
||||
__all__ = ["sample_grid_spacing", "num_points_from_spacing"]
|
||||
@@ -0,0 +1,95 @@
|
||||
"""Poisson-disk surface sampling.
|
||||
|
||||
Uses ``open3d.geometry.TriangleMesh.sample_points_poisson_disk`` when
|
||||
open3d is available; falls back to area-weighted uniform sampling
|
||||
followed by an O(N²) Bridson-style culling on small meshes.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .uniform_area import sample_uniform_area
|
||||
|
||||
|
||||
def _try_open3d():
|
||||
try:
|
||||
import open3d as o3d # type: ignore
|
||||
|
||||
return o3d
|
||||
except ImportError:
|
||||
return None
|
||||
|
||||
|
||||
def sample_poisson_disk(
|
||||
mesh,
|
||||
*,
|
||||
num_points: int,
|
||||
seed: int | None = 42,
|
||||
init_factor: int = 5,
|
||||
**_: Any,
|
||||
) -> np.ndarray:
|
||||
"""Poisson-disk samples on a triangle mesh."""
|
||||
if num_points <= 0:
|
||||
return np.empty((0, 3), dtype=np.float64)
|
||||
|
||||
o3d = _try_open3d()
|
||||
if o3d is not None:
|
||||
# open3d wants its own mesh class.
|
||||
m = o3d.geometry.TriangleMesh()
|
||||
m.vertices = o3d.utility.Vector3dVector(np.asarray(mesh.vertices, dtype=np.float64))
|
||||
m.triangles = o3d.utility.Vector3iVector(np.asarray(mesh.faces, dtype=np.int32))
|
||||
m.compute_vertex_normals()
|
||||
# Open3D's API takes seed via 'seed' kwarg (0.18+) — passing it best-effort.
|
||||
try:
|
||||
pcd = m.sample_points_poisson_disk(
|
||||
number_of_points=int(num_points),
|
||||
init_factor=int(init_factor),
|
||||
seed=int(seed) if seed is not None else -1,
|
||||
)
|
||||
except TypeError:
|
||||
pcd = m.sample_points_poisson_disk(
|
||||
number_of_points=int(num_points),
|
||||
init_factor=int(init_factor),
|
||||
)
|
||||
return np.asarray(pcd.points, dtype=np.float64)
|
||||
|
||||
# ---- Fallback: oversample + grid cull ---------------------------------
|
||||
over = sample_uniform_area(
|
||||
mesh,
|
||||
num_points=num_points * max(init_factor, 5),
|
||||
seed=seed,
|
||||
)
|
||||
# Estimate target spacing from mesh bbox.
|
||||
bbox = over.max(axis=0) - over.min(axis=0)
|
||||
if num_points <= 1:
|
||||
return over[:num_points]
|
||||
target_spacing = float(np.cbrt(np.prod(bbox)) / max(num_points, 1) ** (1 / 3))
|
||||
return _grid_cull(over, target_spacing, num_points, seed)
|
||||
|
||||
|
||||
def _grid_cull(
|
||||
points: np.ndarray,
|
||||
spacing: float,
|
||||
keep_n: int,
|
||||
seed: int | None,
|
||||
) -> np.ndarray:
|
||||
"""Pick at most one point per voxel of size ``spacing``."""
|
||||
rng = np.random.default_rng(seed)
|
||||
perm = rng.permutation(points.shape[0])
|
||||
voxel = np.floor(points[perm] / max(spacing, 1e-6)).astype(np.int64)
|
||||
seen: set[tuple[int, int, int]] = set()
|
||||
out_idx: list[int] = []
|
||||
for k, key in enumerate(map(tuple, voxel)):
|
||||
if key in seen:
|
||||
continue
|
||||
seen.add(key)
|
||||
out_idx.append(perm[k])
|
||||
if len(out_idx) >= keep_n:
|
||||
break
|
||||
return points[np.asarray(out_idx)]
|
||||
|
||||
|
||||
__all__ = ["sample_poisson_disk"]
|
||||
@@ -0,0 +1,53 @@
|
||||
"""Area-weighted uniform triangle sampling."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _triangle_areas(vertices: np.ndarray, faces: np.ndarray) -> np.ndarray:
|
||||
a = vertices[faces[:, 0]]
|
||||
b = vertices[faces[:, 1]]
|
||||
c = vertices[faces[:, 2]]
|
||||
return 0.5 * np.linalg.norm(np.cross(b - a, c - a), axis=1)
|
||||
|
||||
|
||||
def sample_uniform_area(
|
||||
mesh,
|
||||
*,
|
||||
num_points: int,
|
||||
seed: int | None = 42,
|
||||
**_: Any,
|
||||
) -> np.ndarray:
|
||||
"""Sample ``num_points`` from a triangle mesh, weighted by area.
|
||||
|
||||
Returns an ``(N, 3) float64`` ndarray.
|
||||
"""
|
||||
if num_points <= 0:
|
||||
return np.empty((0, 3), dtype=np.float64)
|
||||
|
||||
v = np.asarray(mesh.vertices, dtype=np.float64)
|
||||
f = np.asarray(mesh.faces, dtype=np.int64)
|
||||
areas = _triangle_areas(v, f)
|
||||
total = float(areas.sum())
|
||||
if total <= 0:
|
||||
raise ValueError("mesh has zero total area")
|
||||
|
||||
rng = np.random.default_rng(seed)
|
||||
|
||||
probs = areas / total
|
||||
tri_idx = rng.choice(len(f), size=num_points, p=probs)
|
||||
u = rng.random(num_points)
|
||||
w = rng.random(num_points)
|
||||
flip = u + w > 1.0
|
||||
u[flip] = 1.0 - u[flip]
|
||||
w[flip] = 1.0 - w[flip]
|
||||
a = v[f[tri_idx, 0]]
|
||||
b = v[f[tri_idx, 1]]
|
||||
c = v[f[tri_idx, 2]]
|
||||
return a + (b - a) * u[:, None] + (c - a) * w[:, None]
|
||||
|
||||
|
||||
__all__ = ["sample_uniform_area"]
|
||||
@@ -0,0 +1,21 @@
|
||||
"""Synthetic-scan utilities: noise, rigid transform, optional view simulator."""
|
||||
|
||||
from .noise import add_gaussian_noise
|
||||
from .transform import (
|
||||
RandomRigidTransform,
|
||||
apply_rigid,
|
||||
compose_rigid,
|
||||
invert_rigid,
|
||||
sample_random_rigid,
|
||||
)
|
||||
from .scan_simulator import simulate_scan_view
|
||||
|
||||
__all__ = [
|
||||
"add_gaussian_noise",
|
||||
"RandomRigidTransform",
|
||||
"apply_rigid",
|
||||
"compose_rigid",
|
||||
"invert_rigid",
|
||||
"sample_random_rigid",
|
||||
"simulate_scan_view",
|
||||
]
|
||||
@@ -0,0 +1,57 @@
|
||||
"""Synthetic Gaussian noise generation."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def add_gaussian_noise(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
sigma_mm: float,
|
||||
mode: str = "isotropic",
|
||||
normals: np.ndarray | None = None,
|
||||
seed: int | None = 43,
|
||||
) -> np.ndarray:
|
||||
"""Return a noisy copy of ``points``.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
points : (N, 3)
|
||||
sigma_mm : float
|
||||
Standard deviation in mm. ``0`` is a no-op (returns a copy).
|
||||
mode : {"isotropic", "along_normal"}
|
||||
``isotropic`` — N(0, σ²) noise added independently to x/y/z.
|
||||
``along_normal`` — N(0, σ²) along ``normals`` only. Requires
|
||||
a per-point normal of shape ``(N, 3)``.
|
||||
normals : (N, 3) or None
|
||||
Required when ``mode == "along_normal"``.
|
||||
"""
|
||||
points = np.asarray(points, dtype=np.float64)
|
||||
if sigma_mm == 0:
|
||||
return points.copy()
|
||||
if sigma_mm < 0:
|
||||
raise ValueError("sigma_mm must be ≥ 0")
|
||||
|
||||
rng = np.random.default_rng(seed)
|
||||
if mode == "isotropic":
|
||||
return points + rng.normal(0.0, sigma_mm, size=points.shape)
|
||||
|
||||
if mode == "along_normal":
|
||||
if normals is None:
|
||||
raise ValueError("along_normal mode requires per-point normals")
|
||||
normals = np.asarray(normals, dtype=np.float64)
|
||||
if normals.shape != points.shape:
|
||||
raise ValueError(
|
||||
f"normals shape {normals.shape} != points shape {points.shape}"
|
||||
)
|
||||
norms = np.linalg.norm(normals, axis=1, keepdims=True)
|
||||
norms[norms < 1e-12] = 1.0
|
||||
unit = normals / norms
|
||||
offset = rng.normal(0.0, sigma_mm, size=points.shape[0])[:, None]
|
||||
return points + unit * offset
|
||||
|
||||
raise ValueError(f"unknown noise mode '{mode}'")
|
||||
|
||||
|
||||
__all__ = ["add_gaussian_noise"]
|
||||
@@ -0,0 +1,80 @@
|
||||
"""Single-view scan simulator (optional).
|
||||
|
||||
Models a structured-light / line-laser scanner that only sees points
|
||||
visible from a single camera origin. Implementation is the *hidden-point
|
||||
removal* (HPR) of Katz, Tal & Basri (2007), which works directly on a
|
||||
point cloud — no mesh raycasting needed.
|
||||
|
||||
Open3D ships an HPR primitive; we route to that when available, with a
|
||||
plain-numpy fallback for the simple convex-hull formulation.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _try_open3d():
|
||||
try:
|
||||
import open3d as o3d # type: ignore
|
||||
|
||||
return o3d
|
||||
except ImportError:
|
||||
return None
|
||||
|
||||
|
||||
def simulate_scan_view(
|
||||
points: np.ndarray,
|
||||
*,
|
||||
view_origin,
|
||||
hpr_radius_mm: float,
|
||||
) -> np.ndarray:
|
||||
"""Return only the points visible from ``view_origin``.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
view_origin : (3,)
|
||||
hpr_radius_mm : float
|
||||
HPR sphere radius. Larger = more points kept. Pick something
|
||||
much larger than the cloud diameter (e.g. 100× diameter) to
|
||||
approximate a "all front-facing surfaces" view.
|
||||
"""
|
||||
pts = np.asarray(points, dtype=np.float64)
|
||||
o = np.asarray(view_origin, dtype=np.float64).reshape(3)
|
||||
|
||||
o3d = _try_open3d()
|
||||
if o3d is not None:
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(pts)
|
||||
_, idx = pcd.hidden_point_removal(o, float(hpr_radius_mm))
|
||||
return pts[np.asarray(idx, dtype=np.int64)]
|
||||
|
||||
# ---- numpy fallback: spherical-flip + convex-hull selection ----------
|
||||
# Flip every point through a sphere of radius R centred at the view.
|
||||
diff = pts - o
|
||||
norms = np.linalg.norm(diff, axis=1, keepdims=True)
|
||||
norms[norms < 1e-12] = 1.0
|
||||
R = float(hpr_radius_mm)
|
||||
flipped = pts + 2.0 * (R - norms) * (diff / norms)
|
||||
# Add the viewpoint itself as a phantom point so its convex hull
|
||||
# encloses the visible-from-origin set.
|
||||
augmented = np.vstack([flipped, o[None, :]])
|
||||
try:
|
||||
from scipy.spatial import ConvexHull
|
||||
|
||||
hull = ConvexHull(augmented, qhull_options="QJ")
|
||||
keep = np.asarray(hull.vertices, dtype=np.int64)
|
||||
# Drop the phantom point.
|
||||
keep = keep[keep < pts.shape[0]]
|
||||
return pts[keep]
|
||||
except Exception:
|
||||
# Last-resort: keep points whose dot with (point - origin) along
|
||||
# the centroid axis is positive. Crude but never crashes.
|
||||
centroid = pts.mean(axis=0)
|
||||
view_dir = centroid - o
|
||||
view_dir /= max(np.linalg.norm(view_dir), 1e-12)
|
||||
d = (pts - o) @ view_dir
|
||||
return pts[d > 0]
|
||||
|
||||
|
||||
__all__ = ["simulate_scan_view"]
|
||||
@@ -0,0 +1,162 @@
|
||||
"""Random rigid transforms used to mimic mounting jitter / scan pose noise.
|
||||
|
||||
The transform is stored as a 4×4 homogeneous matrix; the GT used for
|
||||
validation is exactly this matrix. The 4-hole Kabsch registration step
|
||||
should recover its inverse up to noise.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RandomRigidTransform:
|
||||
"""Outcome of one random rigid sample."""
|
||||
|
||||
matrix: np.ndarray # (4, 4) float64
|
||||
rotation_deg: float
|
||||
rotation_axis: tuple[float, float, float]
|
||||
translation_mm: float
|
||||
translation_dir: tuple[float, float, float]
|
||||
seed: int | None
|
||||
|
||||
def to_dict(self) -> dict:
|
||||
return {
|
||||
"matrix": self.matrix.tolist(),
|
||||
"rotation_deg": self.rotation_deg,
|
||||
"rotation_axis": list(self.rotation_axis),
|
||||
"translation_mm": self.translation_mm,
|
||||
"translation_dir": list(self.translation_dir),
|
||||
"seed": self.seed,
|
||||
}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Construction
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _rodrigues(axis: np.ndarray, angle_rad: float) -> np.ndarray:
|
||||
"""Rodrigues rotation around a unit ``axis``."""
|
||||
axis = axis / max(np.linalg.norm(axis), 1e-12)
|
||||
K = np.array(
|
||||
[
|
||||
[0.0, -axis[2], axis[1]],
|
||||
[axis[2], 0.0, -axis[0]],
|
||||
[-axis[1], axis[0], 0.0],
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
s = np.sin(angle_rad)
|
||||
c = np.cos(angle_rad)
|
||||
return np.eye(3) + s * K + (1.0 - c) * (K @ K)
|
||||
|
||||
|
||||
def _resolve_axis(axis_spec, rng) -> np.ndarray:
|
||||
if isinstance(axis_spec, str):
|
||||
spec = axis_spec.lower()
|
||||
if spec == "x":
|
||||
return np.array([1.0, 0.0, 0.0])
|
||||
if spec == "y":
|
||||
return np.array([0.0, 1.0, 0.0])
|
||||
if spec == "z":
|
||||
return np.array([0.0, 0.0, 1.0])
|
||||
if spec == "random":
|
||||
v = rng.normal(size=3)
|
||||
return v / max(np.linalg.norm(v), 1e-12)
|
||||
raise ValueError(f"unknown rotation_axis '{axis_spec}'")
|
||||
arr = np.asarray(axis_spec, dtype=np.float64)
|
||||
if arr.shape != (3,):
|
||||
raise ValueError("rotation_axis must be 'x'/'y'/'z'/'random' or a 3-vector")
|
||||
n = np.linalg.norm(arr)
|
||||
if n < 1e-12:
|
||||
raise ValueError("rotation_axis vector is degenerate")
|
||||
return arr / n
|
||||
|
||||
|
||||
def _resolve_dir(dir_spec, rng) -> np.ndarray:
|
||||
if isinstance(dir_spec, str):
|
||||
if dir_spec == "random":
|
||||
v = rng.normal(size=3)
|
||||
return v / max(np.linalg.norm(v), 1e-12)
|
||||
raise ValueError(f"unknown translation_dir '{dir_spec}'")
|
||||
arr = np.asarray(dir_spec, dtype=np.float64)
|
||||
if arr.shape != (3,):
|
||||
raise ValueError("translation_dir must be 'random' or a 3-vector")
|
||||
n = np.linalg.norm(arr)
|
||||
if n < 1e-12:
|
||||
raise ValueError("translation_dir vector is degenerate")
|
||||
return arr / n
|
||||
|
||||
|
||||
def sample_random_rigid(
|
||||
*,
|
||||
rotation_deg: float,
|
||||
translation_mm: float,
|
||||
rotation_axis: str | Sequence[float] = "random",
|
||||
translation_dir: str | Sequence[float] = "random",
|
||||
seed: int | None = 44,
|
||||
) -> RandomRigidTransform:
|
||||
rng = np.random.default_rng(seed)
|
||||
axis = _resolve_axis(rotation_axis, rng)
|
||||
direction = _resolve_dir(translation_dir, rng)
|
||||
|
||||
R = _rodrigues(axis, np.deg2rad(float(rotation_deg)))
|
||||
t = direction * float(translation_mm)
|
||||
|
||||
M = np.eye(4)
|
||||
M[:3, :3] = R
|
||||
M[:3, 3] = t
|
||||
|
||||
return RandomRigidTransform(
|
||||
matrix=M,
|
||||
rotation_deg=float(rotation_deg),
|
||||
rotation_axis=tuple(axis.tolist()),
|
||||
translation_mm=float(translation_mm),
|
||||
translation_dir=tuple(direction.tolist()),
|
||||
seed=seed,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Application
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def apply_rigid(points: np.ndarray, T: np.ndarray) -> np.ndarray:
|
||||
"""Apply a 4×4 homogeneous transform to an ``(N, 3)`` cloud."""
|
||||
P = np.asarray(points, dtype=np.float64)
|
||||
if P.ndim != 2 or P.shape[1] != 3:
|
||||
raise ValueError(f"expected (N,3), got {P.shape}")
|
||||
R = np.asarray(T, dtype=np.float64)[:3, :3]
|
||||
t = np.asarray(T, dtype=np.float64)[:3, 3]
|
||||
return P @ R.T + t
|
||||
|
||||
|
||||
def compose_rigid(*Ts: np.ndarray) -> np.ndarray:
|
||||
out = np.eye(4)
|
||||
for T in Ts:
|
||||
out = out @ np.asarray(T, dtype=np.float64)
|
||||
return out
|
||||
|
||||
|
||||
def invert_rigid(T: np.ndarray) -> np.ndarray:
|
||||
R = T[:3, :3]
|
||||
t = T[:3, 3]
|
||||
inv = np.eye(4)
|
||||
inv[:3, :3] = R.T
|
||||
inv[:3, 3] = -R.T @ t
|
||||
return inv
|
||||
|
||||
|
||||
__all__ = [
|
||||
"RandomRigidTransform",
|
||||
"sample_random_rigid",
|
||||
"apply_rigid",
|
||||
"compose_rigid",
|
||||
"invert_rigid",
|
||||
]
|
||||
@@ -0,0 +1,148 @@
|
||||
"""Result dataclasses for coupler_panel.
|
||||
|
||||
These types form the **stable public schema** of the package; their
|
||||
field names and semantics are guarded by ``SCHEMA_VERSION`` (see
|
||||
``_version.py``).
|
||||
|
||||
All numeric units are **mm** for distances and **degree** for angles
|
||||
unless otherwise noted.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import asdict, dataclass, field
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Building blocks
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Hole:
|
||||
"""A single detected (or GT) circular hole on the panel."""
|
||||
|
||||
center: tuple[float, float, float] # (x, y, z), mm
|
||||
radius_mm: float
|
||||
fit_residual_rms_mm: float # 0 for GT
|
||||
role: str # "locating" | "central" | "unknown"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class HoleSet:
|
||||
"""The four locating holes plus an optional central hole.
|
||||
|
||||
``locating`` is ordered consistently between GT and detection so that
|
||||
Kabsch pairing is index-aligned.
|
||||
"""
|
||||
|
||||
locating: tuple[Hole, Hole, Hole, Hole]
|
||||
central: Hole | None = None
|
||||
|
||||
def locating_centers(self) -> np.ndarray:
|
||||
"""Return a (4, 3) ndarray of locating-hole centres."""
|
||||
return np.asarray([h.center for h in self.locating], dtype=np.float64)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PanelPlane:
|
||||
"""The fitted (or GT) panel plane."""
|
||||
|
||||
normal: tuple[float, float, float] # unit vector, n_x > 0 enforced
|
||||
point_on_plane: tuple[float, float, float]
|
||||
d_offset: float # n . x + d = 0
|
||||
inlier_ratio: float # 1.0 for GT
|
||||
inlier_residual_rms_mm: float # 0 for GT
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ReferenceFrame:
|
||||
"""Reference geometry derived from the four-hole panel centre."""
|
||||
|
||||
center_C: tuple[float, float, float]
|
||||
# Reference line L = C + lam * e_x (always parallel to the +x axis).
|
||||
line_direction: tuple[float, float, float] = (1.0, 0.0, 0.0)
|
||||
# Reference plane: z = C_z. Stored as plane normal + offset.
|
||||
reference_plane_z: float = 0.0
|
||||
# Longitudinal mid-plane: y = C_y.
|
||||
longitudinal_mid_y: float = 0.0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class StageTiming:
|
||||
"""Per-stage wall-clock and peak-memory cost."""
|
||||
|
||||
stage: str
|
||||
seconds: float
|
||||
peak_kib: float
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Top-level result
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
@dataclass
|
||||
class CouplerMeasurement:
|
||||
"""Top-level result of the coupler-panel measurement pipeline.
|
||||
|
||||
See ``docs/USAGE.md`` for the stable-API contract.
|
||||
"""
|
||||
|
||||
schema_version: str
|
||||
algorithm_version: str
|
||||
|
||||
# --- Primary measurands ---
|
||||
theta_right_deg: float # atan2(n_y, n_x), with right_axis_sign applied
|
||||
theta_ref_deg: float # atan2(n_z, n_x)
|
||||
D_y_max_mm: float # max |y_i - C_y| over panel ROI
|
||||
D_z_max_mm: float # max |z_i - C_z| over panel ROI
|
||||
dx_y_max_mm: float # |A| * D_y_max, A = -n_y / n_x
|
||||
dx_z_max_mm: float # |B| * D_z_max, B = -n_z / n_x
|
||||
|
||||
# --- Geometry ---
|
||||
center_C: tuple[float, float, float]
|
||||
normal: tuple[float, float, float] # n_x > 0
|
||||
holes: HoleSet
|
||||
plane: PanelPlane
|
||||
frame: ReferenceFrame
|
||||
|
||||
# --- Diagnostics ---
|
||||
quality_flags: list[str] = field(default_factory=list)
|
||||
timings: list[StageTiming] = field(default_factory=list)
|
||||
report: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
"""Return a fully JSON-serialisable dict."""
|
||||
return _to_jsonable(asdict(self))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
def _to_jsonable(obj: Any) -> Any:
|
||||
"""Recursively convert numpy / tuple structures into plain Python types."""
|
||||
if isinstance(obj, dict):
|
||||
return {k: _to_jsonable(v) for k, v in obj.items()}
|
||||
if isinstance(obj, (list, tuple)):
|
||||
return [_to_jsonable(v) for v in obj]
|
||||
if isinstance(obj, np.ndarray):
|
||||
return obj.tolist()
|
||||
if isinstance(obj, (np.floating, np.integer)):
|
||||
return obj.item()
|
||||
return obj
|
||||
|
||||
|
||||
__all__ = [
|
||||
"Hole",
|
||||
"HoleSet",
|
||||
"PanelPlane",
|
||||
"ReferenceFrame",
|
||||
"StageTiming",
|
||||
"CouplerMeasurement",
|
||||
]
|
||||
@@ -0,0 +1,21 @@
|
||||
"""Diagnostic visualisation utilities (matplotlib + open3d offscreen).
|
||||
|
||||
All renderers degrade gracefully if matplotlib / open3d are missing —
|
||||
they raise :class:`ImportError` only when actually called. The pipeline
|
||||
calls them via :func:`render_all`, which silently skips on missing deps.
|
||||
"""
|
||||
|
||||
from .cloud_view import render_cloud, render_cloud_compare
|
||||
from .holes_view import render_holes
|
||||
from .plane_view import render_plane_with_refs
|
||||
from .heatmap import render_panel_heatmap
|
||||
from .render_all import render_all
|
||||
|
||||
__all__ = [
|
||||
"render_cloud",
|
||||
"render_cloud_compare",
|
||||
"render_holes",
|
||||
"render_plane_with_refs",
|
||||
"render_panel_heatmap",
|
||||
"render_all",
|
||||
]
|
||||
@@ -0,0 +1,27 @@
|
||||
"""Open3D offscreen renderer factory.
|
||||
|
||||
Wraps the shifting open3d offscreen API so callers don't have to. Returns
|
||||
``None`` if open3d is not available — callers should fall back to
|
||||
matplotlib in that case.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
|
||||
def make_offscreen_renderer(width: int = 1024, height: int = 768) -> Any | None:
|
||||
try:
|
||||
import open3d as o3d # type: ignore
|
||||
except ImportError:
|
||||
return None
|
||||
|
||||
# open3d's offscreen path moved between 0.16 / 0.18.
|
||||
try:
|
||||
renderer = o3d.visualization.rendering.OffscreenRenderer(width, height)
|
||||
return renderer
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
__all__ = ["make_offscreen_renderer"]
|
||||
@@ -0,0 +1,134 @@
|
||||
"""Point cloud rendering using matplotlib.
|
||||
|
||||
By default we render a 2D (y, z) projection — the coupler panel is a thin
|
||||
sheet whose normal is along x, so a top-down y-z view is more readable
|
||||
than a 3D scatter. Pass ``mode="3d"`` to force a 3D scatter (slower and
|
||||
broken on some matplotlib versions, kept for completeness).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _import_plt():
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt # noqa: WPS433
|
||||
|
||||
return plt
|
||||
|
||||
|
||||
def _decimate(points: np.ndarray, max_points: int) -> np.ndarray:
|
||||
n = points.shape[0]
|
||||
if n <= max_points:
|
||||
return points
|
||||
rng = np.random.default_rng(0)
|
||||
idx = rng.choice(n, size=max_points, replace=False)
|
||||
return points[idx]
|
||||
|
||||
|
||||
def render_cloud(
|
||||
points: np.ndarray,
|
||||
out_path: str | Path,
|
||||
*,
|
||||
title: str = "",
|
||||
color: str | np.ndarray = "tab:blue",
|
||||
max_points: int = 80_000,
|
||||
dpi: int = 150,
|
||||
mode: str = "2d",
|
||||
) -> Path:
|
||||
plt = _import_plt()
|
||||
pts = _decimate(np.asarray(points, dtype=np.float64), max_points)
|
||||
|
||||
if mode == "3d":
|
||||
return _render_3d(pts, out_path, title=title, color=color, dpi=dpi)
|
||||
|
||||
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
|
||||
axes[0].scatter(pts[:, 1], pts[:, 2], s=0.5, c=color)
|
||||
axes[0].set_xlabel("y (mm)")
|
||||
axes[0].set_ylabel("z (mm)")
|
||||
axes[0].set_aspect("equal")
|
||||
axes[0].set_title("y-z projection (panel face)")
|
||||
|
||||
axes[1].scatter(pts[:, 0], pts[:, 2], s=0.5, c=color)
|
||||
axes[1].set_xlabel("x (mm)")
|
||||
axes[1].set_ylabel("z (mm)")
|
||||
axes[1].set_aspect("equal")
|
||||
axes[1].set_title("x-z projection (side view)")
|
||||
|
||||
if title:
|
||||
fig.suptitle(title)
|
||||
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
def render_cloud_compare(
|
||||
clouds: dict[str, np.ndarray],
|
||||
out_path: str | Path,
|
||||
*,
|
||||
title: str = "",
|
||||
max_points: int = 50_000,
|
||||
dpi: int = 150,
|
||||
) -> Path:
|
||||
"""Side-by-side y-z projections of several clouds."""
|
||||
plt = _import_plt()
|
||||
n = len(clouds)
|
||||
fig, axes = plt.subplots(1, n, figsize=(6 * n, 6), squeeze=False)
|
||||
cmap = plt.get_cmap("tab10")
|
||||
for i, (name, pts) in enumerate(clouds.items()):
|
||||
ax = axes[0, i]
|
||||
sub = _decimate(np.asarray(pts, dtype=np.float64), max_points)
|
||||
ax.scatter(sub[:, 1], sub[:, 2], s=0.5, c=[cmap(i)])
|
||||
ax.set_title(name)
|
||||
ax.set_xlabel("y (mm)")
|
||||
ax.set_ylabel("z (mm)")
|
||||
ax.set_aspect("equal")
|
||||
if title:
|
||||
fig.suptitle(title)
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
def _render_3d(pts, out_path, *, title, color, dpi):
|
||||
plt = _import_plt()
|
||||
fig = plt.figure(figsize=(8, 6))
|
||||
try:
|
||||
ax = fig.add_subplot(111, projection="3d")
|
||||
ax.scatter(pts[:, 0], pts[:, 1], pts[:, 2], s=0.5, c=color)
|
||||
ax.set_xlabel("x (mm)")
|
||||
ax.set_ylabel("y (mm)")
|
||||
ax.set_zlabel("z (mm)")
|
||||
if title:
|
||||
ax.set_title(title)
|
||||
except Exception:
|
||||
# Fallback to 2D when matplotlib's 3D backend is broken (e.g. mpl 3.10
|
||||
# has a known incompatibility with mpl_toolkits.mplot3d).
|
||||
ax = fig.add_subplot(111)
|
||||
ax.scatter(pts[:, 1], pts[:, 2], s=0.5, c=color)
|
||||
ax.set_aspect("equal")
|
||||
ax.set_xlabel("y (mm)")
|
||||
ax.set_ylabel("z (mm)")
|
||||
if title:
|
||||
ax.set_title(f"{title} (3D backend unavailable)")
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
__all__ = ["render_cloud", "render_cloud_compare"]
|
||||
@@ -0,0 +1,73 @@
|
||||
"""Panel deviation heatmap.
|
||||
|
||||
Colours each panel-ROI point by its signed perpendicular distance to
|
||||
the fitted plane, projected onto a 2D image of the panel face.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _import_plt():
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
return plt
|
||||
|
||||
|
||||
def render_panel_heatmap(
|
||||
panel_points: np.ndarray,
|
||||
*,
|
||||
plane_normal: tuple[float, float, float],
|
||||
plane_d: float,
|
||||
center_C: tuple[float, float, float],
|
||||
out_path: str | Path,
|
||||
title: str = "",
|
||||
cmap: str = "viridis",
|
||||
dpi: int = 150,
|
||||
) -> Path:
|
||||
plt = _import_plt()
|
||||
|
||||
pts = np.asarray(panel_points, dtype=np.float64)
|
||||
n = np.asarray(plane_normal, dtype=np.float64)
|
||||
n = n / max(np.linalg.norm(n), 1e-12)
|
||||
C = np.asarray(center_C, dtype=np.float64)
|
||||
|
||||
# Local 2D frame on the panel.
|
||||
y_world = np.array([0.0, 1.0, 0.0])
|
||||
u = y_world - np.dot(y_world, n) * n
|
||||
if np.linalg.norm(u) < 1e-6:
|
||||
u = np.array([0.0, 0.0, 1.0]) - np.dot(np.array([0.0, 0.0, 1.0]), n) * n
|
||||
u /= np.linalg.norm(u)
|
||||
v = np.cross(n, u)
|
||||
|
||||
rel = pts - C
|
||||
uu = rel @ u
|
||||
vv = rel @ v
|
||||
signed_dist = pts @ n + float(plane_d)
|
||||
|
||||
fig, ax = plt.subplots(figsize=(8, 7))
|
||||
sc = ax.scatter(uu, vv, c=signed_dist, cmap=cmap, s=2)
|
||||
cb = plt.colorbar(sc, ax=ax)
|
||||
cb.set_label("signed distance to fitted plane (mm)")
|
||||
|
||||
ax.set_aspect("equal")
|
||||
ax.set_xlabel("panel u (≈ y axis, mm)")
|
||||
ax.set_ylabel("panel v (≈ z axis, mm)")
|
||||
if title:
|
||||
ax.set_title(title)
|
||||
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
__all__ = ["render_panel_heatmap"]
|
||||
@@ -0,0 +1,70 @@
|
||||
"""4-hole detection diagnostic plot.
|
||||
|
||||
Shows the panel cloud projected to its local 2D frame, with each
|
||||
detected circle drawn on top and the four selected locating holes
|
||||
highlighted.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _import_plt():
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.patches import Circle # noqa: WPS433
|
||||
|
||||
return plt, Circle
|
||||
|
||||
|
||||
def render_holes(
|
||||
uv_points: np.ndarray,
|
||||
candidates: list,
|
||||
selected_indices: list[int],
|
||||
out_path: str | Path,
|
||||
*,
|
||||
title: str = "",
|
||||
dpi: int = 150,
|
||||
) -> Path:
|
||||
plt, Circle = _import_plt()
|
||||
fig, ax = plt.subplots(figsize=(8, 8))
|
||||
if uv_points.shape[0] > 50_000:
|
||||
idx = np.random.default_rng(0).choice(uv_points.shape[0], 50_000, replace=False)
|
||||
sub = uv_points[idx]
|
||||
else:
|
||||
sub = uv_points
|
||||
ax.scatter(sub[:, 0], sub[:, 1], s=0.3, c="lightgray")
|
||||
|
||||
for i, cand in enumerate(candidates):
|
||||
is_sel = i in set(selected_indices)
|
||||
col = "tab:red" if is_sel else "tab:blue"
|
||||
ax.add_patch(
|
||||
Circle(
|
||||
(cand.fit.cx, cand.fit.cy),
|
||||
cand.fit.radius,
|
||||
fill=False,
|
||||
edgecolor=col,
|
||||
lw=2 if is_sel else 1,
|
||||
)
|
||||
)
|
||||
ax.plot(cand.fit.cx, cand.fit.cy, "+", color=col)
|
||||
|
||||
ax.set_aspect("equal")
|
||||
ax.set_xlabel("u (mm)")
|
||||
ax.set_ylabel("v (mm)")
|
||||
if title:
|
||||
ax.set_title(title)
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
__all__ = ["render_holes"]
|
||||
@@ -0,0 +1,105 @@
|
||||
"""Plane + reference geometry overlay (2D projection).
|
||||
|
||||
Renders the y-z view of the panel ROI with the fitted plane normal,
|
||||
reference line L, reference plane z=C_z, and longitudinal mid y=C_y
|
||||
overlaid as annotations.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _import_plt():
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
return plt
|
||||
|
||||
|
||||
def render_plane_with_refs(
|
||||
panel_points: np.ndarray,
|
||||
*,
|
||||
plane_normal: tuple[float, float, float],
|
||||
center_C: tuple[float, float, float],
|
||||
out_path: str | Path,
|
||||
title: str = "",
|
||||
line_length_mm: float = 200.0,
|
||||
dpi: int = 150,
|
||||
) -> Path:
|
||||
plt = _import_plt()
|
||||
|
||||
pts = np.asarray(panel_points, dtype=np.float64)
|
||||
n = np.asarray(plane_normal, dtype=np.float64)
|
||||
n = n / max(np.linalg.norm(n), 1e-12)
|
||||
C = np.asarray(center_C, dtype=np.float64)
|
||||
|
||||
if pts.shape[0] > 80_000:
|
||||
idx = np.random.default_rng(0).choice(pts.shape[0], 80_000, replace=False)
|
||||
pts = pts[idx]
|
||||
|
||||
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
|
||||
|
||||
# Panel face view: y-z, with a small arrow for the normal projected
|
||||
# into y-z (the in-plane part of n).
|
||||
ax = axes[0]
|
||||
ax.scatter(pts[:, 1], pts[:, 2], s=0.5, c="lightgray")
|
||||
# Reference plane z = C_z
|
||||
ax.axhline(C[2], color="tab:green", lw=1, alpha=0.7, label=f"ref plane z = {C[2]:.2f}")
|
||||
# Longitudinal mid y = C_y
|
||||
ax.axvline(C[1], color="tab:orange", lw=1, alpha=0.7, label=f"long. mid y = {C[1]:.2f}")
|
||||
ax.scatter([C[1]], [C[2]], color="black", s=40, zorder=5, label=f"C = ({C[1]:.1f}, {C[2]:.1f})")
|
||||
# Project normal into y-z
|
||||
n_yz = np.array([n[1], n[2]])
|
||||
if np.linalg.norm(n_yz) > 1e-3:
|
||||
scale = line_length_mm * 0.5
|
||||
ax.annotate(
|
||||
"",
|
||||
xy=(C[1] + n_yz[0] * scale, C[2] + n_yz[1] * scale),
|
||||
xytext=(C[1], C[2]),
|
||||
arrowprops=dict(arrowstyle="->", color="tab:red", lw=2),
|
||||
)
|
||||
ax.set_aspect("equal")
|
||||
ax.set_xlabel("y (mm)")
|
||||
ax.set_ylabel("z (mm)")
|
||||
ax.set_title("y-z view: ROI + reference geometry + n's y-z projection")
|
||||
ax.legend(loc="upper right", fontsize="x-small")
|
||||
|
||||
# x-y side view: shows the panel tilt out of x.
|
||||
ax = axes[1]
|
||||
ax.scatter(pts[:, 0], pts[:, 1], s=0.5, c="lightgray")
|
||||
# Reference line L (along +x at y=C_y, z=C_z, projected to x-y → horizontal at y=C_y)
|
||||
L = float(line_length_mm)
|
||||
ax.plot([C[0] - L, C[0] + L], [C[1], C[1]], color="tab:blue", lw=2, label="ref line L")
|
||||
# Plane normal (x, y components)
|
||||
n_xy = np.array([n[0], n[1]])
|
||||
scale = line_length_mm * 0.5
|
||||
ax.annotate(
|
||||
"",
|
||||
xy=(C[0] + n_xy[0] * scale, C[1] + n_xy[1] * scale),
|
||||
xytext=(C[0], C[1]),
|
||||
arrowprops=dict(arrowstyle="->", color="tab:red", lw=2),
|
||||
)
|
||||
ax.scatter([C[0]], [C[1]], color="black", s=40, zorder=5)
|
||||
ax.set_aspect("equal")
|
||||
ax.set_xlabel("x (mm)")
|
||||
ax.set_ylabel("y (mm)")
|
||||
ax.set_title(f"x-y view: n=({n[0]:.4f},{n[1]:.4f},{n[2]:.4f})")
|
||||
ax.legend(loc="upper right", fontsize="x-small")
|
||||
|
||||
if title:
|
||||
fig.suptitle(title)
|
||||
|
||||
out_path = Path(out_path)
|
||||
out_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
fig.tight_layout()
|
||||
fig.savefig(out_path, dpi=dpi)
|
||||
plt.close(fig)
|
||||
return out_path
|
||||
|
||||
|
||||
__all__ = ["render_plane_with_refs"]
|
||||
@@ -0,0 +1,112 @@
|
||||
"""One-shot rendering of every diagnostic image for a single run.
|
||||
|
||||
Reads the artifact directory written by :mod:`coupler_panel.pipeline` and
|
||||
fills in ``images/`` with PNG plots. Silently skips any stage that
|
||||
cannot be loaded (e.g. ``denoised.ply`` if denoise was disabled).
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from ..io_utils import load_point_cloud, Artifacts
|
||||
|
||||
|
||||
def _safe_load(path: Path) -> np.ndarray | None:
|
||||
try:
|
||||
return load_point_cloud(path) if path.exists() else None
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def render_all(run_dir: str | Path, *, dpi: int = 150) -> dict[str, Path]:
|
||||
"""Render every diagnostic image we know how to make for this run."""
|
||||
art = Artifacts(run_dir)
|
||||
out: dict[str, Path] = {}
|
||||
|
||||
# Lazy imports — matplotlib not strictly required to run pipeline.
|
||||
try:
|
||||
from .cloud_view import render_cloud, render_cloud_compare
|
||||
from .plane_view import render_plane_with_refs
|
||||
from .heatmap import render_panel_heatmap
|
||||
except ImportError:
|
||||
return out
|
||||
|
||||
stages = ("cad_mesh", "sampled", "noisy", "transformed", "denoised", "registered")
|
||||
clouds: dict[str, np.ndarray] = {}
|
||||
for st in stages:
|
||||
pts = _safe_load(art.path(st, ext="ply"))
|
||||
if pts is not None and pts.shape[0] > 0:
|
||||
clouds[st] = pts
|
||||
try:
|
||||
out[st] = render_cloud(
|
||||
pts, art.image(f"{st}.png"), title=st, dpi=dpi,
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# Per-stage 2-up comparisons of selected pairs.
|
||||
if "sampled" in clouds and "noisy" in clouds:
|
||||
try:
|
||||
out["sampled_vs_noisy"] = render_cloud_compare(
|
||||
{"sampled": clouds["sampled"], "noisy": clouds["noisy"]},
|
||||
art.image("sampled_vs_noisy.png"),
|
||||
dpi=dpi,
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
if "transformed" in clouds and "registered" in clouds:
|
||||
try:
|
||||
out["transformed_vs_registered"] = render_cloud_compare(
|
||||
{"transformed": clouds["transformed"], "registered": clouds["registered"]},
|
||||
art.image("transformed_vs_registered.png"),
|
||||
dpi=dpi,
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# Plane + reference geometry (read from report.json).
|
||||
report_path = art.report_json
|
||||
if report_path.exists():
|
||||
try:
|
||||
import json
|
||||
|
||||
with report_path.open("r", encoding="utf-8") as fh:
|
||||
report: dict[str, Any] = json.load(fh)
|
||||
|
||||
measured = report.get("measured", report)
|
||||
plane = measured.get("plane")
|
||||
frame = measured.get("frame")
|
||||
roi_pts_path = art.path("panel_roi", ext="ply")
|
||||
roi_pts = _safe_load(roi_pts_path)
|
||||
if roi_pts is None and "registered" in clouds:
|
||||
roi_pts = clouds["registered"]
|
||||
if plane and frame and roi_pts is not None and roi_pts.shape[0] > 0:
|
||||
out["plane_with_refs"] = render_plane_with_refs(
|
||||
roi_pts,
|
||||
plane_normal=tuple(plane["normal"]),
|
||||
center_C=tuple(frame["center_C"]),
|
||||
out_path=art.image("plane_with_refs.png"),
|
||||
title="fitted plane + reference geometry",
|
||||
dpi=dpi,
|
||||
)
|
||||
out["heatmap"] = render_panel_heatmap(
|
||||
roi_pts,
|
||||
plane_normal=tuple(plane["normal"]),
|
||||
plane_d=float(plane["d_offset"]),
|
||||
center_C=tuple(frame["center_C"]),
|
||||
out_path=art.image("deviation_heatmap.png"),
|
||||
title="panel deviation heatmap",
|
||||
dpi=dpi,
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return out
|
||||
|
||||
|
||||
__all__ = ["render_all"]
|
||||
Reference in New Issue
Block a user