Lecture 1 Rigid Body Transformations

Lecture 1 of the Visual Perception module of the Erasmus Mundus MSc in Vision and Robotics (VIBOT)

1. Lecture1
RigidBodyTransformations
Joaquim Salvi
Universitat de Girona
Visual Perception

2. 2
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

3. 3
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

4. 4
Lecture 1: Rigid Body Transformations
Standard base vectors:
1.1 Cartesian coordinates, Points and Vectors
Coordinate System:Complete set of orthonormal vectors
(perpendicular and unit) and coinciding in a point (origin).
Reference System:Unique/World coordinate system used for
referencing points, vectors and other coordinate systems
right-hand frame
{A}A
xˆ A
yˆ A
zˆ 1
ˆ
0
0
A
X





 0
ˆ
1
0
A
Y





 0
ˆ
0
1
A
Z






O

5. 5
Lecture 1: Rigid Body Transformations
Coordinatesof a point in space:zpzpc
ypypb
xpxpa
zcybxa
c
b
a
p
T
T
T















ˆ·ˆ
ˆ·ˆ
ˆˆ
ˆˆˆˆ
{A}A
xˆ A
yˆ A
zˆ
a
b
cp
A
ˆ u
A
pˆ
Referenced with respect to {A}
Corresponding to object/reference u
1.1 Cartesian coordinates, Points and Vectors

6. 6
Lecture 1: Rigid Body Transformations
A “free” vectoris defined by a pair
of points :
Coordinates of the vector :
{A}A
xˆ A
yˆ A
zˆ
Op
A
ˆ q
A
ˆ v
A
ˆ v
A
ˆ 1
2
3
ˆ
A
p
pp
p





 1
2
3
ˆ
A
q
qq
q





 1 1 1
2 2 2
3 3 3
ˆ
A
v q p
v v q p
v q p
   
   
  
   
   
   
1.1 Cartesian coordinates, Points and Vectors

7. 7
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

8. 8
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

9. 9
Lecture 1: Rigid Body Transformations
Inner productbetween two vectors:
1.2 Inner product and Cross product

10. 10
Lecture 1: Rigid Body Transformations
Antisymmetric matrix
1.2 Inner product and Cross product
Cross productbetween two vectors:

11. 11
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

12. 12
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

13. 13
Lecture 1: Rigid Body Transformations
•{B} is a translation of {A} to position
A
(3 4 3)
T
.
•Coodinate axes of {B} are paralel to coordinate axes of {A}.
•The translation can be representated by vectorial addition.
























































































5
7
4
3
4
3
2
3
1
;
AAB
z
A
y
A
x
A
z
A
y
A
x
A
z
B
y
B
x
B
z
A
y
A
x
A
z
B
y
B
x
B
z
A
y
A
x
A
p
p
p
o
o
o
p
p
p
o
o
o
p
p
p
p
p
p
B
p=(1 3 2)
T
A
p=(4 7 5)
T
A
o
B=(3 4 3)
T
{A}A
zˆ B
zˆ A
yˆ B
yˆ A
xˆ B
xˆ
{B}
Translation vector:
1.3 Translations

14. 14
Lecture 1: Rigid Body Transformations
Coordinates are related by:p )oT( =p
B
B
AA B
A
B
z
A
y
A
x
A
A
B
A
tI
o
o
o
oT
























10
1000
100
010
001
)( B
ABA
o p= p 
Translation matrix:
1.3 Translations

15. 15
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

16. 16
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

17. 17
Lecture 1: Rigid Body Transformations
Let {A} and {B} two ortonormal coordinate systems with
the same origin and unit vectors {a
1, a
2, a
3} {b
1, b
2, b
3}.
b
1
a
1
a
2
a
3
b
2
b
3 
 
332211
332211
321
B
321
A
ˆ
·
ˆ
·
ˆ
·ˆ
ˆ·ˆ·ˆ·ˆ
ˆ
ˆ
{B} and {A}in ˆby vector drepresente is Point
bpbpbpp
apapapp
pppp
pppp
pp
ABABABA
AAAAAAA
T
BBB
T
AAA








  
3332321313
3232221212
3132121111
3
1
3
1
3
1
···3
···2
···1
ˆ·
ˆ
ˆ)
ˆ
(ˆˆ
Consider
prprprpk
prprprpk
prprprpk
r·pab·pab·pa·pp
p
BBBA
BBBA
BBBA
kj
j
j
B
j
k
A
j
A
j
B
k
A
j
A
j
B
j
k
AA
k
A
k
A









 
 




































3
2
1
332313
322212
312111
3
2
1
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
p
p
p
bababa
bababa
bababa
p
p
p
B
B
B
B
A
A
A
A
Rotation matrix:
1.4 Rotations

18. 18
Lecture 1: Rigid Body Transformations
Rotation matrix:















332313
322212
312111
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
bababa
bababa
bababa
R
B
A
b
1
a
1
a
2
a
3
b
2
b
31
ˆ
b
A 2
ˆ
b
A 3
ˆ
b
A 















332313
322212
312111
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
bababa
bababa
bababa
R
B
A 1
ˆa
B 2
ˆa
B 3
ˆa
B 
A
B
T
B
A
RR , det( ) 1, rank( ) 3
T
R R I R R  
1.4 Rotations

19. 19
Lecture 1: Rigid Body Transformations
Basic Rotation matrices:
cossin0
sincos0
001
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
332313
322212
312111




























B
A
bababa
bababa
bababa 




























cos0sin
010
sin0cos
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
332313
322212
312111
B
A
bababa
bababa
bababa
Rot(

,
x
)
Rot(

,
y)
Y
X
Z
Z’
X’



Y
X
Z
Y’
Z’
Rot(

,
z
)
Y
X
Z
Z’
X’
Y’








 
















100
0cossin
0sincos
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
332313
322212
312111


B
A
bababa
bababa
bababa
1.4 Rotations

20. 20
Lecture 1: Rigid Body Transformations
Coordinates are related by: p R =p
B
B
AA
Rotation matrix:p )RT( =p
B
B
AA B
A
B
A
B
A
R
rrr
rrr
rrr
RT
























10
0
1000
0
0
0
)(
333231
232221
131211
1.4 Rotations

21. 21
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

22. 22
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

23. 23
Lecture 1: Rigid Body Transformations
Coordinates are related by: p R =p
B
B
AA
Rotation matrix:p )RT( =p
B
B
AA B
A
B
A
B
A
R
rrr
rrr
rrr
RT
























10
0
1000
0
0
0
)(
333231
232221
131211
1.5 Homogeneous coordinates
Coordinates are related by:p )oT( =p
B
B
AA B
A
B
z
A
y
A
x
A
A
B
A
tI
o
o
o
oT
























10
1000
100
010
001
)( B
ABA
o p= p 
Translation matrix:

24. 24
Lecture 1: Rigid Body Transformations













1
p
p
p
=p
z
A
y
A
x
A
A
Point: Vector:













0
v
v
v
=v
z
A
y
A
x
A
A
Composed matrix: p T =p
B
B
AA tR
tR
orrr
orrr
orrr
T
B
A
B
z
A
y
A
x
A
A
B
A

























10
1000
333231
232221
131211
1.5 Homogeneous coordinates

25. 25
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

26. 26
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

27. 27
Lecture 1: Rigid Body Transformations
T=Identity
Traslations and rotations considered as
separated matrices
Trasl./Rot. of
{B}?
Pre-
multiplication
Post-
multiplication
End
More?
({A})fixed and ({B})mobiles.
Mobile Axis
{B}
Fixed Axis
{A}
yes
no
Algorithm
1.6 Composition of transformations

28. 28
Lecture 1: Rigid Body Transformations
Example 1IT
y
A
z
A
x
B
y
B
z
B
A
o
x
A x
1
y
A y
1
z
A z
1
^
^
^
^
^
^
^
^^
^
^
1.6 Composition of transformations

29. 29
Lecture 1: Rigid Body Transformations
x
A
y
A
z
A
x
B
y
B
z
B
A
o
^
^
^
^
^
^IT IoTransT
oTransIT
A
A
)·(
o
)(·


x
A x
1
y
A y
1
z
A z
1
z
A
x
A
y
A
y
2
z
2
x
2
A
o
^
^
^
^
^^
^
^
^
^
1.6 Composition of transformations

30. 30
Lecture 1: Rigid Body Transformations
x
A
y
A
z
A
x
B
y
B
z
B
A
o
^
^
^
^
^
^IT IoTransT
oTransIT
A
A
)·(
o
)(·

 )90,ˆ(·)·(
o
)90,ˆ()·(·
2
2
yRotIoTransT
yRotoTransIT
A
A


x
A x
1
y
A y
1
z
A z
1
z
A
x
A
y
A
y
2
z
2
x
2
A
o
z
A
x
A
y
A
z
B
y
B
^^
^
^
^
^
^
^
^
^
^
^^
^
^
^
^
^
x
B
1.6 Composition of transformations

31. 31
Lecture 1: Rigid Body Transformations
x
A
y
A
z
A
x
B
y
B
z
B
A
o
^
^
^
^
^
^ 
B
A
TA
z
A
y
A
x
A
z
A
y
A
x
A
A
T
o
o
o
o
o
o
o
T
yRotIoTransT


















































































1000
3001
3010
3100
1333
90
1000
cos0sin
010
sin0cos
1000
0cos0sin
0010
0sin0cos
·
·
1000
0100
0010
0001
·
1000
100
010
001
)90,ˆ(·)·(
2





1.6 Composition of transformations

32. 32
Lecture 1: Rigid Body Transformations
x
A
y
A
z
A
x
B
y
B
z
B
A
o
^
^
^
^
^
^
Be careful !!!
The result is different if
the rotation is around the
fixed axis (and not around
the mobile axis)IoTransT
oTransIT
A
A
)·(
o
)(·


z
A
x
A
y
A
y
2
z
2
x
2
A
o
^
^
^
^
^
1.6 Composition of transformations

33. 33
Lecture 1: Rigid Body Transformations
x
A
y
A
z
A
x
B
y
B
z
B
A
o
^
^
^
^
^
^
Be careful !!!
The result is different if
the rotation is about the
fixed axis (and not about
the mobile axis)IoTransT
oTransIT
A
A
)·(
o
)(·


z
A
x
A
y
A
y
2
z
2
x
2
A
o
^
^
^
^
^
z
A
y
A
x
B
z
B
y
B
^
^
^
^IoTransyRotT
oTransIyRotT
A
A
)·()·90,ˆ(
o
)(·)·90,ˆ(
2
2


x
A
^
Example 2
1.6 Composition of transformations

34. 34
Lecture 1: Rigid Body Transformations
x
A
y
A
z
A
x
B
y
B
z
B
A
o
^
^
^
^
^
^
Be careful !!!
The result is different if
the rotation is about the
fixed axis (and not about
the mobile axis) 
B
A
TA
z
A
x
A
y
A
z
A
x
A
z
A
y
A
x
A
A
T
o
oo
o
oo
o
o
o
T
IoTransyRotT



















































































1000
3001
3010
3100
1333
90
1000
·cos·sincos0sin
010
·sin·cossin0cos
1000
0100
0010
0001
·
1000
100
010
001
·
1000
0cos0sin
0010
0sin0cos
)·()·90,ˆ(
2





1.6 Composition of transformations

35. 35
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

36. 36
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

37. 37
Lecture 1: Rigid Body Transformations
Homogeneous Inverse Transformation






1000
pR
T
Example:


































1000
2100
0001
2010
1000
2100
2001
0010
1
TT
































-2
0
-2
2
2
0
-100
001
0-10







 


1000
1 pRR
T
TT
 pR
T
=
1.7 Inverse transformation

38. 38
Lecture 1: Rigid Body Transformations
The homogeneous transformation that maps {B} with respect to {A}, is
A
T
B. What are the coordinates of the point
A
p=(0,1,0) with respect the
coordinate system {B} ?
A
p = T
B
p
T
-1
·
A
p = T
-1
·T
B
p
T
-1
·
A
p =
B
p

































1000
2100
0001
2010
1000
2100
2001
0010
1
TT
Then,
































1
2
0
1
1
0
1
0
1
T
A B
{A}
{B}B
xˆ A
yˆ B
yˆ A
zˆ B
zˆ A
xˆ
p
1.7 Inverse transformation

39. 39
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

40. 40
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

41. 41
Lecture 1: Rigid Body Transformations
RPY angles: Roll, Pitch and yaw 






























 

33
33
22
22
11
11
321
cossin0
sincos0
001
cos0sin
010
sin0cos
100
0cossin
0sincos
),(),(),(






 XRYRZR
AAA
Euler angles: ZYZ, … 








 




















 

100
0cossin
0sincos
cos0sin
010
sin0cos
100
0cossin
0sincos
),(),(),(
33
33
22
22
11
11
321






 ZRYRZR
BBB
1.8 Parametrization of a Rotation matrix

42. 42
Lecture 1: Rigid Body Transformations
RPY angles:



































 
































 

32322
313213132121
321313132121
32322
33
32322
11
11
33
33
22
22
11
11
321
coscossincossin
sincoscossinsincoscossinsinsincossin
cossincossinsincossinsinsincoscoscos
coscossincossin
sincos0
cossinsinsincos
100
0cossin
0sincos
cossin0
sincos0
001
cos0sin
010
sin0cos
100
0cossin
0sincos
),(),(),(














 XRYRZR
AAA
1.8 Parametrization of a Rotation matrix

43. 43
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

44. 44
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

45. 45
Lecture 1: Rigid Body Transformations























32322
313213132121
321313132121
333231
232221
131211
coscossincossin
sincoscossinsincoscossinsinsincossin
cossincossinsincossinsinsincoscoscos



rrr
rrr
rrr
Given a Rotation Matrix R, we decide the parametrization )/atan(
)cos/asin(
)asin(
11121
2323
312
rr
r
r






And we can extract the angles from that parametrization
1.9 Extracting the rotation angles of a rotation matrix

46. 46
Lecture 1: Rigid Body Transformations
cos() = cos(-)
sin() = sin(+ p/2)
atan()=atan(+p)
Angles are defined partially
Angles in the (0,2p) rang are desired























32322
313213132121
321313132121
333231
232221
131211
coscossincossin
sincoscossinsincoscossinsinsincossin
cossincossinsincossinsinsincoscoscos



rrr
rrr
rrr
Given a Rotation Matrix R, we decide the parametrization )/atan(
)cos/asin(
)asin(
11121
2323
312
rr
r
r






And we can extract the angles from that parametrization
1.9 Extracting the rotation angles of a rotation matrix

47. 47
Lecture 1: Rigid Body Transformations
atan2(s,c)
•Problem of tan
-1
:  
2121
1
tantan/
2
tan
2

pp


t
III
III IV
s
c
s<0
c>0
s<0
c>0
s>0
c>0
s>0
c<0
s<0
c<01
 2

Solution:












































p
p
)·sgn(tan),(atan2
,
0
2
)·sgn(),(atan2
,
0
tan),(atan2
,
0
,atan2
1
1
s
c
s
cs
IIIQIIQ
c
scs
IVQIQ
c
c
s
cs
IVQIQ
c
cs 







c
s
cs
1
tan),(atan2 







c
s
cs
1
tan),(atan2 p







c
s
cs
1
tan),(atan2 p







c
s
cs
1
tan),(atan2
1.9 Extracting the rotation angles of a rotation matrix

48. 48
Lecture 1: Rigid Body Transformations)sin/,atan2(-
),atan2(
),atan2(
332312
11211
33323



rr
rr
rr



X























32322
313213132121
321313132121
333231
232221
131211
coscossincossin
sincoscossinsincoscossinsinsincossin
cossincossinsincossinsinsincoscoscos



rrr
rrr
rrr
Given a Rotation Matrix R, we decide the parametrization )/atan(
)cos/asin(
)asin(
11121
2323
312
rr
r
r






And we can extract the angles from that parametrization
1.9 Extracting the rotation angles of a rotation matrix

49. 49
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

50. 50
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix

51. 51
Lecture 1: Rigid Body Transformations
Any mxn matrix M can be expressed in terms of its Singular Value
Decomposition as:
where:
U is an nxn rotation matrix,
V is an mxm rotation matrix, and
D is an mxn diagonal matrix (i.e off-diagonals are all 0).TT
UDVMSVDUDVM  )(;
1.10 Computing the closest rotation matrix

52. 52
Lecture 1: Rigid Body Transformations
M is 3x3, the inverse of M is,
where:
U is an 3x3 rotation matrix,
V is an 3x3rotation matrix, and
D is an 3x3 diagonal matrix (i.e off-diagonals are all 0).
M is a Rotation Matrix if D is a Rotation matrix, i.e. its elements are unitarian.  
TT
UVDUDVUDVM
111
1 

 T
VUMSVD











3
2
1
00
00
00
)(


 T
VUR











)sgn(00
0)sgn(0
00)sgn(
3
2
1



sgn(x)=1 if x>0; sgn(x)=-1 if x<0











3
2
1
1
/100
0/10
00/1



D 










3
2
1
00
00
00



D
1.10 Computing the closest rotation matrix

53. 53
Lecture 1: Rigid Body Transformations
Contents
1. Rigid Body Transformations
1.1 Cartesian coordinates, Points and Vectors.
1.2 Inner product and Cross product
1.3 Translations
1.4 Rotations
1.5 Homogeneous coordinates
1.6 Composition of transformations
1.7 Inverse transformation
1.8 Parametrization of a Rotation matrix
1.9 Extracting the rotation angles of a Rotation matrix
1.10 Computing the closest rotation matrix of a noisy rotation
matrix