Math 2 · Rotations & Lie groups

Why this chapter exists. Robot states are poses, and poses contain rotations. The set of rotations is not a vector space: you can't add two rotation matrices and get a rotation, and you can't add a small "step" to a quaternion and stay unit length. Yet optimizers (GTSAM) need to take small steps and differentiate, and planners (OMPL) need to measure distances, interpolate, and sample. Lie theory is the toolkit that makes "calculus on rotations" work. In GTSAM it is not optional background: traits, retract, localCoordinates, AdjointMap, OptionalJacobian, and the conventions of every factor come straight from this chapter.

1. Frames and the meaning of a rotation matrix

Attach a coordinate frame to the world (\(w\)) and one to the robot body (\(b\)). A point has coordinates \(p^b\) in the body frame and \(p^w\) in the world frame. The rotation \({}^{w}R_{b}\) converts between them:

\[ p^w = {}^{w}R_{b}\;p^b . \]

The columns of \({}^{w}R_{b}\) are the body's x, y, z axes expressed in world coordinates. Frames compose like fractions cancelling: \({}^{w}R_{c} = {}^{w}R_{b}\,{}^{b}R_{c}\), and \({}^{b}R_{w} = ({}^{w}R_{b})^{-1}\).

GTSAM writes this as wRb / wTb (rotation/pose of body in world) and is consistent about it: wTb.transformFrom(p_b) returns \(p^w\), and wTb.transformTo(p_w) returns \(p^b\). When reading any GTSAM code, find out which frames each pose maps between. Half of all bugs in ported SLAM code are frame mix-ups.

2. SO(2) and SO(3)

SO(2): planar rotations \(R(\theta) = \begin{bmatrix}\cos\theta & -\sin\theta\\ \sin\theta&\cos\theta\end{bmatrix}\). Composition adds angles: \(R(\alpha)R(\beta) = R(\alpha+\beta)\). The set of all of them is a circle: \(\theta\) and \(\theta+2\pi\) are the same rotation. That wrap-around is the first sign that we are on a curved space, not a line. GTSAM's Rot2 stores \((\cos\theta,\sin\theta)\) to avoid ever normalizing angles; OMPL's SO2StateSpace stores \(\theta\in[-\pi,\pi)\) and wraps.

SO(3): the 3×3 matrices with

\[ R^\top R = I \quad(\text{orthogonal: preserves lengths and angles}),\qquad \det R = +1 \quad(\text{no reflection}). \]

There are 9 entries and 6 independent constraints (\(R^\top R=I\) is symmetric, so 6 equations), which leaves 3 degrees of freedom. SO(3) is a smooth, curved, 3-dimensional surface sitting inside \(\R^9\), which is what "3-dimensional manifold" means. Facts you'll use:

\(R^{-1}=R^\top\).
Composition is matrix multiplication, which is not commutative: rotating 90° about x then z differs from z then x. Try it with a book.
Every rotation is a rotation by some angle \(\theta\in[0,\pi]\) about some unit axis \(u\) (Euler's rotation theorem), and \(\tr R = 1+2\cos\theta\) (proved in §6).

3. Parameterizations, and why each library picked what it did

Representation	Numbers	Pros	Cons	Used by
Rotation matrix	9	composition = matmul; acting on points is a matmul; no singularities	6 redundant numbers; drifts off SO(3) with roundoff (needs re-orthonormalization)	GTSAM `Rot3` default
Unit quaternion	4	compact; cheap to normalize; great for interpolation (slerp) and uniform sampling	double cover (\(q\) and \(-q\) are the same rotation); 1 redundant number	GTSAM with `GTSAM_USE_QUATERNIONS`; OMPL `SO3StateSpace` (stored x,y,z,w)
Rotation vector (axis·angle) \(\omega=\theta u\)	3	minimal; is the tangent space / Lie algebra coordinates	not unique beyond \(\lVert\omega\rVert=\pi\); composition is messy	tangent vectors everywhere in GTSAM (`Vector3`)
Euler angles (roll, pitch, yaw)	3	human-readable	gimbal lock; 12 conventions; discontinuities	input/output only (`Rot3::Ypr`, `RPY`)

Gimbal lock, concretely. With the ZYX convention \(R = R_z(\psi)R_y(\vartheta)R_x(\phi)\) at pitch \(\vartheta=\pi/2\), a short multiplication shows \(R_y(\tfrac\pi2)R_x(\phi) = R_z(-\phi)R_y(\tfrac\pi2)\), so

\[ R = R_z(\psi - \phi)\,R_y(\tfrac\pi2). \]

Only \(\psi-\phi\) matters: yaw and roll have become the same rotation, and one degree of freedom has vanished. The Jacobian from \((\phi,\vartheta,\psi)\) to rotations becomes singular there. Any optimizer or planner working in Euler angles misbehaves near it, which is why neither library uses Euler angles internally.

3.1 Quaternions in enough depth to port OMPL and GTSAM

A quaternion is \(q = w + x\,\mathbf i + y\,\mathbf j + z\,\mathbf k\) with \(\mathbf i^2=\mathbf j^2=\mathbf k^2=\mathbf{ijk}=-1\). Write it as \((w, \mathbf v)\), a scalar part and a vector part. The Hamilton product is

\[ (w_1,\mathbf v_1)(w_2,\mathbf v_2) = \big(w_1w_2 - \mathbf v_1\cdot\mathbf v_2,\;\; w_1\mathbf v_2 + w_2\mathbf v_1 + \mathbf v_1\times\mathbf v_2\big). \]

The cross product makes it non-commutative, just like rotations. A unit quaternion \(\lVert q\rVert=1\) encoding rotation by \(\theta\) about unit axis \(u\) is

\[ q = \big(\cos\tfrac\theta2,\; u\sin\tfrac\theta2\big), \]

and it rotates a vector \(p\) (written as the pure quaternion \((0,p)\)) by \(p' = q\,p\,q^{*}\), where \(q^*=(w,-\mathbf v)\) is the conjugate (= inverse for unit \(q\)). Expanding gives the matrix

\[ R(q) = \begin{bmatrix} 1-2(y^2+z^2) & 2(xy - wz) & 2(xz+wy)\\ 2(xy+wz) & 1-2(x^2+z^2) & 2(yz - wx)\\ 2(xz - wy) & 2(yz+wx) & 1-2(x^2+y^2) \end{bmatrix}. \]

Double cover. Every entry of \(R(q)\) is quadratic in \(q\), so \(R(-q)=R(q)\). The unit quaternions form the 3-sphere \(S^3\subset\R^4\), and SO(3) is \(S^3\) with antipodal points glued together. Consequences:

Comparing quaternions requires \(\lvert q_1\cdot q_2\rvert\), not \(q_1\cdot q_2\).
Interpolation must first flip one quaternion if \(q_1\cdot q_2<0\) to take the short way (§17).
For two rotations differing by angle \(\Delta\theta\), \(\lvert q_1\cdot q_2\rvert = \cos(\Delta\theta/2)\). Remember the half angle; it matters for OMPL's metric.

Storage-order trap

Eigen's Quaternion constructor takes (w, x, y, z) but stores [x, y, z, w] in .coeffs(). OMPL's state struct is {x, y, z, w}. GTSAM's Rot3::quaternion() returns a Vector4 ordered (w, x, y, z). ROS messages are x, y, z, w. Write the order down whenever you convert.

4. Groups and manifolds, minimally

A group is a set with an associative composition, an identity, and inverses. SO(3) under matrix multiplication is a group. The unit quaternions are a group under the Hamilton product. \(\R^n\) is a group under addition.

A (smooth) manifold of dimension \(d\) is a space that, near every point, looks like a patch of \(\R^d\) that can be bent smoothly. A circle is a 1-manifold, a sphere a 2-manifold, and SO(3) a 3-manifold.

A Lie group is both, with smooth composition and inverse. Its big advantage over a general manifold is that the neighbourhood of any element looks like the neighbourhood of the identity, carried over by multiplication. So you only need to understand one tangent space, the one at the identity, and you can move it anywhere. That one tangent space is the Lie algebra.

5. The tangent space at the identity: deriving so(3)

Take a smooth curve of rotations \(R(t)\) with \(R(0)=I\). Differentiate the constraint \(R(t)^\top R(t)=I\):

\[ \dot R^\top R + R^\top\dot R = 0 \quad\xrightarrow{t=0}\quad \dot R(0)^\top + \dot R(0) = 0 . \]

So the velocity at the identity is a skew-symmetric matrix. The skew-symmetric 3×3 matrices form a 3-dimensional vector space, \(\mathfrak{so}(3) = \{\skew{\omega}:\omega\in\R^3\}\) (chapter 1 §12). The hat map \(\omega\mapsto\skew{\omega}\) and its inverse vee identify it with \(\R^3\).

More generally, at an arbitrary point, \(R^\top\dot R\) is skew-symmetric, so every rotation trajectory satisfies

\[ \dot R = R\,\skew{\omega_b}\qquad\text{(body angular velocity)}\qquad\text{or equivalently}\qquad \dot R = \skew{\omega_w}R\qquad\text{(world angular velocity)}, \]

with \(\omega_w = R\,\omega_b\). Gyroscopes measure \(\omega_b\). This ODE is the bridge from "a vector of angular rates" to "a rotation".

6. The exponential map: deriving Rodrigues' formula

Question. If the body spins at a constant rate \(\omega\) (rad/s, body frame) for 1 second, starting at \(I\), where does it end up?

Solve \(\dot R = R\skew{\omega}\) with \(R(0)=I\). As for scalar \(\dot y = ay\), the solution is the matrix exponential:

\[ R(t) = \exp(t\skew{\omega}),\qquad \exp(K) = \sum_{k\ge0}\frac{K^k}{k!}. \]

Define \(\Exp(\omega) := \exp(\skew{\omega})\), the rotation reached after one second, i.e. rotation by angle \(\theta=\lVert\omega\rVert\) about axis \(\omega/\theta\).

Collapsing the series. Let \(K=\skew{\omega}\) and \(\theta = \lVert\omega\rVert\). From chapter 1 §12, \(K^3 = -\theta^2K\). So all powers reduce to \(K\) or \(K^2\): \(K^{2k+1} = (-\theta^2)^kK\) and \(K^{2k+2} = (-\theta^2)^kK^2\). Group the series:

\[ \exp(K) = I + \underbrace{\Big(\sum_{k\ge0}\frac{(-1)^k\theta^{2k}}{(2k+1)!}\Big)}_{\sin\theta/\theta}K + \underbrace{\Big(\sum_{k\ge0}\frac{(-1)^k\theta^{2k}}{(2k+2)!}\Big)}_{(1-\cos\theta)/\theta^2}K^2 . \]

\[ \boxed{\Exp(\omega) = I + \frac{\sin\theta}{\theta}\skew{\omega} + \frac{1-\cos\theta}{\theta^2}\skew{\omega}^2}\qquad\text{(Rodrigues' formula)} \]

Numerics: what the code actually does. Both coefficients are \(0/0\) at \(\theta=0\). Near zero, use their Taylor series:

\[ A = \frac{\sin\theta}{\theta} \approx 1 - \frac{\theta^2}{6},\qquad B = \frac{1-\cos\theta}{\theta^2}\approx \frac12 - \frac{\theta^2}{24}. \]

Away from zero, compute \(1-\cos\theta\) as \(2\sin^2(\theta/2)\), because \(1-\cos\theta\) suffers catastrophic cancellation for small \(\theta\). Open gtsam/geometry/SO3.cpp, ExpmapFunctor::init: these are exactly the lines there (A = sin_theta / theta; B = one_minus_cos / theta2; with one_minus_cos = 2.0 * s2 * s2, and the nearZero branch A = 1.0 - theta2 * one_6th; B = 0.5 - theta2 * one_24th;). A port that skips the branch returns NaN at the identity, which is the most common state in an optimizer.

Trace identity. \(\tr K = 0\) and \(\tr K^2 = -2\theta^2\), so \(\tr\Exp(\omega) = 3 - 2(1-\cos\theta) = 1+2\cos\theta\).

7. The logarithm map

Invert Rodrigues: given \(R\), find \(\omega\) with \(\Exp(\omega)=R\) and \(\theta\in[0,\pi]\).

Angle from the trace: \(\theta = \arccos\big(\tfrac{\tr R - 1}{2}\big)\).
Axis from the skew part: the \(K^2\) term is symmetric, so \(R - R^\top = 2\frac{\sin\theta}{\theta}\skew{\omega}\), giving

\[ \skew{\omega} = \frac{\theta}{2\sin\theta}(R - R^\top). \]

Two singular cases need special handling, and both are in SO3::Logmap:

\(\theta\to0\): \(\frac{\theta}{2\sin\theta}\to\frac12\). Use the Taylor expansion \(\frac{\theta}{2\sin\theta}\approx\frac12+\frac{\theta^2}{12}\).
\(\theta\to\pi\): \(\sin\theta\to0\) and the skew part vanishes, so the formula loses the axis. Use the symmetric part instead. At \(\theta=\pi\), Rodrigues gives \(R = I + \frac{2}{\pi^2}(\omega\omega^\top - \pi^2I) = 2uu^\top - I\), so \(uu^\top = (R+I)/2\). Take the largest-norm column of \(R+I\), normalize it to get \(u\), and set \(\omega = \pi u\). The sign of \(u\) is arbitrary, because \(\pm\pi u\) give the same rotation. GTSAM computes this case with extra care to keep Jacobians smooth near \(\pi\) (nearPi in DexpFunctor).

\(\Exp\) is onto but not one-to-one. Rotation vectors \(\omega\) and \(\omega(1-2\pi/\theta)\) give the same rotation, so \(\Log\) chooses the representative with \(\theta\le\pi\). Optimizers step in the tangent space with small \(\delta\), far from the cut at \(\pi\). That's why everything works locally even though nothing works globally.

8. Perturbations, ⊕ and ⊖: how optimizers and planners "add" to a rotation

To move a little away from \(R\), multiply by a small rotation. There are two choices:

\[ \text{right (body/local):}\quad R\oplus\delta = R\,\Exp(\delta),\qquad\qquad \text{left (world/global):}\quad \Exp(\delta)\,R . \]

Right perturbation means "rotate by \(\delta\) about the body's own axes". Left means "about the world axes". Both are valid, but a codebase must pick one, because every Jacobian depends on it. GTSAM uses right perturbations everywhere:

\[ \mathtt{x.retract}(\delta) = x\,\Exp(\delta),\qquad \mathtt{x.localCoordinates}(y) = \Log(x^{-1}y) =: y\ominus x . \]

These are inverses: \(x\oplus(y\ominus x) = y\), and \((x\oplus\delta)\ominus x = \delta\). This is precisely check_manifold_invariants in gtsam/base/Manifold.h. The traits<T>::Retract/Local machinery and Values::retract(VectorValues) are this section in code.

(Other libraries such as Sophus and many VIO papers use left perturbations or other conventions. A Jacobian copied from a paper must be converted with the adjoint, §9.)

9. The adjoint: moving tangent vectors between frames

Question. A perturbation \(\delta\) applied on the right of \(R\): what is the equivalent perturbation on the left?

\[ R\,\Exp(\delta) = \Exp(\delta')\,R \;\Longrightarrow\; \Exp(\delta') = R\,\Exp(\delta)\,R^\top = \exp(R\skew{\delta}R^\top) = \exp(\skew{R\delta}), \]

using \(\exp(MKM^{-1}) = M\exp(K)M^{-1}\) (expand the series) and \(R\skew{\delta}R^\top = \skew{R\delta}\) (chapter 1 §12). So \(\delta' = R\delta\). In general, for any Lie group,

\[ \boxed{g\,\Exp(\delta)\,g^{-1} = \Exp(\Ad_g\,\delta)}\qquad\text{and}\qquad \boxed{g\,\Exp(\delta) = \Exp(\Ad_g\delta)\,g}. \]

For SO(3), \(\Ad_R = R\). For SE(3) and SE(2) see §11–12. In GTSAM every Lie type implements AdjointMap(), and Lie.h derives all compose/inverse/between Jacobians from it (§13).

10. SE(3): poses

A rigid transform is a rotation plus a translation. As a 4×4 homogeneous matrix:

\[ T = \begin{bmatrix}R & t\\0&1\end{bmatrix},\qquad T\begin{bmatrix}p\\1\end{bmatrix} = \begin{bmatrix}Rp+t\\1\end{bmatrix},\qquad T_1T_2 = \begin{bmatrix}R_1R_2 & R_1t_2+t_1\\0&1\end{bmatrix},\qquad T^{-1} = \begin{bmatrix}R^\top & -R^\top t\\0&1\end{bmatrix}. \]

Lie algebra. A twist \(\xi = (\omega, v)\in\R^6\). GTSAM orders it rotation first: \(\xi = [\omega;\,v]\). Its hat is

\[ \xi^\wedge = \begin{bmatrix}\skew{\omega} & v\\0&0\end{bmatrix}. \]

Exponential. Compute \(\exp(\xi^\wedge)\) by the series. Powers of \(\xi^\wedge\) have top-left block \(\skew{\omega}^k\) and top-right \(\skew{\omega}^{k-1}v\), which sums to

\[ \Exp(\xi) = \begin{bmatrix}\Exp(\omega) & J_l(\omega)\,v\\0&1\end{bmatrix},\qquad J_l(\omega) = I + \frac{1-\cos\theta}{\theta^2}\skew{\omega} + \frac{\theta-\sin\theta}{\theta^3}\skew{\omega}^2 . \]

\(J_l\) is the left Jacobian of SO(3) (it reappears in §13). This is what Pose3::Expmap computes: the code comment literally says "The translation is t = J_l(w)v" (gtsam/geometry/Pose3.cpp). Physically, \(\Exp(\xi)\) is a screw motion*: rotating about an axis while sliding along it, which is the motion produced by constant body twist for one second. Translation and rotation are coupled; \(t\neq v\) unless \(\omega=0\).

Retraction choice. With GTSAM_POSE3_EXPMAP=ON (default), retract uses this exact exponential. With it off, GTSAM uses a cheaper decoupled retraction, \(T\oplus(\omega,v) = (R\,\text{Retract}(\omega),\; t + Rv)\). Both are valid retractions (they agree to first order), but they give different iterates. Pin one in a port.

11. The SE(3) adjoint, derived

Compute \(T\xi^\wedge T^{-1}\):

\[ \begin{bmatrix}R&t\\0&1\end{bmatrix}\begin{bmatrix}\skew{\omega}&v\\0&0\end{bmatrix}\begin{bmatrix}R^\top&-R^\top t\\0&1\end{bmatrix} = \begin{bmatrix}R\skew{\omega}R^\top & -R\skew{\omega}R^\top t + Rv\\0&0\end{bmatrix} = \begin{bmatrix}\skew{R\omega} & \skew{t}R\omega + Rv\\0&0\end{bmatrix}, \]

where \(-\skew{R\omega}t = t\times R\omega = \skew{t}R\omega\). Reading off \((\omega', v')\):

\[ \boxed{\Ad_T = \begin{bmatrix}R & 0\\ \skew{t}R & R\end{bmatrix}}\qquad\text{(GTSAM order }[\omega;v]). \]

This matches ExtendedPose3::AdjointMap() in gtsam/geometry/ExtendedPose3-inl.h (adj.block(0,0)=R; adj.block(3,0)=skew(t)*R; adj.block(3,3)=R). Libraries that order \([v;\omega]\) have the transposed block layout \(\begin{bmatrix}R&\skew{t}R\\0&R\end{bmatrix}\). Check this whenever you copy formulas.

12. SE(2): planar poses

\(T = (R(\theta), t)\) with tangent \(\xi = (v_x, v_y, \omega)\). GTSAM's Pose2 orders it translation first, unlike Pose3. The exponential:

\[ \Exp(\xi) = \Big(R(\omega),\; V(\omega)\,v\Big),\qquad V(\omega) = \frac1\omega\begin{bmatrix}\sin\omega & -(1-\cos\omega)\\ 1-\cos\omega & \sin\omega\end{bmatrix}, \]

with \(V\to I\) as \(\omega\to0\). GTSAM's Pose2::Expmap writes it as t = (v_ortho - R.rotate(v_ortho)) / w where v_ortho is \(v\) rotated by 90°. Expand \((I-R)R_{\pi/2}v/\omega\) and you get exactly \(V v\). The motion is an arc of a circle, the path of a car driving at constant speed and steering angle. The adjoint in GTSAM's order is

\[ \Ad_T = \begin{bmatrix}\cos\theta & -\sin\theta & t_y\\ \sin\theta & \cos\theta & -t_x\\0&0&1\end{bmatrix} \]

(Pose2::AdjointMap).

13. Derivatives on manifolds

13.1 Definition

For \(f: G\to H\) between Lie groups, define the (right) Jacobian as the matrix \(J\) such that

\[ f(x\oplus\delta) \approx f(x)\oplus J\delta \qquad\Longleftrightarrow\qquad J = \frac{\partial}{\partial\delta}\Log\big(f(x)^{-1}f(x\,\Exp(\delta))\big)\Big|_{\delta=0}. \]

If the output is a plain vector, use \(f(x\oplus\delta)\approx f(x)+J\delta\). This is exactly what every OptionalJacobian in GTSAM returns, and exactly what numericalDerivative11/21/... (gtsam/base/numericalDerivative.h) approximates with central differences in the tangent space. That is the tool used to test every analytic Jacobian in the codebase. The chain rule holds as usual: \(J_{f\circ g} = J_f\,J_g\).

All the derivations below use one trick: move every \(\Exp(\delta)\) to the far right using the adjoint, then read off the coefficient.

13.2 The Jacobians GTSAM's `Lie.h` hard-codes

Composition \(f(A,B)=AB\): \(A\Exp(\delta)B = AB\,(B^{-1}\Exp(\delta)B) = AB\,\Exp(\Ad_{B^{-1}}\delta)\), so

\[ \frac{\partial(AB)}{\partial A} = \Ad_{B^{-1}},\qquad \frac{\partial(AB)}{\partial B} = I . \]

Inverse \(f(A)=A^{-1}\): \((A\Exp(\delta))^{-1} = \Exp(-\delta)A^{-1} = A^{-1}(A\Exp(-\delta)A^{-1}) = A^{-1}\Exp(-\Ad_A\delta)\), so

\[ \frac{\partial A^{-1}}{\partial A} = -\Ad_A . \]

Between \(f(A,B) = A^{-1}B\) (the relative pose, which is what odometry measures): \((A\Exp\delta)^{-1}B = \Exp(-\delta)A^{-1}B = f\,\Exp(-\Ad_{f^{-1}}\delta)\), so

\[ \frac{\partial(A^{-1}B)}{\partial A} = -\Ad_{(A^{-1}B)^{-1}} = -\Ad_{B^{-1}A},\qquad \frac{\partial(A^{-1}B)}{\partial B} = I. \]

Compare with the code: compose: *H1 = g.inverse().AdjointMap(); inverse: *H = -AdjointMap(); Pose3::between: *Hself = -wTb.inverse().AdjointMap() * AdjointMap() \(= -\Ad_{B^{-1}}\Ad_A = -\Ad_{B^{-1}A}\). ✓

13.3 Acting on points

Rotate a point \(f(R,p) = Rp\): \(R\Exp(\delta)p\approx R(I+\skew{\delta})p = Rp + R\skew{\delta}p = Rp - R\skew{p}\delta\), so

\[ \frac{\partial (Rp)}{\partial R} = -R\skew{p},\qquad \frac{\partial(Rp)}{\partial p} = R . \]

Rot3::rotate: *H1 = R * skewSymmetric(-p). ✓

Transform a world point into the body frame, \(f(T,p^w) = R^\top(p^w - t) =: q\). Perturb \(T\to T\Exp(\xi)\), which to first order is \((R(I+\skew{\omega}),\; t+Rv)\):

\[ q' \approx (I-\skew{\omega})R^\top(p^w - t - Rv) \approx q - \skew{\omega}q - v = q + \skew{q}\omega - v, \]

\[ \frac{\partial q}{\partial T} = \begin{bmatrix}\skew{q} & -I\end{bmatrix},\qquad \frac{\partial q}{\partial p^w} = R^\top . \]

That is literally the 3×6 matrix typed out in Pose3::transformTo. ✓

13.4 Derivatives of Exp and Log: the right Jacobian

How does \(\Exp\) respond to a change in its argument? It is not \(\Exp(\omega)\Exp(\delta)\), because \(\Exp(\omega+\delta)\ne\Exp(\omega)\Exp(\delta)\) when rotations don't commute. To first order:

\[ \Exp(\omega+\delta)\approx\Exp(\omega)\,\Exp\big(J_r(\omega)\,\delta\big)\approx \Exp\big(J_l(\omega)\,\delta\big)\,\Exp(\omega), \]

\[ J_r(\omega) = I - \frac{1-\cos\theta}{\theta^2}\skew{\omega} + \frac{\theta - \sin\theta}{\theta^3}\skew{\omega}^2,\qquad J_l(\omega) = J_r(-\omega) = \Exp(\omega)\,J_r(\omega). \]

And for the logarithm:

\[ \Log\big(\Exp(\omega)\Exp(\delta)\big)\approx \omega + J_r^{-1}(\omega)\,\delta,\qquad J_r^{-1}(\omega) = I + \tfrac12\skew{\omega} + \Big(\frac{1}{\theta^2} - \frac{1+\cos\theta}{2\theta\sin\theta}\Big)\skew{\omega}^2 . \]

In GTSAM, SO3::ExpmapDerivative(ω) \(=J_r(\omega)\) and SO3::LogmapDerivative(ω) \(= J_r^{-1}(\omega)\). Both are computed by so3::DexpFunctor, whose cached coefficients are \(B = (1-\cos\theta)/\theta^2\), \(C = (1-A)/\theta^2 = (\theta-\sin\theta)/\theta^3\), and \(D = (1-\frac{A}{2B})/\theta^2\). Check that \(D\) equals the \(J_r^{-1}\) coefficient above, using \(\frac{\sin\theta}{1-\cos\theta} = \frac{1+\cos\theta}{\sin\theta}\). The near-zero branches are \(C\approx\frac16-\frac{\theta^2}{120}\) and \(D\approx\frac1{12}+\frac{\theta^2}{720}\).

Where this bites. A BetweenFactor<Pose3> has error \(e = \Log(z^{-1}x_1^{-1}x_2)\) (traits<T>::Local(measured, between(x1,x2))). Its exact Jacobians are \(J_r^{-1}(e)\cdot(-\Ad_{x_2^{-1}x_1})\) and \(J_r^{-1}(e)\). Near convergence \(e\approx0\) and \(J_r^{-1}\approx I\), which is why some code omits it. GTSAM does include it whenever the type provides Local with Jacobians.

13.5 BCH: why "small" matters

The Baker–Campbell–Hausdorff formula says \(\Log(\Exp(a)\Exp(b)) = a + b + \tfrac12 a\times b + O(\lVert a\rVert\lVert b\rVert(\lVert a\rVert + \lVert b\rVert))\). Tangent vectors only add like vectors to first order. Every linearization in GTSAM is valid for small steps, and Gauss–Newton/LM iterate precisely because the first-order model is only locally right (chapter 4).

14. Uncertainty on manifolds

You can't put a Gaussian directly on SO(3), but you can put one on the tangent space and push it through the retraction:

\[ x = \bar x\oplus\epsilon = \bar x\,\Exp(\epsilon),\qquad \epsilon\sim\mathcal N(0,\Sigma). \]

So a covariance in GTSAM is a covariance of the right-perturbation vector in the body frame, in tangent order. For Pose3 that is 6×6 with rotation (rad) in rows 0–2 and translation (m, body frame) in rows 3–5. Useful consequences:

Propagation through a function. \(y = f(x)\) gives \(\Sigma_y \approx J\Sigma_xJ^\top\) with the manifold Jacobian \(J\) from §13. Example: the covariance of a relative pose is \(\Sigma_{rel} = J_1\Sigma_1J_1^\top + J_2\Sigma_2J_2^\top\) (+ cross terms).
Changing the frame. The same uncertainty expressed as a left (world-frame) perturbation is \(\Sigma_w = \Ad_{\bar x}\Sigma\Ad_{\bar x}^\top\).
Interop. ROS PoseWithCovariance expects \((x,y,z,\text{rotX},\text{rotY},\text{rotZ})\), translation first, typically in the parent frame. Converting from Marginals::marginalCovariance needs both a permutation and an adjoint. This is a frequent porting bug.

15. Retractions in general, and manifolds that aren't groups

A retraction at \(x\) is any smooth map \(\mathcal R_x:T_x\mathcal M\to\mathcal M\) with \(\mathcal R_x(0)=x\) and derivative \(I\) at 0. It agrees with the exponential to first order. Optimizers only need a retraction plus its inverse (local coordinates). Options in GTSAM:

SO(3): exponential (default, GTSAM_ROT3_EXPMAP), or Cayley, \(\text{cay}(\omega) = (I-\tfrac12\skew{\omega})^{-1}(I+\tfrac12\skew{\omega})\), which is cheaper and rational (no trig).
SE(3): exponential (default) or the decoupled first-order chart (§10).
Unit3 (\(S^2\), a bearing or direction; not a group): GTSAM builds an orthonormal tangent basis \(B(p)\in\R^{3\times2}\) at each point. The 2-vector \(\delta\) maps to \(v = B\delta\), and \(p\oplus\delta = \cos(\lVert v\rVert)p + \sin(\lVert v\rVert)\frac{v}{\lVert v\rVert}\) (the sphere's great-circle exponential).
OrientedPlane3, EssentialMatrix (\(SO(3)\times S^2\)), and Line3 are each built from these pieces.

This is why GTSAM's Manifold.h only requires retract/localCoordinates, while Lie.h adds group operations on top.

16. Building bigger groups

Direct product \(G\times H\): components act independently, tangent vectors stack, and Jacobians are block-diagonal. GTSAM: ProductLieGroup.
Semidirect product \(G\ltimes H\): \(G\) acts on \(H\), e.g. SE(3) \(=\) SO(3) \(\ltimes\R^3\), because rotation acts on translation in \(T_1T_2\). GTSAM: SemidirectLieGroup.
\(SE_2(3)\) / NavState: \((R, p, v)\) with \(\begin{bmatrix}R&p&v\\0&1&0\\0&0&1\end{bmatrix}\), so rotation acts on both position and velocity. Its exponential has the same \(J_l\) structure applied to both. GTSAM's NavState (tangent order rotation, position, velocity) and ExtendedPose3<K> implement it. It underlies IMU preintegration (chapter 3 §13).

17. The OMPL view: metrics, geodesics, interpolation

OMPL never differentiates, but it needs a distance and an interpolation on each space, and it gets them from the same geometry.

SO(2). The distance is the shorter arc, \(d(a,b) = \min(\lvert a-b\rvert, 2\pi - \lvert a-b\rvert)\). Interpolation moves along that shorter arc and wraps back to \([-\pi,\pi)\). getMaximumExtent() \(=\pi\) and getMeasure() \(=2\pi\) (base/spaces/src/SO2StateSpace.cpp).

SO(3). The natural geodesic distance is the rotation angle of \(R_1^\top R_2\), \(\theta = \lVert\Log(R_1^\top R_2)\rVert = 2\arccos\lvert q_1\cdot q_2\rvert\). OMPL's SO3StateSpace::distance returns \(\arccos\lvert q_1\cdot q_2\rvert\), half of that. It is still a valid metric (a constant multiple), but its maximum is \(\pi/2\) (getMaximumExtent() \(=\pi/2\)). The measure is set to \(\pi^2\), half the volume \(2\pi^2\) of \(S^3\), because of the double cover. Interpolation is slerp:

\[ \text{slerp}(q_0,q_1;t) = \frac{\sin((1-t)\Omega)\,q_0 + \sin(t\Omega)\,q_1}{\sin\Omega},\qquad \cos\Omega = \lvert q_0\cdot q_1\rvert , \]

with \(q_1\)'s sign flipped if \(q_0\cdot q_1<0\). Derivation: slerp is \(q_0(q_0^{-1}q_1)^t\), i.e. constant angular velocity along the great circle. In Lie terms it's \(R_0\Exp(t\Log(R_0^\top R_1))\). The formula is the planar-geometry identity for points on a circle spanned by \(q_0,q_1\). Code: SO3StateSpace::interpolate (it negates s1 when dq < 0).

SE(2)/SE(3) in OMPL are compound spaces. Translation (RealVectorStateSpace) and rotation (SO2/SO3StateSpace) are separate subspaces with weights (both 1.0 by default), so

\[ d_{SE(3)} = w_t\lVert t_1-t_2\rVert + w_r\arccos\lvert q_1\cdot q_2\rvert, \]

and interpolation is component-wise (linear + slerp), not the screw motion of §10. The weights mix metres and radians. Choose them so both terms reflect how far the robot's body actually moves; chapter 6 discusses this.

GTSAM's interpolation of poses (interpolate<Pose3>) instead uses the Lie-group geodesic \(T_0\Exp(t\Log(T_0^{-1}T_1))\), which is a screw motion. Two codebases, two different notions of "straight line" between poses. A port that shares geometry code between them must keep both.

18. Conventions cheat-sheet

Item	GTSAM	OMPL	Eigen / ROS
Rotation storage	3×3 matrix (or quaternion with flag)	quaternion `{x,y,z,w}`	Eigen stores `x,y,z,w`, constructs `(w,x,y,z)`; ROS `x,y,z,w`
Perturbation	right: \(x\Exp(\delta)\)	n/a (no derivatives)	varies
Pose3 tangent order	\([\omega;\,v]\) (rotation first)	n/a	ROS covariance: translation first
Pose2 tangent order	\([v_x,v_y;\,\omega]\) (translation first)	SE2: x, y, yaw
SO3 distance	\(\lVert\Log(R_1^\top R_2)\rVert\) = angle	\(\arccos\lvert q_1\cdot q_2\rvert\) = half angle
Pose interpolation	Lie geodesic (screw)	component-wise (lerp + slerp)
Pose3 retract	Expmap (flag: first-order)	n/a

19. Check yourself

Derive Rodrigues' formula from the exponential series. Why are the Taylor branches needed, and what are their next terms?
Derive \(\partial(A^{-1}B)/\partial A\) for SO(3) and verify it numerically for a random pair.
Show that \(\Ad_{T}\) for SE(3) in \([v;\omega]\) order is \(\begin{bmatrix}R&\skew{t}R\\0&R\end{bmatrix}\).
Why does OMPL's SO(3) maximum extent equal \(\pi/2\) and not \(\pi\)?
A covariance from Marginals for a Pose3 has a large entry in position (3,3). Which physical quantity is uncertain, and in which frame?
Prove \(\lvert q_1\cdot q_2\rvert = \cos(\Delta\theta/2)\).