M7-T9 — Transforming Points Between Frames

Learning objectives

After this lesson, you should be able to:

• Turn a polar sensor reading into Cartesian coordinates in the sensor frame.
• Apply the mount transform and the robot pose, in the correct order, to reach the world frame, for a point and for a whole scan.
• Transform a world point back into the sensor frame (for example to predict a measurement).
• List and test the common failure modes: wrong order, wrong inverse, wrong units, wrong time.

Introduction and motivation

The wall in the map is 5 cm thick in one place and 40 cm thick in another because the scan is drawn with the robot pose of the previous frame. A drone lands 2 m away from where the camera said the target was, because the camera frame has z forward and the code assumed x forward. Both are small, silent frame mistakes, not sensor faults. A checklist and a known-answer test prevent them.

The intuition

The sensor knows only “this far in this direction from me”. To place that on a map we need to know (1) where the sensor is on the robot (a fixed, calibrated transform) and (2) where the robot is in the world right now (a time-dependent transform). Chaining the two moves the point from the sensor's private coordinates into the shared ones. To predict what the sensor should see, for example when matching a map to a scan, we run the chain backwards with inverses.

Geometric and physical interpretation

Geometric view. A range-and-bearing return is a point in polar coordinates around the sensor. Converting to Cartesian gives a vector from the sensor origin; the mount and pose transforms rotate that vector into world orientation and add the sensor's world position. Physical view. A spinning LiDAR takes about 0.1 s per revolution; if the robot moves at 1 m/s the scan is smeared by 10 cm unless each point uses the pose at its own time (motion distortion correction). A rolling-shutter camera has the same issue row by row.

The mathematics

(r, β)

range and bearing of a planar return (add an elevation angle ε in 3D).

ˢp

the point in sensor coordinates: (r cos β, r sin β) in 2D; (r cos ε cos β, r cos ε sin β, r sin ε) in 3D.

T_BS

the fixed mount transform (sensor in base), found by calibration.

T_WB(t)

the base pose in the world at time t.

P

a 3 × N or 4 × N matrix whose columns are N points (homogeneous in the 4 × N case).

Polar to Cartesian

ˢp = ( r cos β,  r sin β )        3D: ( r cos ε cos β,  r cos ε sin β,  r sin ε )

What it means. The return is a point at distance r in a direction given by the bearing (and elevation). This is the only step that uses the sensor physics; everything after is geometry.

To the world

ʷp = TWB(t) · TBS · ˢp        batch:  ʷP = TWB TBS ˢP

What it means. Apply the mount first, then the pose at the measurement time. For a whole scan stack the points as columns and multiply once; a 4 × 4 matrix times a 4 × N matrix transforms N points.

Back to the sensor

ˢp = TBS⁻¹ · TWB(t)⁻¹ · ʷp,    r = ‖ˢp‖,  β = atan2(ˢp_y, ˢp_x)

What it means. Invert each factor and reverse the order. The predicted range and bearing are what the sensor should report for a known landmark, the basis of every measurement model in a filter or graph.

Raw reading → Cartesian in sensor frame → T_BS → T_WB(t) → world. Reverse with inverses in reverse order. Always verify with a known point.

Step-by-step derivation: the recipe, the batch form and the measurement model

1. Polar to Cartesian. A planar return at range r and bearing β (counter-clockwise from the sensor x-axis) is (r cos β, r sin β); add a z-coordinate of 0 or use the 3D elevation form.
2. Homogeneous point. Append 1: ˢp̃ = (x, y, 1) in 2D, (x, y, z, 1) in 3D.
3. Chain. ᴮp̃ = T_BS ˢp̃ and ʷp̃ = T_WB(t) ᴮp̃, so ʷp̃ = T_WB T_BS ˢp̃: the inner B cancels.
4. Batch. Place N homogeneous points as columns of P (size 4 × N). Matrix multiplication acts on each column independently: T P has columns T pᵢ. Precompute T = T_WB T_BS once; the cost is N matrix-vector products, which a vectorised library does in microseconds.
5. Inverse (prediction). The map ʷp̃ = T ˢp̃ is inverted with the closed form T⁻¹ = [[Rᵀ, −Rᵀt], [0, 1]]; for a product (T_WB T_BS)⁻¹ = T_BS⁻¹ T_WB⁻¹.
6. Measurement model. The predicted reading of a landmark at known world position ʷℓ is h(x) = (‖ˢℓ‖, atan2(ˢℓ_y, ˢℓ_x)) with ˢℓ = T_BS⁻¹ T_WB⁻¹ ʷℓ̃. Filters and pose graphs compare this prediction with the actual reading; the difference is the residual to be minimised (Module 5), and its Jacobian is the subject of Module 8.

Camera convention. A pinhole camera's optical frame usually has x right, y down, z forward, while the vehicle body has x forward, y left, z up. The fixed rotation between them, for example R_BC = [[0, 0, 1], [−1, 0, 0], [0, −1, 0]], is part of T_BC and must be included; omitting it is the classic “everything looks rotated 90°” bug.

Time. If the pose is only known at discrete times t₀ < t₁, interpolate: linearly in the translation and by slerp in the rotation, to get T_WB(t) for a point stamped between them. Use the same pose for all points of a 1 ms burst, but a fresh one for each point of a full sweep.

Sanity tests. (1) A point straight ahead of a robot at the origin facing +x must map to +x. (2) Turning the robot 90° must turn the cloud 90° about the robot, not about the world origin. (3) Moving the robot 1 m must shift every point by 1 m. (4) Round trip: world → sensor → world returns the original point.

A worked example: a LiDAR scan and a drone camera point

Planar LiDAR. Base pose T_WB: position (2, 1), heading 90°. LiDAR mount T_BS: 0.3 m forward, no rotation. Three returns (r, β): (4 m, 0°), (4 m, 30°), (2 m, 90°).

• Sensor Cartesian. (4, 0), (3.464, 2), (0, 2).
• Base coordinates (add 0.3 to x): (4.3, 0), (3.764, 2), (0.3, 2).
• World coordinates. R(90°)(x, y) + (2, 1) = (−y + 2, x + 1): (2, 5.3), (0, 4.764), (0, 1.3). The robot faces world +y, so the straight-ahead return (4 m ahead of the sensor) is at y = 1 + 0.3 + 4 = 5.3 ✓.
• Predicting a measurement. The world landmark (0, 4.764) in the sensor frame: T_BS⁻¹T_WB⁻¹ gives (3.464, 2), range √(12 + 4) = 4.00, bearing atan2(2, 3.464) = 30° ✓: the original reading.

Drone camera (3D). The UAV is at (10, 5, 20) with yaw 90° (R_WB = Rz(90°)). A downward-or-forward sensor is mounted so that its optical axis points along body x: R_BS = Ry(90°), offset (0.1, 0, −0.05). A feature is 4 m along the optical axis: ˢp = (0, 0, 4).

• Base. R_BS ˢp + t_BS = (4, 0, 0) + (0.1, 0, −0.05) = (4.1, 0, −0.05).
• World. R_WB (4.1, 0, −0.05) + (10, 5, 20) = (0, 4.1, −0.05) + (10, 5, 20) = (10, 9.1, 19.95): 4 m in front of the nose, which points along world +y after the 90° yaw ✓.

Robotics application

SLAM and mapping. Every scan point is converted with this recipe before registration or insertion into a map; the pose graph then adjusts T_WB; see LiDAR point clouds for SLAM, point-cloud preprocessing and landmark-based mapping.

Calibration. The mount transforms T_BS come from extrinsic calibration; errors of 1° or 1 cm turn into 3.5 cm of error at 2 m range per degree; see camera-IMU-LiDAR extrinsic calibration.

Navigation. Obstacle points from the LiDAR are converted into the base frame for the local costmap and into the map frame for the global one; see frames, poses and footprints.

UAV perception and GNSS/IMU fusion. Camera features, LiDAR points, GNSS antenna offsets (lever arms) and IMU readings all pass through this chain with their own mount transforms; see UAV coordinate frames and TF frames and consistency.

Engineering implications and limitations

Practical rules

• Have a known-answer test per sensor. A fixture with the robot at a simple pose and a known target; assert the world coordinates. Run it in CI.
• Check the order and the inverse once, in one function. Everything else calls it; no hand-written multiplications scattered around.
• Include the optical-frame rotation for cameras. And check that the determinant of the full rotation is +1.
• Use the pose at the measurement time. Interpolate poses; deskew spinning LiDAR with per-point poses; account for sensor latency.
• Vectorise. Apply the transform to the whole 4 × N (or N × 3) array at once; avoid Python loops over points.
• Mind lever arms. GNSS antenna and IMU are not at the same point; rotation of the body makes their velocities differ by ω × r.
• Propagate uncertainty. A heading error δθ moves a point at range r by r·δθ sideways; far points are the least reliable (Module 9).

Limits

The recipe treats the mount and pose as exact. Calibration error, odometry drift and latency all enter as errors in T_BS and T_WB, and they scale with range. The geometry here gives the nominal map; the covariance machinery of Module 9 gives its uncertainty.

Common misconceptions

Engineering takeaways

1. Sensor reading → Cartesian in the sensor frame → mount transform T_BS → pose T_WB(t) → world point.
2. A whole scan is transformed by one matrix product on a 4 × N array.
3. Predicted measurements use the inverses in reverse order: ˢp = T_BS⁻¹T_WB⁻¹ʷp, then range and bearing.
4. Include camera optical-frame rotations, lever arms, and the pose at the measurement time.
5. Verify every pipeline with a known-answer test, a 90° turn and a round trip.

References and further reading

Primary papers and authoritative documentation.

• Modern Robotics (Lynch and Park), free preprint: chapter 3.3.1, applying transformations to points and frames
• Barfoot, State Estimation for Robotics (author-hosted first edition, free to read): chapter 6 and 8, transforming points and sensor models
• UCSD ECE276A: Sensing and Estimation in Robotics: transforming landmark and scan measurements between frames
• Boyd and Vandenberghe, Introduction to Applied Linear Algebra (VMLS): matrix-matrix products for batches of points; the site did not load when we last checked

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Module 7 gave you the algebra of where things are. Module 8 asks how things move: if the joints of an arm turn at some rate, how fast does the hand move? That map is a matrix, the Jacobian, and everything you learned about rank, singular values and pseudoinverses will be put to work on it.

Related content

Nothing below is required to finish this lesson.

• Before
  M7-T8: Euler Angles and Quaternions

  Compare Euler angles, axis-angle and quaternions, and explain gimbal lock.

• Lab
  M7-L1: Robot Coordinate Frame Explorer

  Chain frames from the world to a sensor on an arm, find the wrong-order and inverse bugs, and bring a measured point back to the world.