∑MathAdvanced

Lie Groups & Lie Algebras

Key Points

•
Lie groups model continuous symmetries like rotations and rigid-body motions, combining algebra (group law) and calculus (smooth manifolds).
•
A Lie algebra is the linearized, tangent-space view of a Lie group at the identity, equipped with a bracket that captures non-commutativity.
•
The exponential map sends a small algebra element (infinitesimal motion) to a finite group element (finite motion).
•
For matrix Lie groups, the Lie bracket is the commutator [X, Y] = XY − YX and the exponential map is the matrix exponential.
•
SO(3) and SE(3) are core examples: 3D rotations and 3D rigid-body motions, with efficient closed-form exp/log formulas.
•
The Baker–Campbell–Hausdorff (BCH) formula combines small motions, revealing correction terms from non-commuting directions.
•
In computation, many operations reduce to small fixed-size matrix arithmetic (e.g., 3×3), which is effectively O(1) per operation.
•
Common pitfalls include mixing vector addition with group composition, ignoring normalization, and misusing small-angle approximations.

Prerequisites

→Linear algebra (matrices, vectors, eigenvalues) — Matrix Lie groups, commutators, and closed-form formulas all rely on matrix arithmetic and properties.
→Multivariable calculus and differential equations — Lie groups are smooth manifolds; exponentials arise from integrating differential equations on groups.
→Group theory basics — Understanding identity, inverses, associativity, and homomorphisms grounds the algebraic side of Lie groups.
→3D geometry and rotations — SO(3) and SE(3) are key examples; intuition about axes, angles, and rigid-body motions is essential.
→Numerical methods and stability — Implementing exp/log and handling small angles require series expansions and care with floating-point errors.
→Vector calculus and cross product — In so(3), the Lie bracket corresponds to the cross product, which simplifies computations.

Detailed Explanation

Tap terms for definitions

01Overview

Lie groups are mathematical structures that unify symmetry and smooth geometry. They are groups (with an associative composition, identity, and inverses) where the group operations are also smooth maps on a differentiable manifold. This means we can use calculus to study symmetry. The associated Lie algebra is the tangent space to the Lie group at the identity, capturing how the group behaves locally through linear objects and a bilinear bracket operation. For matrix Lie groups—subsets of invertible matrices—these ideas become concrete: the group operation is matrix multiplication, the Lie algebra is a vector space of matrices closed under commutators, and the exponential map is the matrix exponential. Famous examples include SO(3), the group of 3D rotations, and SE(3), the group of 3D rigid-body motions (rotation plus translation). These structures are central in robotics, computer graphics, physics, control, and differential geometry because they let us transition smoothly between small, linearized motions (Lie algebra) and finite motions (Lie group). Computation on Lie groups leverages closed-form formulas, series expansions, and numerically stable procedures, allowing accurate modeling of orientation, pose, and continuous transformations.

02Intuition & Analogies

Imagine steering a car: tiny nudges of the wheel and pedals produce small changes that accumulate into a curved trajectory. The small changes are like elements of a Lie algebra—infinitesimal motions—while the final pose of the car is like a point in a Lie group—the result of composing many small moves. In another picture, think of rotating a camera: you can specify an axis and a small angle (an infinitesimal twist) and, by integrating those tiny turns, you get a noticeable orientation. The Lie algebra encodes those axis-angle tweaks; the Lie group encodes the full rotation. The exponential map is the rule that turns a tiny, linear instruction into a finite motion—like following a constant steering command for one unit of time. When motions don’t commute (turning then moving forward is not the same as moving forward then turning), the bracket measures this discrepancy: it tells you how two small motions “interfere.” The BCH formula is the instruction manual for combining two small motions into one equivalent small motion, with correction terms that account for the non-commutativity. In robotics, using Lie groups keeps orientations and poses consistent (e.g., rotation matrices stay orthonormal) while still letting you compute with vectors in the algebra. In graphics and physics, this viewpoint avoids singularities that plague angle parametrizations and yields smooth interpolation. Overall, Lie groups and algebras are like the map (group) and compass (algebra) of continuous symmetry: one tells you where you are, the other tells you how to move.

03Formal Definition

A Lie group G is a smooth manifold equipped with a group structure such that the multiplication map m: G ×

G \to G

, m(g, h) = gh, and the inversion map i:

G \to G

, i(g) =

g^{- 1}

, are smooth (

C^{\infty}

) functions. The Lie algebra

g

of G is the tangent space

T_{e}

G at the identity e, equipped with a bilinear, antisymmetric bracket [

\cdot

\cdot

g

\times

g

→

g

satisfying the Jacobi identity. For matrix Lie groups (closed subgroups of GL(n,

R

)), the Lie algebra is a linear subspace of

R^{n \times n}

that is closed under the commutator [X, Y] = XY − YX. The exponential map

exp

g

→ G is defined by

exp

(X) =

γ

(1), where

γ

R

→ G is the unique one-parameter subgroup with

γ

(0) = e and

\overset{γ}{˙}

(0) = X; for matrix groups,

exp

is the matrix exponential. The adjoint representation Ad: G → Aut(

g

) is given by

A d_{g}

(X) =

\frac{d}{dt}\big

|_{

t=0}

exp

(tX)

g^{- 1} = g X g^{- 1}

, and its differential at the identity is

a d_{X}

(Y) = [X, Y]. Structure constants

c_{ij}^{k}

are coefficients relative to a basis \{

e_{i}

\} of

g

defined by [

e_{i}

e_{j}

] =

\sum_{k}

c_{ij}^{k}

e_{k}

04When to Use

Use Lie groups and Lie algebras whenever your state, transformation, or symmetry is continuous and non-linear, but you still want linear tools locally. Typical cases: (1) 3D rotations and poses: Represent orientations with SO(3) instead of Euler angles to avoid singularities, and rigid motions with SE(3). (2) Robotics and state estimation: Implement motion models and filters (e.g., EKF on manifolds) using exp/log to move between tangent perturbations and group states; use the adjoint to transport covariances. (3) Computer graphics and animation: Interpolate rotations smoothly (slerp on SO(3)), blend motions, and preserve orthonormality without renormalization hacks. (4) Control and planning: Design controllers on configuration manifolds, compute geodesics and linearizations using Lie algebra, and compose small motions via BCH. (5) Numerical integration of dynamics with symmetry (e.g., rigid body): Use Lie group integrators that preserve structure. (6) Differential equations on groups: Solve \dot{g} = gX(t) with products of exponentials. In code, use Lie groups when small increments are naturally vector-like (algebra) but the true state must satisfy constraints (group), such as det(R)=1 and R^{T}R=I for rotations.

⚠️Common Mistakes

Treating group elements like vectors: Adding two rotations or poses component-wise is invalid; use group multiplication or exp/log via the algebra. 2) Ignoring non-commutativity: Assuming exp(X)exp(Y)=exp(X+Y) leads to large errors; use BCH or compose on the group. 3) Numerical drift in rotations: Repeated floating-point arithmetic can make R non-orthonormal; prefer using exp of skew-symmetric matrices, re-orthonormalize if needed, or use quaternions with normalization. 4) Small-angle misuse: Applying first-order approximations outside their validity range causes bias; use closed forms (e.g., Rodrigues) or higher-order terms. 5) Mixing Ad and ad: Ad_{g} (group action) and ad_{X} (algebra action) are different; misuse breaks transforms of twists and covariances. 6) Wrong frame conventions in SE(3): Confusing left- vs right-multiplication or body- vs spatial-twists flips signs and adjoints. 7) Disconnected representations: Using Euler angles can introduce gimbal lock; prefer SO(3) matrices or unit quaternions; when linearizing, use the Lie algebra. 8) Overlooking domain checks: Log maps need valid inputs (e.g., det(R)=1, R^{T}R=I); sanitize or project noisy matrices before log.

Key Formulas

Matrix Exponential Series

exp (A) = k = 0 \sum \infty \frac{A ^{k}}{k !}

Explanation: This defines the exponential of a matrix A via a power series. For small-norm A it converges rapidly and connects the algebra to the group.

Commutator

[X, Y] = X Y - Y X

Explanation: The Lie bracket for matrix Lie algebras is the commutator. It measures the failure of X and Y to commute and drives the correction terms in BCH.

Adjoint and ad

Ad_{g} (X) = g X g^{- 1}, ad_{X} (Y) = [X, Y]

Explanation: Ad describes how a group element conjugates an algebra element; ad is the derived action within the algebra. They connect global and local coordinate changes.

Jacobi Identity

[X, [Y, Z]] + [Y, [Z, X]] + [Z, [X, Y]] = 0

Explanation: The Jacobi identity is a defining property of Lie algebras. It encodes associativity-like behavior for the bracket and ensures consistent algebraic structure.

Adjoint-Exponential Relation

Ad_{e x p (X)} = e^{ad_{X}} = k = 0 \sum \infty \frac{ad _{X}^{k}}{k !}

Explanation: Conjugation by exp(X) equals exponentiating the ad operator. This links group conjugation to algebra commutators and is key for BCH and covariance transport.

Baker–Campbell–Hausdorff (BCH) Series

lo g (exp X exp Y) = X + Y + \frac{1}{2} [X, Y] + \frac{1}{12} [X, [X, Y]] - \frac{1}{12} [Y, [X, Y]] + \dots

Explanation: The log of the product of exponentials equals a series in X, Y, and their commutators. Truncations give practical approximations when X and Y are small.

Rodrigues’ Formula for SO(3)

R = I + \frac{sin θ}{θ} \overset{ω}{^} + \frac{1 - cos θ}{θ ^{2}} \overset{ω}{^}^{2}, θ = ∥ ω ∥

Explanation: This gives the rotation matrix R from an axis-angle vector $ω$ . It is numerically stable for moderate angles and avoids singular Euler angles.

SO(3) Logarithm

θ = arccos (\frac{tr ( R ) - 1}{2}), \overset{ω}{^} = \frac{θ}{2 sin θ} (R - R^{T})

Explanation: This recovers the axis-angle vector from a rotation matrix R. For small $θ$ , use series expansions to avoid dividing by small $sin θ$ .

SE(3) Exponential

T = exp ([\overset{ω}{^} 0 v 0]) = [R 0 V v 1], V = I + \frac{1 - cos θ}{θ ^{2}} \overset{ω}{^} + \frac{θ - sin θ}{θ ^{3}} \overset{ω}{^}^{2}

Explanation: This maps a twist (v, $ω$ ) to a rigid transform T with rotation R and translation Vv. The matrix V corrects for rotation-induced coupling of translation.

Structure Constants

[e_{i}, e_{j}] = k \sum c_{ij}^{k} e_{k}

Explanation: Relative to a basis, the bracket is encoded by constants $c_{ij}^{k}$ . They summarize the algebra’s multiplication table and enable coordinate computations.

Complexity of Dense Matrix Operations

T (n) = O (n^{3}) for dense matrix exp/log on n \times n

Explanation: Naive dense matrix multiplications are O( $n^{3}$ ), so series, Padé, or scaling-and-squaring methods typically cost cubic time. For 3×3 or 4×4, this is effectively constant per call.

Complexity Analysis

The computational cost of Lie group operations depends on representation and dimension. For general n×n matrix Lie groups, dense matrix multiplication dominates at O(

n^{3}

). Consequently, matrix exponentials (via scaling-and-squaring with Padé or truncated series) and logarithms generally take O(

n^{3}

) time and O(

n^{2}

) space for storing matrices. In contrast, many practical robotics/graphics tasks use SO(3) and SE(3), where closed-form formulas (Rodrigues for SO(3), and the SE(3) exp/log with the V matrix) reduce computations to a handful of 3×3 products and vector operations. For these fixed, tiny sizes, the asymptotic complexity is effectively O(1) time and O(1) space per operation, though internally they perform a constant number of O(3^{3}) multiplies. The BCH truncation to second order requires computing commutators and possibly nested commutators, each costing O(

n^{3}

) in the dense case; for so(3) using the hat map, they reduce to cross products, which are O(1). Numerical stability considerations can add overhead: small-angle branches use series expansions to avoid division by tiny numbers, and re-orthonormalization of rotations may be necessary due to floating-point drift (e.g., via SVD or polar decomposition, each O(

n^{3}

) for general n). Memory use is modest: storing a few small matrices and vectors (3×3, 4×4) is constant; in general, O(

n^{2}

) space suffices. Parallelism (SIMD) and caching can further improve constant factors, but do not change big-O rates.

Code Examples

SO(3) exponential and logarithm via Rodrigues’ formula

1 #include <bits/stdc++.h>
2 using namespace std;
3 
4 struct Vec3 { double x,y,z; };
5 struct Mat3 { double a[3][3]; };
6 
7 // Utility: build skew-symmetric matrix from a vector (hat operator)
8 Mat3 hat(const Vec3& w){ Mat3 W{}; W.a[0][0]=0;     W.a[0][1]=-w.z; W.a[0][2]= w.y;
9                          W.a[1][0]= w.z; W.a[1][1]=0;     W.a[1][2]=-w.x;
10                          W.a[2][0]=-w.y; W.a[2][1]= w.x; W.a[2][2]=0; return W; }
11 
12 // vee operator: extract vector from skew-symmetric matrix
13 Vec3 vee(const Mat3& W){ return { (W.a[2][1]-W.a[1][2])/2.0,
14                                   (W.a[0][2]-W.a[2][0])/2.0,
15                                   (W.a[1][0]-W.a[0][1])/2.0 }; }
16 
17 Mat3 I3(){ Mat3 R{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) R.a[i][j]=(i==j); return R; }
18 
19 Mat3 add(const Mat3& A,const Mat3& B){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[i][j]+B.a[i][j]; return C; }
20 Mat3 mul(const Mat3& A,const Mat3& B){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++){ C.a[i][j]=0; for(int k=0;k<3;k++) C.a[i][j]+=A.a[i][k]*B.a[k][j]; } return C; }
21 Mat3 scale(const Mat3& A,double s){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[i][j]*s; return C; }
22 Mat3 sub(const Mat3& A,const Mat3& B){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[i][j]-B.a[i][j]; return C; }
23 Mat3 transpose(const Mat3& A){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[j][i]; return C; }
24 
25 double norm(const Vec3& v){ return sqrt(v.x*v.x+v.y*v.y+v.z*v.z); }
26 
27 double trace(const Mat3& A){ return A.a[0][0]+A.a[1][1]+A.a[2][2]; }
28 
29 // Exponential map so(3) -> SO(3) using Rodrigues' formula with small-angle handling
30 Mat3 so3_exp(const Vec3& w){
31     double theta = norm(w);
32     Mat3 I = I3();
33     if(theta < 1e-8){
34         // First-order: R ≈ I + hat(w)
35         return add(I, hat(w));
36     }
37     Vec3 u{ w.x/theta, w.y/theta, w.z/theta };
38     Mat3 W = hat(u);
39     Mat3 W2 = mul(W,W);
40     double s = sin(theta), c = cos(theta);
41     Mat3 term1 = scale(W, s);
42     Mat3 term2 = scale(W2, 1.0 - c);
43     return add(add(I, term1), term2);
44 }
45 
46 // Logarithm map SO(3) -> so(3): returns axis-angle vector w
47 Vec3 so3_log(const Mat3& R){
48     // Clamp trace to valid range to avoid NaNs due to numerical errors
49     double tr = max(-1.0, min(3.0, trace(R)));
50     double cos_theta = (tr - 1.0)/2.0;
51     cos_theta = max(-1.0, min(1.0, cos_theta));
52     double theta = acos(cos_theta);
53     if(theta < 1e-8){
54         // For tiny angles, use skew-symmetric part directly
55         Mat3 S = scale(sub(R, transpose(R)), 0.5);
56         Vec3 w = vee(S);
57         return w; // already ~ angle-axis for small angles
58     }
59     double denom = 2.0*sin(theta);
60     Mat3 S = scale(sub(R, transpose(R)), 1.0/denom);
61     Vec3 u = vee(S); // unit axis
62     return { u.x*theta, u.y*theta, u.z*theta };
63 }
64 
65 int main(){
66     // Example: rotate around z by 45 degrees
67     Vec3 w{0,0,M_PI/4};
68     Mat3 R = so3_exp(w);
69     Vec3 w_back = so3_log(R);
70     cout.setf(std::ios::fixed); cout<<setprecision(6);
71     cout << "Recovered axis-angle: ("<<w_back.x<<","<<w_back.y<<","<<w_back.z<<")\n";
72     // Expect approximately (0,0,pi/4)
73     return 0;
74 }
75

Implements the hat/vee operators, Rodrigues’ formula for the exponential map from so(3) to SO(3), and the corresponding logarithm. It handles small angles with series-based branches for numerical stability.

Time: O(1) for 3×3 operations (constant-time); O(n^3) in general for n×n dense matricesSpace: O(1) extra space

SE(3) exponential and logarithm (rigid-body transforms)

1 #include <bits/stdc++.h>
2 using namespace std;
3 
4 struct Vec3 { double x,y,z; };
5 struct Mat3 { double a[3][3]; };
6 struct SE3 { double T[4][4]; };
7 
8 Mat3 I3(){ Mat3 R{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) R.a[i][j]=(i==j); return R; }
9 Mat3 add(const Mat3& A,const Mat3& B){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[i][j]+B.a[i][j]; return C; }
10 Mat3 sub(const Mat3& A,const Mat3& B){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[i][j]-B.a[i][j]; return C; }
11 Mat3 mul(const Mat3& A,const Mat3& B){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++){ C.a[i][j]=0; for(int k=0;k<3;k++) C.a[i][j]+=A.a[i][k]*B.a[k][j]; } return C; }
12 Mat3 scale(const Mat3& A,double s){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[i][j]*s; return C; }
13 Mat3 transpose(const Mat3& A){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[j][i]; return C; }
14 
15 Mat3 hat(const Vec3& w){ Mat3 W{}; W.a[0][0]=0;     W.a[0][1]=-w.z; W.a[0][2]= w.y;
16                          W.a[1][0]= w.z; W.a[1][1]=0;     W.a[1][2]=-w.x;
17                          W.a[2][0]=-w.y; W.a[2][1]= w.x; W.a[2][2]=0; return W; }
18 
19 Vec3 vee(const Mat3& W){ return { (W.a[2][1]-W.a[1][2])/2.0,
20                                   (W.a[0][2]-W.a[2][0])/2.0,
21                                   (W.a[1][0]-W.a[0][1])/2.0 }; }
22 
23 double norm(const Vec3& v){ return sqrt(v.x*v.x+v.y*v.y+v.z*v.z); }
24 
25 Mat3 so3_exp_rodrigues(const Vec3& w){
26     double theta = norm(w); Mat3 I = I3();
27     if(theta < 1e-8) return add(I, hat(w));
28     Vec3 u{w.x/theta,w.y/theta,w.z/theta}; Mat3 W=hat(u); Mat3 W2=mul(W,W);
29     double s=sin(theta), c=cos(theta);
30     return add(add(I, scale(W,s)), scale(W2, 1.0-c));
31 }
32 
33 // Compute V(theta) matrix used in SE(3) exponential
34 Mat3 V_matrix(const Vec3& w){
35     double theta = norm(w); Mat3 I = I3();
36     if(theta < 1e-8){
37         // V ≈ I + 1/2 W + 1/6 W^2 for small angles
38         Mat3 W = hat(w); Mat3 W2 = mul(W,W);
39         return add(add(I, scale(W, 0.5)), scale(W2, 1.0/6.0));
40     }
41     Vec3 u{w.x/theta,w.y/theta,w.z/theta}; Mat3 W=hat(u); Mat3 W2=mul(W,W);
42     double s=sin(theta), c=cos(theta);
43     Mat3 term1 = I;
44     Mat3 term2 = scale(W, (1.0 - c)/(theta)); // note W is unit-based; divide by theta
45     Mat3 term3 = scale(W2, (theta - s)/(theta)); // then divide by theta^2 overall
46     // Correct scaling for unit W: coefficients are (1-c)/theta^2 and (theta-s)/theta^3
47     term2 = scale(W, (1.0 - c)/(theta*theta));
48     term3 = scale(W2, (theta - s)/(theta*theta*theta));
49     return add(add(term1, term2), term3);
50 }
51 
52 // Exponential map se(3)->SE(3) for twist (v, w)
53 SE3 se3_exp(const Vec3& v, const Vec3& w){
54     Mat3 R = so3_exp_rodrigues(w);
55     Mat3 V = V_matrix(w);
56     // t = V * v
57     double t[3];
58     for(int i=0;i<3;i++) t[i] = V.a[i][0]*v.x + V.a[i][1]*v.y + V.a[i][2]*v.z;
59     SE3 T{};
60     for(int i=0;i<3;i++) for(int j=0;j<3;j++) T.T[i][j]=R.a[i][j];
61     T.T[0][3]=t[0]; T.T[1][3]=t[1]; T.T[2][3]=t[2];
62     T.T[3][0]=T.T[3][1]=T.T[3][2]=0; T.T[3][3]=1;
63     return T;
64 }
65 
66 // Logarithm map SE(3)->se(3): returns (v, w)
67 pair<Vec3,Vec3> se3_log(const SE3& T){
68     Mat3 R{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) R.a[i][j]=T.T[i][j];
69     Vec3 t{T.T[0][3], T.T[1][3], T.T[2][3]};
70     // so(3) log
71     double tr = max(-1.0, min(3.0, R.a[0][0]+R.a[1][1]+R.a[2][2]));
72     double cos_theta = (tr - 1.0)/2.0; cos_theta = max(-1.0, min(1.0, cos_theta));
73     double theta = acos(cos_theta);
74     Mat3 W{};
75     Vec3 w;
76     if(theta < 1e-8){
77         Mat3 S{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) S.a[i][j]=0.5*(R.a[i][j]-R.a[j][i]);
78         w = vee(S);
79         // V^{-1} ≈ I - 1/2 W + 1/12 W^2 for small angles
80         Mat3 Wm = hat(w); Mat3 W2 = mul(Wm,Wm);
81         Mat3 Vinv = add(add(I3(), scale(Wm, -0.5)), scale(W2, 1.0/12.0));
82         double v_arr[3];
83         v_arr[0] = Vinv.a[0][0]*t.x + Vinv.a[0][1]*t.y + Vinv.a[0][2]*t.z;
84         v_arr[1] = Vinv.a[1][0]*t.x + Vinv.a[1][1]*t.y + Vinv.a[1][2]*t.z;
85         v_arr[2] = Vinv.a[2][0]*t.x + Vinv.a[2][1]*t.y + Vinv.a[2][2]*t.z;
86         return { {v_arr[0],v_arr[1],v_arr[2]}, w };
87     } else {
88         Mat3 S{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) S.a[i][j]=(R.a[i][j]-R.a[j][i])/(2.0*sin(theta));
89         Vec3 u = vee(S);
90         w = {u.x*theta, u.y*theta, u.z*theta};
91         // Compute V^{-1}
92         Vec3 uh{u.x,u.y,u.z}; Mat3 Uhat = hat(uh); Mat3 U2 = mul(Uhat,Uhat);
93         double A = sin(theta)/theta;
94         double B = (1.0 - cos(theta))/(theta*theta);
95         // V = I + B*Uhat + C*U2 with C = (theta - sin theta)/theta^3
96         double C = (theta - sin(theta))/(theta*theta*theta);
97         // Known closed-form for V^{-1} coefficients
98         double halfcot = 0.5 * (theta * (cos(theta)/(sin(theta)))) ; // not directly used
99         // Use series-derived expression: V^{-1} = I - 0.5 Uhat + (1/theta^2)*(1 - A/(2B)) * U2
100         double coeff2 = (1.0/(theta*theta)) * (1.0 - A/(2.0*B));
101         Mat3 Vinv = add(add(I3(), scale(Uhat, -0.5*theta)), scale(U2, coeff2*theta*theta));
102         // Note: above scaling manipulations ensure dimensionless correctness
103         // Safer alternative: compute V and invert numerically (since 3x3)
104         // We'll do that instead for robustness.
105         Mat3 V = add(add(I3(), scale(Uhat, B*theta)), scale(U2, C*theta*theta));
106         // Build dense matrix for inversion
107         double M[3][3]; for(int i=0;i<3;i++) for(int j=0;j<3;j++) M[i][j]=V.a[i][j];
108         // Invert M (Gauss-Jordan)
109         double Ainv[3][6];
110         for(int i=0;i<3;i++) for(int j=0;j<6;j++) Ainv[i][j] = (j<3)? M[i][j] : (i==(j-3));
111         for(int i=0;i<3;i++){
112             // pivot
113             int p=i; for(int r=i+1;r<3;r++) if(fabs(Ainv[r][i])>fabs(Ainv[p][i])) p=r;
114             for(int j=0;j<6;j++) swap(Ainv[i][j], Ainv[p][j]);
115             double piv = Ainv[i][i];
116             for(int j=0;j<6;j++) Ainv[i][j]/=piv;
117             for(int r=0;r<3;r++) if(r!=i){ double f=Ainv[r][i]; for(int j=0;j<6;j++) Ainv[r][j]-=f*Ainv[i][j]; }
118         }
119         double Vinv_arr[3][3]; for(int i=0;i<3;i++) for(int j=0;j<3;j++) Vinv_arr[i][j]=Ainv[i][j+3];
120         double v_arr[3];
121         v_arr[0] = Vinv_arr[0][0]*t.x + Vinv_arr[0][1]*t.y + Vinv_arr[0][2]*t.z;
122         v_arr[1] = Vinv_arr[1][0]*t.x + Vinv_arr[1][1]*t.y + Vinv_arr[1][2]*t.z;
123         v_arr[2] = Vinv_arr[2][0]*t.x + Vinv_arr[2][1]*t.y + Vinv_arr[2][2]*t.z;
124         return { {v_arr[0],v_arr[1],v_arr[2]}, w };
125     }
126 }
127 
128 int main(){
129     // Twist: rotate 30 deg about z, translate (1,0,0)
130     Vec3 w{0,0,M_PI/6}; Vec3 v{1,0,0};
131     SE3 T = se3_exp(v,w);
132     auto vw = se3_log(T);
133     cout.setf(std::ios::fixed); cout<<setprecision(6);
134     cout << "Recovered w: ("<<vw.second.x<<","<<vw.second.y<<","<<vw.second.z<<")\n";
135     cout << "Recovered v: ("<<vw.first.x<<","<<vw.first.y<<","<<vw.first.z<<")\n";
136     return 0;
137 }
138

Computes the SE(3) exponential from a twist (v, ω) using the rotation R and the V matrix, and inverts it by computing the SO(3) log and inverting V numerically (robust for 3×3). Small-angle branches use series for stability.

Time: O(1) for fixed 3×3/4×4 operations; O(n^3) in general due to matrix multiplications and inversionSpace: O(1) extra space

BCH approximation on so(3) up to second order

1 #include <bits/stdc++.h>
2 using namespace std;
3 
4 struct Vec3{ double x,y,z; };
5 
6 // Cross product corresponds to commutator in so(3) under hat
7 Vec3 cross3(const Vec3& a, const Vec3& b){ return { a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x }; }
8 
9 Vec3 add(const Vec3& a,const Vec3& b){ return {a.x+b.x, a.y+b.y, a.z+b.z}; }
10 Vec3 sub(const Vec3& a,const Vec3& b){ return {a.x-b.x, a.y-b.y, a.z-b.z}; }
11 Vec3 scale(const Vec3& a,double s){ return {a.x*s, a.y*s, a.z*s}; }
12 
13 double norm(const Vec3& v){ return sqrt(v.x*v.x+v.y*v.y+v.z*v.z); }
14 
15 // BCH up to terms: X + Y + 1/2[X,Y] + 1/12[X,[X,Y]] - 1/12[Y,[X,Y]]
16 Vec3 bch_so3(const Vec3& X, const Vec3& Y){
17     Vec3 XxY = cross3(X,Y);
18     Vec3 X_XY = cross3(X, XxY);
19     Vec3 Y_XY = cross3(Y, XxY);
20     Vec3 Z = add(add(X, Y), scale(XxY, 0.5));
21     Z = add(Z, scale(X_XY, 1.0/12.0));
22     Z = add(Z, scale(Y_XY, -1.0/12.0));
23     return Z;
24 }
25 
26 int main(){
27     // Two small rotations
28     Vec3 X{0.01, 0.00, 0.00}; // ~0.57 deg about x
29     Vec3 Y{0.00, 0.02, 0.00}; // ~1.15 deg about y
30     Vec3 Z = bch_so3(X,Y);
31     cout.setf(std::ios::fixed); cout<<setprecision(6);
32     cout << "BCH result (axis-angle): ("<<Z.x<<","<<Z.y<<","<<Z.z<<")\n";
33     // Check magnitude to ensure small-angle validity
34     cout << "|Z| ≈ " << norm(Z) << " rad\n";
35     return 0;
36 }
37

Uses the isomorphism between so(3) and R^3 under the hat map: the commutator corresponds to the cross product. Implements the BCH series truncated at second order, valid when X and Y are small.

Time: O(1) for 3D vectorsSpace: O(1)

1	#include <bits/stdc++.h>
2	using namespace std;
3
4	struct Vec3 { double x,y,z; };
5	struct Mat3 { double a[3][3]; };
6
7	// Utility: build skew-symmetric matrix from a vector (hat operator)
8	Mat3 hat(const Vec3& w){ Mat3 W{}; W.a[0][0]=0; W.a[0][1]=-w.z; W.a[0][2]= w.y;
9	W.a[1][0]= w.z; W.a[1][1]=0; W.a[1][2]=-w.x;
10	W.a[2][0]=-w.y; W.a[2][1]= w.x; W.a[2][2]=0; return W; }
11
12	// vee operator: extract vector from skew-symmetric matrix
13	Vec3 vee(const Mat3& W){ return { (W.a[2][1]-W.a[1][2])/2.0,
14	(W.a[0][2]-W.a[2][0])/2.0,
15	(W.a[1][0]-W.a[0][1])/2.0 }; }
16
17	Mat3 I3(){ Mat3 R{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) R.a[i][j]=(i==j); return R; }
18
19	Mat3 add(const Mat3& A,const Mat3& B){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[i][j]+B.a[i][j]; return C; }
20	Mat3 mul(const Mat3& A,const Mat3& B){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++){ C.a[i][j]=0; for(int k=0;k<3;k++) C.a[i][j]+=A.a[i][k]*B.a[k][j]; } return C; }
21	Mat3 scale(const Mat3& A,double s){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[i][j]*s; return C; }
22	Mat3 sub(const Mat3& A,const Mat3& B){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[i][j]-B.a[i][j]; return C; }
23	Mat3 transpose(const Mat3& A){ Mat3 C{}; for(int i=0;i<3;i++) for(int j=0;j<3;j++) C.a[i][j]=A.a[j][i]; return C; }
24
25	double norm(const Vec3& v){ return sqrt(v.xv.x+v.yv.y+v.z*v.z); }
26
27	double trace(const Mat3& A){ return A.a[0][0]+A.a[1][1]+A.a[2][2]; }
28
29	// Exponential map so(3) -> SO(3) using Rodrigues' formula with small-angle handling
30	Mat3 so3_exp(const Vec3& w){
31	double theta = norm(w);
32	Mat3 I = I3();
33	if(theta < 1e-8){
34	// First-order: R ≈ I + hat(w)
35	return add(I, hat(w));
36	}
37	Vec3 u{ w.x/theta, w.y/theta, w.z/theta };
38	Mat3 W = hat(u);
39	Mat3 W2 = mul(W,W);
40	double s = sin(theta), c = cos(theta);
41	Mat3 term1 = scale(W, s);
42	Mat3 term2 = scale(W2, 1.0 - c);
43	return add(add(I, term1), term2);
44	}
45
46	// Logarithm map SO(3) -> so(3): returns axis-angle vector w
47	Vec3 so3_log(const Mat3& R){
48	// Clamp trace to valid range to avoid NaNs due to numerical errors
49	double tr = max(-1.0, min(3.0, trace(R)));
50	double cos_theta = (tr - 1.0)/2.0;
51	cos_theta = max(-1.0, min(1.0, cos_theta));
52	double theta = acos(cos_theta);
53	if(theta < 1e-8){
54	// For tiny angles, use skew-symmetric part directly
55	Mat3 S = scale(sub(R, transpose(R)), 0.5);
56	Vec3 w = vee(S);
57	return w; // already ~ angle-axis for small angles
58	}
59	double denom = 2.0*sin(theta);
60	Mat3 S = scale(sub(R, transpose(R)), 1.0/denom);
61	Vec3 u = vee(S); // unit axis
62	return { u.xtheta, u.ytheta, u.z*theta };
63	}
64
65	int main(){
66	// Example: rotate around z by 45 degrees
67	Vec3 w{0,0,M_PI/4};
68	Mat3 R = so3_exp(w);
69	Vec3 w_back = so3_log(R);
70	cout.setf(std::ios::fixed); cout<<setprecision(6);
71	cout << "Recovered axis-angle: ("<<w_back.x<<","<<w_back.y<<","<<w_back.z<<")\n";
72	// Expect approximately (0,0,pi/4)
73	return 0;
74	}
75

1	#include <bits/stdc++.h>
2	using namespace std;
3
4	struct Vec3{ double x,y,z; };
5
6	// Cross product corresponds to commutator in so(3) under hat
7	Vec3 cross3(const Vec3& a, const Vec3& b){ return { a.yb.z - a.zb.y, a.zb.x - a.xb.z, a.xb.y - a.yb.x }; }
8
9	Vec3 add(const Vec3& a,const Vec3& b){ return {a.x+b.x, a.y+b.y, a.z+b.z}; }
10	Vec3 sub(const Vec3& a,const Vec3& b){ return {a.x-b.x, a.y-b.y, a.z-b.z}; }
11	Vec3 scale(const Vec3& a,double s){ return {a.xs, a.ys, a.z*s}; }
12
13	double norm(const Vec3& v){ return sqrt(v.xv.x+v.yv.y+v.z*v.z); }
14
15	// BCH up to terms: X + Y + 1/2[X,Y] + 1/12[X,[X,Y]] - 1/12[Y,[X,Y]]
16	Vec3 bch_so3(const Vec3& X, const Vec3& Y){
17	Vec3 XxY = cross3(X,Y);
18	Vec3 X_XY = cross3(X, XxY);
19	Vec3 Y_XY = cross3(Y, XxY);
20	Vec3 Z = add(add(X, Y), scale(XxY, 0.5));
21	Z = add(Z, scale(X_XY, 1.0/12.0));
22	Z = add(Z, scale(Y_XY, -1.0/12.0));
23	return Z;
24	}
25
26	int main(){
27	// Two small rotations
28	Vec3 X{0.01, 0.00, 0.00}; // ~0.57 deg about x
29	Vec3 Y{0.00, 0.02, 0.00}; // ~1.15 deg about y
30	Vec3 Z = bch_so3(X,Y);
31	cout.setf(std::ios::fixed); cout<<setprecision(6);
32	cout << "BCH result (axis-angle): ("<<Z.x<<","<<Z.y<<","<<Z.z<<")\n";
33	// Check magnitude to ensure small-angle validity
34	cout << "\|Z\| ≈ " << norm(Z) << " rad\n";
35	return 0;
36	}
37