"""Turn a 2D detection into a 3D point with the pinhole camera model. K is the 3 x 3 intrinsic matrix [[fx, 0, cx], [0, fy, cy], [0, 0, 1]]. The camera frame has z along the optical axis, x to the right in the image and y down the image. Three ways to get the missing depth: backproject depth Z is known (depth camera, or a calibrated working distance) depth_from_width the real width of the object is known ray_plane the object rests on a known plane, such as the table Run as a script for the worked example from the lesson: python lift.py """ from __future__ import annotations import numpy as np from numpy.typing import ArrayLike, NDArray def box_center(box: ArrayLike) -> tuple[float, float]: x1, y1, x2, y2 = np.asarray(box, dtype=float) return 0.5 * (x1 + x2), 0.5 * (y1 + y2) def backproject(u: float, v: float, depth: float, K: ArrayLike) -> NDArray[np.float64]: """Point in the camera frame at pixel (u, v) and depth Z along the optical axis.""" K = np.asarray(K, dtype=float) fx, fy, cx, cy = K[0, 0], K[1, 1], K[0, 2], K[1, 2] return np.array([(u - cx) * depth / fx, (v - cy) * depth / fy, depth]) def depth_from_width(width_px: float, width_m: float, fx: float) -> float: """Depth of an object of known width that appears width_px pixels wide (fronto-parallel).""" return fx * width_m / width_px def pixel_ray(u: float, v: float, K: ArrayLike) -> NDArray[np.float64]: """Direction of the ray through pixel (u, v) in the camera frame, with z component 1.""" return np.linalg.solve(np.asarray(K, dtype=float), np.array([u, v, 1.0])) def ray_plane(u: float, v: float, K: ArrayLike, n: ArrayLike, d: float) -> NDArray[np.float64]: """Intersect the ray through (u, v) with the plane n . X = d (camera frame).""" ray = pixel_ray(u, v, K) n = np.asarray(n, dtype=float) denom = float(n @ ray) if abs(denom) < 1e-12: raise ValueError("ray is parallel to the plane") t = d / denom if t <= 0: raise ValueError("plane is behind the camera along this ray") return t * ray def to_base(T_base_cam: ArrayLike, p_cam: ArrayLike) -> NDArray[np.float64]: """Map a point from the camera frame to the robot base frame with a 4 x 4 transform.""" T = np.asarray(T_base_cam, dtype=float) return T[:3, :3] @ np.asarray(p_cam, dtype=float) + T[:3, 3] def main() -> None: # Example intrinsics for a 640 x 480 image; use your own calibration. K = np.array([[600.0, 0.0, 320.0], [0.0, 600.0, 240.0], [0.0, 0.0, 1.0]]) box = [376, 276, 424, 324] # a detected 4 cm cube, 48 px wide u, v = box_center(box) z = depth_from_width(box[2] - box[0], 0.04, K[0, 0]) p_cam = backproject(u, v, z, K) print(f"centre ({u:.0f}, {v:.0f}) px, depth {z:.3f} m, p_cam = {np.round(p_cam, 4)} m") # Camera 0.54 m above the table, looking straight down, so the table is the plane z = 0.54 # in the camera frame. The cube centre is 2 cm above the table: intersect with z = 0.52. p_centre = ray_plane(u, v, K, n=[0.0, 0.0, 1.0], d=0.54 - 0.02) print(f"ray meets the plane z = 0.52 m at {np.round(p_centre, 4)} m") # Base frame: origin on the table, x forward, z up. The camera sits at (0.30, 0, 0.54) m # and looks down, with x_cam = -y_base, y_cam = -x_base, z_cam = -z_base. T = np.eye(4) T[:3, :3] = np.array([[0.0, -1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, -1.0]]) T[:3, 3] = [0.30, 0.0, 0.54] print(f"cube centre in the base frame: {np.round(to_base(T, p_centre), 4)} m") if __name__ == "__main__": main()