Download pdf - Lecture 1 Rigid Body Transformations

Lecture 1Rigid Body Transformations

Joaquim SalviUniversitat de Girona

Visual Perception

http://www.vibot.org/

http://www.vibot.org/

http://europa.eu.int/comm/education/programmes/mundus/index_en.html

http://europa.eu.int/comm/education/programmes/mundus/index_en.html

2

Lecture 1: Rigid Body Transformations

Contents

1. Rigid Body Transformations1.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


Contents



1.3 Translations

1.4 Rotations







4


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}

Ax

Ay

Az

1

ˆ 0

0

AX

0

ˆ 1

0

AY

0

ˆ 0

1

AZ

O

5


Coordinates of a point in space:

zpzpc

ypypb

xpxpa

zcybxa

c

b

a

p

T

T

T

ˆ·ˆ

ˆ·ˆ

ˆˆ

ˆˆˆˆ

{A}

Ax

Ay

Az

a

b

c

pA ˆ u

Ap

Referenced with respect to {A}

Corresponding to object/reference u


6


A “free” vector is defined by a pair of points :

Coordinates of the vector : {A}

Ax

Ay

Az

O

pA ˆ q

A ˆv

A ˆ

vA ˆ

1

2

3

Â

p

p p

p

1

2

3

Â

q

q q

q

1 1 1

2 2 2

3 3 3

Â

v q p

v v q p

v q p


7


Contents



1.3 Translations

1.4 Rotations







8


Contents



1.3 Translations

1.4 Rotations







9


Inner product between two vectors:


10


Antisymmetric matrix


Cross product between two vectors:

11


Contents



1.3 Translations

1.4 Rotations







12


Contents



1.3 Translations

1.4 Rotations







13


• {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

Bp=(1 3 2) T

Ap=(4 7 5) T

AoB=(3 4 3)T

{A}

Az Bz

Ay

By

AxBx

{B}

Translation vector:

1.3 Translations

14


Coordinates are related by:

p )oT( =pB

B

AA

B

A

B

z

A

y

A

x

AA

B

AtI

o

o

o

oT

10

1000

100

010

001

)(

B

ABAo p= p

Translation matrix:

1.3 Translations

15


Contents



1.3 Translations

1.4 Rotations







16


Contents



1.3 Translations

1.4 Rotations







17


Let {A} and {B} two ortonormal coordinate systems with

the same origin and unit vectors {a1, a2, a3} {b1, b2, b3}.

b1

a1

a2

a3

b2b3

332211

332211

321

B

321

A

ˆ·ˆ·ˆ·ˆ

ˆ·ˆ·ˆ·ˆ

ˆ

ˆ

{B} and {A}in ˆby vector drepresente is Point

bpbpbpp

apapapp

pppp

pppp

pp

ABABABA

AAAAAAA

TBBB

TAAA

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


Rotation matrix:

332313

322212

312111

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ

bababa

bababa

bababa

RB

A

b1

a1

a2

a3

b2b3

1bA

2bA

3bA

332313

322212

312111

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ

bababa

bababa

bababa

RB

A

1aB

2aB

3aB

A

BT

B

ARR

, det( ) 1, rank( ) 3T

R R I R R

1.4 Rotations

19


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

Ro

t(, x

)R

ot(, y)

Y

X

ZZ’

X’

Y

X

Z

Y’Z’

Ro

t(, z

)

Y

X

ZZ’

X’

Y’

100

0cossin

0sincos

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ

ˆˆˆˆˆˆ

332313

322212

312111

B

A

bababa

bababa

bababa

1.4 Rotations

20


Coordinates are related by: p R =pB

B

AA

Rotation matrix:

p )RT( =pB

B

AA

B

A

B

A

B

AR

rrr

rrr

rrr

RT

10

0

1000

0

0

0

)(333231

232221

131211

1.4 Rotations

21


Contents



1.3 Translations

1.4 Rotations







22


Contents



1.3 Translations

1.4 Rotations







23


Coordinates are related by: p R =pB

B

AA

Rotation matrix:

p )RT( =pB

B

AA

B

A

B

A

B

AR

rrr

rrr

rrr

RT

10

0

1000

0

0

0

)(333231

232221

131211


Coordinates are related by:

p )oT( =pB

B

AA

B

A

B

z

A

y

A

x

AA

B

AtI

o

o

o

oT

10

1000

100

010

001

)(

B

ABAo p= p

Translation matrix:

24


1

p

p

p

=pz

A

y

A

x

A

A

Point: Vector:

0

v

v

v

=vz

A

y

A

x

A

A

Composed matrix:

p T =pB

B

AA tRtR

orrr

orrr

orrr

T

B

A

B

z

A

y

A

x

AA

B

A

10

1000

333231

232221

131211


25


Contents



1.3 Translations

1.4 Rotations







26


Contents



1.3 Translations

1.4 Rotations







27


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


28


Example 1

IT

yA

zA

xB

yB

zB

AoxA x1

yA y1

zA z1

^

^^

^^

^

^

^ ^

^

^


29


xA

yA

zA

xB

yB

zB

Ao

^

^

^^

^^

IT

IoTransT

oTransIT

A

A

)·(

o

)(·

xA x1

yA y1

zA z1

zA

xA

yA

y2

z2

x2

Ao

^

^

^

^

^ ^

^^

^

^


30


xA

yA

zA

xB

yB

zB

Ao

^

^

^^

^^

IT

IoTransT

oTransIT

A

A

)·(

o

)(·

)90,ˆ(·)·(

o

)90,ˆ()·(·

2

2

yRotIoTransT

yRotoTransIT

A

A

xA x1

yA y1

zA z1

zA

xA

yA

y2

z2

x2

Ao

zA

xA

yA

zB

yB

^ ^

^

^

^

^

^

^

^

^

^

^ ^

^

^^

^

^

xB


31


xA

yA

zA

xB

yB

zB

Ao

^

^

^^

^^

B

A

TA

z

A

y

A

x

A

z

A

y

A

x

A

A

To

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


32


xA

yA

zA

xB

yB

zB

Ao

^

^

^^

^^

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

)(·

zA

xA

yA

y2

z2

x2

Ao

^

^

^

^

^


33


xA

yA

zA

xB

yB

zB

Ao

^

^

^^

^^

Be careful !!!


the rotation is about the

fixed axis (and not about

the mobile axis)

IoTransT

oTransIT

A

A

)·(

o

)(·

zA

xA

yA

y2

z2

x2

Ao

^

^

^

^

^

zA

yA

xB

zB

yB

^

^

^

^

IoTransyRotT

oTransIyRotT

A

A

)·()·90,ˆ(

o

)(·)·90,ˆ(

2

2

xA^

Example 2


34


xA

yA

zA

xB

yB

zB

Ao

^

^

^^

^^

Be careful !!!


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

To

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


35


Contents



1.3 Translations

1.4 Rotations







36


Contents



1.3 Translations

1.4 Rotations







37


Homogeneous Inverse Transformation

1000

pRT

Example:

1000

2100

0001

2010

1000

2100

2001

0010

1TT

-2

0

-2

2

2

0

-100

001

0-10

1000

1 pRRT

TT

pRT

=


38


The homogeneous transformation that maps {B} with respect to {A}, is ATB. What are the coordinates of the point Ap=(0,1,0) with respect the

coordinate system {B} ?

Ap = T Bp

T-1·Ap = T-1·T Bp

T-1·Ap = Bp

1000

2100

0001

2010

1000

2100

2001

0010

1TT

Then,

1

2

0

1

1

0

1

0

1T

A B

{A}

{B}Bx

Ay

By

Az

Bz

Ax p


39


Contents



1.3 Translations

1.4 Rotations







40


Contents



1.3 Translations

1.4 Rotations







41


RPY angles: Roll, Pitch and yaw

33

33

22

22

11

11

321

cossin0

sincos0

001

cos0sin

010

sin0cos

100

0cossin

0sincos

),(),(),(

XRYRZRAAA

Euler angles: ZYZ, …

100

0cossin

0sincos

cos0sin

010

sin0cos

100

0cossin

0sincos

),(),(),( 33

33

22

22

11

11

321

ZRYRZRBBB


42


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

),(),(),(

XRYRZRAAA


43


Contents



1.3 Translations

1.4 Rotations







44


Contents



1.3 Translations

1.4 Rotations







45


32322

313213132121

321313132121

333231

232221

131211

coscossincossin



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


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



rrr

rrr

rrr


)/atan(

)cos/asin(

)asin(

11121

2323

312

rr

r

r



47


atan2(s,c)

• Problem of tan-1: 2121

1tantan/

2tan

2

pp

t

III

III IV

s

cs<0

c>0

s<0

c>0

s>0

c>0

s>0

c<0

s<0

c<0

1

2

Solution:

p

p

)·sgn(tan),(atan2,

0

2)·sgn(),(atan2

,

0

tan),(atan2,

0

,atan2

1

1

sc

scs

IIIQIIQ

c

scsIVQIQ

c

c

scs

IVQIQ

c

cs

c

scs

1tan),(atan2

c

scs

1tan),(atan2

p

c

scs

1tan),(atan2

p

c

scs

1tan),(atan2


48


)sin/,atan2(-

),atan2(

),atan2(

332312

11211

33323

rr

rr

rr

X

32322

313213132121

321313132121

333231

232221

131211

coscossincossin



rrr

rrr

rrr


)/atan(

)cos/asin(

)asin(

11121

2323

312

rr

r

r



49


Contents



1.3 Translations

1.4 Rotations







50


Contents



1.3 Translations

1.4 Rotations







51


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).

TTUDVMSVDUDVM )(;

1.10 Computing the closest rotation matrix

52


M is 3x3, the inverse of M is,

where:

U is an 3x3 rotation matrix,

V is an 3x3 rotation 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.

TTUVDUDVUDVM

1111

TVUMSVD

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


Contents



1.3 Translations

1.4 Rotations





