Camera models, calibration, and hand-eye

Three lessons of kinematics gave you an arm that can put its gripper anywhere in its workspace to a few millimeters — entirely in its own base frame. Nothing in that math knows where the object is. The camera is the only instrument connecting the robot's coordinates to the world's, and it is a strange one: a projective encoder that destroys a dimension on capture and must be fitted, not trusted. The π₀-class policies you will train later in the OpenPI stack consume raw pixels and learn this geometry implicitly — precisely why they cannot tell you why a grasp missed. The classical oracle this phase builds needs the geometry explicit: a calibrated map from pixel $(u, v)$ plus depth to a metric point in the base frame, with error bars on every term.

The pinhole model: a projective encoder with four load-bearing numbers

Strip the lens away and a camera is a box with a hole. Put the pinhole at the origin of the camera frame, point the $z$-axis out through the hole, and place the image plane at distance $f$ — the focal length — in front of it. A point $P = (X, Y, Z)$ in camera coordinates projects along the straight ray through the pinhole, and similar triangles do the rest:

The sensor samples that plane on a pixel grid, turning metric image coordinates into pixels through four numbers: $f_x$ and $f_y$ are the focal length divided by the pixel pitch in each direction — units of pixels — and $(c_x, c_y)$ is the principal point, where the optical axis actually pierces the sensor. On a RealSense-class module, expect $f_x \approx 447$ px for the 848×480 depth stream (87° horizontal FOV) and roughly 910 px for the 1280×720 color stream. The principal point sits near the image center but never exactly on it — a calibration returning exactly the center is a sign nothing was actually estimated.

A world-frame point first passes through the camera's extrinsics — a rigid transform into the camera frame, the same $SE(3)$ objects from the forward-kinematics lesson — then through $K$:

Note what the projection destroyed: the scale $s$. Every point along the ray through a pixel lands on that same pixel, so a single RGB camera cannot distinguish a small near object from a large far one. Everything downstream is a strategy for recovering the lost dimension — depth sensors here, registration in the next lesson.

One correction before the model is usable: real lenses bend rays. The standard Brown–Conrady model perturbs normalized coordinates with a radial polynomial ($k_1 r^2 + k_2 r^4 + k_3 r^6$) plus two tangential terms ($p_1$, $p_2$) for lens–sensor misalignment — worth 1–3 px at the edges of a narrow lens, tens of pixels on wide-FOV modules. For your rig: RealSense-class depth streams arrive rectified (distortion removed, coefficients zero), so deprojection can ignore it; the color stream is not, so fiducial detection in RGB must model distortion or undistort first.

Depth cameras: manufacturing the Z you lost

There are three mainstream ways to rebuild the missing coordinate, worth knowing at the systems level because their failure signatures differ — and you will meet all three in this phase's point clouds.

Your camera is in the first row, so derive its behavior. Two rectified imagers sit a baseline $B$ apart along the $x$-axis. The same world point lands at $u_L$ in the left image and $u_R$ in the right; the disparity between them encodes depth:

Depth is the reciprocal of disparity, and that reciprocal is the entire noise story. The stereo matcher localizes disparity to roughly constant subpixel precision — call it $\sigma_d \approx 0.08$ px — independent of distance. Propagate the constant through the reciprocal:

Put RealSense-class numbers in: $B = 50$ mm and $f_x = 447$ px give $f_x B \approx 22.4$ px·m. At 0.4 m, $\sigma_Z \approx 0.6$ mm; at 1 m, 3.6 mm; at 2 m, 14 mm. This one formula should drive your mounting decisions. A WidowX AI has a 0.769 m reach, so a camera 0.4–0.6 m from the action lives in the sub-2 mm regime, while one bolted 1.5 m away for a nicer wide view pays quadratically for the framing.

Beyond the $Z^2$ law, stereo depth has structured artifacts you should recognize on sight. Flying pixels appear at depth discontinuities: the correlation window straddles foreground and background, producing points that float in the space between them — exactly along object boundaries, which is exactly where a grasp planner looks. Invalid pixels (holes, depth 0) appear on IR-dark and specular surfaces, in occlusion shadows only one imager sees, and closer than the minimum range — about 0.2–0.3 m at full resolution, so mount with margin. The map also flickers temporally — a static scene wanders frame to frame — which is why every validation in this lesson averages at least 100 frames.

Intrinsic calibration: fitting K like a tiny model

Factory calibration covers more than you might expect: the depth stream ships calibrated and rectified, intrinsics queryable at runtime. You calibrate yourself when you use the color camera for metric work (fiducials, PnP), after a lens knock or thermal cycle, and whenever you need to verify rather than trust. The workflow is stubbornly physical. Print a checkerboard — say 9×6 inner corners, 25 mm squares — and mount it dead flat on glass or aluminum; a 1 mm paper bow displaces corners by several pixels in tilted views. Better, use a ChArUco board: embedded markers give unambiguous corner identities and tolerate partial views. Lock focus and exposure. Capture 20–40 views that fill the frame, push corners into the image corners, and tilt the board 30–45° about both axes. cv2.calibrateCamera then runs Levenberg–Marquardt over intrinsics, distortion, and a nuisance pose per view, finishing in seconds on CPU — your RTX contributes nothing; the data is everything.

Foundations: From Metric Rays to Discrete Pixels

Before deriving the full projection chain, we must rigorously distinguish the continuous geometric space from the discrete digital sensor. The pinhole model is a geometric abstraction that maps 3D points to a 2D plane, but physical cameras do not record a continuous plane; they record a grid of discrete samples. Confusing these two domains is the primary source of unit errors in robotics vision. We define the image plane as the continuous surface where the optical axis intersects the focal plane, and the sensor array as the physical silicon grid that samples this plane.

Let $(x, y)$ denote the metric image coordinates in meters, measured from the principal point on the continuous image plane. Let $(u, v)$ denote the pixel coordinates in integer units, measured from the top-left corner of the sensor array. The mapping between these two spaces is governed by the pixel pitch, denoted $p_x$ and $p_y$ (in meters per pixel), which is the physical distance between the centers of adjacent pixels. The relationship is affine: $u = \frac{x}{p_x} + c_x$ and $v = \frac{y}{p_y} + c_y$, where $(c_x, c_y)$ is the principal point expressed in pixel units.

This dimensional analysis is critical. The intrinsic matrix $K$ contains $f_x$ and $f_y$ in units of pixels, not meters. If you treat $f_x$ as a metric length, your deprojection will be off by a factor of the pixel pitch (typically $10^{-5}$ to $10^{-4}$). The abstraction exists to decouple the optical geometry (focal length) from the sensor manufacturing (pixel density). A camera with a 10 mm focal length and 10 µm pixels has $f_x = 1000$ px; a camera with the same optics but 5 µm pixels has $f_x = 2000$ px. The geometry is identical; the digital representation is not.

In stereo vision, the baseline $B$ is the distance between the optical centers of the two cameras, not the physical width of the camera housing. The housing may be 50 mm wide, but if the lenses are inset, the baseline is smaller. This distinction is vital because the depth formula $Z = \frac{f_x B}{d}$ depends linearly on $B$. A 10% error in $B$ results in a 10% error in all depth measurements. Furthermore, the disparity $d$ is measured in pixels, so the product $f_x B$ has units of pixels·meters. This hybrid unit is a constant for a given camera rig and simplifies noise propagation calculations.

Worked Example: Deriving Depth Noise from Disparity

We now derive the depth noise $\sigma_Z$ from the disparity noise $\sigma_d$ using the chain rule. This derivation reveals why stereo depth degrades quadratically with distance. Assume the stereo matcher has a constant standard deviation $\sigma_d$ in disparity space, independent of depth $Z$. This is a standard assumption for subpixel matching algorithms, which localize features to a fixed precision regardless of the object's distance.

The variance of $Z$ is approximated by the square of the partial derivative times the variance of $d$: $\sigma_Z^2 = \left( \frac{\partial Z}{\partial d} \right)^2 \sigma_d^2$. Taking the square root gives the standard deviation: $\sigma_Z = \left| \frac{\partial Z}{\partial d} \right| \sigma_d = \frac{f_x B}{d^2} \sigma_d$. Substituting $d = \frac{f_x B}{Z}$ into this expression yields the final form: $\sigma_Z = \frac{Z^2}{f_x B} \sigma_d$. This equation shows that depth noise scales with the square of the distance $Z$.

Let us compute $\sigma_Z$ for $Z = 1.0$ m. First, calculate the constant $f_x B = 447 \times 0.05 = 22.35$ px·m. The disparity at $Z = 1.0$ m is $d = \frac{22.35}{1.0} = 22.35$ px. Now, apply the noise formula: $\sigma_Z = \frac{(1.0)^2}{22.35} \times 0.08 = \frac{1.0}{22.35} \times 0.08 \approx 0.0447 \times 0.08 \approx 0.00358$ m, or 3.6 mm. If we double the distance to $Z = 2.0$ m, the disparity halves to $d = 11.175$ px, and the noise quadruples to $\sigma_Z \approx 14.3$ mm. This quadratic growth is the fundamental limitation of stereo vision at long ranges.

The headline metric is RMS reprojection error: project the board's known 3D corners through the fitted model, measure pixel distance to the detected corners, and take the root mean square over all corners and views. For a 640-wide image, a careful calibration lands at 0.15–0.3 px; under 0.5 px is acceptable; anything above 1 px is a structural problem, not bad luck. The causes are boringly repeatable:

• A non-flat board. A 1 mm bow shifts corners by pixels in oblique views; the optimizer absorbs it into the distortion coefficients, poisoning them.
• All views fronto-parallel. Without tilt, focal length and board distance are nearly indistinguishable; the solver returns a confident $f_x$ with enormous covariance.
• No corners near the image edges. The distortion polynomial is unconstrained exactly where it is largest; it invents geometry at the periphery.
• Autofocus or auto-exposure left on. Focus changes focal length mid-session; the dataset samples several different cameras and fits none.
• Motion blur and rolling shutter. Waving the board by hand shears the geometry; trigger capture on detected stillness.
• Wrong square-size metadata. The invisible one: reprojection error is unaffected — square size rescales board poses, not pixels — but every metric measurement downstream is silently scaled.

Extrinsics and hand-eye: where is the camera, really?

Intrinsics say how the camera maps rays to pixels; extrinsics say where those rays live in the robot's world. Two mounting patterns dominate. Eye-to-hand: the camera is fixed to the table or frame; the unknown is the constant base-to-camera transform. Eye-in-hand: the camera rides the wrist; the unknown is the constant gripper-to-camera transform, and the camera's base-frame pose is FK times that mount. The trade is occlusion against motion: a fixed camera watches the arm block its own view at the grasp moment; a wrist camera keeps the target centered through the approach but blurs while moving and costs a little payload — a RealSense-class module with cable is 75–100 g against the WidowX AI's 1.5 kg rating, a rounding error. The standard answer for your rig, and the layout π₀-style setups use, is both: one over-the-shoulder view, one wrist view — which is exactly how the WidowX AI Follower ships, with an Intel RealSense D405 already on an arm mount for the wrist view.

Calibrating either mount is the same problem: a rigid transform you cannot measure with a ruler. The trick is to let the arm do the measuring. Take the eye-in-hand case and fix a fiducial board to the table. At arm pose $i$, the board's base-frame pose factors as FK (gripper in base) times the unknown mount $X$ (camera in gripper) times the detection (board in camera, from PnP on its corners). The board never moves, so the product is identical at every pose; equating two poses eliminates the board entirely:

Split $AX = XB$ into rotation and translation parts: $R_A R_X = R_X R_B$ and $(R_A - I)\,t_X = R_X t_B - t_A$. Classical solvers — Tsai–Lenz and descendants, wrapped by cv2.calibrateHandEye — solve the rotation equation first (each motion pair says the rotation axes of $A$ and $B$ correspond under $R_X$), then substitute into the linear translation equation. The structure dictates the data: rotations about at least two non-parallel axes, because one axis leaves $R_X$ free to spin about it, and $(R_A - I)$ must be exercised for $t_X$ to appear at all. A practical session: 15–30 poses, wrist rotated ±30° or more about two distinct axes, board sharp and fully visible, FK pose and image stored as a synchronized pair — your Phase 01 timestamping discipline, cashed in. Validate on held-out poses: predict where the board should appear under the solved $X$ and measure the pixel gap.

From pixels to millimeters: the error budget

Now invert the projection and follow errors through it. Given a pixel and its depth, the lateral coordinate and its two sensitivities are:

Read the partial derivatives as prices. At $Z = 0.45$ m with $f_x = 447$ px, one pixel of image error costs $Z / f_x \approx 1.0$ mm of lateral position — a round number worth memorizing, which doubles at a 0.9 m mount. Depth error passes straight into the point's $z$ and leaks laterally through $\tan\theta$: near the edge of the 87° FOV the factor approaches 0.95, so a 5 mm depth bias drags the point almost 5 mm sideways too. Rotation error costs displacement proportional to lever arm, $\varepsilon \approx \theta L$: half a degree (8.7 mrad) over 0.45 m is 3.9 mm. Now the budget for a concrete task — a 25 mm cube, camera 0.45 m away, eye-to-hand, the WidowX AI's 40 mm gripper opening leaving roughly ±7 mm of usable centering tolerance:

The punchline is the last column. The noise rows combine in quadrature to barely 1.3 mm — harmless, and averaging 100 frames divides them by 10. The bias rows do not average: they add, worst case in the same direction, and at 8–12 mm they can consume the entire ±7 mm tolerance while every individual number looked respectable. Hence the phase goal of separating failure classes: a grasp that misses consistently in one base-frame direction is a calibration bug; one that misses in a different direction every trial is a perception bug. Same failure rate, opposite fixes.

Time to make this executable. Write these two functions yourself, once, and test them against the vendor implementation (rs2_deproject_pixel_to_point in pyrealsense2) so that every convention — pixel origin, axis directions, distortion handling — is verified rather than assumed. They are the atoms of the next three lessons.

import numpy as np

def project(points_cam, K, dist=None):
    """(N, 3) camera-frame points in meters -> (N, 2) pixel coordinates.

    dist: optional Brown-Conrady coefficients (k1, k2, p1, p2, k3),
    OpenCV ordering. Pass dist=None for rectified streams.
    """
    p = np.asarray(points_cam, dtype=np.float64)
    x = p[:, 0] / p[:, 2]
    y = p[:, 1] / p[:, 2]
    if dist is not None:
        k1, k2, p1, p2, k3 = dist
        r2 = x * x + y * y
        radial = 1.0 + k1 * r2 + k2 * r2**2 + k3 * r2**3
        x, y = (x * radial + 2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x),
                y * radial + p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y)
    u = K[0, 0] * x + K[0, 2]
    v = K[1, 1] * y + K[1, 2]
    return np.stack([u, v], axis=1)

def deproject(pixels, depth_m, K):
    """(N, 2) pixels + (N,) depths in meters -> (N, 3) camera-frame points.

    Assumes a rectified stream (RealSense-class depth streams are).
    Depth 0 means an invalid pixel: filter those out before calling.
    """
    px = np.asarray(pixels, dtype=np.float64)
    z = np.asarray(depth_m, dtype=np.float64)
    x = (px[:, 0] - K[0, 2]) / K[0, 0] * z
    y = (px[:, 1] - K[1, 2]) / K[1, 1] * z
    return np.stack([x, y, z], axis=1)

# Round-trip sanity check with RealSense-class depth intrinsics
K = np.array([[447.0, 0.0, 424.0],
              [0.0, 447.0, 240.0],
              [0.0, 0.0, 1.0]])
pt = np.array([[0.10, -0.05, 0.45]])   # 10 cm right, 5 cm up, 45 cm out
uv = project(pt, K)                    # -> approx [523.3, 190.3]
assert np.allclose(deproject(uv, pt[:, 2], K), pt)

Validation closes the loop between calibration numbers and physical reality: measure an object whose size you know to better precision than the camera can resolve, and attribute the disagreement. A caliper-measured aluminum block is ideal.

import numpy as np

def robust_depth(depth_image_m, u, v, half=2):
    """Median depth in a (2*half+1)^2 window, ignoring invalid zeros.

    Single-pixel depth at an object edge is exactly where flying
    pixels live; the median rejects them.
    """
    patch = depth_image_m[v - half : v + half + 1, u - half : u + half + 1]
    valid = patch[patch > 0]
    return float(np.median(valid)) if valid.size else 0.0

def edge_width_mm(uv_a, uv_b, z_a, z_b, K):
    """Metric distance between two pixel+depth measurements, in mm."""
    pts = deproject(np.array([uv_a, uv_b], dtype=np.float64),
                    np.array([z_a, z_b], dtype=np.float64), K)
    return 1000.0 * float(np.linalg.norm(pts[1] - pts[0]))

def report(widths_mm, true_mm):
    w = np.asarray(widths_mm, dtype=np.float64)
    bias = w.mean() - true_mm
    print("frames:        ", w.size)
    print("true width:    ", true_mm, "mm")
    print("measured mean: ", round(float(w.mean()), 3), "mm")
    print("bias:          ", round(float(bias), 3), "mm  (systematic; will NOT average away)")
    print("std (noise):   ", round(float(w.std()), 3), "mm  (random; shrinks as 1/sqrt(N))")

# Per distance (0.30 m, 0.45 m, 0.60 m):
#   1. place a caliper-measured block (e.g. 60.00 mm) facing the camera
#   2. locate its two top-edge endpoints in the aligned depth frame
#   3. widths.append(edge_width_mm(...)) over ~100 frames,
#      using robust_depth() at each endpoint
#   4. report(widths, 60.00)

Where this goes next: Inverse kinematics: analytic, numeric, and constrained gave you the map from a desired gripper pose to joint angles; this lesson built the map from pixels to metric points worth aiming that IK at. The missing middle is turning a calibrated point cloud into the 6-DoF pose of the object you want to grasp — that is Pose estimation and point-cloud registration (see MIT Robotic Manipulation, Chapter 4 (opens in a new tab)), where your intrinsics become the projection model inside PnP and ICP, and your measured noise decides which correspondences to trust. Bring the numbers: the error-budget table is the first row of the failure-attribution ledger this phase exists to build.