feat: initial commit - massive-cloud point cloud processing system

alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/__init__.py

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

alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/noise.py

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

alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/scan_simulator.py

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

alg/delivery_v1.1/coupler_panel_release/coupler_panel/simulation/transform.py

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