NA 568 - Winter 2021

Matrix Lie Groups for Robotics

Maani Ghaffari

February 9, 2021
 References

1 Tapp K (2016) Matrix groups for undergraduates, volume 79. American

Mathematical Soc.
2 Baker A (2012) Matrix groups: An introduction to Lie group theory.
Springer Science & Business Media.
3 Murray, R. (1994). A Mathematical Introduction to Robotic Manipulation.
CRC Press.
4 G. S. Chirikjian, Stochastic Models, Information Theory, and Lie Groups,
Volume 2: Analytic Methods and Modern Applications. Springer Science
& Business Media, 2011.
5 T. D. Barfoot and P. T. Furgale, Associating uncertainty with
three-dimensional poses for use in estimation problems, IEEE Trans. on
Robotics, vol. 30, no. 3, pp. 679–693, 2014.
6 A. W. Long, K. C. Wolfe, M. J. Mashner, and Chirikjian, The banana
distribution is Gaussian: A localization study with exponential coordinates.
Robotics: Science and Systems, 2013.
7 E. Eade, Lie groups for 2D and 3D transformations, accessed: 2018-02-01.
[Online]. Available: http://ethaneade.com/lie.pdf
 Matrix Groups

I A matrix group is a group of invertible matrices.

I This is a simple and purely algebraic definition.

3
 Matrix Groups

I Some application examples of matrix groups are:

I Computer graphics where matrix groups are used for three-dimensional

rotation and translation of objects.

I The theory of differential equations relies on matrix groups and, in

particular, matrix exponentiation.

I The shape of the universe. Cosmologists find it far easier to model the
space as a simply connected space under the action of a matrix group.

I Quantum computing is based on the group of unitary matrices.

I In linear algebra the theory of matrix group provides a uniform tools which
are essential in disciplines ranging from topology and geometry to discrete
math and statistics.

I Riemannian geometry relies heavily on matrix groups.

4
 Group Definition

A group is a nonempty set G together with a binary group

operation ·, e.g., g · h where g,h ∈ G, that satisfies the
following properties:

1 Closure: if g,h ∈ G then also g · h ∈ G;

2 Associativity: for all g,h,l ∈ G, (g · h) · l = g · (h · l);

3 Identity: there exist a unique identity element e ∈ G such

that e · g = g · e = g for all g ∈ G;

4 Inverse: if g ∈ G there exists an element g −1 ∈ G such

that g −1 · g = g · g −1 = e.

5
 General Linear Groups

I In general we can work with the set of all m by n matrices

with entries in K denoted Mm,n (K), where K = {R, C, H}.

I While it is possible to work with complex numbers C and

quaternions H as entries of the matrix, we shall limit our
attention to the field of reals R.

I The reason is twofold: first, in most of the problems we

encounter we deal with real numbers; second, it turns out
that all matrix groups are real matrix groups using the
following theorem.

6
 General Linear Groups

Definition
The general linear group over K is:
GLn (K) = {A ∈ Mn (K) : det(A) 6= 0}.

Theorem

1 GLn (C) is isomorphic to a subgroup of GL2n (R).

2 GLn (H) is isomorphic to a subgroup of GL2n (C).
It follows that GLn (H) is isomorphic to a subgroup of
GL4n (R).

7
 Affine Groups

I The n-dimensional affine group over R is

  
A t n
Aff n (R) = : A ∈ GLn (R), t ∈ R ⊆ GLn+1 (R).
0 1

I This is a closed
  subgroup of GLn+1 (R). If we identify
x
x ∈ Rn with ∈ Rn+1 , then as a consequence of the
1
formula
    
A t x Ax + t
=
0 1 1 1
we obtain an action of Aff n (R) on Rn .

8
 Affine Groups

I Transformations of Rn with the form x 7→ Ax + t with A

invertible is called affine transformation and preserves
lines. The associated geometry is affine geometry.

I Examples of affine transformations include translation,

scaling, similarity transformation, reflection, rotation, shear
mapping, and compositions of them in any combination
and sequence.

9
 Affine Groups

I The vector space Rn itself can be viewed as the translation

subgroup of Aff n (R),
  
In t n
Transn (R) = : t ∈ R ⊆ Aff n (R),
0 1
and this is a closed subgroup.

I There is also the closed subgroup

  
A 0
: A ∈ GLn (R) ⊆ Aff n (R),
0 1
which will be identified with GLn (R).

10
 Orthogonal Groups

Definition
The orthogonal group over R is denoted O(n) and defined as:
O(n) = {A ∈ GLn (R) : A · AT = In },
where ”·” denotes the standard matrix multiplication as the
group operation and is dropped hereafter, i.e., AAT .

11
 Orthogonal Groups

I Looking closer into the orthogonal group, we see that

det(AAT ) = det(A)2 = det(In ) = 1 , and therefore,
det(A) = ±1. Thus we have O(n) = O(n)+ ∪ O(n)−
where
O(n)+ = {A ∈ O(n) : det(A) = 1},
O(n)− = {A ∈ O(n) : det(A) = −1}.

12
 Orthogonal Groups

I Notice that O(n)+ ∩ O(n)− = ∅, so O(n) is the disjoint

union of the subsets O(n)+ and O(n)− .

I The important subgroup SO(n) = O(n)+ ≤ O(n) is the

n × n special orthogonal group.

13
 Orthogonal Groups

I One of the main reasons for the study of the orthogonal

groups O(n) and SO(n) is their relationships with
isometries, where an isometry of Rn is a
distance-preserving bijection f : Rn → Rn , i.e.
kf (x) − f (y)k = kx − yk x,y ∈ Rn

I If such an isometry fixes the origin, 0, then it is a linear

transformation, often referred to as linear isometry, and so
with respect with the standard basis it corresponds to a
matrix A ∈ GLn (R)

14
 Special Orthogonal Groups

Remark
The special orthogonal group SO(n) is the simultaneous
rotation of n perpendicular planes! For example, SO(3) is the
rotation group of R3 and defines the simultaneous rotation of
three perpendicular planes which construct the
three-dimensional (3D) Euclidean space.

15
 Isometry Groups

I Elements of SO(n) often called direct isometries or

rotations, while elements of O(n)− are sometimes called
indirect isometries.

I We define the full isometry group of Rn as

Isomn (R) = {f : Rn → Rn : f is an isometry},
which clearly contains the subgroup of translations.

16
 Isometry Groups

I In fact, Isomn (R) ⊆ Aff n (R) and is actually a closed

subgroup, hence is a matrix subgroup,
  
A t n
Isomn (R) = : A ∈ O(n), t ∈ R .
0 1

17
 Isometry Groups

I The special Euclidean group is the isometry group that

requires A to be a valid right-handed rotation matrix:
  
A t n
SE(n) = : A ∈ SO(n), t ∈ R .
0 1

I This is the group of valid rigid body transformations of Rn .

18
 Manifolds

Formally, a (smooth) manifold is a topological space M such

that there is a collection of pairs (Uα ,ϕα ) where
1 Uα forms an open cover of M , i.e.

[
Uα = M,
α

2 For each α
ϕα : Uα → Rn
is a diffeomorphism onto an open set,
3 If Uα ∩ Uβ 6= ∅, then
ϕβ ◦ ϕ−1
α : ϕα (Uα ∩ Uβ ) → ϕβ (Uα ∩ Uβ ) ,

is smooth.

19
 Manifolds

This definition means we can “chop” up M into pieces Uα

that each look like Rn .

Figure: https://en.wikipedia.org/wiki/Manifold

20
 Manifolds
Many common objects are manifolds.
1 Every Euclidean space, Rn .

2 The 2-sphere, S 2

3 The Torus T 2

https://www.jpl.nasa.gov/edu/teach/activity/ocean-world-earth-globe-toss-game/
https://en.wikipedia.org/wiki/Torus

21
 Manifolds

Just about everything falls under the category of manifolds.

For example, everything that can be described as a level-set of
a function counts.
Definition
Let f : Rn → R be differentiable. The set
f −1 (y) = {x ∈ Rn : f (x) = y} is called a level-set of f .
Moreover, y is called a regular value of f if for all x ∈ f −1 (y),
∇f 6= 0.
Theorem (Preimage theorem)
Let f : Rn → R be a differentiable function and y be a regular
value. Then f −1 (y) is a manifold.

22
 Manifolds

The preimage theorem immediately shows that the sphere is a

manifold. Let f (x,y,z) = x2 + y 2 + z 2 . Then
∇f = [2x,2y,2z]T 6= 0 and so the set
S 2 = f −1 (1) = x2 + y 2 + z 2 = 1


is a manifold.

23
 Tangent Spaces

In order to study the geometry of a manifold, we need the

notion of a tangent space. Let γ be some curve in some
manifold M , then its derivative γ̇ is a tangent vector.

24
 Tangent Spaces

If x ∈ M is a point in the manifold, then the space of all

possible tangent vectors is called the tangent space and is
denoted by Tx M .
It is important to point out that Tx M is a vector space and
dim Tx M = dim M.

25
 Tangent Space

I A matrix group is an algebraic object. However, it can also

be seen as a geometric object since it is a subset of a
Euclidean space:
G ⊂ GLn (R) ⊂ Mn (R) ∼
2
= Rn .

I In addition, looking at a matrix group as a subset of a

Euclidean space means we can discuss its tangent space.

26
 Tangent Space

Definition (Tangent space)

Let G ⊂ Rm be a subset, and let g ∈ G. The tangent space to
G at g is:
Tg G = {γ 0 (0) : γ : (−, ) → G is differentiable with γ(0) = g} .

Tg G means the set of initial velocity vectors of differentiable

paths through g in G. The term differentiable means that,
when we consider γ as a path in Rm , the m components of γ
are differentiable functions from (−, ) to R.

27
 Lie Algebras

Definition (Lie algebra)

The Lie algebra of a matrix group G ⊂ GLn (R) is the tangent
space to G at the identity e. It is denoted g = g(G) = Te G.

I In particular, g is a subspace of the Euclidean space

Mn (R) which shows that matrix groups are ”nice” sets.

I Note that the choice of identity is due to the fact that all
groups contain at least the identity element.

28
 Lie Algebras

Proposition
The Lie algebra g of a matrix group G ⊂ GLn (R) is a real
subspace of Mn (R).

Definition
The dimension of a matrix group G means the dimension of its
Lie algebra.

29
 Example: Lie Algebras of GLn (R)

Let us consider the Lie algebra of GLn (R), denoted gln (R).
Proposition
gln (R) = Mn (R). In particular, dim(GLn (R)) = n2 .

Proof.
Let A ∈ Mn (R). The path γ(t) = I + t.A in Mn (R) satisfies
γ(0) = I and γ 0 (0) = A. Also, γ restricted to sufficiently small
interval (−, ) lies in GLn (R). To justify this, notice
det(γ(0)) = 1. Since the determinant function is continuous,
det(γ(t)) is close to 1 (and is therefore non-zero) for t close to 0.
This demonstrates that A ∈ gln (R).

30
 Example: Lie Algebras of (Real) Orthogonal Group

The set o(n) = {A ∈ Mn (R) : A + AT = 0} is denoted

so(n) and called skew-symmetric matrices. We wish to show
that so(n) is the Lie algebra of O(n).

Theorem
The Lie algebra of O(n) equals so(n).

31
 Example: Lie Algebras of (Real) Orthogonal Group

Proof.
Suppose γ : (−, ) → O(n) is differentiable with γ(0) = I. Using the
product rule to differentiate both sides of
γ(t) · γ(t)T = I
gives γ 0 (0) + γ 0T (0) = 0, so γ 0 (0) ∈ so(n). This demonstrates that
g(O(n)) ⊂ so(n).

Proving the other inclusion means explicitly constructing a path in O(n)

in the direction of any A ∈ so(n). It is simpler and sufficient to do so for
all A in a basis of so(n).

32
 Example: Lie Algebras of (Real) Orthogonal Group

Proof.
The natural basis of so(n) is the set

{Eij − Eji : 1 ≤ i < j ≤ n}

where Eij denotes the matrix with ij-entry 1 and other entries zero. For
example,
so(3) = span{E12 − E21 , E13 − E31 , E23 − E32 }
     
 0 1 0 0 0 1 0 0 0 
= −1 0 0 ,  0 0 0 , 0 0 1
0 0 0 −1 0 0 0 −1 0
 

32
 Example: Lie Algebras of (Real) Orthogonal Group

Proof.
The path
γij (t) , I + sin(t)Eij − sin(t)Eji + (−1 + cos(t))(Eii + Ejj )

lies in SO(n), has γij (0) = I, and has initial direction

0
γij (0) = Eij − Eji .
Rγij (t) : Rn → Rn rotates the subspace span{ei , ej } by an angle t and
does nothing to the other basis vectors. For example, the path
 
cos(t) 0 sin(t)
γ13 (t) =  0 1 0 
− sin(t) 0 cos(t)
0
in SO(3) satisfies γ13 (0) = E13 − E31 .

32
 Example: Lie Algebras of (Real) Orthogonal Group

Remark
Suppose x is a row vector. Rγ(t) (x) , x · γ(t) is the right
multiplication of matrix γ(t) with vector x. Similarly,
Lγ(t) , (γ(t) · xT )T is the left multiplication and
Lγ(t) = Rγ T (t) . Unlike the previous example, we often work
with column vectors and left multiplication, i.e.,
Lγ(t) = γ(t) · x.

33
 Example: Lie Algebras of (Real) Orthogonal Group

Corollary
n(n−1)
dim(SO(n)) = 2
.

Proof.
Skew-symmetric matrices have zeros on the diagonal, arbitrary
real numbers above, and entries below determined by those
above, so dim(so(n)) = n(n−1)
2
.

34
 Examples

1 If G is a Lie group and e ∈ G is the identity element, then

g = Te G.

35
 Examples

1 If G is a Lie group and e ∈ G is the identity element, then

g = Te G.
2 Let G = SO(n). We know that son = Te SO(n) is the
space of all skew-symmetric matrices:
son = A ∈ Mn (R) : AT = −A .


What is Tg SO(n) for g 6= e? Let γ be a curve in SO(n) such

that γ(0) = g. Then we know that γ̇(0) ∈ Tg SO(n).

35
 Examples

Consider γ(t) = g exp(At) where A is skew. Then we know

that γ lies in SO(n) and γ(0) = g.
Differentiating, we see that γ̇(0) = gA.

Tg SOn = gA : AT = −A = g · son .


For a general matrix group G,

Tg G = g · g.

36
 Matrix Exponentiation; Series in Mn (R)

I When the power series of the function f (x) = exp(x) is

applied to a matrix A ∈ Mn (R), the result is called matrix
exponentiation:
1 2 1 3 1 4
exp(A) = I + A + A + A + A + ··· .
2! 3! 4!

I This series converges for all A ∈ Mn (R).

37
The “Best” Path in a Matrix Group: One-Parameter Groups

I If A ∈ gln (R), we would like to find the ”most natural”

path γ(t) in GLn (R) with γ(0) = I and γ 0 (0) = A.

I We will attempt to choose γ(t) such that for all x ∈ Rn ,

the path
t 7→ Rγ(t) (x)
is an integral curve of Rγ(t) .

38
The “Best” Path in a Matrix Group: One-Parameter Groups

I A vector field on Rm means a continuous function

F : Rm → Rm . By picturing F (v) as a vector drawn at
v ∈ Rm , we think of a vector field as associating a vector
to each point of Rm .

Definition (Integral curve)

A path α : (−, ) → Rm is called an integral curve of a vector
field F : Rm → Rm if α0 (t) = F (α(t)) for all t ∈ (−, ).

39
The “Best” Path in a Matrix Group: One-Parameter Groups

Proposition
Let A ∈ gln (R). The path γ : R → Mn (R) defined as
γ(t) = exp(tA) is differentiable, and γ 0 (t) = A · γ(t) = γ(t) · A.
The “Best” Path in a Matrix Group: One-Parameter Groups

Proposition
Let A ∈ gln (R). The path γ : R → Mn (R) defined as
γ(t) = exp(tA) is differentiable, and γ 0 (t) = A · γ(t) = γ(t) · A.

Proof.
Each of the n2 entries of
1 1
γ(t) = I + tA + t2 A2 + t3 A3 + · · ·
2 6
is a power series in t, which, from familiar real calculus, can be
termwise differentiated, giving:
1
γ 0 (t) = 0 + A + tA2 + t2 A3 + · · · .
2
This equals γ(t) · A or A · γ(t) depending whether you factor an
A out on the left or right.
The “Best” Path in a Matrix Group: One-Parameter Groups

Proposition
Let A ∈ Mn (R) and let γ(t) = exp(tA).

1 For all x ∈ Rn , α(t) = Rγ(t) (x) is an integral curve of

RA . Also, α(t) = Lγ(t) (x) is an integral curve of LA .

2 γ(t) is itself an integral curve of the vector field on Mn (R)

whose value at g is A · g, and is also an integral curve of
the vector field whose value at g is g · A.

41
The “Best” Path in a Matrix Group: One-Parameter Groups

The second result from the previous proposition has the

following useful implications:

Remark
Suppose a left-translation is given via the smooth map
Lg : G → G where h 7→ gh. Its differential gives rise to an
isomorphism of tangent spaces, (Lg )∗ : g → Tg G, i.e.,
Tg G ∼
= (Lg )∗ g. Then we have:
γ(t) = g exp(tA).
A similar argument for the right-translation can be expressed.
γ(t) = exp(tA)g.

42
 Body vs. Spatial Velocities

Using the robotics terminology we have the following

interpretations:

Remark
In γ(t) = g exp(tA), the velocity A is considered to be in the
body frame, whereas in γ(t) = exp(tA)g the velocity A is in
the spatial frame (relative to a fixed (inertial) coordinate
frame). The relation between the body and spatial velocities
motivates us to study the adjoint action in the following.

43
 Conjugation, Adjoint, and the Lie Bracket

Let G be a matrix group with Lie algebra g. For all g ∈ G, the

conjugation map Cg : G → G, define as

Cg (a) = gag −1 ,
is a smooth isomorphism. The derivative d(Cg )I : g → g is a
vector space isomorphism, which we denote as Adg (adjoint):
Adg = d(Cg )I

44
 Adjoint and the Lie Bracket

To derive a simple formula for Adg (B), notice that any B ∈ g

can be represented as B = b0 (0), where b(t) is a differentiable
path in G with b(0) = I. The product rule gives:

d 
Adg (B) = d(Cg )I (B) =  gb(t)g −1 = gBg −1 .
dt t=0
So we learn that (notice the similarity transformation):
Adg (B) = gBg −1 .

45
 Adjoint and the Lie Bracket

I If all elements of G commute with g, then Adg is the

identity map on g.

I So in general, Adg measures the failure of g to commute

with elements of G near I.

I More specifically, Adg (B) measures the failure of g to

commute with elements of G near I in the direction of B.

46
 The Lie Bracket

Investigating this phenomena when g is itself close to I leads

one to define:

Definition (Lie bracket)

The Lie bracket of two vectors A and B in g is:

d 
[A, B] =  Ada(t) (B),
dt t=0
where a(t) is any differentiable path in G with a(0) = I and
a0 (0) = A.

47
 The Lie Bracket

The following alternative definition is easier to calculate and

verifies that the previous definition is independent of the
choice of path a(t).

Proposition
For all A, B ∈ g, [A, B] = AB − BA.
Proof.
Left as exercise.

48
 The Lie Bracket

I Notice that [A, B] ∈ g.

I It measures the failure of elements of G near I in the

direction of A to commute with elements of G near I in
the direction of B.

49
 Properties of the Lie Bracket

Proposition
For all A, A1 , A2 , B, B1 , B2 , C ∈ g and λ1 , λ2 ∈ R,

1 (Bilinearity) [λ1 A1 + λ2 A2 , B] = λ1 [A1 , B] + λ2 [A2 , B].

2 (Bilinearity) [A, λ1 B1 + λ2 B2 ] = λ1 [A, B1 ] + λ2 [A, B2 ].

3 (Anticommutativity) [A, B] = −[B, A].

4 (Jacobi identity) [[A,B],C] + [[B,C],A] + [[C,A],B] = 0.

50
 Example: Lie Bracket on so(3) and Cross Product

See so3 cross example.m for numerical examples and

details.

     
0 0 0 0 0 1 0 −1 0
G1 = 0 0 −1 , G2 =  0 0 0 , G3 = 1 0 0
0 1 0 −1 0 0 0 0 0
[G1 , G2 ] = G3 , [G2 , G3 ] = G1 , [G3 , G1 ] = G2
e1 × e2 = e3 , e2 × e3 = e1 , e3 × e1 = e2

51
 The Adjoint Action

I Let G ⊂ GLn (R) be matrix group of dimension d, with Lie

algebra g. For every g ∈ G, Adg : g → g is a vector space
isomorphism.

I Once we choose a basis B of g, this isomorphism can be

represented as LA for some A ∈ GLd (R) (left matrix
multiplication).

I In other words, after fixing a basis of g, we can regard the

map g 7→ Adg as a function from G to GLd (R).

52
 Baker-Campbell-Hausdorff Series

For X, Y, Z ∈ g with sufficiently small norm, the equation

exp(X) exp(Y) = exp(Z) has a power series solution for Z in
terms of repeated Lie bracket of X and Y. The beginning of
the series is:

1 1 1
Z = X+Y+ [X,Y]+ [X,[X,Y]]+ [Y,[Y,X]]+· · · .
2 12 12

53
 Baker-Campbell-Hausdorff (BCH) Series

The existence of such a series means that the group operation is

completely determined by the Lie bracket operation; the product of
exp(X) and exp(Y) can be expressed purely in term of repeated Lie
brackets of X and Y.

One important consequence of the Baker-Campbell-Hausdorff series is

the following correspondence between Lie algebras and matrix groups.

Theorem (The Lie correspondences theorem)

There is a natural one-to-one correspondences between sub-algebras of
gln (R) and path-connected subgroups of GLn (R).

54
 Manifolds: Matrix Groups are Manifolds

A manifold is a set which is locally diffeomorphic to Euclidean

space:

Definition
A subset X ⊂ Rm is called a manifold of dimension n if for all
p ∈ X there exists a neighborhood V of p in X which is
diffeomorphic to an open set U ⊂ Rn .

Theorem
Any matrix group of dimension n is a manifold of dimension n.

55
 Lie Groups

I Lie groups are the generalization of matrix groups.

I As such, all of the theory of matrix groups we discussed

are also true for Lie groups.
Definition
A Lie group is a manifold, G, with a smooth group operation
G × G → G.

56
 Lie Groups

I In other words, a Lie group is a manifold that is also a

group.

I Since matrix groups are (embedded) manifolds and have a

smooth group operation, so matrix groups are Lie groups.

I All important structure of matrix groups carry over to Lie

groups.

57
 Useful Lie Groups in Robotics

I Group of 3D rotation matrices, SO(3); it can model

rotations without any singularities or ambiguities.

I Group of direct spatial isometries (3D Rigid Body

Transformations), SE(3).

I Group of K direct isometries, SEK (3); for example, it is

used for modeling IMU sensors and robot pose plus
landmarks and/or contact points.

I Group of 3D similarity transformations, Sim(3); it is more

general than SE(3) and includes a scale factor and used in
monocular vision where the scale is not known.

58
 Group of 3D Rotation Matrices, SO(3)

Let φ ∈ R3 be a vector that can be identified with an element

of the Lie algebra, φ∧ ∈ so(3). The corresponding rotation
matrix, R ∈ SO(3), can be computed using the group’s
exponential map:
   
∧ sin θ ∧ 1 − cos θ
R = exp(φ ) = I3 + φ + (φ∧ )2 ∈ SO(3)
θ θ2

where θ , ||φ||.

The inverse operation maps the rotation matrix back to a

3 × 3 skew-symmetric matrix.
θ
φ∧ = log(R) = (R − RT ) ∈ so(3)
2 sin θ
 
−1 tr(R) − 1
where θ , cos
2
59
 Group of 3D Rotation Matrices, SO(3)

The matrix representation for the adjoint for SO(3) is simply

the rotation matrix itself.
AdR = R
The left Jacobian of SO(3) and its inverse can be calculated
using:
   
1 − cos θ ∧ θ − sin θ
Jl (φ) = I3 + φ + (φ∧ )2
θ2 θ3
 
−1 1 ∧ 1 1 + cos θ
Jl (φ) = I3 − φ + − (φ∧ )2
2 θ2 2θ sin θ

where θ , ||φ||.

60
 Group of Direct Spatial Isometries, SE(3)

Let ξ ∈ R6 be a vector that can be identified with an element

of the Lie algebra, ξ ∧ ∈ se(3). The corresponding pose,
H ∈ SE(3), can be computed using the exponential map:
 ∧  ∧   
∧ φ φ ρ R p
ξ = = and H =
ρ 01×3 0 01×3 1

exp(φ∧ ) Jl (φ)ρ
 
H = exp(ξ ∧ ) = ∈ SE(3),
01×3 1
where exp(φ∧ ) and Jl (φ) are the exponential map and the
left Jacobian of SO(3).

The logarithm maps the pose back to the tangent space.

log(R) J−1
 
∧ l (log(R)) p
ξ = log(H) = ∈ se(3)
01×3 0

61
 Group of Direct Spatial Isometries, SE(3)

The matrix representation for the adjoint of SE(3) is given by:

 
R 03×3
AdH = ∧ ∈ R6×6
p R R

The left Jacobian of SE(3) and its inverse are:

 
Jl (φ) 03×3
Jl (ξ) = ∈ R6×6
Q(φ,ρ) Jl (φ)
J−1
 
−1 (φ) 03×3
Jl (ξ) = l ∈ R6×6
−J−1l (φ)Q(φ,ρ)J −1
l (φ) J −1
l (φ)
1 ∧ θ − sin θ ∧ ∧
Q(φ,ρ) , ρ + (φ ρ + ρ∧ φ∧ + φ∧ ρ∧ φ∧ )
2 θ3
θ2
1− − cos θ
− 2
(φ∧ φ∧ ρ∧ + ρ∧ φ∧ φ∧ − 3φ∧ ρ∧ φ∧ )
θ4
 
θ2 3
1  1 − 2 − cos θ θ − sin θ − θ6
− −3  (φ∧ ρ∧ φ∧ φ∧ + φ∧ φ∧ ρ∧ φ∧ )
2 θ4 θ5

62
 Group of K Direct Isometries, SEK (3)
Let ξ ∈ R3(K+1) be a vector identified with an element of the
Lie algebra, ξ ∧ ∈ seK (3). The corresponding group element,
X ∈ SEK (3), is computed using the exponential map:
φ ∧
 ∧
ρ1 · · · ρK ···
    
φ R p1 pK
 ρ1  01×3 0 ··· 0  01×3 1 ··· 0 
∧
ξ = .  = . .. ..  and X =  .. .. .. 
     
. . . . ..
 .   . . . .   . . . . 
ρK 01×3 0 ··· 0 01×3 0 ··· 1
exp(φ∧ ) Jl (φ)ρ1 · · · Jl (φ)ρK
 
 01×3 1 ··· 0 
X = exp(ξ∧ ) =  .. .. ..  ∈ SEK (3),
 
..
 . . . . 
01×3 0 ··· 1

where exp(φ∧ ) and Jl (φ) are the exponential map and the
left Jacobian of SO(3).
J−1 J−1
 
log(R) l (log(R)) p1 ··· l (log(R)) pK
 01×3 0 ··· 0 
ξ∧ = log(H) =  .  ∈ seK (3)
 
.. .. ..
 .. . . . 
01×3 0 ··· 0

63
 Group of K Direct Spatial Isometries, SEK (3)

The matrix representation for the adjoint of SEK (3) is given

by:
 
R 03×3 · · · 03×3
 p1 ∧ R R · · · 03×3 
AdX = 
 ... .. .. ..  ∈ R3(K+1)×3(K+1)
. . . 
pK ∧ R 03×3 · · · R

The left Jacobian of SEK (3) is:

 
Jl (φ) 03×3 · · · 03×3
 Q(φ,ρ )
1Jl (φ) · · · 03×3  3(K+1)×3(K+1)
Jl (ξ) =  ..  ∈ R
 
.. .. ..
 . . . . 
Q(φ,ρK ) 03×3 · · · Jl (φ)

64
 First-Order Approximation using BCH

I The adjoint representation of a Lie group is a linear map that

captures the non-commutative structure of the group.

I The following properties from adjoint representation and BCH

formula for the first order approximation are useful.

X exp(ξ ∧ )X−1 = exp((AdX ξ)∧ )

=⇒ X exp(ξ ∧ ) = exp((AdX ξ)∧ )X

=⇒ exp(ξ ∧ )X = X exp((AdX−1 ξ)∧ )

I In the above equations X ∈ G and ξ ∧ ∈ g.

65
 First-Order Approximation using BCH

I The BCH formula can be used to compound two matrix

exponentials.

I If both terms are small, by keeping the first two terms ignoring
the higher order terms, we have:

BCH(ξ 1 ∧ , ξ 2 ∧ ) = ξ 1 ∧ + ξ 2 ∧ + HOT,
exp(ξ 1 ∧ ) exp(ξ 2 ∧ ) ≈ exp(ξ 1 ∧ + ξ 2 ∧ ).

66
 First-Order Approximation using BCH

I When both terms are not small and assuming ξ is small, by

keeping the linear terms in ξ, we have:
log(exp(r ∧ ) exp(ξ ∧ ))∨ ≈ r + J−1
r (r)ξ,

log(exp(ξ ∧ ) exp(r ∧ ))∨ ≈ r + J−1

l (r)ξ.
where Jr and Jl are the right and left Jacobians of the Lie
group G, respectively.

I The left and right Jacobians are related through the adjoint
map,

Jr (ξ) = Adexp(−ξ ∧ ) Jl (ξ).

67
 Example: Uncertainty Propagation on Matrix Lie Groups
I Suppose a process model on matrix Lie group G where the state at
any two successive keyframes at times-steps k + 1 and k is related
using input such as Uk ∈ G. The deterministic process model is as
follows.
Xk+1 = fui (Xk ) , Xk Uk

I Substituting in the noisy process model we have

Xk+1 = fui (Xk ) exp(wk ∧ )

where wk ∼ N (0,Σwk ).

I Notice that the noise is defined in the Lie algebra and mapped to the
nonlinear error using the Lie exponential map (matrix exponential).

I wk ∈ Rn is defined in the Euclidean vector space that is isomorphic

to the Lie algebra where n is the dimension of the Lie algebra.
68
 Example: Uncertainty Propagation on Matrix Lie Groups

I We use the left invariant error to track the covariance of the spatial
error as seen in the body-fixed frame: η = exp(ξ ∧ ) = X−1 X̄.

I Then, the state X is of the following form where X̄ is the mean or

estimated value of the true state: X = X̄ exp(−ξ ∧ ).

I We substitute X = X̄ exp(−ξ ∧ ) in the noisy process model as

follows:
X̄k+1 exp(−ξ k+1 ∧ ) = X̄k exp(−ξ k ∧ )Uk exp(wk ∧ ),

I and using the adjoint to shift all noise terms to the right, we get:
∧
X̄k+1 exp(−ξ k+1 ∧ ) = X̄k Uk exp((−AdU−1 ξ k ) ) exp(wk ∧ )
k

69
 Example: Uncertainty Propagation on Matrix Lie Groups

I This previous equation shows the relation between noise terms at

timesteps k and k + 1. Thus,
∧
exp(−ξ k+1 ∧ ) = exp((−AdU−1 ξ k ) ) exp(wk ∧ ).
k

I Since all noise terms are small, after applying the BCH formula and
keeping the first two terms we arrive at the approximate error
dynamics:

ξ k+1 = AdU−1 ξ k − wk .
k

I From here, one can easily show that

Σk+1 = AdU−1 Σk AdT

U−1
+ Σwk .
k k

70
 Example: Uncertainty Propagation on SE(2)

See odometry propagation se2.m for code.

71
 Example: Uncertainty Propagation on SE(3)

See odometry propagation se3.m for code.

72