Robotics - Homogeneous coordinates and transformations. Simone Ceriani

Transcription

1 Robotics - Homogeneous coordinates and transformations Simone Ceriani ceriani@elet.polimi.it Dipartimento di Elettronica e Informazione Politecnico di Milano 5 March 0

2 /49 Outline Introduction D space 3 3D space 4 Rototranslation - D 5 Rototranslation - 3D 6 Composition 7 Projective D Geometry 8 Projective Transformations

3 3/49 Outline Introduction D space 3 3D space 4 Rototranslation - D 5 Rototranslation - 3D 6 Composition 7 Projective D Geometry 8 Projective Transformations

4 4/49 Calendar st part Wed 4/03 Homogeneous coordinate Thu 9/03 Computer Vision () Thu /04 Computer Vision () nd part Thu 0/05 Localization () Thu 07/06 Localization () Thu 4/06 Slam

5 5/49 Outline Introduction D space 3 3D space 4 Rototranslation - D 5 Rototranslation - 3D 6 Composition 7 Projective D Geometry 8 Projective Transformations

6 6/49 Homogeneous coordinates Introduction Introduced in 87 (Möbius) Used in projective geometry Suitable for points at the infinity Easily code points (D-3D) lines (D) conics (D) planes (3D) quadrics (3D) Transformation simpler than Cartesian

7 7/49 Points in Homogeneous coordinates - D space - Definition Homogeneous D space Given a point p e = [ ] X R in Cartesian coordinates Y x we can define p h = y R 3 in homogeneous coordinates w X = x/w under the relation Y = y/w w 0 i.e., there is an arbitrary scale factor (w)

8 8/49 Points in Homogeneous coordinates - D space - Example Example y 3 O + p e 3 4 x [ ] 3 p e = (euclidean) 3 p h = p e 6 p h = 4 p e.5 p h 3 = p e 0.5 p h p h p h3 Note A Cartesian point can be represented by infinitely many homogeneous coordinates

9 9/49 Points in Homogeneous coordinates - D space - Properties Note A Cartesian point can be represented by infinitely many homogeneous coordinates Property x given p h = y, w 0 w λ x for λ 0 ˆp h = λ y p h λ w Proof p e = [ ] x/w y/w for λ 0 ˆp e = λ x λ w λ y λ w = [ ] x/w y/w Notes w = : normalized homogeneous coordinates normalization : [ x y w ] T [ x/w, y/w, ] T, w 0 hom cart : [ x y w ] T [ x/w, y/w ] T, w 0 cart hom : [ x y ] T [ x, y, ] T

10 0/49 Points in Homogeneous coordinates - D space - Improper points What s more than Cartesian? All Cartesian points can be expressed in homogeneous coordinates: p e [ p e, ] T Are homogeneous coordinates more powerful than Cartesian ones? Yes Improper points With w = 0 we can express points at the infinity [ x/0, y/0 ] T p h = [ x, y, 0 ] T codes a direction not directly expressed in Cartesian coordinates Property p h = [ x, y, 0 ] T [ λx, λ y, 0 ] T λ 0

11 /49 Points in Homogeneous coordinates - D space - Directions Example Example y 3 d y d 45 d x O 3 4 x d x = 0 0 : x-axis d y = : y-axis 0 0 / d 45 = / : 45 axis 0 0 Note A direction can be represented by infinitely many homogeneous directions A unit vector is the direction with d = (i.e., x + y = )

12 /49 Points in Homogeneous coordinates - D space - Final Remarks Points λx p h = λy p e = λw with w 0, λ 0 [ ] x/w y/w Origin 0 p h = 0 p e = w with w 0 [ ] 0 0 Improper Points - Directions λx d h = λy 0 with (x 0 y 0) && λ 0 Invalid homogeneous point 0 p h = 0 0 homogeneous D space is defined on R 3 [ 0, 0, 0 ] T

13 3/49 Outline Introduction D space 3 3D space 4 Rototranslation - D 5 Rototranslation - 3D 6 Composition 7 Projective D Geometry 8 Projective Transformations

14 4/49 Points in Homogeneous coordinates - 3D space - Definition Homogeneous 3D space X Given a point p e = Y R 3 in Cartesian coordinates Z x we can define p h = y z R4 in homogeneous coordinates w X = x/w Y = y/w under the relation Z = z/w w 0 i.e., there is an arbitrary scale factor (w)

15 5/49 Points in Homogeneous coordinates - 3D space - Summary Points λx p h = λy x/w λz pe = y/w z/w λw with w 0, λ 0 Origin 0 p h = pe = 0 0 w with w 0 Improper Points - Directions λx d h = λy λz with 0 (x 0 y 0 z 0) && λ 0 Invalid homogeneous point 0 p h = homogeneous 3D space is defined on R 4 [ 0, 0, 0, 0 ] T

16 6/49 Outline Introduction D space 3 3D space 4 Rototranslation - D 5 Rototranslation - 3D 6 Composition 7 Projective D Geometry 8 Projective Transformations

17 7/49 Translation - Cartesian D Start with an example y y 3 O p (O ) + O p (O ) p (O ) p (O ) +p (O ) 3 4 x x [ ] p (O ) = [ ] O = p (O ) [ ] p (O ).5 =.5 p (O ) = p (O ) + p (O ) [ ] 3.5 =.5

18 8/49 Translation - Homogeneous D - Same example but with homogeneous coordinate y y 4 λ 3 p (O ) = λ λ O p (O ) + O p (O ) p (O ) p (O ) +p (O ) 3 4 Anything better? x x O p (O ).5 6 λ.5 p (O ) = p (O ) = p (O ) + p (O ) NO λ.5 λ valid only if p (O ) w = p(o ) w e.g., we can normalize points (w = ) e.g.,

19 9/49 Translation - Homogeneous D - Translation: the right way with homogeneous coordinates x O O = y O : position of the second reference frame (normalized) x p p (O) = y p : point w.r.t. the second reference frame (homogeneous) w p 0 x O x p x p + x O w p p = 0 y O y p = y p + y O w p 0 0 w p w p

20 0/49 Translation - Homogeneous D - 3 Let s try on the example y 3 p (O ) + O p (O ) p (O ) +p (O ) 4 λ p (O ) = λ λ O p (O ) normalized.5 6 λ.5 p (O ) = λ.5 λ O 3 4 x 0 λ.5 λ.5 + λ λ.5 = λ.5 + λ λ λ

21 /49 Rotation - Cartesian D Derivation from example y 3 + p (O ) cos(θ)p y y 3 sin(θ)p x O O sin(θ)p cos(θ)p 3 4 y x θ 3 4 x x O O rotated of θ = 30 [ ] [ ] p (O) px = = p y cos(θ + 90 ) = sin(θ) sin(θ + 90 ) = cos(θ) [ ] p (O) cos(θ)px sin(θ)p y = = sin(θ)p x + cos(θ)p y [ ] cos(30 ) sin(30 ) sin(0 ) + cos(30 = ) [ ]

22 /49 Rotation - Homogeneous D [ ] From p (O) cos(θ)px sin(θ)p y = sin(θ)p x + cos(θ)p y [ ] cos(θ) sin(θ) Rewrite with matrices p (O) = p (O ) sin(θ) cos(θ) cos(θ) sin(θ) 0 Pass to homogeneous p (O ) h = sin(θ) cos(θ) 0 p (O ) h 0 0 where p (O ) h = λp x λp y λ

23 3/49 Rototranslation - Homogeneous D Putting things together y y 3 O t O + p (O ) 3 4 One step: p (O ) h = θ 3 x x O translated of t w.r.t. O O rotated of θ w.r.t. O Two steps: cos(θ) sin(θ) 0 p (O ) h = sin(θ) cos(θ) 0 p (O ) h 0 0 p (O ) h = cos(θ) sin(θ) t x sin(θ) cos(θ) t y 0 0 p (O ) h 0 t x 0 t y 0 0 p (O ) h

24 4/49 Rototranslation - Homogeneous D - Some notes cos(θ) sin(θ) t x n x m x t x Consider sin(θ) cos(θ) t y = n y m y t y = n x [ ] R t = T 0 n = n y is the direction vector (improper point) of the x axis 0 m x m = m y is the direction vector (improper point) of the x axis 0 n = m = they are unit vectors [n x, m x] = [n y, m y ] = are unit vectors too R is an orthogonal matrix R = R T T is homogeneous too! i.e., T λt T p λt p

25 5/49 Rototranslation - Homogeneous D - Get parameters T T T 3 Consider T = T T T cos(θ) sin(θ) t x remember sin(θ) cos(θ) t y 0 0 t = [ T3 T 3 ] θ = atan(t, T ) atan(y, x) = ATAN y θ θ + π arctan(y/x) = [0, π] atan(y, x) [ π, π] arctan(y/x) x > 0 arctan(y/x) + π y 0, x < 0 arctan(y/x) π y < 0, x < 0 + π y > 0, x = 0 π y < 0, x = 0 undefined x = 0, y = 0 x

26 6/49 Outline Introduction D space 3 3D space 4 Rototranslation - D 5 Rototranslation - 3D 6 Composition 7 Projective D Geometry 8 Projective Transformations

27 7/49 Rotations in 3D - Rotate around x O z z y y x ρ, roll ( rollio ) around x-axis Rotate around y z O y z x x θ, pitch ( beccheggio ) around y-axis Rotate around z z y O y x x φ, yaw ( imbardata ) around z-axis R x = cos(ρ) sin(ρ) 0 sin(ρ) cos(ρ) R y = cos(θ) 0 sin(θ) 0 0 sin(θ) 0 cos(θ) R z = cos(φ) sin(φ) 0 sin(φ) cos(φ) 0 0 0

28 8/49 Rotations in 3D - To create a 3D rotation Compose 3 planar rotation 4 possible conventions non-commutative matrix products Common convention Rotate around x (roll) Rotate around y (pitch) Rotate around z (yaw) Derive a complete rotation matrix Let p ORxyz in the rotated system p ORyz p ORz = R xp ORxyz = R y p ORyz p O = R zp ORz in the original system R xyz = R z R y R x R xyz = cos(φ) cos(θ) cos(φ) sin(θ) sin(ρ) sin(φ) cos(ρ) cos(φ) sin(θ) cos(ρ)+sin(φ) sin(ρ) sin(φ) cos(θ) sin(φ) sin(θ) sin(ρ)+cos(φ) cos(ρ) sin(φ) sin(θ) cos(ρ) cos(φ) sin(ρ) sin(θ) cos(θ) sin(ρ) cos(θ) cos(ρ)

29 9/49 Rototranslations - Homogeneous 3D O () z y t x y z Complete Transformation p (O) w.r.t. O reference O () (O p ) x Let consider O rotated as O, but translated by t R rotation of O wrt O p (O ) = R p (O ) p (O ) = t + p (O ) In Homogeneous Coordinates [ ] [ ] p (O ) I t R 0 h = 0 0 where p (O ) h and p (O ) h p (O ) h = is p (O ) in homogeneous coordinates is p (O ) in homogeneous coordinates [ ] R t p (O ) 0 h

30 30/49 Rototranslation - Homogeneous 3D - Some notes n x m x a x t x [ ] Consider n y m y a y t y R t n z m z a z t z = = T n = n x n y n z, m = m x m y m z 0 0, a = a x a y n = m = a = unit vectors a z 0 are the direction vectors of the x,y,z, axes [n x, m x, a x ] = [ny, m y, a y] = [nz, m z, a z ] = are unit vectors too R is an orthogonal matrix R = R T T is homogeneous too! i.e., T λt T p λt p

31 3/49 Rototranslation - Homogeneous 3D - Get parameters T T T 3 T 4 Consider T = T T T 3 T 4 T 3 T 3 T 33 T remember t = T 4 T 4 T 34 cos(φ) cos(θ) cos(φ) sin(θ) sin(ρ) sin(φ) cos(ρ) cos(φ) sin(θ) cos(ρ)+sin(φ) sin(ρ) sin(φ) cos(θ) sin(φ) sin(θ) sin(ρ)+cos(φ) cos(ρ) sin(φ) sin(θ) cos(ρ) cos(φ) sin(ρ) φ = atan (T,T ) θ = atan ( T 3, T3 +T 33 ) ρ = atan (T 3,T 33 ) sin(θ) cos(θ) sin(ρ) cos(θ) cos(ρ)

32 3/49 Outline Introduction D space 3 3D space 4 Rototranslation - D 5 Rototranslation - 3D 6 Composition 7 Projective D Geometry 8 Projective Transformations

33 33/49 Transformations - Why? Think about... W T (W ) p (W ) T (W ) WR R p (R) W is the world reference frame. R is the robot reference frame. T (W ) WR [ ] (W ) R WR = WR t (W ) WR is the transformation matrix 0 map points in R reference frame in W frame t (W ) WR R (W ) WR is the position of R w.r.t. W is the transformation that codes position and orientation of the robot w.r.t. W. The robot perceives the red point, it knows the point p (R) in robot reference frame. p (W ) = T (W ) WR p(r) is the point in world coordinates. is the rotation applied to a reference frame rotated as W with origin on O

34 34/49 Transformations - Inversion Inverse transformation W is the world reference frame. W p (W ) T (R) RW T (W ) WR R p (R) R is the robot reference frame. T (W ) WR is the transformation that codes position and orientation of the robot w.r.t. W. ) You know the red point (p (W ) in world coordinates, Position of the world w.r.t. the robot T (R) RW = ( T (W ) WR ) = [R (W ) T WR R (W ) WR 0 T (W ) t WR p (R) = T (R) RW p(w ) is the point in robot coordinates. ] is the transformation matrix

35 35/49 Transformations - Composition Composition of transformations 0 T (0) 03 T (0) 0 T (0) 0 3 T () 3 T () T (0) 0 : pose of w.r.t. 0 T () : pose of w.r.t. T (0) 0 = T(0) 0 T() : pose of w.r.t. 0 T () 3 : pose of 3 w.r.t. T (0) 03 = T(0) 0 T() T() 3 = T(0) 0 T() 3 : pose of 3 w.r.t. 0 Graphical method Post-multiplication following arrows verse Pre-multiplication coming back in arrows verse Note: Not unique convention about arrow direction!!

36 36/49 Transformations - Composition Example Example 0 T (0) 03 T (0) 0 3 p (3) p () T () 3 T () T (0) 0 : pose of w.r.t. 0 T () : pose of w.r.t. T (0) 03 : pose of 3 w.r.t. 0 p (3) : position of p w.r.t 3 p () : position of p w.r.t? Solution T () 3 = ( ( Note: T () T () p () = T () 3 p(3) ) ( ) ( T (0) 0 T (0) 0 ) (0) T 03 = T() T() 0 T(0) 03 ) ( = ) = ( T (0) 0 T() T (0) 0 )

37 37/49 Transformations - Composition - Practical Case Change reference system of motion W T (R) S T (S 0 ) S 0 S S R 0 T (R) S S R T (R 0) R 0 R T (R0) R 0 R : pose of robot R at time t = w.r.t. robot at time t = 0 i.e., the relative motion of the robot T (R) S : pose of a sensor S w.r.t. the robot R Note: fixed in time T (S 0) S 0 S : pose of sensor S at time t = w.r.t. sensor at time t = 0? i.e., the relative motion of the sensor Solution ( T (S 0) S 0 S = T (R) RS ) T (R 0 ) R 0 R T (R) RS

38 38/49 Outline Introduction D space 3 3D space 4 Rototranslation - D 5 Rototranslation - 3D 6 Composition 7 Projective D Geometry 8 Projective Transformations

39 39/49 Lines in homogeneous coordinates Lines definition Slope-intercept form: y = mx + q vertical lines m = Linear equation: a x + b y + c = 0, (a, b) R {0, 0} Homogeneous coordinates [ ] T Line: l = a, b, c [ ] T Point: p = x, y, p lies on l l T p = p T l = 0 Homogeneous property: l λ l p λ p

40 40/49 Point from Lines & Lines from Points Intersection of lines Line joining two points p = l l = l l y l + p l y l = p p p + p + l x l = [, 0, ] T x = l = [ 0,, ] T y = p = [,, ] T x p = [,, ] T p = [,, ] T l = [,, 0 ] T y = x a x a = a y b = a z b x b y b z Cross product reminder u x u y u z a y b z a zb y a b = det a x a y a z = a zb x a xb z b x b y b z a xb y a y b x

41 4/49 Ideal points and l Intersection of parallel lines p = l l = l l y l p l 3 x l = [, 0, ] T x = l = [, 0, ] T x = p = [ 0,, 0 ] T improper point direction of y axis Line that join improper points (l ) p = [ x, y, 0 ] T p = [ x, y, 0 ] T p p [ 0, 0, ] T l = [ 0, 0, ] T : join pair of improper points, [ ] i.e., l T T x, y, 0 = 0

42 4/49 Duality principle Duality p l p T l = 0 l T p = 0 p = l l l = p p To any theorem in D projective geometry there correspond a dual theorem, derived by interchanging the role of points and lines

43 43/49 Conics Definition n d degree equations planar curve Equation: ax + bxy + cy + dx + ey + f = 0 Homogeneous: ax + bxy + cy + dxw + eyw + fw = 0 Matrix form: [ ] T x = x, y, w a b/ d/ C = b/ c e/ d/ e/ f x T Cx = 0 C is homogeneous too, i.e., 6 parameters, 5 D.O.F.

44 44/49 Conics - Summary Proper conics: rank (C) = 3 Circle Hyperbola Ellipse Parabola Degenerate conics: rank (C) < 3 l m C = lm T + ml T -lines, rank (C) = l C = ll T repeated line, rank (C) =

45 45/49 Conics Parameters Estimation Parameters estimation Given a point x i, y i, it satisfies axi + bx i y i + cyi + dx i + ey i + f = 0 [ ] [ ] T Rewrite: xi, x i y i, yi, x i, y i, a, b, c, d, e, f = 0 Stacking constraints on 5 points: x x y y a 0 x y x x y y x y b 0 x3 x 3y 3 y3 x 3 y 3 c x 4 x 4y 4 y4 x 4 y 4 d = 0 0 x5 x 5y 5 y5 x 5 y 5 Solve the linear system e f 0 0 Details Multiple View Geometry in computer vision - Hartley, Zisserman, Chapter.

46 46/49 Outline Introduction D space 3 3D space 4 Rototranslation - D 5 Rototranslation - 3D 6 Composition 7 Projective D Geometry 8 Projective Transformations

47 47/49 Projective Transformations - Definition Definition A projectivity is an invertible mapping h( ) : R R such that x,x,x 3 lie on the same line h(x ),h(x ),h(x 3) do i.e., a projectivity maintains collinearity Theorem A mapping h( ) : R R is a projectivity a non-singular 3 3 matrix H such that p R expressed with its homogeneous vector p h h(p h ) = H p h

48 48/49 Projective Transformations - Practice Projective Transformation x = H x x y = w h h h 3 x h h h 3 y h 3 h 3 h 33 w Note H has 9 elements H is homogeneous too: λh H normalized if h 33 = only 8 D.O.F. synonymous Projectivity Projective transformation Collineation Homography

49 Introduction D space 3D space Rototranslation - D Rototranslation - 3D Composition Projective D Geometry Projective Transformations Projective Transformations - Mapping between planes Original Image Estimation Rectified Image Note Take four point on first image xi H has 8 D.O.F. (λh = H) Map on four known destination points x0i 0 x0 = H x x = H x on hij. Solve x0 = H x3 30 x4 = H x4 each point impose constraint Details Multiple View Geometry in computer vision Hartley Zisserman Chapter 4. 49/49