Lie Groups: SO(3), SE(3) & the Exponential Map

Chapter 1: Groups & Lie Groups

Before we can talk about the exponential map, we need to understand what kind of mathematical object SO(3) is. The answer: it is a Lie group. A Lie group is both a group (in the algebraic sense) and a smooth manifold (in the geometric sense). Both parts matter.

Group axioms

A group (G, ⊗) is a set G with a binary operation ⊗ satisfying four axioms:

SO(3) satisfies all four with matrix multiplication as ⊗. Note what is absent: commutativity. R₁R₂ ≠ R₂R₁ in general — SO(3) is a non-abelian group. Rotations in 3D do not commute, a fact that will return to haunt us in Chapter 7.

Proof that SO(3) is non-abelian — with numbers

Let R₁ = Rz(90°) and R₂ = Rx(90°). Compute both orders:

R_zR_x ≠ R_xR_z — the "final orientation" depends on which rotation came first. This is not a numerical artifact. It reflects the fundamental topology of SO(3).

SE(3) is also a group under matrix multiplication. Its identity element is the 4×4 identity. Its inverse is [[R^T, −R^Tt],[0, 1]], as derived in L1.

Manifold structure

A smooth manifold is a space that looks locally flat — every neighborhood of every point is homeomorphic to ℝⁿ for some n. SO(3) is a 3-dimensional manifold embedded in ℝ⁹ (the space of 3×3 matrices): locally, near any rotation, the set of valid rotations looks like a 3D flat space. SE(3) is a 6-dimensional manifold (3 for rotation, 3 for translation).

A Lie group is a smooth manifold that is also a group, with smooth group operations. SO(3), SE(3), O(d), and the general linear group GL(d,ℝ) are all matrix Lie groups. The key benefit: they admit a tangent space at every point, giving us the machinery to do calculus — and optimization — on them.

Chapter 2: The Lie Algebra so(3)

Every matrix Lie group G has an associated Lie algebra g — the tangent space at the identity element. Concretely: imagine a smooth curve through the identity in SO(3). The derivative of that curve at the identity is a matrix in the tangent space. The Lie algebra is the set of all such derivatives.

For SO(3), what matrices can appear as derivatives of rotation-matrix curves at the identity? A curve R(t) with R(0) = I and R(t) ∈ SO(3) for all t satisfies R(t)^TR(t) = I. Differentiating at t=0:

So the tangent vectors at the identity must be skew-symmetric matrices — matrices satisfying A^T = −A. The Lie algebra of SO(3) is:

A 3×3 skew-symmetric matrix has exactly 3 free parameters (the upper triangle determines the rest via the antisymmetry). So so(3) is a 3-dimensional vector space — matching the 3 degrees of freedom of rotation in 3D.

The general form

Any element of so(3) can be written:

where φ = [φ₁, φ₂, φ₃]^T ∈ ℝ³. The 3-vector φ uniquely specifies the skew matrix and vice versa. This bijection between ℝ³ and so(3) is formalized as the hat map (Chapter 3).

Similarly, the Lie algebra of SE(3) is se(3) — 4×4 matrices of the form [Φ, ρ; 0, 0] where Φ ∈ so(3) and ρ ∈ ℝ³. The 6-vector ξ = [φ, ρ]^T is called a twist.

Worked dimension count

SO(3) is a 3×3 matrix group but has only 3 degrees of freedom. Where do the other 6 entries go? The constraint R^TR = I imposes 6 independent equations (the 9 entries of I, but the matrix is symmetric so only 6 are independent). So: 9 free parameters minus 6 constraints = 3 DOF. ✓

SE(3): a 4×4 matrix with the bottom row fixed to [0,0,0,1] leaves 12 free entries. The R block imposes 6 constraints. The t block is free (3 entries). Net: 3 + 3 = 6 DOF. ✓

The Lie algebra dimension equals the manifold dimension — by construction. This is why so(3) is 3-dimensional (exactly 3 free parameters in a skew-symmetric matrix) and se(3) is 6-dimensional.

Chapter 3: Hat Map, Vee Map & Angular Velocity

The bijection between ℝ³ and so(3) is so useful it gets two dedicated operators. The hat map (·)^ turns a 3-vector into its corresponding skew-symmetric matrix. The vee map (·)∨ does the reverse.

And the inverse: given the skew matrix on the right, (·)∨ extracts the vector [φ₁, φ₂, φ₃]^T.

Worked numbers — hat and vee

Let φ = [1, 2, 3]^T. Then:

Verify: transpose gives [[0,−3,2],[3,0,−1],[−2,1,0]]^T = [[0,3,−2],[−3,0,1],[2,−1,0]] = −φ̂. ✓ Skew-symmetric.

Vee: given [[0,−3,2],[3,0,−1],[−2,1,0]], extract upper triangle entries: (1,3) = 2 → φ₂, (1,2) = −3 → φ₃, (2,3) = −1 → φ₁. So (·)∨ = [1, 2, 3]^T. ✓

Angular velocity interpretation

The hat map is not just notation. If a rigid body has angular velocity ω = [ω₁, ω₂, ω₃]^T (radians/second), then the time derivative of its rotation matrix is:

This is the kinematic equation of rotation. The skew matrix ω̂ acts as the "infinitesimal generator" of rotation — it tells you which direction in ℝ^3×3 the rotation is moving. The hat map converts the physical quantity (angular velocity vector) into the algebraic object (element of so(3)) that drives the kinematics.

Key algebraic properties of the hat map

These identities come up constantly in derivations and implementations:

The last row says the Lie bracket of so(3) corresponds to the cross product of ℝ³ — this is why so(3) and (ℝ³, ×) are isomorphic as Lie algebras. It also gives a concrete formula for the BCH commutator in so(3): [φ̂, ψ̂] = (φ × ψ)̂.

python
import numpy as np

# Verify key hat-map identities with concrete numbers
phi = np.array([1, 2, 3])
psi = np.array([4, -1, 2])
v   = np.array([1, 0, 0])

# Identity 1: φ̂v = φ×v
print(hat(phi) @ v)            # skew-matrix-times-v
print(np.cross(phi, v))         # cross product
# Both: [ 0.  3. -2.] ✓

# Identity 4: û̂² = uuᵀ − I
u = phi / np.linalg.norm(phi)
u_hat = hat(u)
print(np.round(u_hat @ u_hat, 4))       # û̂²
print(np.round(np.outer(u,u) - np.eye(3), 4))  # uuᵀ−I
# Identical ✓

# Identity 7: [φ̂,ψ̂] = (φ×ψ)̂
lie_bracket = hat(phi) @ hat(psi) - hat(psi) @ hat(phi)
cross_hat   = hat(np.cross(phi, psi))
print(np.allclose(lie_bracket, cross_hat))  # True ✓

Chapter 4: Rodrigues' Formula — Derived from the Power Series

The matrix exponential of a square matrix A is defined by the power series:

For a general matrix, this series does not simplify. But for skew-symmetric matrices in so(3), it collapses to a beautiful closed form — Rodrigues' formula. The key is a remarkable property of the hat map.

Step 1: the cubing identity

Let φ ∈ ℝ³ with ‖φ‖ = θ (the rotation angle). Define the unit axis û = φ/θ. Then φ̂ = θû̂. Compute û̂³:

Now û̂u = û × u = 0 (cross product of a vector with itself is zero), so uu^Tû̂ = u(û^Tû̂) = 0. Therefore:

This is the critical identity. It means the powers of û̂ cycle: û̂, û̂², −û̂, −û̂², +û̂, ... Substituting φ̂ = θû̂:

Step 2: split the series

Expand exp(φ̂) and separate odd and even powers:

The first series in parentheses is exactly sin(θ). The second is exactly 1 − cos(θ). Therefore:

Substituting back û = φ/θ and û̂ = φ̂/θ:

This is Rodrigues' formula — the closed-form matrix exponential for so(3) elements. The exponential map exp: so(3) → SO(3) is exactly Rodrigues' formula. What Lecture 1 presented as an axiom, we have now derived from the power series.

Worked numbers — 90° about z

Let φ = [0, 0, π/2]^T. Then θ = π/2, û = [0,0,1]^T.

Compare with L1's Rz(90°) = [[0,−1,0],[1,0,0],[0,0,1]]. ✓ Exact match. The exponential map and the geometric rotation formula are the same thing.

python
import numpy as np

def hat(phi):
    """3-vector → 3×3 skew-symmetric matrix."""
    p1, p2, p3 = phi
    return np.array([[   0, -p3,  p2],
                     [ p3,   0, -p1],
                     [-p2,  p1,   0]])

def vee(Phi):
    """3×3 skew-symmetric matrix → 3-vector."""
    return np.array([Phi[2,1], Phi[0,2], Phi[1,0]])

def rodrigues(phi):
    """Rodrigues' formula: exp map so(3) → SO(3)."""
    theta = np.linalg.norm(phi)
    if theta < 1e-10:
        return np.eye(3) + hat(phi)  # first-order approx
    u_hat = hat(phi / theta)
    return np.eye(3) + np.sin(theta)*u_hat + (1-np.cos(theta))*u_hat@u_hat

# Test: 90° about z-axis
phi = np.array([0, 0, np.pi/2])
R = rodrigues(phi)
print(np.round(R, 4))
# [[ 0. -1.  0.]
#  [ 1.  0.  0.]
#  [ 0.  0.  1.]]

# Verify it's a rotation: R^T R = I, det = 1
print(np.allclose(R.T @ R, np.eye(3)))  # True
print(np.round(np.linalg.det(R), 4))    # 1.0

Chapter 5: The Logarithm Map

The exponential map goes from the Lie algebra to the Lie group: exp: so(3) → SO(3). The inverse is the logarithm map log: SO(3) → so(3). Given a rotation matrix R, the log recovers the tangent vector φ = θû that, when exponentiated, produces R.

Extracting the rotation angle

From Rodrigues' formula R = I + sin(θ)û̂ + (1−cos(θ))û̂², take the trace:

Where we used tr(û̂) = 0 (diagonal is all zeros) and tr(û̂²) = tr(uu^T − I) = û·û − 3 = 1 − 3 = −2.

Extracting the rotation axis

From Rodrigues: R − R^T = 2sin(θ)û̂. So:

The vee of this gives φ = θû. Special cases: if R = I then θ = 0 and log(I) = 0. Near θ = π the formula is numerically unstable and a different branch formula is needed (beyond this lesson).

Worked numbers

Let R = Rz(90°) = [[0,−1,0],[1,0,0],[0,0,1]]. Recover φ:

We recover exactly the input φ = [0,0,π/2]^T. ✓ Round-trip complete.

The θ = π singularity

The formula log(R) = θ/(2sin(θ)) · (R − R^T) breaks when θ = π because sin(π) = 0. At θ = π, we have R − R^T = 0 (verify: sin(π) = 0, so 2sin(θ)û̂ = 0). But R ≠ I! The issue is that at θ = π, the rotation axis û is not recoverable from R − R^T alone.

Instead, at θ = π: R + R^T = 2(cos(π)I + (1−cos(π))uu^T) = 2(−I + 2uu^T). So R + I = 2uu^T (the columns of R + I give 2u), and the axis can be recovered from any nonzero column of R + I. In practice, choose the column with the largest norm for numerical stability.

Additionally, at θ = π there is a 2-fold ambiguity: the rotations exp(πû̂) = exp(−πû̂) = exp(π(−û)̂) are the same (π rotation about û = −π rotation about −û). This is an inherent topological property of SO(3) — its fundamental group π₁(SO(3)) = ℤ₂, which is why quaternions (the double cover) are often preferred near θ = π.

python
import numpy as np

def log_so3(R):
    """Log map SO(3) → so(3). Returns skew matrix φ̂."""
    cos_theta = (np.trace(R) - 1) / 2
    cos_theta = np.clip(cos_theta, -1, 1)  # numerical safety
    theta = np.arccos(cos_theta)
    if theta < 1e-10:
        return np.zeros((3,3))
    return theta / (2 * np.sin(theta)) * (R - R.T)

# Round-trip test
phi_in = np.array([0, 0, np.pi/2])
R = rodrigues(phi_in)
Phi_hat = log_so3(R)
phi_out = vee(Phi_hat)
print(np.round(phi_out, 4))  # [0. 0. 1.5708] = [0, 0, π/2] ✓

# Note: past θ=π, log returns the "short" path (|θ| ≤ π)
phi_big = np.array([0, 0, 5])  # > π
R2 = rodrigues(phi_big)
phi_recovered = vee(log_so3(R2))
print(np.round(phi_recovered, 4))  # wrapped: ≠ [0,0,5]

Chapter 6: SE(3): Twists & the exp Map

Rigid-body poses T ∈ SE(3) form a Lie group too. The Lie algebra se(3) consists of 4×4 matrices of the form:

where φ̂ ∈ so(3) is the rotational part and ρ ∈ ℝ³ is the translational part. The 6-vector ξ = [φ; ρ]^T ∈ ℝ⁶ is called a twist — it encodes both angular velocity (φ) and linear velocity (ρ) simultaneously.

The SE(3) exponential map

Applying the matrix exponential to ξ̂ ∈ se(3) gives:

The rotation block is exactly Rodrigues applied to φ. The translation block is J_l(φ)ρ, not just ρ — the rotation "bends" the translation. J_l(φ) is the left Jacobian (also called the left-invariant Jacobian) of SO(3):

When θ → 0, J_l(φ) → I, and the SE(3) exp map reduces to T = [I, ρ; 0, 1] — a pure translation. This makes sense: a small twist with no rotation just translates.

When φ ≠ 0, J_l couples the rotation and translation. Physically: if you rotate while translating, the effective displacement in world coordinates depends on the rotation. The Jacobian accounts for this coupling.

Worked numbers: SE(3) exp with rotation + translation

Let ξ = [φ; ρ] = [[0,0,π/2]; [1,0,0]]^T — a 90° z-rotation combined with a 1 m x-translation in the body twist. Compute the left Jacobian:

The translation in the resulting T is J_l[1,0,0]^T ≠ [1,0,0]^T — the rotation bends the translation. Physically: if you drive 1 m forward while rotating 90°, the effective endpoint is a curved arc, not a straight line. The Jacobian computes the chord of that arc exactly.

python
import numpy as np
from scipy.linalg import expm  # reference: matrix exponential

def hat_se3(xi):
    """6-vector [phi; rho] → 4×4 se(3) matrix."""
    phi, rho = xi[:3], xi[3:]
    Xi = np.zeros((4,4))
    Xi[:3,:3] = hat(phi)
    Xi[:3,3]  = rho
    return Xi

def left_jacobian(phi):
    """Left Jacobian J_l(phi) for SO(3)."""
    theta = np.linalg.norm(phi)
    if theta < 1e-10:
        return np.eye(3)
    ph = hat(phi)
    return (np.eye(3) + (1-np.cos(theta))/theta**2 * ph
            + (theta-np.sin(theta))/theta**3 * ph@ph)

def exp_se3(xi):
    """SE(3) exp map: 6-vector → 4×4 pose matrix."""
    phi, rho = xi[:3], xi[3:]
    R = rodrigues(phi)
    t = left_jacobian(phi) @ rho
    T = np.eye(4)
    T[:3,:3] = R
    T[:3,3]  = t
    return T

# Test: pure z-rotation + translation
xi = np.array([0, 0, np.pi/2, 1, 0, 0])  # 90° z, 1m x-translation
T = exp_se3(xi)
print(np.round(T, 4))
# R block = Rz(90°); t block = J_l([0,0,π/2])·[1,0,0] ≠ [1,0,0]
print(np.allclose(expm(hat_se3(xi)), T))  # True — matches scipy reference

Chapter 7: BCH Formula & Adjoint (Brief)

Two more structural facts about Lie groups that come up constantly in robotics: the Baker-Campbell-Hausdorff formula and the Adjoint.

Baker-Campbell-Hausdorff (BCH)

In ordinary calculus, e^ae^b = e^a+b. For matrix exponentials of non-commuting matrices A, B:

where [A,B] = AB − BA is the Lie bracket. For so(3), the Lie bracket is [φ̂₁, φ̂₂] = φ̂₁φ̂₂ − φ̂₂φ̂₁, which corresponds to the cross product: [φ₁]^[φ₂]^ = (φ₁ × φ₂)^.

The practical consequence: composing two rotations exp(A)exp(B) is not exp(A+B). The error depends on the commutator [A,B]. When A and B are small (near the identity), the BCH correction is second-order in the magnitudes, so for small perturbations it is often ignored. This is the justification for the first-order approximation used in iterative solvers.

Adjoint

The Adjoint of a group element g ∈ G is a linear map Ad_g: g → g that transforms Lie algebra elements between frames. For SO(3):

This says: if you have a tangent vector φ expressed in the body frame, multiplying by R converts it to the world frame. For SE(3), the 6×6 Adjoint matrix is:

The Adjoint is used extensively in kinematics (converting velocities between frames) and in the derivation of Jacobians for pose estimation. You will see it again in the optimization lectures.

Why BCH matters for SLAM

In a factor-graph optimizer (like g2o or GTSAM), you often need to combine two pose updates: a "motion model" update and a "measurement" update. Each is expressed as an exp of a tangent vector. Naively concatenating: T_new = T · exp(δ₁) · exp(δ₂). If you want to express this as a single exp: T · exp(BCH(δ₁, δ₂)), you need BCH. In practice, optimizers sidestep this by iterating with small steps where BCH ≈ addition. But knowing BCH exists — and when it matters — prevents subtle bugs in large-angle scenarios.

Numerical verification of BCH

Let A = [0,0,0.5]̂ (0.5 rad about z) and B = [0,0.4,0]̂ (0.4 rad about y). Then:

For these angles (~28° each), the BCH correction has Frobenius norm ≈ 0.1. For 5° each, it would be ≈ 0.004 — small but nonzero. At 1°, it is ~10⁻⁴. This is why linearized solvers work: they iterate with steps of a few degrees at most, making BCH negligible.

python
import numpy as np
from scipy.linalg import expm

# BCH verification: exp(A)exp(B) ≠ exp(A+B) for non-small angles
A_vec = np.array([0, 0, 0.5])  # 0.5 rad about z
B_vec = np.array([0, 0.4, 0])  # 0.4 rad about y
A = hat(A_vec); B = hat(B_vec)

R_AB  = expm(A) @ expm(B)      # exp(A)·exp(B)
R_sum = expm(A + B)             # exp(A+B) — first-order approx

diff = np.linalg.norm(R_AB - R_sum, 'fro')
print(f"‖exp(A)exp(B) − exp(A+B)‖_F = {diff:.4f}")  # ≈ 0.033

# BCH first-order correction: ½[A,B] = ½(AB−BA)
commutator = 0.5 * (A @ B - B @ A)
R_bch1 = expm(A + B + commutator)
diff2 = np.linalg.norm(R_AB - R_bch1, 'fro')
print(f"‖exp(A)exp(B) − exp(A+B+½[A,B])‖_F = {diff2:.4f}")  # ≈ 0.001 — much smaller

Chapter 8: Showcase: Retraction Optimizer

This is the payoff. We simulate a simple rotation optimization: given a "target" rotation R_goal, we want to find it starting from R_init = I. A gradient descent step in SO(3) uses the retraction:

where δ = log(R^T R_goal)∨ is the tangent error (how far R is from R_goal, expressed in the tangent space at R) and α is the step size. Compare this with a naive additive step R ← R − α·δ̃ that ignores the manifold — the naive step immediately leaves SO(3).

The canvas shows both paths. The circular arc (left) represents SO(2) rotations (the 2D analog of SO(3)) as angles on the unit circle. The retraction step (teal) stays on the circle. The additive step (red) drifts off it.

The full algorithm spelled out

Here is the retraction-based rotation optimizer in pseudocode, annotated with every Lie-theoretic step:

The convergence rate is linear for constant α (gradient descent). For Gauss-Newton (used in SLAM), α is replaced by the Levenberg-Marquardt damping — much faster. But the retraction structure is identical. The exp map replaces the "add" operation at every single step.

Why does exp(α δ̂) keep us on SO(3)? Because exp maps so(3) → SO(3) by the Rodrigues formula, and SO(3) is closed under composition: R · exp(α δ̂) ∈ SO(3) whenever R ∈ SO(3) and exp(α δ̂) ∈ SO(3).

python
import numpy as np

def retract_step(R, R_goal, alpha):
    """One retraction step toward R_goal. Stays in SO(3)."""
    delta_hat = log_so3(R.T @ R_goal)     # tangent error at R
    delta = vee(delta_hat)                 # error as 3-vector
    R_new = R @ rodrigues(alpha * delta)   # retraction: R ← R·exp(α·δ̂)
    return R_new

def additive_step(R, R_goal, alpha):
    """Naive additive step — BREAKS SO(3) structure."""
    delta = R_goal - R
    R_new = R + alpha * delta             # NOT a rotation matrix!
    return R_new

# Simulate: optimize from identity toward Rz(120°)
R = np.eye(3)
R_goal = rodrigues(np.array([0, 0, 2*np.pi/3]))

for i in range(10):
    R = retract_step(R, R_goal, alpha=0.3)
    det = np.linalg.det(R)
    err = np.linalg.norm(vee(log_so3(R.T @ R_goal)))
    print(f"Step {i+1}: det={det:.6f}, error={err:.4f} rad")
# det stays ≈1.0 throughout; error decreases to 0

Chapter 9: Connections & Cheat Sheet

The complete Lie theory cheat sheet

The "rotations don't add" demo — verified

We claimed R₁ + R₂ is not a rotation. Here are the numbers. Let R₁ = Rz(45°), R₂ = Rz(45°). Then:

Now try R₁ · R₂ = Rz(45°)·Rz(45°) = Rz(90°). Orthonormal ✓, det=1 ✓. Composition via multiplication preserves SO(3); addition does not.

Distances on SO(3): three flavors

The log map also gives us a principled notion of "how far apart" two rotations are — something impossible to define without Lie theory.

The angular distance is the "right" geodesic distance — the length of the shortest path along the SO(3) manifold. The chordal distance is the length of the chord cutting through the ambient ℝ⁹ space. They relate by: dist_c = 2√2·|sin(θ/2)|, where θ = dist_θ. For small angles (θ ≪ 1): dist_c ≈ √2 · dist_θ. They agree to first order, but diverge for large rotations.

In SLAM, the angular distance appears in error terms for rotation optimization (because it is geodesic and intrinsic to SO(3)). The chordal distance appears in initialization and closed-form solvers (because it is algebraically simpler to minimize). Knowing which one your optimizer uses matters: minimizing chordal distance does NOT minimize angular distance except at the optimum.

python
import numpy as np

def dist_angular(Ra, Rb):
    """Geodesic (angular) distance between two rotations."""
    err = Ra.T @ Rb         # relative rotation
    phi = vee(log_so3(err)) # tangent error
    return np.linalg.norm(phi)

def dist_chordal(Ra, Rb):
    """Chordal distance (Frobenius norm of difference)."""
    return np.linalg.norm(Ra - Rb, 'fro')

Ra = rodrigues(np.array([0, 0, np.pi/4]))  # Rz(45°)
Rb = rodrigues(np.array([0, 0, np.pi/2]))  # Rz(90°)
print(f"Angular dist: {dist_angular(Ra,Rb):.4f} rad ({dist_angular(Ra,Rb)*180/np.pi:.1f}°)")
# 0.7854 rad (45.0°)
print(f"Chordal dist: {dist_chordal(Ra,Rb):.4f}")
# ≈ 0.7654 (≈ √2·sin(22.5°)·2 — slightly different)

What's next

Related lessons

• VNAV L1 — 3D Geometry: Rotations, Rigid Transforms & Frames (prerequisite)
• Kalman Filter — uses SE(3) retraction in the update step for VIO
• Extended Kalman Filter (EKF) — linearizes about a manifold point using the Jacobian
• Classical SLAM — pose-graph optimization directly uses the retraction

Putting it all together: a full retraction step with numbers

Let's trace a complete optimization step. Current R = I (identity). Goal: R_goal = Rz(60°).

Notice: at every step, R is a valid rotation matrix (determinant = 1, columns orthonormal). The retraction guarantees this automatically. The naive additive step R ← R + 0.5·δ̂ would give R + a small skew matrix — not in SO(3), but close enough to corrupt subsequent computations.

This geometric convergence (error halves each step for α = 0.5) is exactly what you see in the canvas above. Run 3-4 steps to verify.