Phase VI — Lie Groups & Manifold Optimization | Week 13 | 2.5 hours Standard Gauss-Newton breaks on manifolds — learn the retraction trick that makes it work.
The OKS navigation estimator refines robot poses on SE(2)/SE(3) — a manifold, not a vector space. Naive $R + \Delta R$ leaves SO(3), producing invalid rotations. Manifold Gauss-Newton uses Exp/Log maps to keep every iterate on the manifold, which is exactly how pose refinement works in the robot's localization stack.
In Euclidean space, Gauss-Newton updates via $x_{k+1} = x_k + \delta$. On SO(3), this breaks:
$$R + \Delta R \notin \text{SO}(3)$$
Adding a matrix to a rotation matrix destroys orthogonality. The fundamental issue: manifolds are not vector spaces, so additive updates are meaningless.
Key insight: instead of updating in the ambient space, we linearize in the tangent space and map back via the exponential map.
Define the retraction (or box-plus) operator:
$$x \oplus \delta = x \cdot \text{Exp}(\delta)$$
where $\delta \in \mathbb{R}^n$ is a tangent-space perturbation and $\text{Exp}: \mathbb{R}^n \to \mathcal{M}$ is the exponential map. For SO(3), $\delta \in \mathbb{R}^3$ and $\text{Exp}$ is Rodrigues' formula.
The inverse operation (box-minus):
$$x_1 \ominus x_2 = \text{Log}(x_2^{-1} \cdot x_1)$$
Given a cost $f(x) = \frac{1}{2}\|r(x)\|^2$ where $x \in \mathcal{M}$, we perturb around the current estimate $x_k$:
$$r(x_k \oplus \delta) \approx r(x_k) + J_k \delta$$
where $J_k = \frac{\partial r(x_k \oplus \delta)}{\partial \delta}\big|_{\delta=0}$ is the Jacobian with respect to the tangent-space perturbation.
The normal equations become:
$$J_k^T J_k \, \delta^* = -J_k^T r(x_k)$$
The complete update:
$$\delta^* = -(J_k^T J_k)^{-1} J_k^T r_k, \qquad x_{k+1} = x_k \cdot \text{Exp}(\delta^*)$$
Equivalently with the pseudoinverse: $\delta^* = -J_k^\dagger r_k$.
Convergence: under standard regularity conditions, manifold GN inherits quadratic convergence near the optimum, just like its Euclidean counterpart.
"""Manifold Gauss-Newton on SE(2)."""
import numpy as np
# --- SE(2) helpers ---
def se2_exp(xi):
"""Exp map: R^3 -> SE(2). xi = [dx, dy, dtheta]."""
dx, dy, dth = xi
c, s = np.cos(dth), np.sin(dth)
if abs(dth) < 1e-10:
return np.array([[1, 0, dx], [0, 1, dy], [0, 0, 1]])
V = np.array([[s / dth, -(1 - c) / dth],
[(1 - c) / dth, s / dth]])
t = V @ np.array([dx, dy])
return np.array([[c, -s, t[0]], [s, c, t[1]], [0, 0, 1]])
def se2_log(T):
"""Log map: SE(2) -> R^3."""
th = np.arctan2(T[1, 0], T[0, 0])
c, s = np.cos(th), np.sin(th)
if abs(th) < 1e-10:
return np.array([T[0, 2], T[1, 2], th])
V_inv = np.array([[s / th, (1 - c) / th],
[-(1 - c) / th, s / th]]) / (s**2 / th + (1 - c)**2 / th) * th
# Simpler: invert V directly
half_th = th / 2
V_inv = (half_th / np.tan(half_th)) * np.eye(2) + np.array([[0, half_th], [-half_th, 0]])
rho = V_inv @ T[:2, 2]
return np.array([rho[0], rho[1], th])
def se2_inv(T):
R = T[:2, :2]
t = T[:2, 2]
T_inv = np.eye(3)
T_inv[:2, :2] = R.T
T_inv[:2, 2] = -R.T @ t
return T_inv
# --- Manifold Gauss-Newton for circle-of-poses fitting ---
def make_circle_poses(n, radius, noise_std=0.05):
"""Generate n poses arranged on a circle with noise."""
poses = []
for i in range(n):
angle = 2 * np.pi * i / n
x = radius * np.cos(angle) + np.random.randn() * noise_std
y = radius * np.sin(angle) + np.random.randn() * noise_std
th = angle + np.pi / 2 + np.random.randn() * noise_std
c, s = np.cos(th), np.sin(th)
T = np.array([[c, -s, x], [s, c, y], [0, 0, 1]])
poses.append(T)
return poses
def residual_and_jacobian(T_est, T_meas):
"""Residual: Log(T_meas^{-1} T_est), Jacobian w.r.t. tangent perturbation."""
err_T = se2_inv(T_meas) @ T_est
r = se2_log(err_T)
# Jacobian of Log(T_meas^{-1} (T_est Exp(delta))) at delta=0
# Approximate as identity (right Jacobian ≈ I for small residuals)
J = np.eye(3)
return r, J
def manifold_gauss_newton(poses_init, poses_meas, max_iter=20, tol=1e-8):
"""Refine poses_init to match poses_meas using manifold GN."""
poses = [T.copy() for T in poses_init]
n = len(poses)
costs = []
for it in range(max_iter):
total_r, total_J = [], []
for i in range(n):
r_i, J_i = residual_and_jacobian(poses[i], poses_meas[i])
row = np.zeros((3, 3 * n))
row[:, 3*i:3*i+3] = J_i
total_r.append(r_i)
total_J.append(row)
r = np.concatenate(total_r)
J = np.vstack(total_J)
cost = 0.5 * r @ r
costs.append(cost)
H = J.T @ J + 1e-6 * np.eye(3 * n)
delta = np.linalg.solve(H, -J.T @ r)
if np.linalg.norm(delta) < tol:
break
for i in range(n):
poses[i] = poses[i] @ se2_exp(delta[3*i:3*i+3])
return poses, costs
# --- Demo ---
np.random.seed(42)
n_poses, radius = 8, 2.0
true_poses = make_circle_poses(n_poses, radius, noise_std=0.0)
meas_poses = make_circle_poses(n_poses, radius, noise_std=0.1)
init_poses = make_circle_poses(n_poses, radius, noise_std=0.3)
refined, costs = manifold_gauss_newton(init_poses, meas_poses)
print(f"Iterations: {len(costs)}, Final cost: {costs[-1]:.6f}")
print(f"Cost reduction: {costs[0]:.4f} -> {costs[-1]:.6f}")
Starting from $r(x_k \oplus \delta) \approx r_k + J_k \delta$, derive the normal equations and show that the update $x_{k+1} = x_k \cdot \text{Exp}(\delta^*)$ stays on the manifold.
Given $N$ noisy rotations $\{R_i\}$, find the Fréchet mean $R^* = \arg\min_R \sum_i \|\text{Log}(R^{-1} R_i)\|^2$ using manifold GN.
def rotation_average(rotations, max_iter=50):
R = rotations[0].copy()
for _ in range(max_iter):
deltas = [so3_log(R.T @ Ri) for Ri in rotations]
mean_delta = np.mean(deltas, axis=0)
if np.linalg.norm(mean_delta) < 1e-10:
break
R = R @ so3_exp(mean_delta)
return R
Run manifold GN on the circle-of-poses problem with increasing noise levels (σ = 0.01, 0.1, 0.5, 1.0). Plot iterations-to-convergence vs noise. At what noise level does convergence fail?
Implement manifold GN with left perturbation ($x_{k+1} = \text{Exp}(\delta) \cdot x_k$) and compare convergence with the right perturbation version. When does the choice matter?
Add a damping parameter $\lambda$ to get $(J^TJ + \lambda I)\delta = -J^Tr$. Implement adaptive $\lambda$ (increase on cost increase, decrease on decrease).
Compute the Jacobian $\frac{\partial r(x \oplus \delta)}{\partial \delta}$ numerically using central differences on the manifold and compare with the analytical Jacobian.
def numerical_jacobian(x, r_func, eps=1e-7):
n = 3 # tangent space dim
J = np.zeros((3, n))
for i in range(n):
ei = np.zeros(n); ei[i] = eps
r_plus = r_func(x @ se2_exp(ei))
r_minus = r_func(x @ se2_exp(-ei))
J[:, i] = (r_plus - r_minus) / (2 * eps)
return J