Section 4.6: Camera, body, and world frames

"A pixel tells you what the sensor saw. A frame tells you what the robot can do about it."

A Careful Control Loop

Figure 4.6A captures the stakes: a pixel detection is inert until it is chained through the camera, body, and world frames. A wrist camera spots a coffee cup, reports pixel (312, 204) with full confidence, and the arm reaches a clean 8 cm to the left of the handle: the detector was never wrong, the frame chain was. This section develops the camera-to-robot coordinate chain that prevents exactly that failure. First we define the camera optical frame (the axes fixed to the lens) and the body frame (the axes fixed to the robot base). Then we show how camera intrinsics (the lens parameters that relate pixels to rays: focal lengths and principal point) convert a pixel and depth into a 3D camera-frame point. Finally we compose extrinsics (the rigid transform that gives the camera's position and orientation relative to the body) so the same point becomes a body-frame or world-frame target. Figure 4.6B lays out this pixel-to-world chain end to end.

The key question is practical: when a perception model marks a pixel, what additional calibration, depth, and transform information turns that pixel into a robot action?

A pixel coordinate is a question; a world-frame point is the answer the robot can act on.

Theory

A pinhole camera model maps a 3D camera-frame point \((X, Y, Z)\) to pixel coordinates \((u, v)\) using focal lengths \((f_x, f_y)\) and principal point \((c_x, c_y)\):

$$u = f_x\frac{X}{Z} + c_x, \qquad v = f_y\frac{Y}{Z} + c_y.$$

Back-projection reverses this mapping when depth \(Z\) is known:

$$X = (u-c_x)\frac{Z}{f_x}, \qquad Y = (v-c_y)\frac{Z}{f_y}.$$

The axis-convention gap

This derivation assumes a calibrated pinhole model after distortion correction, with matching image resolution and synchronized depth. OpenCV convention uses \(x\) right, \(y\) down, and \(z\) forward. Robotics body frames use \(x\) forward, \(y\) left, and \(z\) up. This mismatch is the axis-convention gap, and it forces you to make camera-to-body extrinsics explicit. On a physical robot, ignoring the gap produces a categorical error. A point detected below the image center points downward in camera coordinates, but the body frame treats that direction as leftward. The arm then reaches along the wrong axis entirely. To fix this, apply a fixed rotation: rotate 90 degrees about the camera \(x\)-axis, then 90 degrees about the resulting \(z\)-axis. This swaps \(y\)-down into \(z\)-up and \(z\)-forward into \(x\)-forward, aligning the optical frame to the body frame. Every physical sensor that supplies depth, including cameras and structured-light depth sensors, defines its own optical frame. That per-sensor variation makes the axis-convention gap a persistent source of error.

In practice, "the world frame" is rarely one single frame. Most stacks maintain a continuous odom frame, produced by a state estimator (the software module, typically an extended Kalman filter or factor-graph optimizer, that fuses wheel encoders, IMU readings, and visual odometry into a running pose estimate), which never jumps but slowly drifts over minutes of travel; and a separate map frame that is corrected against a stored map or loop closure and therefore can jump, but does not drift over the long run. The transform chain in this section treats "world" as whichever of these two is authoritative for the task, so a grasp planner must know which one it is reading before trusting a world-frame point.

Both \(T_{\text{body},\text{cam}}\) and \(T_{\text{world},\text{body}}\) below are 4x4 homogeneous transform matrices, the same representation introduced in section 4.4: the upper-left 3x3 block is a rotation and the rightmost column is a translation, packaged so that composing two frame changes is a single matrix product.

Worked Example

Running the pinhole equations and the axis-convention gap on concrete numbers turns the abstract transform into inspectable code. Code Fragment 4.6.1 back-projects a detected pixel into the camera frame, then shifts it into a simple body frame. The example is small by design: one pixel, one depth value, one camera offset.

# Back-project one detected pixel into the camera frame.
# Then translate it into the robot body frame using a known camera offset.
# This exposes the difference between image evidence and action-ready geometry.
import numpy as np

fx, fy = 600.0, 600.0
cx, cy = 320.0, 240.0
u, v, depth = 380.0, 210.0, 2.0

x_camera = (u - cx) * depth / fx
y_camera = (v - cy) * depth / fy
point_camera = np.array([x_camera, y_camera, depth])

camera_origin_in_body = np.array([0.30, 0.00, 0.80])
point_body = camera_origin_in_body + point_camera

print("camera frame:", point_camera.round(3).tolist())
print("body frame:", point_body.round(3).tolist())

Expected output: the camera-frame point shows where the pixel ray lands at 2 meters of depth. The body-frame point shifts by the camera mounting offset, which is the piece perception logs often omit when debugging reach errors.

The same pattern composes a full sensor-to-world chain. An Inertial Measurement Unit (IMU) reports acceleration in its own frame; the controller needs it in the body frame; the navigation stack needs it in the world frame. Each hop is one homogeneous transform, and the composition is left to right along the chain:

$$T_{\text{world},\text{imu}} = T_{\text{world},\text{body}}\; T_{\text{body},\text{imu}}.$$

# Chain a measurement from the IMU frame to the body frame to the world frame.
# Each hop is one homogeneous transform; the order follows the frame graph.
import numpy as np
from scipy.spatial.transform import Rotation as Rot

def make_T(rpy_deg, translation):
    T = np.eye(4)
    T[:3, :3] = Rot.from_euler("xyz", rpy_deg, degrees=True).as_matrix()
    T[:3, 3] = translation
    return T

# IMU mounted rotated 90 deg about z and offset on the body.
T_body_imu = make_T([0, 0, 90], [0.05, 0.0, 0.10])
# Body pose in the world: yawed 30 deg and translated.
T_world_body = make_T([0, 0, 30], [2.0, 1.0, 0.0])

T_world_imu = T_world_body @ T_body_imu

p_imu = np.array([1.0, 0.0, 0.0, 1.0])     # a point 1 m along the IMU x-axis
p_body = T_body_imu @ p_imu
p_world = T_world_imu @ p_imu

print("in body frame: ", p_body[:3].round(3).tolist())
print("in world frame:", p_world[:3].round(3).tolist())

Practical Recipe

Knowing which library anchors the intrinsics and extrinsics is only half the job; the following recipe orders those calibration steps so each transform is verified before the next one depends on it.

1. Fix intrinsics first: run OpenCV's calibrateCamera on a charuco board (a calibration target combining a checkerboard with ArUco fiducial markers, so corners stay identifiable even under partial occlusion) and verify that reprojection error (the pixel distance between a detected corner and where the fitted camera model predicts it) is below 0.5 px before touching extrinsics. A depth camera such as the Intel RealSense D435 ships with factory intrinsics, but wrist-mounted copies on a Franka Panda routinely drift 2-3 px after mechanical stress; re-calibrate after any crash or hard stop.
2. Measure the extrinsic transform with a known target at a known distance, then compare the back-projected world-frame point to a tape-measure ground truth. A 1-degree pitch error in the camera mount of a wrist camera at 0.5 m depth produces an 8 mm forward offset in the body frame, which is enough to cause a parallel-jaw gripper to close 1 cm behind the target object; a 5-degree tilt (easy to introduce by hand-tightening a single mount screw unevenly) at 1 m depth produces 87 mm of error, which means the gripper misses the object entirely rather than clipping it, yet the detector logs show a confident, high-score detection every time.
3. Log every frame hop with a timestamp: camera-frame point, body-frame point, world-frame point, depth sensor timestamp, and robot pose timestamp. A mismatch above 20 ms between depth capture and odometry update is typically among the most common causes of grasp-position errors on mobile manipulators such as Spot Arm and TIAGo, in practice ranking alongside axis-convention and calibration-drift bugs rather than dominating them outright.
4. Record failures as structured cases with the axis convention and frame name attached: a "left-right flip" bug almost always traces to an OpenCV optical-frame \(y\)-down convention misread as \(y\)-up, not a bad detector.
5. Run one perturbation test by physically rotating the camera mount 5 degrees and confirming that the world-frame point shifts by the predicted amount before deploying to a new robot configuration.

Production Pattern

Camera, body, and world frames sits inside the Part II robotics contract: geometry defines where things are, kinematics defines what motion is possible, dynamics defines what motion costs, control defines how errors are corrected, and sensing defines what the agent can know on time.

Write frame relationships into the same audit trail before projecting or back-projecting any observation. The pattern then carries its full weight: an intuitive role, a formal interface, a runnable check, and a reproducible failure mode.

Use this recipe when turning Camera, body, and world frames into code, a simulator experiment, or a robot diagnostic. The point is not to use every library. The point is to keep the hand-built baseline and the maintained-tool path comparable.

1. Name every frame with a parent, child, unit convention, and timestamp policy.
2. Write one hand-checked transform chain and verify identity, inverse, and composition tests.
3. Run the same transform through ROS 2 tf2 or SciPy Rotation, then compare one point and one direction vector.
4. Record a frame audit with source sensor, latency, and expected sign convention.
5. Debug failed behavior by replaying the transform tree before changing policy or controller code.

Camera-to-body mistakes typically corrupt downstream perception results whenever the affected pixels feed a 3D estimate, though pure 2D tasks such as image classification are unaffected. Section 4.7 covers common frame mistakes and how to debug them. Verify optical-frame convention, extrinsics, depth scale, and timestamp alignment before blaming detection or planning. Consider a specific case. A manipulation robot using an Intel RealSense D435 mounted 30 degrees downward on the wrist reports grasps that sit 8 cm too far forward. The detector is correct. The bug is that the extrinsic calibration stored a pure translation and ignored the 30-degree pitch, so the back-projected \(Z\) depth in the camera frame maps to a mix of forward and downward displacement in the body frame. Replacing the translation-only offset with a full \(T_{\text{body},\text{cam}}\) (rotation plus translation) reduces the grasp position error to under 5 mm.