massive-cloud/alg/delivery_v1.2/coupler_panel_release/coupler_panel/api.py

"""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",
]