Introduction to Vectors and Frames

Transcription

1 Intoduction to Vectos and Fames CIS Russell Taylo Saah Gaham

2 Infomation Flow Diagam Model the Patient Plan the Pocedue Eecute the Plan Real Wold

3 Coodinate Fame Tansfomation F = [ R, p] 0 F y z y 0 z 0

4

5 b F = [R,p]

6 b F = [R,p]

7 b F = [ I,0]

8 = R b R F = [R,0] b

9 = R b R p F = [R,p] b = R + p = R + b p

10 Coodinate Fames = F b = [R,p] b = R b + p b F = [R,p]

11

12

13

14

15 Fowad and Inese Fame Tansfomations ], [ p R F = p b R b p R b F + = = = ], [ p R R p R b b F = = = ) ( ], [ p R R F = Fowad Inese

16 Composition Assume F R p F R p = = [, ], [, ] Then b p p R R R p p R b R R p b R p R p b R F b F F b F F = + + = + = + = = ], [ ) ( ], [ ) ( ) ( 2 So F F [R,p ] [R,p ] [R R,R p p ] = = +

17 Vectos [ ] z y ow z y col = = y z : length z y + + = dot poduct : a = w w u = : poduct coss cosθ w ( ) = z z y y w w w + + = = y y z z y z z y w w w w w w sinθ, w u = w w u = w

18 Vectos as Displacements z w +w y = + z z y y w w w w = z z y y w w w w w -w y w

19 Vectos as Displacements Between Paallel Fames z 0 z = 0 w 0 w y y 0 0

20 Rotations: Some Notation Rot(, Rotation by angle α about ais a R a( Rotation by angle α about ais a Ra ()@ Rot(, a a ) Ryz ( αβγ,, )@ R (, α) Ry (, β) Rz (, γ ) R ( αβγ,, )@ Rz (, α) Ry (, β) Rz (, γ ) zyz

21 Rotations: A few useful facts Rot( sa, α) = a a and Rot(, aα ) b = b a Rot(, aα) = Rot(, aˆα ) whee aˆ = a Rot(, aˆα) Rot(, aˆβ) = Rot(, aˆ α+ β) Rot(, aˆα) = Rot(, aˆ α ) Rot(,0) a = b b i.e., Rot(,0) a = I = the identity otation Rot(, aˆ α ) = b aˆ baˆ+ Rot(, aˆα ) b aˆ baˆ Rot ( ) ( ) ( ) Rot(, aˆα) Rot( bˆ, β) = Rot( bˆ, β) Rot( Rot( bˆ, β) aˆ, α )

22 Rotations: moe facts T If = [,, ] then a otation R may be descibed in y z T tems of the effects of R on othogonal unit ectos, ˆ = [,0,0], T T yˆ= [0,,0], zˆ = [0,0,] R = + + y y z z whee ˆ = R ˆ y = R y = R zˆ z Thus R b R c = bc g ( ) g( )

23 Rotations in the plane R R cosθ y sinθ = y sinθ y cosθ + cosθ sinθ = sinθ cosθ y θ = [, y] T

24 Rotations in the plane cosθ sinθ 0 R [ ˆ yˆ ] = sinθ cosθ 0 [ R ˆ R yˆ ] = θ

25 3D Rotation Matices [ ˆ ˆ ˆ] [ ˆ ˆ ˆ] R y z = R R y R z = ˆ ˆ ˆ y z T ˆ T T R = R ˆ ˆ ˆ ˆ y y z ˆ z T T T ˆ ˆ ˆ ˆ ˆ ˆ g gy g z 0 0 T T T = ˆ ˆ ˆ ˆ ˆ ˆ y y y y z = 0 0 g g g T T T ˆ ˆ ˆ ˆ ˆ ˆ 0 0 z z y z z g g g

26 Popeties of Rotation Matices Inese of a Rotation Mati equals its tanspose: R - = R T R T R=R R T = I The Deteminant of a Rotation mati is equal to +: det(r)= + Any Rotation can be descibed by consecutie otations about the thee pimay aes,, y, and z: R = R z,q R y,f R,y

27 Canonical 3D Rotation Matices Note: Right-Handed Coodinate System R 0 0 ( θ) = Rot (, θ) = 0 cos (?) sin (?) 0 sin (?) cos (?) R y cos (?) 0 sin (?) ( θ) = Rot( y, θ) = 0 0 sin (?) 0 cos (?) R z cos (?) sin (?) 0 ( θ) = Rot (, z θ) = sin (?) cos (?) 0 0 0

28 Homogeneous Coodinates Widely used in gaphics, geometic calculations Repesent 3D ecto as 4D quantity / Fo ou puposes, we will keep the scale s = s y/ s = y = z/ s z s

29 Repesenting Fame Tansfomations as Matices 0 0 p 0 0 p y y + p= = P 0 0 pz z R R 0 = 0 I p R 0 R p P = R = = [ Rp, ] = F R p F= = ( R) + p 0

30

31 Fame tansfomation fom 3 point pais b 3 F CT b b F ob

32 Fame tansfomation fom 3 point pais F CT Fob k = FCT bk k = Fob FCT bk = F b k C k b b 2 F = F F C F ob ob CT b 3 3 2

33 Fame tansfomation fom 3 point pais = F b = R b + p k C k C k C Define 3 3 m = k bm = bk 3 3 u = a = b b k k m k k m FCa k = RCa k+ pc ( ) RCak+ pc = RC bk bm + pc R a = R b + p R b p RCa k = k m= uk p = u R b C k C k C C m C C m C m Sole These!! b m b a 2 a a 3 3 m u 3 u 2 b 3 b 2 u 2

34 Rotation fom multiple ecto pais Gien a system Ra k = u k fo k=, L, n the poblem is to estimate R. This will equie at least thee such point pais. Late in the couse we will coe some good ways to sole this system. Hee is a not-so-good way that will poduce oughly coect answes: Step : Fom matices U= u u and A a a [ L ] = [ L ] Step 2: Sole the system RA= U fo R. E.g., by R= UA T Step 3: Renomalize R to guaantee RR= I. n n

35 Renomalizing Rotation Mati Gien "otation" mati T R= y z, modify it so RR= I. Step : Step 2: Step 3: a= y z b= a R z nomalized a b z = a b z

36 Calibating a pointe F pt But what is b tip?? b tip = F b tip pt tip F lab

37 Calibating a pointe b = Fb post k tip = Rb + p k tip k F pt F k b tip b post F lab

38 Calibating a pointe Fo each measuement k, we hae b = Rb + p post k tip k I. e., Rb b = p k tip post k Set up a least squaes poblem MM b tip M Rk I b post p k MM M F pt b tip b tip b tip btip F pt F pt Fpt

39 Kinematic Links F k F k- L k q k F = F F k k k, k [ Rk, pk] = [ Rk, pk ] Rk, k, pk, k = [ R, p ] [ Rot(, θ ), LRot(, θ ) ] k k k k k k k

40 Kinematic Chains F0 = [, I0] R3= R0,R,2R2,3= Rot(, θ) Rot( 2, θ2) Rot( 3, θ3 ) p ( ) 3= p0,+ R0, p,2+ R,2p2,3 = LRot (, θ ) + LRot 2 (, θ) Rot( 2, θ2 ) + LRot(, θ ) Rot(, θ ) Rot(, θ ) F 2 L 2 q 2 q 3 L 3 F 3 F F 0 L q

41 Kinematic Chains If = 2= 3 = z, R3= Rot(, zθ) Rot(, z θ2) Rot(, z θ3 ) = Rot(, z θ+ θ2+ θ3 ) p ( ) 3= p0,+ R0, p,2+ R,2p2,3 = LRot (, zθ) + LRot 2 (, zθ) Rot(, zθ2 ) + LRot 3 (, zθ) Rot(, z θ2) Rot(, zθ3 ) = LRot (, zθ) + LRot 2 ( z, θ+ θ2 ) + LRot( z, θ + θ + θ ) q L 2 q 2 q 3 F 3 L 3 F 0 L

42 Kinematic Chains If = = = z, 2 3 cos( θ+ θ2+ θ3) sin( θ+ θ2+ θ3) 0 R3= sin( θ+ θ2+ θ3) cos( θ+ θ2+ θ3) 0 0 Lcos( θ) + L2cos( θ+ θ2) + L3cos( θ+ θ2+ θ3) p3= Lsin( θ) + L2sin( θ+ θ2) + L3sin( θ+ θ2+ θ3 ) 0

43 Small Fame Tansfomations Repesent a "small" pose shift consisting of a small otation followed by a small displacement p as F= [ R, p] Then F = R + p R

44 Small Rotations a small otation R a( a otation by a small angle α about ais a Rot(, a a ) b a b+ b fo a sufficiently small Ra a otation that is small enough fo this appoimation R( λ a) R( µ b) R( λ a+ µ b) (Lineaity fo small otation) s Eecise: Wok out the lineaity poposition by substitutin o

45 Appoimations to Small Fames Fa (, [ Ra (), p] Fa (, p) = Ra ( ) + p + a + p a = skew() a 0 az az 0 a y ay a 0 z

46 actual nominal Eos & sensitiity Often, we do not hae an accuate alue fo a tansfomation, so we need to model the eo. We model this as a composition of a "nominal" fame and a small displacement F = F F * Often, we will use the notation F fo F and will just use F fo F nominal * F = F F actual. Thus we may wite something like F = F F = + * * o (less often). We also use, etc. Thus, if we use the fome fom (eo on the ight), and hae nominal elationship = F b, we get * * * = F b = F F ( b+ b)

47 F = [R,p] b

48 F * = F F * = + b b b * = +

49 Eos & Sensitiity Suppose that we know nominal alues fo and that T T [ -ε,-ε,-ε] [ εεε,, ] What does this tell us about F= [ R, p]? F * = F F F, b, and * = + b b b * = +

50 Eos & Sensitiity so * * * = F b = F + F ( b b) ( = () + ( ) R Ra b b + p) + p + + ( R b b a + b a + b p) + p = ( R b p R b a + b a b+ p) + R ( b+ a + b p) if a b a b is negligible (it usually is) R + b a + b p = R + b R a b+ R p ( ) * =

51 Digession: otation tiple poduct Epessions like R a b is linea in a, but is not always conenient to wok with. Often we would pefe something like MRb (, ) a. R a b= R b a = R skew( b) a = skew( ) T R b a

52 Eos & Sensitiity Peious epession was R b + a b+ p ( ) Substituting tiple poduct and eaanging gies b R R R skew( b) p a So ε b ε ε R R R skew( b) p ε ε a ε

53 Eos & Sensitiity Now, suppose we know that a system of linea constaints b β, ε ε ε ε b ε R R R skew( b) ε p β I 0 0 β β a β β β this will gie us

54 Eo Popagation in Chains F k F k- L k q k F = F F * * * k k k, k F F = F F F F k k k k k, k k, k ( ) F = F F F F F k k k k k, k k, k k, k k k, k k, k = F F F F

55 Suppose that you hae Rk, k = Ra ( k) + I skew( ak ) p = e k, k k Eecise q 3 F 3 Wok out appoimate fomulas fo [ R, 3 p3] in tems of L, θ a and e. You should k k, k, k k come up with a fomula that is linea in Lk, ak, and e F k. F 2 L 2 q 2 L 3 F 0 L q

56 Paametic Sensitiity Suppose you hae an eplicit fomula like Lcos( θ) + L2cos( θ+ θ2) + L3cos( θ+ θ2+ θ3 ) p 3= Lsin( θ) + L2sin( θ+ θ2) + L3sin( θ+ θ2+ θ3 ) 0 and know that the only aiation is in paametes like Lk and θk. Then you can estimate the aiation in p3 as a function of aiation in and θ by emembeing you calculus. L k k p p L L θ θ 3 3 p3

57 Ginding this out gies: Paametic Sensitiity p3 p 3 L p3 L θ θ whee T L= [ L, L2, L3] T θ = [ θθθ,, ] p 3 = sin( θ) sin( θ+ θ2) sin( θ+ θ2+ θ3) L p3 = θ cos( θ ) cos( θ + θ ) cos( θ + θ + θ ) Lsin( θ) L2sin( θ+ θ2) L3sin( θ+ θ2+ θ3) L2sin( θ+ θ2) L3sin( θ+ θ2+ θ3) L3sin( θ+ θ2+ θ3) Lcos( θ) + L2cos( θ+ θ2) + L3cos( θ+ θ2+ θ3) L2cos( θ+ θ2) + L3cos( θ+ θ2+ θ3) L3cos( θ+ θ2+ θ3) 0 0 0