"""Pinhole camera geometry for robot vision, in NumPy. Conventions ----------- World to camera: X_c = R @ X_w + t (R, t are the extrinsics) Camera axes: x right, y down, z forward along the optical axis Pixels: [u, v, 1]^T ~ K @ X_c, K = [[fx, s, cx], [0, fy, cy], [0, 0, 1]] Distortion: Brown-Conrady on normalised coordinates, coefficients (k1, k2, p1, p2, k3) in the same order as OpenCV Contents -------- intrinsic_matrix, look_at build K and a camera pose project 3D points to pixels (optionally distorted) distort, undistort_normalized radial + tangential lens model and its inverse pixel_to_ray, backproject_to_plane pixel to world ray, ray to plane intersection normalize_points, estimate_homography DLT homography with Hartley normalisation apply_homography, transfer_error use and score a homography plane_homography H = K [r1 r2 t] for the world plane z = 0 intrinsics_from_homographies Zhang's closed-form K from >= 3 plane views rigid_transform least-squares R, t between 3D point sets (Kabsch) Run this file for the worked example: python camera.py """ from __future__ import annotations from collections.abc import Sequence import numpy as np from numpy.typing import NDArray FloatArray = NDArray[np.float64] # --------------------------------------------------------------------------- # Camera model # --------------------------------------------------------------------------- def intrinsic_matrix(fx: float, fy: float, cx: float, cy: float, skew: float = 0.0) -> FloatArray: return np.array([[fx, skew, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]]) def look_at(camera_pos: Sequence[float], target: Sequence[float], up: Sequence[float] = (0.0, 0.0, 1.0)) -> tuple[FloatArray, FloatArray]: """Extrinsics (R, t) of a camera at camera_pos whose optical axis points at target. Image y points 'down', i.e. away from the world 'up' vector. """ c = np.asarray(camera_pos, dtype=float) z = np.asarray(target, dtype=float) - c z /= np.linalg.norm(z) x = np.cross(z, np.asarray(up, dtype=float)) if np.linalg.norm(x) < 1e-9: # looking along 'up': pick any x x = np.cross(z, np.array([0.0, 1.0, 0.0])) x /= np.linalg.norm(x) y = np.cross(z, x) R = np.vstack((x, y, z)) # rows: camera axes in world coordinates return R, -R @ c def distort(xn: FloatArray, coeffs: Sequence[float]) -> FloatArray: """Apply Brown-Conrady distortion to normalised image points (N x 2).""" k1, k2, p1, p2, k3 = (list(coeffs) + [0.0] * 5)[:5] x, y = xn[:, 0], xn[:, 1] r2 = x * x + y * y radial = 1.0 + k1 * r2 + k2 * r2**2 + k3 * r2**3 xd = x * radial + 2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x) yd = y * radial + p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y return np.column_stack((xd, yd)) def undistort_normalized(xd: FloatArray, coeffs: Sequence[float], iterations: int = 50) -> FloatArray: """Invert distort() by fixed-point iteration (fine for moderate distortion).""" k1, k2, p1, p2, k3 = (list(coeffs) + [0.0] * 5)[:5] xn = xd.copy() for _ in range(iterations): x, y = xn[:, 0], xn[:, 1] r2 = x * x + y * y radial = 1.0 + k1 * r2 + k2 * r2**2 + k3 * r2**3 dx = 2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x) dy = p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y xn = np.column_stack(((xd[:, 0] - dx) / radial, (xd[:, 1] - dy) / radial)) return xn def project(K: FloatArray, R: FloatArray, t: FloatArray, X_w: FloatArray, dist: Sequence[float] | None = None) -> tuple[FloatArray, FloatArray]: """Project world points (N x 3) to pixels (N x 2). Also returns depth z_c (N,). Points with z_c <= 0 are behind the camera; their pixels are meaningless. """ X_w = np.atleast_2d(np.asarray(X_w, dtype=float)) X_c = X_w @ R.T + t z = X_c[:, 2] xn = X_c[:, :2] / z[:, None] if dist is not None: xn = distort(xn, dist) uv = xn @ K[:2, :2].T + K[:2, 2] return uv, z def pixel_to_ray(K: FloatArray, R: FloatArray, t: FloatArray, uv: FloatArray, dist: Sequence[float] | None = None) -> tuple[FloatArray, FloatArray]: """Camera centre (3,) and unit ray directions (N x 3) in world coordinates.""" uv = np.atleast_2d(np.asarray(uv, dtype=float)) ones = np.ones((uv.shape[0], 1)) xn = (np.linalg.inv(K) @ np.hstack((uv, ones)).T).T[:, :2] if dist is not None: xn = undistort_normalized(xn, dist) d_c = np.hstack((xn, ones)) d_w = d_c @ R # R^T d for each row d_w /= np.linalg.norm(d_w, axis=1, keepdims=True) centre = -R.T @ t return centre, d_w def backproject_to_plane(K: FloatArray, R: FloatArray, t: FloatArray, uv: FloatArray, normal: Sequence[float] = (0.0, 0.0, 1.0), offset: float = 0.0, dist: Sequence[float] | None = None) -> FloatArray: """Intersect pixel rays with the plane normal . X = offset (world frame). Returns N x 3.""" n = np.asarray(normal, dtype=float) c, d = pixel_to_ray(K, R, t, uv, dist) denom = d @ n if np.any(np.abs(denom) < 1e-12): raise ValueError("ray parallel to the plane") s = (offset - c @ n) / denom if np.any(s <= 0.0): raise ValueError("plane is behind the camera for at least one pixel") return c + s[:, None] * d # --------------------------------------------------------------------------- # Homographies # --------------------------------------------------------------------------- def normalize_points(pts: FloatArray) -> tuple[FloatArray, FloatArray]: """Similarity T so that T @ [p, 1] has zero mean and mean distance sqrt(2).""" pts = np.asarray(pts, dtype=float) mean = pts.mean(axis=0) scale = np.sqrt(2.0) / np.mean(np.linalg.norm(pts - mean, axis=1)) T = np.array([[scale, 0.0, -scale * mean[0]], [0.0, scale, -scale * mean[1]], [0.0, 0.0, 1.0]]) return T, apply_homography(T, pts) def apply_homography(H: FloatArray, pts: FloatArray) -> FloatArray: pts = np.atleast_2d(np.asarray(pts, dtype=float)) ph = np.hstack((pts, np.ones((pts.shape[0], 1)))) @ H.T return ph[:, :2] / ph[:, 2:3] def estimate_homography(src: FloatArray, dst: FloatArray, normalize: bool = True) -> FloatArray: """DLT estimate of H with dst ~ H src from N >= 4 point pairs (N x 2 each). Each pair gives two rows of A h = 0; h is the right singular vector of A with the smallest singular value. Returns H scaled so that H[2, 2] = 1. """ src = np.asarray(src, dtype=float) dst = np.asarray(dst, dtype=float) if src.shape != dst.shape or src.shape[0] < 4: raise ValueError("need at least 4 matching point pairs") if normalize: T1, s = normalize_points(src) T2, d = normalize_points(dst) else: T1 = T2 = np.eye(3) s, d = src, dst rows = [] for (x, y), (u, v) in zip(s, d): rows.append([-x, -y, -1.0, 0.0, 0.0, 0.0, u * x, u * y, u]) rows.append([0.0, 0.0, 0.0, -x, -y, -1.0, v * x, v * y, v]) _, _, vt = np.linalg.svd(np.asarray(rows)) Hn = vt[-1].reshape(3, 3) H = np.linalg.inv(T2) @ Hn @ T1 return H / H[2, 2] def transfer_error(H: FloatArray, src: FloatArray, dst: FloatArray) -> float: """RMS distance between H(src) and dst, in the units of dst.""" diff = apply_homography(H, src) - np.asarray(dst, dtype=float) return float(np.sqrt(np.mean(np.sum(diff**2, axis=1)))) def plane_homography(K: FloatArray, R: FloatArray, t: FloatArray) -> FloatArray: """Homography from world-plane coordinates (X, Y) on z = 0 to pixels: K [r1 r2 t].""" H = K @ np.column_stack((R[:, 0], R[:, 1], t)) return H / H[2, 2] # --------------------------------------------------------------------------- # Calibration helpers # --------------------------------------------------------------------------- def _v(H: FloatArray, i: int, j: int) -> FloatArray: hi, hj = H[:, i], H[:, j] return np.array([hi[0] * hj[0], hi[0] * hj[1] + hi[1] * hj[0], hi[1] * hj[1], hi[2] * hj[0] + hi[0] * hj[2], hi[2] * hj[1] + hi[1] * hj[2], hi[2] * hj[2]]) def intrinsics_from_homographies(Hs: Sequence[FloatArray]) -> FloatArray: """Closed-form K from >= 3 homographies of a planar target (Zhang's linear step). With B = K^-T K^-1, each view gives h1^T B h2 = 0 and h1^T B h1 = h2^T B h2, because r1 and r2 are orthonormal. B is recovered up to scale from the stacked constraints, and K from a Cholesky factorisation of B. """ V = [] for H in Hs: V.append(_v(H, 0, 1)) V.append(_v(H, 0, 0) - _v(H, 1, 1)) _, _, vt = np.linalg.svd(np.asarray(V)) b = vt[-1] B = np.array([[b[0], b[1], b[3]], [b[1], b[2], b[4]], [b[3], b[4], b[5]]]) if B[0, 0] < 0: # fix the arbitrary sign B = -B L = np.linalg.cholesky(B) # B = L L^T, L = K^-T K = np.linalg.inv(L.T) return K / K[2, 2] def rigid_transform(A: FloatArray, B: FloatArray) -> tuple[FloatArray, FloatArray]: """Least-squares R, t with B ~ R A + t for matched 3D points (N x 3), N >= 3.""" A = np.asarray(A, dtype=float) B = np.asarray(B, dtype=float) ca, cb = A.mean(axis=0), B.mean(axis=0) U, _, Vt = np.linalg.svd((A - ca).T @ (B - cb)) D = np.diag([1.0, 1.0, np.sign(np.linalg.det(Vt.T @ U.T))]) R = Vt.T @ D @ U.T return R, cb - R @ ca # --------------------------------------------------------------------------- # Worked example # --------------------------------------------------------------------------- def main() -> None: K = intrinsic_matrix(800.0, 800.0, 320.0, 240.0) np.set_printoptions(precision=4, suppress=True) # 1. A point given in the camera frame (R = I, t = 0) uv, z = project(K, np.eye(3), np.zeros(3), np.array([[0.10, -0.05, 0.50]])) print(f"camera-frame point (0.10, -0.05, 0.50) m -> pixel {uv[0]}, depth {z[0]:.2f} m") # 2. Camera 0.6 m above the table, looking straight down R = np.diag([1.0, -1.0, -1.0]) C = np.array([0.0, 0.0, 0.6]) t = -R @ C for h in (0.0, 0.025): X = backproject_to_plane(K, R, t, np.array([[480.0, 160.0]]), offset=h) print(f"pixel (480, 160) on plane z = {h:.3f} m -> world {X[0]}") # 3. Tilted camera, table-plane homography from 6 touched points rng = np.random.default_rng(0) R, t = look_at([0.0, -0.35, 0.45], [0.0, 0.20, 0.0]) table_xy = np.array([[-0.15, 0.10], [0.0, 0.10], [0.15, 0.10], [-0.15, 0.30], [0.0, 0.30], [0.15, 0.30]]) pix, _ = project(K, R, t, np.column_stack((table_xy, np.zeros(6)))) pix_noisy = pix + rng.normal(0.0, 0.5, pix.shape) H = estimate_homography(table_xy, pix_noisy) cube_pix, _ = project(K, R, t, np.array([[0.07, 0.22, 0.0]])) cube_xy = apply_homography(np.linalg.inv(H), cube_pix) print(f"cube at (0.070, 0.220) m located at {cube_xy[0]} m " f"(error {np.linalg.norm(cube_xy[0] - [0.07, 0.22]) * 1e3:.2f} mm)") if __name__ == "__main__": main()