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

@@ -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)