diff --git a/src/presa_mano/perception/stl_sampler.py b/src/presa_mano/perception/stl_sampler.py new file mode 100644 index 0000000..712ccb1 --- /dev/null +++ b/src/presa_mano/perception/stl_sampler.py @@ -0,0 +1,68 @@ +"""Parse binary/ASCII STL bytes and uniformly sample the surface. + +Output unit follows the STL convention (no unit conversion - STL files for +robotics are by convention in millimetres, but we don't enforce it here). +""" +from __future__ import annotations + +import io + +import numpy as np +from stl import mesh as stl_mesh + + +def parse_stl_bytes(stl_bytes: bytes) -> tuple[np.ndarray, np.ndarray]: + """Parse STL bytes (binary or ASCII) -> (vertices (M,3) float64, + triangles (T,3) int32). + + Vertices are deduplicated so the same point used by multiple triangles + appears once. Raises numpy_stl exceptions on malformed input. + """ + m = stl_mesh.Mesh.from_file(filename="dummy.stl", fh=io.BytesIO(stl_bytes)) + # m.vectors shape: (T, 3, 3) - three vertices per triangle, raw. + if m.vectors is None or len(m.vectors) == 0: + raise ValueError("STL parse yielded no triangles (empty or malformed)") + tris_raw = np.asarray(m.vectors, dtype=np.float64).reshape(-1, 3) + unique, inverse = np.unique(tris_raw, axis=0, return_inverse=True) + triangles = inverse.reshape(-1, 3).astype(np.int32) + return unique, triangles + + +def sample_surface( + vertices: np.ndarray, + triangles: np.ndarray, + n: int, + seed: int = 0, +) -> np.ndarray: + """Sample n points uniformly on the mesh surface, weighted by triangle area. + + Algorithm: + 1. Compute area for each triangle. + 2. Cumulative area -> pick triangle index via inverse CDF. + 3. For each pick, sample a uniform barycentric coordinate: + (1-sqrt(r1)) A + sqrt(r1)(1-r2) B + sqrt(r1) r2 C with r1, r2 ~ U(0,1). + """ + if triangles.size == 0: + return np.empty((0, 3), dtype=np.float64) + A = vertices[triangles[:, 0]] + B = vertices[triangles[:, 1]] + C = vertices[triangles[:, 2]] + areas = 0.5 * np.linalg.norm(np.cross(B - A, C - A), axis=1) + total = areas.sum() + if total <= 0.0: + # degenerate mesh: fall back to vertex sampling + rng = np.random.default_rng(seed) + idx = rng.integers(0, vertices.shape[0], size=n) + return vertices[idx] + cdf = np.cumsum(areas) / total + rng = np.random.default_rng(seed) + u = rng.random(n) + tri_idx = np.searchsorted(cdf, u, side="right") + tri_idx = np.clip(tri_idx, 0, triangles.shape[0] - 1) + r1 = rng.random(n) + r2 = rng.random(n) + sqrt_r1 = np.sqrt(r1) + w_a = (1.0 - sqrt_r1)[:, None] + w_b = (sqrt_r1 * (1.0 - r2))[:, None] + w_c = (sqrt_r1 * r2)[:, None] + return (w_a * A[tri_idx] + w_b * B[tri_idx] + w_c * C[tri_idx]).astype(np.float64) diff --git a/tests/test_stl_sampler.py b/tests/test_stl_sampler.py new file mode 100644 index 0000000..0439c5b --- /dev/null +++ b/tests/test_stl_sampler.py @@ -0,0 +1,88 @@ +"""Unit tests for stl_sampler.""" +from __future__ import annotations + +import io +import struct + +import numpy as np +import pytest + +from presa_mano.perception.stl_sampler import parse_stl_bytes, sample_surface + + +def _make_cube_stl_bytes(side_mm: float = 10.0) -> bytes: + """Synthesize a binary STL of an axis-aligned cube centered at origin, + from -side/2 to +side/2 on each axis. 12 triangles, no normals needed.""" + s = side_mm / 2.0 + v = [ + (-s, -s, -s), ( s, -s, -s), ( s, s, -s), (-s, s, -s), + (-s, -s, s), ( s, -s, s), ( s, s, s), (-s, s, s), + ] + # 6 faces x 2 tris (vertex order: any orientation works for our use) + tris = [ + (0, 1, 2), (0, 2, 3), # z=-s + (4, 6, 5), (4, 7, 6), # z=+s + (0, 4, 5), (0, 5, 1), # y=-s + (3, 2, 6), (3, 6, 7), # y=+s + (0, 3, 7), (0, 7, 4), # x=-s + (1, 5, 6), (1, 6, 2), # x=+s + ] + buf = io.BytesIO() + buf.write(b"\x00" * 80) # header + buf.write(struct.pack("= 8% dei punti (perfetto = 100/6 ~= 16.7% + per faccia, distribuita su 4 octanti adiacenti).""" + stl_bytes = _make_cube_stl_bytes(side_mm=10.0) + verts, tris = parse_stl_bytes(stl_bytes) + pts = sample_surface(verts, tris, n=2000, seed=1) + counts = np.zeros(8, dtype=np.int64) + for p in pts: + # bin (x>0, y>0, z>0) -> 0..7 + idx = (int(p[0] > 0) << 2) | (int(p[1] > 0) << 1) | int(p[2] > 0) + counts[idx] += 1 + frac = counts / pts.shape[0] + assert (frac > 0.08).all(), f"octant fractions too uneven: {frac}" + + +def test_sample_surface_seed_deterministic(): + stl_bytes = _make_cube_stl_bytes(side_mm=10.0) + verts, tris = parse_stl_bytes(stl_bytes) + a = sample_surface(verts, tris, n=100, seed=42) + b = sample_surface(verts, tris, n=100, seed=42) + np.testing.assert_array_equal(a, b) + + +def test_parse_stl_bad_payload_raises(): + with pytest.raises(Exception): # numpy-stl raises various; we just want it to not silently pass + parse_stl_bytes(b"not a real STL")