16.

485: Visual Navigation for Autonomous Vehicles (VNAV) Fall 2021

Lecture 4: Lie Groups

Lecturer: Luca Carlone Scribes: -

Disclaimer: These notes have not been subjected to the usual scrutiny reserved for formal publications.
They may be distributed outside this class only with the permission of the Instructor(s).

This lecture reviews geometric concepts introduced in Lectures 2-3 in terms of differential geometry and Lie
Groups. In particular, we will cover:

• basic concepts about Lie Groups;

• Exponential and Logarithm maps;

• distances between group elements.

Why do we need to review the concepts from Lecture 2 in a group-theoretic perspective? Because the Lie
group perspective allows for a unified treatment of rotations and poses (e.g., useful later on when we discuss
optimization on-manifold), and to introduce the notion of “distance” between poses and between rotations
in a more formal manner.
An introductory reference to this topic is [1, p. 205-256]. A great reference on Lie group theory for robotics
is [8] and the corresponding presentation: https://youtu.be/QR1p0Rabuww. A more extensive discussion
on distances and metrics for 3D rotations can be found in [6, 7].

4.1 Groups and Lie Groups

A group G is a (finite or infinite) set of elements together with a binary group operation ⊗ that satisfies the
following conditions:

• closure: for any A, B ∈ G, it holds A ⊗ B ∈ G;

• associativity: for any A, B, C ∈ G, it holds (A ⊗ B) ⊗ C = A ⊗ (B ⊗ C);

• identity element: there exists an identity element I ∈ G such that A ⊗ I = I ⊗ A = A for any A ∈ G;

• inverse: for any A ∈ G there exist an inverse element A−1 such that A ⊗ A−1 = A−1 ⊗ A = I.

Note that the notion of vector space is much stronger than the notion of group, since a vector space has
two operations (addition and multiplication by scalar) and satisfies a large set of axioms (associativity of
addition, commutativity of addition, identity element of addition, inverse element of addition, and other
properties regarding the scalar product).

Example 4.1.1 (General Linear Group GL(d, R)). Set of invertible Rd×d matrix with matrix multiplication
as group operation.

The following are groups of interest for VNAV, all using the matrix multiplication as group operation.

4-1
4-2 Lecture 4: Lie Groups

. 
Example 4.1.2 (Orthogonal Group O(d)). The group of orthogonal matrices O(d) = R ∈ Rd×d : RT R = Id .
Note: an orthogonal matrix can only have determinant +1 or −1.
Example 4.1.3 (Special Orthogonal Group SO(d)). The group of rotation matrices - it is the subset of d×d
. 
matrices defined as SO(d) = R ∈ Rd×d : RT R = Id , det(R) = +1 . This is sometimes called the group
of proper rotations. Note that SO(d) ⊂ O(d) since O(d) also includes orthogonal matrices with determinant
−1 that represent improper rotations or reflections (also: left-handed coordinate frames).
Example 4.1.4 (Special Euclidean Group SE(d)). The group of (d + 1) × (d + 1) matrices representing rigid
transformations, i.e., poses expressed in homogeneous coordinates.

In general, all the groups above are non-abelian (i.e., the group operation is not commutative).

Figure 4.1: 2-dimensional manifold embedded in 3D space and tangent space at x.

Manifold. An d-dimensional manifold M is a topological space where the neighborhood of every point
x ∈ M is homeomorphic to Rd . Informally: a low-dimensional surface embedded in a higher-dimensional
space which looks “flat” at any point.
Tangent space: A d-dimensional manifold M embedded has a tangent space for every point x ∈ M. The
tangent space at x is denoted as Tx M and has dimension d.
Chart & Atlas: invertible map between a subset of a d-dimensional topological manifold and a subset of the
Euclidean space Rd . A chart is a local “map” and the collection of charts covering the entire manifold is
called an atlas.
Differentiable manifolds: a manifold where we can move from one chart to another using a differentiable
function (the transition map). If the transition map is smooth (infinitely differentiable), the manifold is
called a smooth manifold.
Riemannian manifold: A smooth manifold equipped with a notion of distance (the Riemannian distance)
given by a smooth positive definite symmetric bilinear metric defined on the tangent space at each point.
Riemannian manifolds allow generalizing the intuitive notion of Euclidean distance to “curved surfaces”:
while the minimum-distance path between two points in Euclidean space is a straight line, the minimum-
distance path on a Riemannian manifold is a geodesic, i.e., the shortest continuous curve connecting the two
points on the manifold.
Lie Groups. A group is said to be a Lie group embedded in RN if:

• G is a manifold in RN ;
• the group operations (composition and inverse) are smooth (infinitely differentiable).
Example 4.1.5 (Rotations and poses). SO(d), O(d), SE(d) are matrix Lie groups.
Lecture 4: Lie Groups 4-3

4.2 Lie algebras

Every matrix Lie group is associated with a Lie algebra, which consists of a vector space, called the tangent
space, and a binary operation called the Lie Bracket. For the moment, we will not worry about defining
the Lie Bracket, but we focus on the tangent space. The interested reader can find the definition of the Lie
Bracket operation for different matrix groups in [1, Section 6].
Example 4.2.1 (Lie algebra of SO(3)). The vector space corresponding to the Lie algebra of SO(3) is:
  
 0 −φ3 φ2 
so(3) =  φ3 0 −φ1  : φ = [φ1 φ2 φ3 ]T ∈ R3 (4.1)
−φ2 φ1 0
 

which corresponds to the set of skew-symmetric matrix in R3×3 .

For notational convenience we define the hat (·)∧ and the vee (·)∨ operators as follows:
   ∨
0 −φ3 φ2 0 −φ3 φ2
. .
(φ)∧ =  φ3 0 −φ1  and  φ3 0 −φ1  = φ (4.2)
−φ2 φ1 0 −φ2 φ1 0

Example 4.2.2 (Lie algebra of SE(3)). The vector space corresponding to the Lie algebra of SE(3) is:
 ∧  
φ ρ 3
se(3) = : ρ, φ ∈ R (4.3)
0T3 0

For notational convenience we “overload” the hat (·)∧ and the vee (·)∨ operators to work on vectors ξ ∈ R6
as vectors:  ∧  ∧   ∧ ∨  
φ . φ ρ φ ρ . φ
ξ∧ = = and = ξ = (4.4)
ρ 0T
3 0 0T
3 0 ρ
The matrix ξ ∧ is also called a “screw” matrix [9].

4.3 Exponential and Logarithm map

The exponential map and the logarithm map relate elements of a matrix Lie group with elements in the
corresponding Lie algebra. In particular, the exponential map produces a matrix Lie group element G from
.
a Lie algebra element A = a∧ via a matrix exponential:
∞
X 1 n
G = exp(A) = A (4.5)
n=0
n!

Similarly, the logarithm map produces a Lie algebra element A from a matrix Lie group element G via a
matrix logarithm:
∞
X (−1)n−1
A = log(G) = (G − I)n (4.6)
n=1
n

Example 4.3.1 (Exponential and Logarithm maps for SO(3)). Any element of so(3) is a 3×3 skew symmetric
matrix, and this allows simplifying the expression of the exponential map for SO(3), which can be written
in closed-form as follows:
    T
∧ φ φ φ
R = exp(φ ) = cos(kφk)I3 + sin(kφk) + (1 − cos(kφk)) (4.7)
kφk × kφk kφk
4-4 Lecture 4: Lie Groups

Now we observe that the expression above resembles the Rodrigues’ rotation formula. Indeed, if we interpret
. . φ
θ = kφk as a rotation angle and u = kφk as a rotation axis (a unit vector) the expression above is identical
to the Rodrigues’ rotation formula, suggesting that vectors in the tangent space are in the form

φ = θu (4.8)

In hindsight, we found a new rotation parametrization. We will refer to φ as the exponential coordinates of
the rotation. The vector φ is also called a rotation vector or Euler vector.
The logmap for SO(3) can be simply computed as (see, e.g., https://en.wikipedia.org/wiki/Axis%E2%
80%93angle_representation):

0 if R = I3
log(R) = θ (4.9)
2 sin θ [R − RT ] otherwise

where θ is the rotation angle.

Example 4.3.2 (Exponential and Logarithm maps for SE(3)). Using the special structure of the 4 × 4 skew
matrix in se(3), we can simplify the expression of the exponential map for SE(3), which can be written in
closed-form as follows:
∧ ! 
exp(φ∧ ) Jl (φ)ρ
 
∧ φ
T = exp(ξ ) = exp = (4.10)
ρ 0T
3 1

where Jl (φ) is called the left-jacobian (for SO(3)) and has the following expression:

. 1 − cos(kφk) ∧ kφk − sin(kφk) ∧ ∧

Jl (φ) = I3 + φ + φ φ (4.11)
kφk2 kφk3

The logarithm map can be also computed in closed-form:

 ∧    ∧
∧ φ R t φ
ξ = = log = (4.12)
ρ 0T
3 1 Jl−1 (φ)t

where φ = log(R)∨ . Again, the inverse of the left Jacobian can be expressed in closed-form as:
 
. 1 1 1 + cos(kφk)
Jl−1 (φ) = I3 − φ∧ + 2
− φ∧ φ∧ (4.13)
2 kφk 2kφk sin(kφk)

Derivations for these matrices (and more) are given around page 40 of [4].

The exponential maps for SO(3) and SE(3) are surjective-only in the sense that there exist many Lie algebra
elements that produce the same Lie group element.

4.4 Distances

In many applications, one needs to quantify how “different” two rotations or poses are. For this purpose, we
need the notion of distance.
Lecture 4: Lie Groups 4-5

4.4.1 Distances between rotations

There are multiple possible definitions of what a distance between two rotations is. The choice of distance
is often dictated by analytical and computational convenience. An excellent review of these metrics is given
in [6, 7]. We review these distance metrics below.
Before delving into details, we recall that a “metric” (or distance) dist(a, b) between two generic elements
“a” and “b” satisfies the following properties:

dist(a, b) ≥ 0 (non-negativity) (4.14)

dist(a, b) = 0 ⇐⇒ a = b (identity) (4.15)
dist(a, b) = dist(b, a) (symmetry) (4.16)
dist(a, c) ≤ dist(a, b) + dist(b, c) (triangle inequality) (4.17)

4.4.2 Angular (or geodesic) distance in SO(3)

A fairly intuitive metric for the distance between two rotations RA ∈ SO(3) and RB ∈ SO(3) can be
T
obtained by (i) computing the relative rotation RAB = RA RB and (ii) computing the rotation angle θAB of
the rotation RAB (as in the axis-angle representation), (iii) taking the absolute value of the rotation angle
(unless we restrict θ ∈ [0, π) . . . ). Intuitively, this is the rotation angle (around some axis) that can align
RA to RB . Such a metric, is called the angular distance, and, using the formula to compute the rotation
angle from a rotation matrix we saw in the previous lecture, we write this metric as:

!
T
tr RA RB − 1
distθ (RA , RB ) = arccos (4.18)
2

Recalling that the norm of the exponential coordinates is the rotation angle, the previous metric can be
written as:
T
distθ (RA , RB ) = k log(RA RB )∨ k = k log(RB
T
RA )∨ k (4.19)

It can be shown that this distance is a geodesic distance, i.e., it is the length of the minimum path between
RA and RB on the manifold SO(3).
Bi-invariance: for 3 rotations RA , RB , RC :

distθ (RA , RB ) = distθ (RC RA , RC RB ) = distθ (RA RC , RB RC ) (4.20)

4.4.3 Chordal distance in SO(3)

While the angular distance is a geodesic distance, in some applications it is more convenient to use a simpler
expression for the distance, which often makes computation and analysis easier. Therefore we define the
chordal distance between two rotations RA ∈ SO(3) and RB ∈ SO(3) as:

distc (RA , RB ) = kRA − RB kF = kRB − RA kF (4.21)

where k · kF is the Frobenius norm, which for a matrix M is defined as

sX sX sX q
kM kF = Mij2 = kvec(M )k = kMi,: k2 = kM:,j k2 = tr (M M T ) (4.22)
ij i j
4-6 Lecture 4: Lie Groups

Figure 4.2: (a) Angular and (b) chordal distance in SO(2).

Fig. 4.2 sheds some light on the nature of the chordal distance.
We can also relate the chordal distance to the angular distance for 3D rotations as follows:
q

distc (RA , RB ) = kRA − RB kF = kRB − RA kF = tr (RB − RA )(RB T − RT ) (4.23)
A
q
p p
TR
tr (2I) − 2tr RA 6 − 2(1 + 2 cos(θ)) = 2 1 − cos(θ).
B = (4.24)

Now recalling that sin2 (θ/2) = 21 (1 − cos(θ)):

p q √
distc (RA , RB ) = 2 1 − cos(θ) = 2 2 sin2 (θ/2) = 2 2| sin(θ/2)| (4.25)

which also holds for the 2D case [2] (which considers squared distances). Note that for small angles θ,
sin(θ/2) ≈ θ/2 and: √
distc (RA , RB ) ≈ 2 distθ (RA , RB ) (4.26)

Bi-invariance: for 3 rotations RA , RB , RC :

distc (RA , RB ) = distc (RC RA , RC RB ) = distc (RA RC , RB RC ) (4.27)

4.4.4 Quaternion distance

When the two rotations are given as unit quaternions qA and qB one can compute the distance between
them using the quaternion distance:

distq (qA , qB ) = kqA − qB k = kqB − qA k (4.28)

This distance has been used in several works in the literature. Unfortunately, the distance above has a
number of shortcomings. For instance, we know that qB and −qB represent the same rotation, however in
general:
distq (qA , qB ) 6= distq (qA , −qB ) (4.29)

This problem can be alleviated by redefining the quaternion distance as follows [3]:

distq (qA , qB ) = min kqA − b qB k (4.30)

b∈{−1;+1}

which however has the drawback of including a binary variable, which typically makes computation and
analysis trickier. The metric in (4.30) is a pseudo-metric since it does not satisfy the “identity” property
in (4.15), see [7] for further discussion.
Lecture 4: Lie Groups 4-7

4.4.5 Distances between poses

An excellent survey and discussion about distances in SE(3) is given in [5]. We remark that the metrics
below “mix” angular quantities (radians) with Euclidean quantities (meters), hence typically there is some
weighting factor that makes the quantities dimensionless.

4.4.6 Double Geodesic distance in SE(3)

. .
Given two poses TA = (RA , tA ) and TB = (RB , tB ) a straightforward way to generalize (4.19) to SE(3) is:

distg (TA , TB ) = k log(TA−1 TB )∨ k = k log(TB−1 TA )∨ k (4.31)

In the robotics literature, however, authors prefer to use the double geodesic distance between the two poses,
which is defined as:
q p
distdg (TA , TB ) = k log(RA T R )∨ k2 + kt − t k2 = distθ (RA , RB )2 + ktB − tA k2 (4.32)
B B A

which simply treats SE(3) as the Cartesian product of SO(3) and R3 .

Left-invariance: for 3 poses TA , TB , TC :

distg (TA , TB ) = distg (TC TA , TC TB ) (4.33)

distdg (TA , TB ) = distdg (TC TA , TC TB ) (4.34)

4.4.7 Chordal distance in SE(3)

We define the chordal distance between two poses (in homogeneous coordinates) TA ∈ SE(3) and TB ∈ SE(3)
as:
distc (TA , TB ) = kTA − TB kF = kTB − TA kF (4.35)
It is easy to prove by inspection that:
p
distc (TA , TB ) = distc (RA , RB )2 + ktB − tA k2 (4.36)

Left-invariance: for 3 poses TA , TB , TC :

distc (TA , TB ) = distc (TC TA , TC TB ) (4.37)

References
[1] T. Barfoot. State Estimation for Robotics. Cambridge University Press, 2017.

[2] L. Carlone and F. Dellaert. Duality-based verification techniques for 2D SLAM. In IEEE Intl. Conf. on
Robotics and Automation (ICRA), pages 4589–4596, 2015. (pdf) (code).

[3] L. Carlone, R. Tron, K. Daniilidis, and F. Dellaert. Initialization techniques for 3D SLAM: a survey
on rotation estimation and its use in pose graph optimization. In IEEE Intl. Conf. on Robotics and
Automation (ICRA), pages 4597–4604, 2015. (pdf) (code) (supplemental material: (pdf)).
4-8 Lecture 4: Lie Groups

[4] G. S. Chirikjian. Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods
and Modern Applications (Applied and Numerical Harmonic Analysis). Birkhauser, 2012.
[5] G.S. Chirikjian. Partial bi-invariance of SE(3) metrics. J. Comput. Inf. Sci. Eng, 15(1):185–200, 2015.
[6] R. Hartley, J. Trumpf, Y. Dai, and H. Li. Rotation averaging. IJCV, 103(3):267–305, 2013.
[7] Du Huynh. Metrics for 3d rotations: Comparison and analysis. Journal of Mathematical Imaging and
Vision, 35:155–164, 10 2009.
[8] J. Solà, J. Deray, and D. Atchuthan. A micro Lie theory for state estimation in robotics. arXiv:
1812.01537, abs/1812.01537, 2018.
[9] Y. Wang and G.S. Chirikjian. Error propagation on the euclidean group with applications to manipulator
kinematics. IEEE Trans. Robotics, 22(4):591–602, 2006.