Exercises: Lie Groups & Manifold Optimization

Chapter 09: Lie Groups, Lie Algebras, and Optimization on Manifolds

Self-assessment guide: These exercises cover the mathematical foundations of rotation, rigid-body motion, and optimization on non-Euclidean spaces — the backbone of modern robotics state estimation. Complete them in order; later exercises build directly on earlier results.

Difficulty key: ⭐ Foundational | ⭐⭐ Intermediate | ⭐⭐⭐ Advanced | ⭐⭐⭐⭐ Challenge

Section A — Group Theory Foundations

A1. ⭐ Verify whether $(\mathbb{R}^{2 \times 2}, \cdot)$ — all $2 \times 2$ real matrices under multiplication — forms a group. Check each axiom: 1. Closure 2. Associativity 3. Identity element 4. Inverse element

Which axiom fails? What minimal restriction on the set fixes it?

A2. ⭐ Consider the set of $2\times 2$ rotation matrices $\text{SO}(2)$. Every element has the form:

$$R(\theta) = \begin{pmatrix}\cos\theta & -\sin\theta \\ \sin\theta & \cos\theta\end{pmatrix}$$

1. Show that $R(\alpha) R(\beta) = R(\alpha + \beta)$.
2. What is the identity element?
3. What is $R(\theta)^{-1}$?
4. Name another group that $\text{SO}(2)$ is isomorphic to.

A3. ⭐⭐ Prove that $\text{O}(n) = \{Q \in \mathbb{R}^{n\times n} \mid Q^T Q = I\}$ is a subgroup of $\text{GL}(n, \mathbb{R})$.

Then prove that $\text{SO}(n) = \{Q \in \text{O}(n) \mid \det Q = +1\}$ is a subgroup of $\text{O}(n)$.

A4. ⭐⭐ Let $\mathfrak{so}(3) = \{\Omega \in \mathbb{R}^{3\times 3} \mid \Omega^T = -\Omega\}$ be the set of $3\times 3$ skew-symmetric matrices.

1. What is the dimension of $\mathfrak{so}(3)$? Exhibit a basis.
2. Define the "hat" map $(\cdot)^\wedge : \mathbb{R}^3 \to \mathfrak{so}(3)$. Show it is a linear isomorphism.
3. Show that $[\omega]_\times v = \omega \times v$ for any $\omega, v \in \mathbb{R}^3$.

A5. ⭐⭐⭐ The matrix groups $\text{GL}(n)$, $\text{O}(n)$, $\text{SO}(n)$, $\text{SL}(n)$ all have manifold structure.

1. $\text{GL}(n)$ is an open subset of $\mathbb{R}^{n \times n}$. What is its dimension?
2. What constraint defines $\text{O}(n)$? How many independent constraints? What is $\dim \text{O}(n)$?
3. What is $\dim \text{SO}(3)$? $\dim \text{SE}(3)$?
4. Why can't we parametrize $\text{SO}(3)$ globally with 3 parameters without singularities?

Section B — SO(2) and SO(3)

B1. ⭐ Compute the following compositions in $\text{SO}(3)$. Use the axis-angle interpretation.

1. $R_x(90°) R_y(90°)$ — rotation of 90° about $x$, then 90° about $y$.
2. $R_y(90°) R_x(90°)$ — same rotations in reverse order.
3. Are these equal? What does this say about $\text{SO}(3)$?

B2. ⭐⭐ Derive Rodrigues' rotation formula. Given axis $\hat{n}$ ($\|\hat{n}\| = 1$) and angle $\theta$, show that:

$$R(\hat{n}, \theta) = I + \sin\theta\,[\hat{n}]_\times + (1-\cos\theta)\,[\hat{n}]_\times^2$$

Hint: Decompose any vector $v$ into components parallel and perpendicular to $\hat{n}$.

B3. ⭐⭐ Unit quaternion $q = (w, x, y, z)$ with $\|q\| = 1$ represents a rotation. Given $q_1 = \frac{1}{\sqrt{2}}(1, 0, 0, 1)$ and $q_2 = \frac{1}{2}(1, 1, 1, 1)$:

1. What rotation does $q_1$ represent? (Axis and angle.)
2. Compute the quaternion product $q_1 \cdot q_2$.
3. Verify that $\|q_1 \cdot q_2\| = 1$.

B4. ⭐⭐⭐ Prove the double cover property: the map $\phi: S^3 \to \text{SO}(3)$ defined by $q \mapsto R(q)$ satisfies $\phi(q) = \phi(-q)$.

Then explain why this matters for interpolation in robotics. (OKS connection: OKS AMR heading representation.)

B5. ⭐⭐⭐ Implement SLERP (Spherical Linear Interpolation) between two quaternions. Given $q_0, q_1$ and $t \in [0,1]$:

$$\text{SLERP}(q_0, q_1, t) = q_0 \frac{\sin((1-t)\Omega)}{\sin\Omega} + q_1 \frac{\sin(t\Omega)}{\sin\Omega}$$

where $\cos\Omega = q_0 \cdot q_1$.

1. What happens when $\Omega \approx 0$? How should you handle it?
2. Write Python code that handles all edge cases.
3. Interpolate from $q_0 = (1,0,0,0)$ (identity) to $q_1 = (0,0,0,1)$ (180° about $z$) at $t = 0.5$. What rotation does the midpoint represent?

import numpy as np

def slerp(q0, q1, t):
    """Spherical linear interpolation between unit quaternions."""
    q0 = np.asarray(q0, dtype=float)
    q1 = np.asarray(q1, dtype=float)

    dot = np.dot(q0, q1)

    # Handle double cover: ensure shortest path
    if dot < 0:
        q1 = -q1
        dot = -dot

    dot = np.clip(dot, -1.0, 1.0)

    # For nearly identical quaternions, use linear interpolation
    if dot > 0.9995:
        result = q0 + t * (q1 - q0)
        return result / np.linalg.norm(result)

    omega = np.arccos(dot)
    sin_omega = np.sin(omega)

    s0 = np.sin((1 - t) * omega) / sin_omega
    s1 = np.sin(t * omega) / sin_omega

    return s0 * q0 + s1 * q1

Section C — Exponential and Logarithmic Maps

C1. ⭐ Compute $\exp(\theta\, e_3^\wedge)$ where $e_3 = (0,0,1)^T$, i.e., find $\exp\begin{pmatrix}0&-\theta&0\\\theta&0&0\\0&0&0\end{pmatrix}$ by direct series expansion.

C2. ⭐⭐ The closed-form exponential map for $\text{SO}(3)$ is:

$$\exp(\omega^\wedge) = I + \frac{\sin\theta}{\theta}\omega^\wedge + \frac{1-\cos\theta}{\theta^2}(\omega^\wedge)^2$$

where $\theta = \|\omega\|$. Show that this is equivalent to Rodrigues' formula (Exercise B2).

C3. ⭐⭐⭐ Derive the logarithmic map for $\text{SO}(3)$: given $R \in \text{SO}(3)$, find $\omega$ such that $\exp(\omega^\wedge) = R$.

1. Show that $\theta = \arccos\frac{\text{tr}(R) - 1}{2}$.
2. Show that $[\hat{n}]_\times = \frac{R - R^T}{2\sin\theta}$ when $\theta \neq 0, \pi$.
3. What special handling is needed for $\theta = 0$ and $\theta = \pi$?

C4. ⭐⭐⭐ The Baker-Campbell-Hausdorff (BCH) formula gives the composition in the Lie algebra:

$$\log(\exp(A)\exp(B)) = A + B + \frac{1}{2}[A,B] + \frac{1}{12}([A,[A,B]] - [B,[A,B]]) + \cdots$$

where $[A,B] = AB - BA$ is the Lie bracket.

1. For $\mathfrak{so}(3)$, show that $[\omega_1^\wedge, \omega_2^\wedge] = (\omega_1 \times \omega_2)^\wedge$.
2. Why does the first-order approximation $\log(\exp(A)\exp(B)) \approx A + B$ fail for large rotations?
3. (OKS connection) On an AMR fusing wheel odometry rotation $\delta\theta_{\text{odom}}$ with gyroscope $\delta\theta_{\text{gyro}}$ over a timestep, when is the first-order BCH acceptable?

C5. ⭐⭐⭐⭐ Write a numerically stable Python function that computes both $\exp: \mathfrak{so}(3) \to \text{SO}(3)$ and $\log: \text{SO}(3) \to \mathfrak{so}(3)$, handling all edge cases ($\theta \approx 0$, $\theta \approx \pi$). Verify round-trip accuracy on 10,000 random rotations.

import numpy as np

def hat(omega):
    """R^3 -> so(3): vector to skew-symmetric matrix."""
    return np.array([
        [0, -omega[2], omega[1]],
        [omega[2], 0, -omega[0]],
        [-omega[1], omega[0], 0]
    ])

def vee(Omega):
    """so(3) -> R^3: skew-symmetric matrix to vector."""
    return np.array([Omega[2,1], Omega[0,2], Omega[1,0]])

def so3_exp(omega):
    """Exponential map: R^3 -> SO(3)."""
    theta = np.linalg.norm(omega)
    if theta < 1e-10:
        # First-order Taylor: exp(Ω) ≈ I + Ω
        return np.eye(3) + hat(omega)

    K = hat(omega) / theta  # unit skew matrix
    return (np.eye(3) 
            + np.sin(theta) * K 
            + (1 - np.cos(theta)) * K @ K)

def so3_log(R):
    """Logarithmic map: SO(3) -> R^3."""
    cos_theta = np.clip((np.trace(R) - 1) / 2, -1.0, 1.0)
    theta = np.arccos(cos_theta)

    if theta < 1e-10:
        # Near identity: log(R) ≈ R - I (skew part)
        return vee(0.5 * (R - R.T))

    if np.abs(theta - np.pi) < 1e-6:
        # Near 180°: extract axis from R + I
        M = R + np.eye(3)
        # Pick column with largest norm
        col_norms = np.linalg.norm(M, axis=0)
        k = np.argmax(col_norms)
        n_hat = M[:, k] / col_norms[k]
        return theta * n_hat

    # General case
    skew = (R - R.T) / (2 * np.sin(theta))
    return theta * vee(skew)

# Verification
errors = []
for _ in range(10000):
    omega = np.random.randn(3) * np.pi * np.random.rand()
    R = so3_exp(omega)
    omega_recovered = so3_log(R)
    R_recovered = so3_exp(omega_recovered)
    errors.append(np.linalg.norm(R - R_recovered, 'fro'))

print(f"Max round-trip error: {max(errors):.2e}")  # Should be < 1e-12
print(f"Mean round-trip error: {np.mean(errors):.2e}")

Section D — SE(2)/SE(3) and Jacobians

D1. ⭐ The group $\text{SE}(2)$ represents rigid-body motion in the plane. Its elements are:

$$T = \begin{pmatrix} R & t \\ 0 & 1 \end{pmatrix} \in \mathbb{R}^{3\times 3}, \quad R \in \text{SO}(2),\; t \in \mathbb{R}^2$$

1. Compute $T_1 T_2$ for $T_1 = (R_1, t_1)$ and $T_2 = (R_2, t_2)$.
2. What is $T^{-1}$?
3. (OKS connection) An OKS AMR at pose $(x_1, y_1, \theta_1)$ observes a landmark at relative position $(\ell_x, \ell_y)$ in body frame. What is the landmark's world-frame position?

D2. ⭐⭐ The Lie algebra $\mathfrak{se}(3)$ represents twists (infinitesimal rigid motions). A twist $\xi = (\omega, v)^T \in \mathbb{R}^6$ maps to:

$$\xi^\wedge = \begin{pmatrix}\omega^\wedge & v \\ 0 & 0\end{pmatrix} \in \mathbb{R}^{4\times 4}$$

1. What is the dimension of $\mathfrak{se}(3)$?
2. Write out $\xi^\wedge$ explicitly for $\xi = (0, 0, \omega_z, v_x, v_y, 0)^T$. What kind of motion does this represent?
3. (OKS connection) What twist corresponds to the OKS AMR driving straight forward at speed $v$ while turning at rate $\omega$?

D3. ⭐⭐⭐ The left Jacobian of $\text{SO}(3)$ appears when differentiating the exponential map:

$$J_l(\omega) = \frac{\partial}{\partial \delta}\exp((\omega + \delta)^\wedge)\bigg|_{\delta=0}$$

Its closed form is:

$$J_l(\omega) = I + \frac{1-\cos\theta}{\theta^2}\omega^\wedge + \frac{\theta - \sin\theta}{\theta^3}(\omega^\wedge)^2$$

1. Verify that $J_l(0) = I$.
2. Show that the right Jacobian satisfies $J_r(\omega) = J_l(-\omega)$.
3. Why do these Jacobians matter for optimization on $\text{SO}(3)$?

D4. ⭐⭐⭐ The adjoint representation of $\text{SE}(3)$ is the $6\times 6$ matrix:

$$\text{Ad}_T = \begin{pmatrix}R & 0 \\ [t]_\times R & R\end{pmatrix}$$

for $T = (R, t) \in \text{SE}(3)$.

1. Show that changing the perturbation side satisfies $T \exp(\xi^\wedge) = \exp((\text{Ad}_T \xi)^\wedge) T$.
2. What is $\text{Ad}_T$ for a pure translation $T = (I, t)$?
3. What is $\text{Ad}_T$ for a pure rotation $T = (R, 0)$?

D5. ⭐⭐⭐⭐ Derive the perturbation model for a point observation in $\text{SE}(3)$. A camera at pose $T = (R, t)$ observes a 3D point $p$. The observation function is:

$$h(T) = R^T(p - t)$$

Compute $\frac{\partial h}{\partial \delta\xi}$ using the left perturbation model $T' = \exp(\delta\xi^\wedge) T$.

Section E — Manifold Optimization

E1. ⭐ In Euclidean optimization, we update $x \gets x + \Delta x$. Explain why this fails for $R \in \text{SO}(3)$, and describe the retraction-based update:

$$R \gets R \cdot \exp(\delta\phi^\wedge)$$

Why does this stay on the manifold?

E2. ⭐⭐ Formulate a simple rotation averaging problem. Given $N$ noisy rotation measurements $\tilde{R}_i \approx R_{\text{true}}$, find:

$$R^* = \arg\min_{R \in \text{SO}(3)} \sum_{i=1}^N \|\log(R^{-1}\tilde{R}_i)\|^2$$

1. Write the first-order optimality condition.
2. Derive the Gauss-Newton update step using the right perturbation model.
3. What is the geometric interpretation of this cost function?

E3. ⭐⭐⭐ (OKS connection) Solve a small 2D pose-graph SLAM problem on $\text{SE}(2)$. Given:

• 3 robot poses: $T_0, T_1, T_2 \in \text{SE}(2)$
• Odometry constraints: $T_0^{-1}T_1 \approx \Delta_{01}$, $T_1^{-1}T_2 \approx \Delta_{12}$
• Loop closure: $T_0^{-1}T_2 \approx \Delta_{02}$
• $T_0$ is fixed (anchor/prior)

Set up the cost function and Jacobian structure. Describe (but don't fully solve) one Gauss-Newton iteration.

E4. ⭐⭐⭐⭐ Compare the convergence behavior of manifold Gauss-Newton vs. naive Euler-angle parametrization near gimbal lock. Consider estimating a rotation near $R_{\text{true}} = R_y(89°)$:

1. In Euler angle $(Z\text{-}Y\text{-}X)$ parametrization, what happens to the Jacobian near $\beta = 90°$?
2. How does the Lie algebra perturbation model avoid this issue?
3. Design a numerical experiment to demonstrate the difference. Describe the setup and expected results.

# Setup: estimate R from 100 noisy rotation measurements near R_y(89°)
# Method A: Gauss-Newton in Euler angles (α, β, γ)
# Method B: Gauss-Newton with SO(3) exponential map perturbation
# 
# For each method:
# - Initialize at R_y(88°)
# - Run 50 iterations
# - Track cost and step size per iteration
#
# Expected results:
# - Method A: cost oscillates or diverges after ~5 iterations
#   (Hessian condition number > 10^10 near β=89°)
# - Method B: converges in 3-5 iterations to correct solution
#   (Hessian condition number ~ 10^1)
#
# Vary β from 0° to 89.99° and plot condition number of H
# to show gimbal lock is a parametrization artifact.

E5. ⭐⭐⭐⭐ Prove that the Gauss-Newton method on $\text{SO}(3)$ with the exponential map retraction has local quadratic convergence under standard assumptions (small residuals, full-rank Jacobian).

Hint: Show that the retraction $R \cdot \exp(\delta\phi^\wedge)$ is a second-order approximation to the geodesic, and apply the standard Gauss-Newton convergence theory in the tangent space.

Section F — Applications and Integration

F1. ⭐⭐ (OKS connection) The OKS AMR navigation estimator fuses wheel encoders (providing $\Delta\text{SE}(2)$ increments) with an IMU gyroscope (providing angular rate $\omega_z$). The discrete-time state update is:

$$T_{k+1} = T_k \cdot \exp(\xi_k^\wedge \cdot \Delta t)$$

where $\xi_k = (0, 0, \omega_z, v_x, 0, 0)^T$.

1. Expand $\exp(\xi_k^\wedge \Delta t)$ for the $\text{SE}(2)$ case.
2. What is the exact closed-form solution for the circular arc (constant $v_x, \omega_z$)?
3. Show that the straight-line case ($\omega_z = 0$) falls out as a limit.

F2. ⭐⭐⭐ Hand-eye calibration finds the unknown rigid transform $X \in \text{SE}(3)$ satisfying $AX = XB$ where $A, B$ are known motions. Given $n$ pairs $(A_i, B_i)$:

1. Show that the rotation part decouples: $R_A R_X = R_X R_B$.
2. Using axis-angle representations $\alpha_i = \log(R_{A_i})^\vee$ and $\beta_i = \log(R_{B_i})^\vee$, show that the rotation satisfies:

$$R_X(\beta_i - \alpha_i) = -[\alpha_i + \beta_i]_\times \cdot ?$$

Hmm, let's use a cleaner approach. Show that for each pair, $(\alpha_i + \beta_i)$ must lie in a known relationship determined by $R_X$.

3. How many motion pairs are needed for a unique solution?

F3. ⭐⭐⭐ Design a trajectory interpolation scheme on $\text{SE}(3)$. Given keyframe poses $T_0, T_1, \ldots, T_N$ at times $t_0 < t_1 < \cdots < t_N$:

1. Define SLERP interpolation between two poses $T_a, T_b$.
2. Extend to cubic B-spline interpolation in the Lie algebra.
3. Why is Lie algebra interpolation better than interpolating Euler angles + position separately?

F4. ⭐⭐⭐⭐ IMU preintegration on $\text{SO}(3)$. An IMU provides angular velocity $\omega_k$ at each timestep $\Delta t$. The preintegrated rotation between keyframes $i$ and $j$ is:

$$\Delta R_{ij} = \prod_{k=i}^{j-1} \exp((\omega_k - b_g)\Delta t \cdot \mathbf{1}^\wedge)$$

where $b_g$ is the gyroscope bias.

1. Why preintegrate instead of integrating directly into the world frame?
2. Derive the first-order bias correction: if the bias estimate changes from $\bar{b}_g$ to $\bar{b}_g + \delta b_g$, show that:

$$\Delta R_{ij}(\bar{b}_g + \delta b_g) \approx \Delta R_{ij}(\bar{b}_g) \cdot \exp\left(\frac{\partial \Delta R_{ij}}{\partial b_g}\delta b_g\right)$$

3. What is the Jacobian $\frac{\partial \Delta R_{ij}}{\partial b_g}$?

F5. ⭐⭐⭐⭐ (OKS connection) Design a mini visual-inertial odometry (VIO) pipeline for the OKS AMR. The robot has: - A forward-facing camera (10 Hz) - An IMU (200 Hz) - Wheel encoders (50 Hz)

Describe the state vector, the Lie group structure of each component, the residual types, and the factor graph. Your answer should address:

1. What is the state at each keyframe?
2. What factors connect consecutive keyframes?
3. How do wheel encoder and IMU constraints differ in their Lie group formulation?
4. What is the computational complexity of one optimization step?

Summary

Difficulty distribution: ⭐ ×6 (A1, A2, B1, C1, D1, E1) | ⭐⭐ ×8 (A3, A4, B2, B3, C2, D2, E2, F1) | ⭐⭐⭐ ×10 (A5, B4, B5, C3, C4, D3, D4, E3, F2, F3) | ⭐⭐⭐⭐ ×6 (C5, D5, E4, E5, F4, F5)

OKS robotics exercises: B4 (heading double cover), D1 (body-to-world transform), E3 (pose-graph SLAM), F1 (odometry integration), F5 (VIO pipeline design).