Section 4.6: Camera, body, and world frames

A Careful Control Loop

This section develops the camera-to-robot coordinate chain used by embodied perception. First we define the camera optical frame and the body frame. Then we show how camera intrinsics convert a pixel and depth into a 3D camera-frame point. Finally we compose extrinsics so the same point becomes a body-frame or world-frame target.

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?

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}.$$

This derivation assumes a calibrated pinhole model after distortion correction, matching image resolution, synchronized depth, and a camera optical frame convention. OpenCV convention usually uses $x$ right, $y$ down, and $z$ forward. Robotics body frames often use $x$ forward, $y$ left, and $z$ up. That mismatch is why camera-to-body extrinsics must be explicit.

Worked Example

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 intentionally small: 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 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

1. Write the observation, action, and success metric before choosing a model.
2. Build a baseline that is simple enough to debug by inspection.
3. Add the library implementation only after the baseline behavior is understood.
4. Record failures as structured cases: perception error, state error, planning error, control error, or evaluation error.
5. Run at least one perturbation test before trusting the result.

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 camera, body, and world frames into the same audit trail before projecting or back-projecting observations. This makes the section useful to students, builders, and researchers at the same time: the idea has an intuitive role, a formal interface, a runnable check, and a failure mode that can be reproduced.

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 corrupt every downstream perception result. Verify optical-frame convention, extrinsics, depth scale, and timestamp alignment before blaming detection or planning.