Where is the camera? Rigid motion on a curved space

Lesson 2 pinned a point with two rays, but only because it was told where the second camera is. Here that is the unknown: a rotation and a translation. The translation is a vector. The rotation is not: the average of two rotations 90° apart has determinant 0.50, a "camera" that halves volumes, and descent on the nine entries of a rotation matrix passes through shrunken, sheared and even mirrored frames on the way to its target. This lesson shows why (rotations are a curved three-dimensional surface in nine dimensions), shows that no three-number labelling of them is safe everywhere (Euler angles lock), and derives the tool that is: differentiate the constraint, find the flat space of small rotations, and map it back with the exponential. That moves a pose without leaving the space of poses. It does not say which way to move.

1 · The unknown is a pose, and the first instinct fails

Lesson 2 ended with a point cloud in the frame of the sensor that took it. A second sensor, or the same one a moment later, gives a cloud in its own frame. A world point at x_A in A's frame is at x_B = R x_A + t in B's: a 3×3 rotation matrix R and a vector t, twelve numbers that make a pose, the "where is the second camera" that lesson 2 was told and must now find. Poses compose, (R₁, t₁)∘(R₂, t₂) = (R₁R₂, R₁t₂ + t₁) (the right one acts first), and invert, (R, t)⁻¹ = (R^T, −R^Tt); G3.se3 in the shared geom3.js does both. This lesson leaves Flatland: in a plane a rotation is one angle on a circle, rotations commute, and the circle's single seam is handled by one wrap (below). The full difficulty exists only in 3D, so the code here is true 3D.

The translation is harmless, a vector in a flat space whose average is a midpoint. The trouble is R. When two estimates of a rotation disagree, the first thing anyone tries is the entrywise average M = (R₁ + R₂)/2. For R₁ = I and R₂ a rotation by θ about any axis:

A rotation has determinant 1 and M^TM = I: it keeps lengths and angles. At 90° the average has halved every volume and shortened lengths in the plane of the turn by 29%; at 180° it flattens that plane to a point. (In closed form det M = cos²(θ/2) and ‖M^TM − I‖ = √2 sin²(θ/2).) The same failure shows up in one dimension. In a plane a rotation is an angle, and the average of 359° and 1° is 180° by arithmetic when the right answer is 0°. The cure is to average differences, wrapped the short way round: 359° + ½·wrap(1° − 359°) = 359° + ½·(+2°) = 360° ≡ 0°. A circle has one seam and the wrap steps over it. In 3D there is no single seam to hide, and the rest of the lesson is the 3D version of "average differences, not values".

2 · Nine numbers, six constraints: a curved set

A rotation matrix has orthonormal columns, R^TR = I, and det R = +1. The first equation compares two symmetric 3×3 matrices, so it is six independent equations (three unit lengths, three right angles) on nine unknowns: 9 − 6 = 3 degrees of freedom, matching a rotation's axis (two numbers) and angle (one). The condition det R = +1 excludes the mirror images. So the rotations are a three-dimensional surface in the nine-dimensional space of matrices, the way a sphere is a two-dimensional surface in three, and the midpoint of two points of a sphere lies inside it. M is that midpoint.

A step fails the same way, more slowly. Correct a rotation by a small turn δ (a 3-vector: turn by ‖δ‖ about the axis δ, in the frame's own axes) by adding the first-order change: R ← R + R[δ]_× = R(I + [δ]_×), where [δ]_× is the skew-symmetric matrix with [δ]_×v = δ × v. Skewness gives (I + [δ]_×)^T(I + [δ]_×) = I + [δ]_×^T[δ]_×, so one step leaves ‖R^TR − I‖ = √2‖δ‖² and det R = 1 + ‖δ‖²: 0.0141 and 1.01 for ‖δ‖ = 0.1. Second order, and small, but it compounds: turning a frame through a full circle in 63 steps of 0.1 rad, as a naive integrator or optimiser would, leaves its axes 1.37 times too long and its determinant at 1.87. A computer that adds corrections to a rotation matrix soon estimates something that is not a camera.

3 · Coordinates for the set, and why none is safe everywhere

If the matrix is the problem, label the rotations by fewer numbers that obey no constraint, and let the optimiser add to those. A good labelling gives every rotation a label, makes a small change of label a small change of rotation everywhere, and uses exactly three numbers, so that the optimiser has neither redundant nor missing directions. Four are in common use:

Euler angles. Yaw ψ turns about the vertical z-axis, pitch θ about y, roll φ about x: R = R_z(ψ)R_y(θ)R_x(φ), and any triple is a rotation. Differentiating each factor shows how the knobs turn the frame: dψ about the world z-axis a₁ = (0, 0, 1), dθ about a₂ = (−sin ψ, cos ψ, 0), dφ about the frame's own x-axis a₃ = (cos ψ cos θ, sin ψ cos θ, −sin θ). So the angular velocity, in world axes, is ω = A·(dψ, dθ, dφ)/dt with A = [a₁ a₂ a₃], and |det A| = cos θ. At θ = ±90° we get a₃ = ∓a₁: yaw and roll turn the frame about the same axis, and the three knobs produce only a plane of angular velocities. The singular values of A are √(1 + sin θ), 1 and √(1 − sin θ), so turning the frame at 1 rad/s about its hardest direction takes a combined knob speed (the length of the vector of the three rates) of √(1 + sin θ)/cos θ rad/s:

That is gimbal lock. A different order of axes moves it to a different pose and does not remove it; a body whose x-axis points straight up or down is exactly there.

Rotation vector and quaternion. The vector ω = θn (axis n, angle θ) gives the rotation exp([ω]_×) of §4 and is regular near zero, but it folds over itself on the sphere ‖ω‖ = π and does not add: the quarter-turns about x and about y have vectors (π/2, 0, 0) and (0, π/2, 0) whose sum has length 127.3°, while doing one turn and then the other is a single rotation by 120.0°. The quaternion h = (cos θ/2, sin θ/2·n) composes by the Hamilton product (16 multiplications against 27 for a matrix product, no trigonometry) and has no lock. But h and −h are one rotation, and a step leaves the unit sphere as R + δR leaves the rotations: ‖h + δh‖² = 1 + ‖δh‖² for a step tangent to the sphere. A quaternion is a good way to store a rotation. It does not remove the curvature.

No labelling by three numbers is safe everywhere, and this is not bad luck. The rotations form a closed surface without an edge, like a sphere, and as no flat map of the Earth is faithful everywhere, no smooth one-to-one labelling of all rotations by three real numbers exists. So ask for less: not one labelling of the whole set, but a good one around the rotation we are at, rebuilt after every step. The rotation vector is perfectly regular near ω = 0, and ω = 0 is wherever we stand if we re-centre.

4 · Differentiate the constraint: the tangent space and the exponential

Take any smooth path of rotations R(t) and differentiate R^TR = I: (dR/dt)^TR + R^T(dR/dt) = 0. So Ω = R^TdR/dt satisfies Ω^T = −Ω: it is skew-symmetric, with zero diagonal and three free numbers, which we collect as Ω = [ω]_× = (0, −ω_z, ω_y; ω_z, 0, −ω_x; −ω_y, ω_x, 0). Hence

dR/dt = R [ω]_×,   ω ∈ ℝ³ free

At every rotation R the possible velocities are R[ω]_× for a free 3-vector: a flat three-dimensional space (9 − 6 = 3 again), the tangent space at R, with ω the angular velocity in the frame's own axes. Hold ω constant. The linear equation has the solution R(t) = R(0)·exp(t[ω]_×), where exp(A) = I + A + A²/2! + A³/3! + ⋯. Write ω = θn with n a unit vector. Since n × (n × (n × v)) = −n × v, [n]_׳ = −[n]_×; the odd powers of θ[n]_× are ±θ^2m+1[n]_×, the even ones ±θ^2m[n]_ײ, and summing both series gives Rodrigues' formula

exp([ω]_×) = I + sin θ [n]_× + (1 − cos θ) [n]_ײ

the rotation by angle θ about axis n, and a rotation exactly: exp(A)^T = exp(−A) is its inverse because A and −A commute, and det exp(A) = e^{tr A} = 1. The first-order step of §2 is the first two terms of this series; the remaining terms put it back on the surface. The inverse, the logarithm, follows from tr R = 1 + 2 cos θ and R − R^T = 2 sin θ [n]_×: for 0 < θ < π, θ = arccos((tr R − 1)/2) and ω = θn (at exactly π there are two answers, ±n). With exp and log we can do what the nine entries would not let us:

The mean is the repair of §1: for two rotations 90° apart it is the rotation by 45°, a rotation, 45° from each. The distance (G3.m3.dist) is an angle, not a matrix difference: the entrywise distance is the chord ‖R_a − R_b‖_F = 2√2 sin(α/2) for a turn α, which is 2.00 where the arc is 1.57 at 90°, and 2.83 where it is 3.14 at 180°. Order matters: R·exp([δ]_×) turns the frame about its own axes, exp([δ]_×)·R about the fixed world axes, and they differ because rotations do not commute (R_x(90°)R_y(90°) and R_y(90°)R_x(90°) are 120.0° apart). We use the first form from here on.

Check the map δ ↦ R·exp([δ]_×) against the three requirements of §3. Every rotation near R is R·exp([δ]_×) for some small δ, so every one has a label. At δ = 0 the derivative is R[·]_×, which loses no direction whatever R is. And it uses three numbers. The price is that it folds over at ‖δ‖ = π and is a good chart only near δ = 0, which is why we re-centre after every step.

5 · The same for a pose: SE(3)

A pose (R, t) is a point of a six-dimensional curved set, called SE(3), and moves the same way. Its velocity, seen from the pose, is a twist ξ = (ρ, ω): the velocity of the origin and the angular velocity, in the pose's own axes. As a 4×4 matrix T = (R, t; 0, 1) it obeys dT/dt = T ξ̂ with ξ̂ = ([ω]_×, ρ; 0, 0), and a constant twist carries it to T·exp(ξ̂). With K = [ω]_× the powers are ξ̂ⁿ = (Kⁿ, Kⁿ⁻¹ρ; 0, 0), so

exp(ξ̂) = ( exp(K), Vρ ; 0, 1 ),   V = I + K/2! + K²/3! + ⋯ = I + ((1 − cos θ)/θ²) K + ((θ − sin θ)/θ³) K²

The translation after the motion is Vρ, not ρ. A body moving forward at 1 m/s while turning at π/2 rad/s follows a quarter circle of radius 2/π, and after one second it is at (0.64, 0.64) m, not at (1, 0). Only without rotation (V = I) do translations add. The rest is as for rotations: the update is T ← T∘exp(ξ), the correction from T_a to T_b is log(T_a⁻¹T_b), and G3.se3.exp and G3.se3.log compute both.

6 · The rule, run against the alternatives

The rule: to improve a pose, differentiate the loss with respect to a 3-vector δ in the tangent space at δ = 0, step in that space, and apply the step with the exponential. The earlier sections ruled out the alternatives by argument; the widget runs them. A rigid body carries three markers, 1, 2 and 3 units out along its red, green and blue axes (a lopsided tripod, so that the problem is not symmetric). A target pose says where they should be, q_k = R*p_k, and the loss is the squared distance f(R) = ½ Σ_k ‖R p_k − q_k‖². One loss, one start, four ways to move:

Each step is a round number chosen to be stable for its method. For (iii) the loss curves upward by Σ‖δ × p_k‖² = δ^T(tr M·I − M)δ with M = Σp_kp_k^T = diag(1, 4, 9), that is by 13, 10 and 5 along the axes, so with step 0.1 the error components shrink by factors of 0.3, 0 and 0.5 per iteration (the slowest, 0.5, predicts about 20 iterations to 10⁻⁶). Method (i) sees curvatures 1, 4 and 9, and its best fixed step, 0.2, leaves a slowest factor of 0.8, about 62 iterations. Method (ii) sees curvatures up to 13(1 + sin θ) in the angle variables, so its step is 1/13 divided by 1 + |sin θ|, at most half the stability bound at the target. The step is not what makes Euler slow: a step 1.5 times larger finishes in 277 and 9618 iterations at pitches 60° and 85°, instead of 418 and 14395, and a step 2.5 times larger converges at none of the pitches 0°, 20°, 60°, 80° and 85°.

RM.gradTan = function (R, S) {
  var g = [0, 0, 0], Rt = m3.T(R);
  for (var k = 0; k < 3; k++) g = v3.add(g, v3.cross(m3.mulv(Rt, S.Q[k]), RM.P[k]));
  return g;
};
...
    if (method === 'exp') {
      Rn = m3.mul(R, m3.exp(v3.scale(RM.gradTan(R, S), -eta)));
    } else if (method === 'euler') {
      var G = RM.gradR(R, S), ax = RM.eulerAxes(ang[0], ang[1]), etak = eta / (1 + Math.abs(Math.sin(ang[1])));
      for (j = 0; j < 3; j++) { var dR = m3.mul(m3.skew(ax[j]), R), s = 0; for (i = 0; i < 9; i++) s += G[i] * dR[i]; ang[j] -= etak * s; }
      Rn = m3.eulerZYX(ang[0], ang[1], ang[2]);
    } else {
      var Gn = RM.gradR(R, S);
      Rn = R.map(function (v, q) { return v - eta * Gn[q]; });
      if (method === 'proj') Rn = m3.nearestRotation(Rn);
    }
...
m3.exp = function (w) {
  var th = v3.norm(w), K = m3.skew(w), K2 = m3.mul(K, K), a, b;
  if (th < 1e-8) { a = 1 - th * th / 6; b = 0.5 - th * th / 24; } else { a = Math.sin(th) / th; b = (1 - Math.cos(th)) / (th * th); }
  return m3.add(m3.add(m3.id(), m3.scale(K, a)), m3.scale(K2, b));
};

What to try. The page opens on (i), the nine entries, with exact markers and a start 100° from the target. With nine free numbers the loss pulls each marker straight toward its own target, so each path is a chord of the sphere the marker should stay on: red and green cut through the inside of theirs, and blue, whose step overshoots its target, zig-zags across it. The run needs 65 iterations, and on the way ‖RᵀR − I‖ reaches 2.77 and the determinant falls to 0.28; drag iterate shown to watch the frame shrink and shear. Raise the start to 170° and the determinant goes through zero to −0.21: the frame is turned inside out and recovers only because the target happens to be a rotation. Switch the markers to noisy. Now (i) settles on a matrix with ‖RᵀR − I‖ = 0.18 and determinant 1.06, 0.087 from the best rotation and never closer, with a loss of 0.0000 against 0.0048 for the best rotation: nine free numbers fit three noisy markers perfectly by not being a rotation. Methods (i′) and (iii) are rotations at every iterate and reach the best rotation in 20 and 22 iterations; for (iii) the count depends on how far the start is (18 at 10°, 27 at 170°) and on nothing else. Now (ii), Euler angles, with target pitch 0°: 28 iterations. Raise the pitch: 418 at 60°, 3778 at 80°, 14395 at 85°, and at 89° the run has not arrived after 20000 (it stops 0.010 short) while the dotted cos(pitch) curve sits near zero. The count grows like 1/cos²(pitch): iterations × cos²(pitch) is between 105 and 114 from 60° to 85°, because the weakest curvature in the angle variables lies between 5 and 13 times 1 − sin(pitch).

7 · What a step cannot tell us

A pose is now six free numbers, and the rule moves it exactly. The rule needs a step, and in the widget every optimiser was handed the answer: the loss was written with the target. Two scans come with no target. Take scan A and scan B of the same room corner and crate, 1200 points each, each in its own sensor frame (§1) and each a different random sample of the same surfaces. Apply the true pose to A and its points still do not sit on B's: the average distance from a point of A to the nearest point of B is 0.099 m, not zero, because the scans sample the surface in different places. Move A off the truth by a rigid motion of 15° and 27 cm and the average becomes 0.436 m. That number says how wrong the pose is. It does not say which of the six directions to move in: of 1000 random steps of 2° and 2 cm from the start, between 49.5% and 50.1% lower it (three seeds), and the average change is 0.25 mm against a standard error of 0.77 mm. A step with no direction is a coin flip.

Common mistakes / failure modes

Checkpoint exercise

Where this points next

A pose is now six free numbers whose update never leaves the set of poses: small corrections are 3-vectors in the tangent space, applied with the exponential, and the distance between two rotations is an angle. What this cannot do is choose the step. In the widget every optimiser knew the target; two scans come with none, and the pose that makes them coincide is the unknown itself. Even at the true pose a point of scan A is 0.099 m from its nearest neighbour in B on average, because no point of one lies on a point of the other; from a start 15° and 27 cm off it is 0.436 m; and that number gives no direction, since about half of the random steps lower it and half raise it. Lesson 4 stays in the plane, where a rotation is one angle and the exponential step is plain addition, so it meets this lesson in its simplest form; what it derives carries to 3-D (its closed-form solve through an SVD), and lesson 5 returns to full 3-D with the machinery above. How do we turn "the scans should coincide" into an equation we can solve for the pose?

Interview prompts

• Why can't you average rotation matrices, and what do you do instead? (§1, §4 — the entrywise average leaves the set (determinant 0.50 at 90°); average in the tangent space, R_a·exp(½ log(R_a^TR_b)).)
• How many degrees of freedom has a rotation, and where does the count come from? (§2 — R^TR = I is six independent equations on nine unknowns, so 9 − 6 = 3.)
• What is gimbal lock, as a statement about a Jacobian? (§3 — the matrix A from angle rates to angular velocity has |det A| = cos(pitch); at ±90° its rank is 2 and yaw and roll turn the frame about one axis.)
• Derive the tangent space of the rotations at R. (§4 — differentiate R^TR = I: R^TdR/dt is skew-symmetric, so dR/dt = R[ω]_× with ω free.)
• Where does Rodrigues' formula come from? (§4 — exp of θ[n]_× with [n]_׳ = −[n]_×; the series splits into sin θ and 1 − cos θ.)
• Why is the translation of exp(ξ) not equal to ρ? (§5 — the body turns while it moves, so its origin follows an arc: the translation is Vρ.)
• How do you update a rotation inside an optimiser, and why on the right? (§4, §6 — differentiate f(R·exp([δ]_×)) at δ = 0, step, and set R ← R·exp([δ]_×); the right factor turns about the frame's own axes.)
• Why is ‖R_a − R_b‖ not the angle between two rotations? (§4 — it is the chord 2√2 sin(α/2); the angle is ‖log(R_a^TR_b)‖.)

Companion reads: Computer Graphics · 02 Transforms and spaces (the matrices as a graphics tool; this lesson adds why they cannot be averaged or stepped), Computer Graphics · 13 Animation and motion (quaternions, slerp and gimbal lock from the animator's side), and Computer Vision · 04 Cameras and projection geometry (the extrinsics (R, t) that this lesson makes estimable).