feat(perception): project camera-frame 3D points to image pixels

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

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)