Perception · Lesson 6 of 12

Camera geometry for robots: pinhole model and calibration

Project 3D points with the pinhole model, correct lens distortion, calibrate a camera, and turn a pixel into a table position for a robot arm with homographies.

intermediate50 minUpdated Fact-checked Free

What you will learn

  • Project a 3D point to a pixel with the intrinsic matrix K and the extrinsics R and t
  • Describe radial and tangential lens distortion and how calibration estimates it
  • Explain how a checkerboard calibration recovers K from plane homographies
  • Back-project a pixel to a ray and intersect it with the table plane to locate an object
  • Estimate a homography with the normalised DLT and use it to map pixels to table coordinates

Before you start

  • Spatial maths: rotations and homogeneous transforms (lesson 2)
  • Least squares and the singular value decomposition

After this lesson you can take a pixel where a camera sees an object and turn it into a position the robot can reach. You will project 3D points with the pinhole model, know what calibration estimates and why, and use homographies to map between an image and the table the robot works on.

The pinhole model

An ideal camera maps every 3D point along a straight ray through one point, the camera centre CC, onto an image plane. Put the camera frame at CC with zcz_c along the optical axis, xcx_c to the right and ycy_c down in the image. A point (Xc,Yc,Zc)(X_c, Y_c, Z_c) in this frame lands on the image plane at distance ff where, by similar triangles,

x=f XcZc,y=f YcZc.x = f\,\frac{X_c}{Z_c}, \qquad y = f\,\frac{Y_c}{Z_c}.
Left: a top view with the camera centre C on the left, the optical axis z_c pointing right and x_c pointing up. A vertical image plane sits at distance f from C. A red ray runs from C to a 3D point X at distance Z_c along the axis and X_c off it, and crosses the image plane at height f X_c over Z_c. Right: a pixel grid with origin at the top-left corner, u to the right and v down, the principal point near the centre and the projected point (u, v) to its right, with the equations u = f_x X_c / Z_c + c_x and v = f_y Y_c / Z_c + c_y.
Pinhole projection. A 3D point and the camera centre define a ray, and the ray crosses the image plane at f X_c / Z_c. In pixels, the coordinates are scaled by the focal length in pixels and shifted by the principal point.

Division by ZcZ_c is what makes the image perspective: an object twice as far away appears half as large. It also throws away depth. Every point on the ray projects to the same pixel.

The intrinsic matrix K

Pixels are counted from the top-left corner of the sensor, not from the optical axis, and they have a physical size. The intrinsic matrix collects the conversion:

[uv1]∼K[XcYcZc],K=[fxscx0fycy001].\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} \sim K \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix}, \qquad K = \begin{bmatrix} f_x & s & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}.

The symbol ∼\sim means “equal up to scale”: compute the right side, then divide by its third entry, which is ZcZ_c.

  • fxf_x, fyf_y: focal length in pixels, the physical focal length divided by the pixel pitch. As an example, a 4 mm lens on a sensor with 3 µm pixels gives fx≈1333f_x \approx 1333 px. They differ only if the pixels are not square.
  • cxc_x, cyc_y: the principal point, where the optical axis meets the sensor. It is near the image centre but rarely exactly on it.
  • ss: skew, zero for practically every modern sensor.

Extrinsics R and t

Points usually come in a world or robot-base frame. The extrinsics move them into the camera frame:

Xc=R Xw+t,[uv1]∼K [R∣t][Xw1].X_c = R\,X_w + t, \qquad \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} \sim K\,[R \mid t]\begin{bmatrix} X_w \\ 1 \end{bmatrix}.

RR and tt are the rotation and translation from spatial maths, in the direction world to camera. The camera centre in world coordinates is C=−RTtC = -R^\mathsf{T} t. The 3×4 matrix P=K[R∣t]P = K[R \mid t] is the camera matrix.

Worked example

Example values for a 640 × 480 camera: fx=fy=800f_x = f_y = 800 px, cx=320c_x = 320, cy=240c_y = 240.

A point at (0.10,−0.05,0.50)(0.10, -0.05, 0.50) m in the camera frame projects to

u=800⋅0.100.50+320=480,v=800⋅−0.050.50+240=160.u = 800 \cdot \frac{0.10}{0.50} + 320 = 480, \qquad v = 800 \cdot \frac{-0.05}{0.50} + 240 = 160 .

Now mount the camera 0.6 m above the table, looking straight down. With the world frame on the table (zz up), the camera’s zcz_c points along world −z-z, xcx_c along world xx, and ycy_c along world −y-y:

R=[1000−1000−1],C=(0,0,0.6),t=−RC=(0,0,0.6).R = \begin{bmatrix} 1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & -1 \end{bmatrix}, \qquad C = (0, 0, 0.6), \qquad t = -RC = (0, 0, 0.6).

A point on the table at (0.12,0.06,0)(0.12, 0.06, 0) m has Xc=(0.12,−0.06,0.60)X_c = (0.12, -0.06, 0.60) and projects to the same pixel, (480,160)(480, 160).

def project(K, R, t, X_w, dist=None):
    X_c = X_w @ R.T + t                 # world -> camera, one point per row
    z = X_c[:, 2]                       # depth; z <= 0 means behind the camera
    xn = X_c[:, :2] / z[:, None]        # normalised image coordinates
    if dist is not None:
        xn = distort(xn, dist)          # lens model, next section
    uv = xn @ K[:2, :2].T + K[:2, 2]
    return uv, z

Return the depth with the pixels. A point behind the camera still produces a pixel after the division, and only the sign of ZcZ_c tells you it is not real.

Lens distortion

Real lenses bend straight lines, most visibly near the image edges. The usual model is the Brown-Conrady model, applied to normalised coordinates x=Xc/Zcx = X_c/Z_c, y=Yc/Zcy = Y_c/Z_c before KK:

r2=x2+y2xd=x (1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)yd=y (1+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy\begin{aligned} r^2 &= x^2 + y^2 \\ x_d &= x\,(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + 2p_1 xy + p_2(r^2 + 2x^2) \\ y_d &= y\,(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + p_1(r^2 + 2y^2) + 2p_2 xy \end{aligned}

The kk terms are radial distortion: the image is stretched or compressed with distance from the centre. Negative k1k_1 gives barrel distortion (straight lines bow outwards). The pp terms are tangential distortion, caused by a lens that is slightly tilted relative to the sensor. The coefficient order (k1,k2,p1,p2,k3)(k_1, k_2, p_1, p_2, k_3) in the download matches the order OpenCV uses.

The size is easy to underestimate. With k1=−0.2k_1 = -0.2, a point at r=0.5r = 0.5 (400 px from the centre when f=800f = 800 px) moves inwards by 5 %, which is 20 px. At 0.6 m that is about 15 mm on the table. Distortion must be removed before any of the geometry below gives millimetre accuracy.

The model maps undistorted to distorted coordinates. Going back, which you need for every measured pixel, has no closed form. undistort_normalized uses a fixed-point iteration. With k1=−0.25k_1 = -0.25 and points out to r≈0.57r \approx 0.57, five iterations reach an error of 10−610^{-6} and ten reach 10−1010^{-10} in normalised units.

Camera calibration

Calibration estimates KK and the distortion coefficients, and as a by-product the pose of the calibration target in every image. The standard method for a single camera is Zhang’s method with a flat checkerboard.

One view gives a homography

Put the world frame on the board, so every corner has Zw=0Z_w = 0. The projection then drops the third column of RR:

[uv1]∼K [ r1  r2  t ][XwYw1]=H[XwYw1].\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} \sim K\,[\,r_1 \; r_2 \; t\,]\begin{bmatrix} X_w \\ Y_w \\ 1 \end{bmatrix} = H \begin{bmatrix} X_w \\ Y_w \\ 1 \end{bmatrix}.

The 3×3 matrix HH is a homography, a projective map from one plane to another. It can be estimated from the detected corners alone (see the DLT below).

Several views give K

The columns of H=[h1 h2 h3]H = [h_1\ h_2\ h_3] are λKr1\lambda K r_1, λKr2\lambda K r_2, λKt\lambda K t for some scale λ\lambda. Because r1r_1 and r2r_2 are orthonormal, each view gives two equations on the symmetric matrix B=K−TK−1B = K^{-\mathsf{T}} K^{-1}:

h1TB h2=0,h1TB h1=h2TB h2.h_1^\mathsf{T} B\, h_2 = 0, \qquad h_1^\mathsf{T} B\, h_1 = h_2^\mathsf{T} B\, h_2 .

BB has six entries and is known only up to scale, so it has five unknowns, and three views in different orientations determine it. Stacking the equations gives a homogeneous system Vb=0Vb = 0, solved with the SVD. KK then follows from BB; the download uses a Cholesky factorisation of BB for this step. The download implements this linear step in intrinsics_from_homographies. On synthetic data (6 views of a 9 × 6 corner board, 0.2 px corner noise) it returns fx=818.6f_x = 818.6, fy=808.8f_y = 808.8, cx=329.8c_x = 329.8, cy=235.4c_y = 235.4 px against the true 820, 810, 330 and 235.

Refinement

The linear solution ignores distortion and minimises an algebraic error rather than a geometric one. Real calibration tools use it only as a starting point. They then run a nonlinear least-squares optimisation, usually Levenberg-Marquardt, over KK, the distortion coefficients and one (R,t)(R, t) per view, minimising the reprojection error: the distance in pixels between each detected corner and where the model projects it.

The final RMS reprojection error is the number to check. A common rule of thumb is to aim for less than one pixel. A large error, or one much larger in some images, points to motion blur, a board that is not flat, or a wrong corner count.

Practical rules that follow from the maths:

  • Tilt the board in many directions. Fronto-parallel views all give nearly the same constraint and leave KK poorly determined. Zhang (2000) names parallel board positions as a degenerate configuration of the method.
  • Fill the image corners in some views. Distortion is largest there, and the model cannot be estimated from points near the centre. In Zhang’s own experiment, the views in which the board covered only the centre of the image gave unreliable distortion estimates.
  • Fix the focus and the resolution. Both change KK, and autofocus invalidates a calibration.

From a pixel to the table

A calibrated camera turns a pixel into a ray. Undistort the pixel, apply K−1K^{-1} to get a direction in the camera frame, and rotate it into the world:

dc=K−1[uv1],dw=RTdc,X(s)=C+s dw, s>0.d_c = K^{-1}\begin{bmatrix} u \\ v \\ 1 \end{bmatrix}, \qquad d_w = R^\mathsf{T} d_c, \qquad X(s) = C + s\,d_w,\ s > 0 .

To locate an object you need one more fact, and for a robot on a table the table itself provides it. Intersect the ray with the plane nTX=hn^\mathsf{T} X = h:

s=h−nTCnTdw,X=C+s dw.s = \frac{h - n^\mathsf{T} C}{n^\mathsf{T} d_w}, \qquad X = C + s\,d_w .

For the downward camera, pixel (480,160)(480, 160) gives dc=(0.2,−0.1,1)d_c = (0.2, -0.1, 1) and dw=(0.2,0.1,−1)d_w = (0.2, 0.1, -1). With C=(0,0,0.6)C = (0, 0, 0.6) and the table plane z=0z = 0, s=0.6s = 0.6 and X=(0.12,0.06,0)X = (0.12, 0.06, 0) m, the point we started from.

Now suppose the pixel is the centre of the top face of a 25 mm cube. The correct plane is z=0.025z = 0.025 m, which gives s=0.575s = 0.575 and X=(0.115,0.0575,0.025)X = (0.115, 0.0575, 0.025) m. Using the table plane instead would put the cube 5.6 mm too far from the point under the camera, enough to make a gripper miss a small object. Intersect with the plane the detected feature actually lies on.

def backproject_to_plane(K, R, t, uv, normal=(0.0, 0.0, 1.0), offset=0.0, dist=None):
    n = np.asarray(normal)
    c, d = pixel_to_ray(K, R, t, uv, dist)      # camera centre, unit rays (world)
    s = (offset - c @ n) / (d @ n)
    return c + s[:, None] * d

The function in the download also rejects rays parallel to the plane and planes behind the camera.

Hand-eye calibration

The ray above is in the world frame you used for RR and tt. For a robot, that frame must be the robot base, so you need the transform between camera and base. Finding it is hand-eye calibration. There are two set-ups:

  • Eye-to-hand: the camera is fixed and watches the arm, a common set-up for a desktop arm such as the SO-101. You want the constant transform from camera to robot base.
  • Eye-in-hand: the camera rides on the wrist. You want the constant transform from camera to gripper. The classical formulation moves the arm to several poses, observes a fixed target each time, and solves AX=XBAX = XB for the unknown transform XX, where AA are relative gripper motions from forward kinematics and BB the matching relative camera motions. Tsai and Lenz (1989) solve this with at least three arm poses: first the rotation, then the translation, each by linear least squares.

For the fixed camera there is a direct route when you can measure 3D points in both frames. Let the gripper hold a marker, move it to a dozen positions, record each position in the base frame from forward kinematics, and measure the same positions in the camera frame (with a depth camera, or from a marker of known size). The best rigid transform between the two point sets has a closed-form SVD solution, rigid_transform in the download. With 12 points and 1 mm of noise, the test recovers the rotation to 0.13° and the translation to 0.5 mm.

Homographies for planar scenes

If everything the robot picks lies on the table, you can skip the full calibration chain. Points on a plane map to the image through a homography, so a single 3×3 matrix converts table coordinates to pixels and its inverse converts pixels to table coordinates.

The direct linear transform

Each correspondence (x,y)→(u,v)(x, y) \to (u, v) with [u,v,1]T∼H[x,y,1]T[u, v, 1]^\mathsf{T} \sim H[x, y, 1]^\mathsf{T} gives two linear equations in the nine entries of HH, after you multiply out the division:

[−x−y−1000uxuyu000−x−y−1vxvyv]h=0.\begin{bmatrix} -x & -y & -1 & 0 & 0 & 0 & ux & uy & u \\ 0 & 0 & 0 & -x & -y & -1 & vx & vy & v \end{bmatrix} h = 0 .

Stack the rows for N≥4N \ge 4 points into AA and take hh as the right singular vector of AA for the smallest singular value. This minimises ∥Ah∥\lVert Ah \rVert subject to ∥h∥=1\lVert h \rVert = 1, which removes the scale ambiguity. No three of the points may be collinear.

Normalisation

Before building AA, translate each point set so its centroid is at the origin and scale it so the mean distance from the origin is 2\sqrt{2}. Hartley (1997) proposed this normalisation for the eight-point algorithm and notes that it applies directly to estimating projective transformations between point sets. Estimate H~\tilde H for the normalised points, then undo the two transforms: H=T2−1H~T1H = T_2^{-1} \tilde H T_1.

T1, s = normalize_points(src)                  # table points, e.g. metres
T2, d = normalize_points(dst)                  # pixels
A = build_rows(s, d)                           # 2N x 9
_, _, vt = np.linalg.svd(A)
H = np.linalg.inv(T2) @ vt[-1].reshape(3, 3) @ T1
H /= H[2, 2]

Without normalisation, the entries of AA mix products such as pixels times millimetres, of order 10510^5, with ones, and the result depends on the units and origin you happened to choose for the table. With it, the estimate is the same whether you measure in metres from the robot base or in millimetres from a table corner. test_normalised_dlt_is_invariant_to_world_units_and_origin checks exactly that. In double precision with well-spread points the accuracy gain is often small, but the step is cheap and removes a whole class of surprises.

Calibrating the table with the robot

A practical procedure for an arm and a fixed camera:

  1. Move the gripper tip to six or more points spread over the work area, touching the table. Read each position from forward kinematics.
  2. Detect the tip (or a small marker on it) in the image at each point.
  3. Estimate HH from table coordinates to pixels.
  4. At run time, map each detected object pixel through H−1H^{-1} to get its table position in the robot base frame.

This folds intrinsics, extrinsics and the camera-to-base transform for that one plane into a single matrix. It does not model distortion, so undistort the pixels first or keep the work area near the image centre. It is only valid for points on that plane; for objects of known height, calibrate a second plane at that height or use the ray intersection above.

The download’s worked example does this with a tilted camera 0.45 m above the table and six touched points with 0.5 px detection noise. A cube placed at (0.070,0.220)(0.070, 0.220) m is located with an error of 0.28 mm.

Line plot of RMS location error on the table in millimetres against pixel noise from 0 to 2 pixels, for homographies fitted to 4, 6 and 12 calibration points. All three lines rise linearly from zero. At 2 pixels of noise the errors are about 3.6 mm for 4 points, 2.7 mm for 6 points and 2.1 mm for 12 points.
RMS error of locating points anywhere in a 0.4 m × 0.3 m work area with a table-to-image homography, against the pixel noise on the calibration points, for 4, 6 and 12 calibration points on a regular grid. Each value averages 300 random trials with a camera 0.45 m above the table.

The error grows linearly with pixel noise, and more calibration points reduce it. At 0.5 px noise, four points give about 0.85 mm RMS over the work area and twelve give 0.52 mm. Spread the points to the edges of the work area: outside the calibrated region you are extrapolating and the error grows.

Code and tests

Download camera.py Download test_camera.py
python camera.py
python -m pytest -q
# 13 passed

All tests use synthetic cameras and seeded noise. They cover the worked projection, the downward-camera back-projection numbers, the project and back-project round trip with distortion, undistortion, exact homography recovery without noise, cube location with noise, invariance of the normalised DLT, Zhang’s linear calibration and the rigid transform for hand-eye calibration.

Exercise

Locate a cube from one pixel

Use the downward camera from the worked example: fx=fy=800f_x = f_y = 800 px, cx=320c_x = 320, cy=240c_y = 240, centre 0.6 m above the table, R=diag(1,−1,−1)R = \mathrm{diag}(1, -1, -1). A detector reports the top-face centre of a 30 mm cube at pixel (200,300)(200, 300). Lens distortion has already been removed.

  1. Compute the ray direction in world coordinates.
  2. Find the cube’s top-face centre.
  3. How large is the error if you intersect with the table plane instead?
Show solution
  1. dc=((200−320)/800, (300−240)/800, 1)=(−0.15,0.075,1)d_c = ((200 - 320)/800,\ (300 - 240)/800,\ 1) = (-0.15, 0.075, 1). dw=RTdc=(−0.15,−0.075,−1)d_w = R^\mathsf{T} d_c = (-0.15, -0.075, -1) (not normalised, which is fine for the intersection).
  2. Plane z=0.03z = 0.03: s=(0.03−0.6)/(−1)=0.57s = (0.03 - 0.6)/(-1) = 0.57, so X=(0−0.57⋅0.15, 0−0.57⋅0.075, 0.03)=(−0.0855,−0.0428,0.030)X = (0 - 0.57 \cdot 0.15,\ 0 - 0.57 \cdot 0.075,\ 0.03) = (-0.0855, -0.0428, 0.030) m.
  3. Plane z=0z = 0: s=0.6s = 0.6, X=(−0.090,−0.045,0)X = (-0.090, -0.045, 0) m. The horizontal error is 0.00452+0.002252=5.0\sqrt{0.0045^2 + 0.00225^2} = 5.0 mm, directed away from the point below the camera.

Check: backproject_to_plane(K, R, t, np.array([[200.0, 300.0]]), offset=0.03) with t=(0,0,0.6)t = (0, 0, 0.6).

Exercise

Count the constraints

  1. A homography has nine entries. Why do four point pairs suffice, and why does it fail if three of the four points lie on a line?
  2. Zhang’s linear step needs at least three views of the board. With the extra assumption that the skew is zero, how many views are enough?
  3. You calibrate with ten images, all taken with the board facing the camera squarely at the same distance. The reported reprojection error is low. Why can KK still be wrong?
Show solution
  1. HH is defined up to scale, so it has eight degrees of freedom. Each pair gives two equations (one for uu, one for vv), and four pairs give eight. If three points are collinear, their equations are not independent. A homography maps lines to lines, so once two of the points fix where their line goes, the third point on it adds only one new equation instead of two. Four such points give seven independent equations for eight unknowns, and AA has a null space of more than one dimension.
  2. BB has five unknowns after removing scale. Zero skew sets B12=0B_{12} = 0, which removes one unknown, leaving four. Each view gives two equations, so two views in different orientations suffice.
  3. Views that all look the same give nearly identical constraints on BB, so the stacked system is close to rank-deficient. In particular, focal length and distance trade off against each other when the board is never tilted. The optimiser finds a KK that fits these images well but is wrong for other viewing angles. Low reprojection error on the calibration images does not prove a good calibration; tilted views and views that cover the image corners do.

Summary

  • The pinhole model projects a world point with [u,v,1]T∼K[R∣t]Xw[u, v, 1]^\mathsf{T} \sim K[R \mid t]X_w. KK holds the focal length in pixels and the principal point; RR and tt move points from the world into the camera frame.
  • Lens distortion is modelled with radial (k1,k2,k3k_1, k_2, k_3) and tangential (p1,p2p_1, p_2) terms on normalised coordinates. A few percent of radial distortion moves edge pixels by tens of pixels and must be removed before metric measurements.
  • Zhang’s method calibrates from several views of a flat board: each view gives a homography, each homography gives two linear constraints on KK, and a nonlinear refinement minimises the reprojection error.
  • One pixel gives only a ray. Intersecting the ray with a known plane, such as the table or the top face of an object, gives a 3D position; using the wrong plane causes a systematic error of millimetres.
  • For objects on a table, a homography estimated with the normalised DLT from robot-touched points maps pixels directly to table coordinates in the robot base frame.

Check your understanding

Pick an answer for each question, then check. Answers stay hidden until you submit.

1. A camera has fx = fy = 800 px and principal point (320, 240). Where does the camera-frame point (0.10, -0.05, 0.50) m project?
2. Why can a single camera not give the 3D position of an arbitrary point from one pixel?
3. How many point correspondences does the DLT need at least to estimate a homography, and why?
4. You locate a 30 mm cube from its top-face centre but intersect the ray with the table plane z = 0 instead of z = 0.03 m. What kind of error do you get?
5. In Zhang's calibration method, what does one view of a planar checkerboard contribute?

Further reading

Found a mistake or something unclear?Report a correction.