Calibration and coordinate frames

"Every one of my sensors is certain it sits at the center of the universe. My job is to talk them out of it before they collide."

A Diplomatic AI Agent

Frames, poses, and the rigid-transform chain

A coordinate frame is an origin plus three orthonormal axes attached to a physical thing: the camera's optical center, the lidar's rotation axis, the vehicle's rear axle, a surveyed point on the factory floor. A 3D point has different numeric coordinates in each frame even though it is one point in the world. The what of calibration is finding, for every pair of frames you care about, the rigid transform that converts coordinates from one to the other; the why is that perception, planning, and control each live in a different frame and must exchange geometry constantly.

A rigid transform from frame \(B\) to frame \(A\) is a rotation \(R \in SO(3)\) and a translation \(t \in \mathbb{R}^3\), packed into a \(4\times 4\) homogeneous matrix so that composition becomes matrix multiplication:

The leading superscript names the frame the coordinates are expressed in. This bookkeeping is not pedantry: the single most common calibration bug is applying a transform backwards, and \({}^{A}T_{B}\) versus its inverse \({}^{B}T_{A} = {}^{A}T_{B}^{-1}\) differ by exactly that mistake. Transforms compose along a chain, \({}^{A}T_{C} = {}^{A}T_{B}\,{}^{B}T_{C}\), which is what lets you move a lidar point through the sensor mount, into the body frame, and out into the world in one product. Because \(R\) is orthonormal, its inverse is its transpose, and the inverse transform is \(R^\top\) with translation \(-R^\top t\), never a naive negation.

Intrinsic calibration: turning pixels into rays

For a depth camera derived from a pinhole model, the intrinsics are the focal lengths \(f_x, f_y\) and principal point \((c_x, c_y)\) that map a 3D point in the camera frame to a pixel, plus lens-distortion coefficients that bend straight lines. The forward projection of a point \((X, Y, Z)\) is

Inverting it is what makes depth 3D: given a pixel \((u,v)\) and its measured depth \(Z\), you recover \(X = (u - c_x)\,Z / f_x\) and \(Y = (v - c_y)\,Z / f_y\). This back-projection is the bridge between the depth map and the point cloud of Section 40.2, and every point cloud is only as accurate as the intrinsics behind it. A principal point wrong by five pixels or a focal length off by one percent shears the whole cloud, so a plane looks slightly bowled and registration in Section 40.4 never quite converges. Radial distortion, if left uncorrected, curves what should be straight walls, an error that grows toward the image edges precisely where wide-baseline stereo is already weakest.

Intrinsics are estimated by showing the sensor a known target, classically a checkerboard, from many poses and minimizing the reprojection error between detected corners and their projected model positions. The when: do this once per device at build time, then re-check whenever thermal cycling or a lens knock is suspected, because a depth camera's baseline can drift with a few degrees of temperature change.

Extrinsic calibration: putting every sensor in one frame

Extrinsic calibration finds \({}^{A}T_{B}\) between two sensors so their observations of the same scene overlap. The three cases you will meet constantly are camera-to-camera (the stereo baseline of Section 40.1), camera-to-depth (aligning a color image to a depth map so each pixel gets both), and camera-to-lidar or lidar-to-IMU for a full robot stack. Each is solved by observing shared structure: mutual views of a board for cameras, a board whose plane is visible to both camera and lidar for the cross-modal case, and a motion sequence for camera-to-IMU, where the classic hand-eye formulation \(AX = XB\) recovers the unknown mounting pose \(X\) from the two sensors' relative motions \(A\) and \(B\).

The code below composes an extrinsic and does the color-to-depth alignment that a wearable or an RGB-D camera performs on every frame: back-project depth pixels to 3D, move them into the color camera's frame, and project them into the color image so each depth sample inherits a pixel of color.

import numpy as np

def align_depth_to_color(depth, K_d, K_c, T_c_d):
    """Map each depth pixel into the color camera's image.
    depth: HxW depth in meters (depth-camera frame)
    K_d, K_c: 3x3 intrinsics for depth and color cameras
    T_c_d: 4x4 rigid transform, depth frame -> color frame
    """
    H, W = depth.shape
    u, v = np.meshgrid(np.arange(W), np.arange(H))
    z = depth.reshape(-1)
    valid = z > 0
    # back-project depth pixels to 3D rays in the depth frame
    x = (u.reshape(-1) - K_d[0, 2]) * z / K_d[0, 0]
    y = (v.reshape(-1) - K_d[1, 2]) * z / K_d[1, 1]
    pts_d = np.stack([x, y, z, np.ones_like(z)], axis=0)   # 4xN homogeneous
    pts_c = T_c_d @ pts_d                                    # into color frame
    # project into the color image
    proj = K_c @ pts_c[:3]
    uu = proj[0] / proj[2]
    vv = proj[1] / proj[2]
    return uu[valid], vv[valid], pts_c[2, valid]            # color pixels + their depth

Notice the single line pts_c = T_c_d @ pts_d is where extrinsic error enters and the two K matrices are where intrinsic error enters, which is exactly why the key insight above insists you separate them. The output pixels (uu, vv) tell you where each depth sample lands in color; a systematic constant offset means the extrinsic translation is off, while a fan-shaped spread that grows with depth means the rotation is off.

The transform tree, time, and staying honest

A real platform has more than two frames, so the transforms are organized as a tree with the world at the root and each sensor a leaf, exactly the tf tree that middleware like ROS maintains. Any frame-to-frame query is answered by walking the tree and multiplying transforms along the path. Some edges are static (a bolted-down camera relative to the chassis) and some are dynamic (the chassis relative to the world, updated by odometry or SLAM). Keeping this tree consistent is a running concern, not a one-time setup, and it links tightly to the localization and mapping of Chapter 52.

Two failure modes dominate. The first is temporal: transforming a lidar point captured at time \(t\) using a body pose from time \(t + \delta\) smears the cloud whenever the platform moves, so calibration is inseparable from the timestamp synchronization of Chapter 3. At highway speed a 20 ms sync error is a 30 cm position error. The second is drift: mounts flex, thermal expansion shifts baselines, and vibration slowly rotates a bracket, so extrinsics that were perfect at the factory decay in the field. Mature systems therefore monitor calibration continuously, watching reprojection residuals or plane-fit consistency, and either recalibrate online or raise a health flag.

What's Next

In Section 40.4, we take the clouds that calibration has placed in a common frame and clean them: removing outliers, downsampling for speed, estimating normals, and registering overlapping scans with ICP so that many partial views become one coherent model. Calibration puts sensors into approximate agreement; registration is how you refine that agreement point by point.