From b85f4b9a48f6d932e7d04a66da2ae741aa75e6bb Mon Sep 17 00:00:00 2001
From: Adriano Dal Pastro <adrianodalpastro@tielogic.com>
Date: Wed, 13 May 2026 10:02:20 +0200
Subject: [PATCH] feat(perception): project camera-frame 3D points to image
 pixels

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
---
 src/presa_mano/perception/projection.py | 45 +++++++++++++++++
 tests/test_projection.py                | 67 +++++++++++++++++++++++++
 2 files changed, 112 insertions(+)
 create mode 100644 src/presa_mano/perception/projection.py
 create mode 100644 tests/test_projection.py

diff --git a/src/presa_mano/perception/projection.py b/src/presa_mano/perception/projection.py
new file mode 100644
index 0000000..063ac92
--- /dev/null
+++ b/src/presa_mano/perception/projection.py
@@ -0,0 +1,45 @@
+"""Project 3D points (camera frame) to 2D pixels using OpenCV intrinsics."""
+from __future__ import annotations
+
+from typing import Any
+
+import numpy as np
+
+
+def K_from_intrinsics(intr: dict[str, Any]) -> np.ndarray:
+    """Build a 3x3 K from a {fx,fy,cx,cy,width,height} dict.
+    Width/height are not used here but must be present so callers can pass
+    the same dict to `project_camera_to_pixels`."""
+    return np.array([
+        [float(intr["fx"]), 0.0, float(intr["cx"])],
+        [0.0, float(intr["fy"]), float(intr["cy"])],
+        [0.0, 0.0, 1.0],
+    ], dtype=np.float64)
+
+
+def project_camera_to_pixels(
+    points_cam: np.ndarray,
+    K: np.ndarray,
+    image_size: tuple[int, int],
+) -> np.ndarray:
+    """Project (N,3) points in OpenCV camera frame to (M,2) int pixels.
+
+    Drops points with Z <= 0 (behind the camera) and points that project
+    outside `image_size` (W,H). The unit of `points_cam` is free: K@P/P.z
+    cancels it as long as K is in pixels.
+    """
+    if points_cam.size == 0:
+        return np.empty((0, 2), dtype=np.int32)
+    W, H = image_size
+    pts = np.asarray(points_cam, dtype=np.float64)
+    z = pts[:, 2]
+    keep_z = z > 0.0
+    if not keep_z.any():
+        return np.empty((0, 2), dtype=np.int32)
+    pts = pts[keep_z]
+    proj = (K @ pts.T).T          # (M, 3)
+    uv = proj[:, :2] / proj[:, 2:3]
+    u = np.round(uv[:, 0]).astype(np.int32)
+    v = np.round(uv[:, 1]).astype(np.int32)
+    in_frame = (u >= 0) & (u < W) & (v >= 0) & (v < H)
+    return np.stack([u[in_frame], v[in_frame]], axis=1)
diff --git a/tests/test_projection.py b/tests/test_projection.py
new file mode 100644
index 0000000..191b363
--- /dev/null
+++ b/tests/test_projection.py
@@ -0,0 +1,67 @@
+"""Unit tests for camera-to-pixel projection."""
+from __future__ import annotations
+
+import numpy as np
+
+from presa_mano.perception.projection import K_from_intrinsics, project_camera_to_pixels
+
+
+def _K():
+    return np.array([[600.0,   0.0, 320.0],
+                     [  0.0, 600.0, 240.0],
+                     [  0.0,   0.0,   1.0]])
+
+
+def test_K_from_intrinsics_matches_manual():
+    intr = {"fx": 600.0, "fy": 600.0, "cx": 320.0, "cy": 240.0,
+            "width": 640, "height": 480}
+    K = K_from_intrinsics(intr)
+    np.testing.assert_array_equal(K, _K())
+
+
+def test_project_principal_point():
+    """Punto sull'asse ottico a 1m -> cx,cy."""
+    pts = np.array([[0.0, 0.0, 1000.0]])  # mm
+    out = project_camera_to_pixels(pts, _K(), (640, 480))
+    assert out.shape == (1, 2)
+    assert out[0, 0] == 320 and out[0, 1] == 240
+
+
+def test_project_behind_camera_dropped():
+    """Z<=0 deve essere scartato."""
+    pts = np.array([[0.0, 0.0, -1000.0], [10.0, 20.0, 0.0]])
+    out = project_camera_to_pixels(pts, _K(), (640, 480))
+    assert out.shape == (0, 2)
+
+
+def test_project_outside_frame_dropped():
+    """Punto che proietta fuori bbox immagine viene scartato."""
+    pts = np.array([
+        [0.0, 0.0, 1000.0],         # -> (320, 240): inside
+        [10000.0, 0.0, 1000.0],     # -> u molto fuori bbox: dropped
+        [0.0, -10000.0, 1000.0],    # -> v molto fuori bbox (sopra): dropped
+    ])
+    out = project_camera_to_pixels(pts, _K(), (640, 480))
+    assert out.shape == (1, 2)
+    assert out[0, 0] == 320 and out[0, 1] == 240
+
+
+def test_project_known_offset():
+    """Punto in (0.1, 0.0, 1.0) m -> u = cx + fx*(0.1/1.0) = 320 + 60 = 380."""
+    pts = np.array([[100.0, 0.0, 1000.0]])  # mm
+    out = project_camera_to_pixels(pts, _K(), (640, 480))
+    assert out[0, 0] == 380 and out[0, 1] == 240
+
+
+def test_project_unit_invariance():
+    """Stesso punto in m vs mm proietta sullo stesso pixel (K@P/P.z elide unità)."""
+    pts_mm = np.array([[100.0, 0.0, 1000.0]])
+    pts_m  = pts_mm / 1000.0
+    out_mm = project_camera_to_pixels(pts_mm, _K(), (640, 480))
+    out_m  = project_camera_to_pixels(pts_m,  _K(), (640, 480))
+    np.testing.assert_array_equal(out_mm, out_m)
+
+
+def test_project_empty_input():
+    out = project_camera_to_pixels(np.empty((0, 3)), _K(), (640, 480))
+    assert out.shape == (0, 2)