Commande par vision

Commande par vision

CHAPITRE 2 – ESTIMATION DE LA MESURE

[email protected]

Plan

  EsBmaBon de la mesure :

◦  Avec 4 points coplainaires

◦  Avec au moins 4 points non coplanaires

◦  Avec le Jacobien image

◦   Avec projecBon de marqueurs

◦   UBlisaBon d’OpenCV

EsFmaFon de pose

A parFr de 4 points coplanaires

  Soit la configuraBon suivante:

  La cible est consBtuée de 4 points P

, P

, P

, P

coplanaires dans (P

x y) où P

est l’origine de R

. Trouver M

, la matrice homogène de transformaBon entre R et R .

z x

y R _c

M _co x

z y

R _o P ₀

0

P

=

X

Y

0 1

⎡

⎣

⎢ ⎢

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥ ⎥

⎥

EsFmaFon de pose

A parFr de 4 points coplanaires

  Soit le modèle d’une projecBon perspecBve:

z

p

= K Π M

P

avec

p

=

x

y

1

⎡

⎣

⎢ ⎢

⎢ ⎢

⎤

⎦

⎥ ⎥

⎥ ⎥

et

P

=

X

Y

Z

1

⎡

⎣

⎢ ⎢

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥ ⎥

⎥ K =

G

0 u

0 G

v

0 0 1

⎛

⎝

⎜ ⎜

⎜

⎞

⎠

⎟ ⎟

⎟

et Π =

1 0 0 0 0 1 0 0 0 0 1 0

⎛

⎝

⎜ ⎜

⎜

⎞

⎠

⎟ ⎟

⎟

EsFmaFon de pose

A parFr de 4 points coplanaires   Si :

  Alors, en considérant pour simplifier que u ₀ = v ₀ = 0 : M

=

r

r

r

t

r

r

r

t

r

r

r

t

0 0 0 1

⎡

⎣

⎢ ⎢

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥ ⎥

⎥

EsFmaFon de pose

A parFr de 4 points coplanaires   On obBent :

  En divisant tout par t _z : x

= ( θ

X

+ θ

Y

+ θ

)

θ

X

+ θ

Y

+ 1 y

= ( θ

X

+ θ

Y

+ θ

)

θ

X

+ θ

Y

+ 1

⎧

⎨

⎪ ⎪

⎩

⎪ ⎪

EsFmaFon de pose

A parFr de 4 points coplanaires Avec :

Notes :

◦  G

et G

sont les grandissements suivant x et y du capteur.

◦  Ils dépendent de la taille du pixel.

◦   Ils sont égaux lorsque le pixel est carré.

◦   Ils permeZent d’obtenir une coordonnée dans l’image en pixels.

◦   On a :

θ

= r

G

t

θ

= r

G

t

θ

= t

G

t

! θ

= r

t

f f

EsFmaFon de pose

A parFr de 4 points coplanaires Il vient :

x

( θ

X

+ θ

Y

+ 1 ) ⁼ ( ^θ

^X

^{+ θ}

^Y

^{+ θ}

)

y

( θ

X

+ θ

Y

+ 1 ) ⁼ ( ^θ

^X

^{+ θ}

^Y

^{+ θ}

)

⎧

⎨ ⎪

⎩⎪

x

= ( θ

X

+ θ

Y

+ θ

) ^{− x}

( ^θ

^X

^{+ θ}

^Y

)

y

= ( θ

X

+ θ

Y

+ θ

) ^{− y}

( ^θ

^X

^{+ θ}

^Y

)

⎧

⎨ ⎪

⎩⎪

EsFmaFon de pose

A parFr de 4 points coplanaires

Sous forme matricielle :

EsFmaFon de pose

A parFr de 4 points coplanaires

En appliquant ceZe équaBon aux 4 points : x

y

! x

y

⎡

⎣

⎢ ⎢

⎢ ⎢

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥ ⎥

⎥ ⎥

⎥

"#$

=

M

M

M

M

⎛

⎝

⎜ ⎜

⎜ ⎜

⎜

⎞

⎠

⎟ ⎟

⎟ ⎟

⎟

M_8×8

" # % $ %

θ

θ !

⎡

⎣

⎢ ⎢

⎢ ⎢

⎤

⎦

⎥ ⎥

⎥ ⎥

"#$

d'où : Θ =M

B

EsFmaFon de pose

A parFr de 4 points coplanaires

En uBlisant le fait que r ₁₁ ² + r ₂₁ ² + r ₃₁ ² = 1 :

On en déduit tous les termes de M . θ

t

G

⎛

⎝⎜

⎞

⎠⎟

2

+ θ

t

G

⎛

⎝ ⎜ ⎞

⎠ ⎟

2

+ ( ) θ

t

^{= 1}

⇒ t

= 1

θ

G

⎛

⎝⎜

⎞

⎠⎟

2

+ θ

G

⎛

⎝ ⎜ ⎞

⎠ ⎟

2

+ θ

car t

> 0

EsFmaFon de pose

A parFr de 4 points coplanaires Conclusions :

◦  Méthode simple exploitant les propriétés de planéité de la cible.

◦  Le calcul implique l’inversion d’une matrice 8x8.

◦  Le calcul se fait en un nombre d’opéraBons fixe.

◦   CeZe méthode est sensible au bruit : il n’y a aucune garanBe que la matrice de rotaBon esBmée respecte les propriétés d’ortho-

normalité.

◦  PotenBellement, il faudrait normaliser ceZe matrice et rendre ses

lignes/colonnes perpendiculaires 2 à 2.

EsFmaFon de pose

A parFr d’au moins 4 points non coplanaires : POSIT

  Référence bibliographique :

  D. DeMenthon et L. S. Davis, Model-Based Object Pose in 25 Lines of Code, Interna'onal Journal of Computer Vision, volume 15, pages 123 à 141, 1995.

  Méthode itéraBve, basée sur l’approximaBon de projecBon

orthographique.

EsFmaFon de pose

POSIT : définiFons

i j

k C f

O

Plan moyen

H

Z

u w v M

M

Plan image

M

R

R

EsFmaFon de pose

POSIT : définiFons

k C f

H

Z

u

w v M

M

P

N

p

m

m

EsFmaFon de pose

POSIT : objecFfs

  Soit :

M

=

o

i

i

i

Coord. de i

! ##

" ## $

o

j

j

j

Coord. de j dans repère objet

! ## " ## $

o

k

k

k

ok=^oi×^oj

! ### " ### $

T

= x

Z

f T

= y

Z

f T

= Z

0 0 0 1

⎛

⎝

⎜ ⎜

⎜ ⎜

⎜ ⎜

⎜ ⎜

⎜ ⎜

⎜

⎞

⎠

⎟ ⎟

⎟ ⎟

⎟ ⎟

⎟ ⎟

⎟ ⎟

⎟

avec :

c

m

= x

y

⎛

⎝ ⎜

⎜

⎞

⎠ ⎟

⎟

EsFmaFon de pose

POSIT : calculs

  Pour déterminer M _co il suffit de :

◦  Déterminer les coordonnées dans R

de i et j

◦  Déterminer la profondeur Z

le long de l’axe opBque du plan moyen.

  Démontrer les relaBons suivantes : M

M

! !!!!! "

i f

Z

i = x

(1 + ε

) − x

(1) M

M

! !!!!! "

i f

Z

j = y

(1 + ε

) − y

(2)

avec

m = ⎡ x y f ⎤

et ε = 1

M M

! !!!!! "

i k

EsFmaFon de pose

POSIT : calculs

i j

k C f

O H

Z

u

w v M

M

P

N

p

m

m

Utiliser les triangles

Cm

O et N

P

M

EsFmaFon de pose

POSIT : calculs

  On a :   Avec :

  Les triangles C m _i O et N _i P _i M _i sont semblables dans le

rapport entre M _i P _i et C O . Ce rapport est égale à :

EsFmaFon de pose

POSIT : calculs

  On obBent :

  Produit scalaire par i :

  On obBent l’équaBon (1) :

  Le produit scalaire par j donne l’équaBon (2).

M

M

! !!!!! "

i f

Z

i = x

(1 + ε

) − x

avec ε

= 1

Z

! M !!!!!

M "

i k

EsFmaFon de pose

POSIT : calculs

  Les équaBons (1) et (2) peuvent s’écrire dans R _o :

  Sous forme matricielle pour tous les n points M _i :

  Avec :

o

( M

M

)

^{I = ζ}

^avec

^{I =} _Z ^f

0

o

i et ζ

= x

( ) 1 + ε

^{− x}

o

( M

M

)

^{J = η}

^avec

^{J =} _Z ^f

0

o

j et η

= y

( ) 1 + ε

^{− y}

A

I = x ′ A

J = y ′

⎧ ⎨

⎩ A =

o

( M

M

)

!

o

( )

⎛ ⎜

⎜ ⎜

⎞ ⎟

⎟ ⎟ x ′ =

ζ

ζ !

⎡ ⎢

⎢ ⎢

⎤ ⎥

⎥ ⎥ y ′ =

η

η !

⎡ ⎢

⎢ ⎢

⎤ ⎥

⎥ ⎥

EsFmaFon de pose

POSIT : calculs

  Si on a au moins 4 points, la matrice A est de dimension 3×3 et est inversible. Si n>3 on uBlise la pseudo-inverse :

  Et on en déduit :   Et finalement :

o

I = B x ′

o

J = B y ′

⎧ ⎨

⎩ avec B = ( ) A

A

^A

o

i =

I I

o

j =

J J

o

k =

i ×

j

T

= Z

= 2 f

I + J T

= x

Z

f T

= y

Z

f

EsFmaFon de pose

POSIT : algorithme

n nombre de points, A de dimension 3 × ( n − 1)

Calcul de B, pseudo-inverse de A . ε

= 0 ∀ i = 1 … n

IniBalisaBon

Calcul de la pose

o

I = B x ′

J = B y ′

o

i =

I I

o

j =

J J

o

k =

i ×

j Evaluation de x ′ et y ′ .

T

= Z

= 2 f ( I + J )

^T

^{= x}

^Z _f

^T

^{= y}

^Z _f

Mise à jour

ε = 1 ! !!!!! "

∀ = Arrêt si tous les ε

EsFmaFon de pose

POSIT : algorithme

i j

k C f

O

Plan moyen

H

Z

u w v M

M

Plan image

M

R

R

EsFmaFon de pose

POSIT : algorithme

k C f

Plan moyen

H

Z

u w v M

M

Plan image

M

R

EsFmaFon de pose

POSIT : définiFons

i j

k C f

O

Plan moyen

H

Z

u w v M

M

Plan image

M

R

R

EsFmaFon de pose

POSIT : algorithme

k C f

Plan moyen

H

Z

u w v M

M

Plan image

M

R

EsFmaFon de pose

POSIT : exercice

  Une caméra munie d'un objecBf de focale 1 cm a dans son champ de vision un objet consBtué de 4 points dont les

coordonnées dans le repère R _o sont les suivantes :

x z y

z

x y M

M

M

M

R

R

M

? M

( 0 0 0 ) ^M

( ^{0.1 0 0} )

M

( 0 0.1 0 ) ^M

( ^{0 0 0.1} )

EsFmaFon de pose

POSIT : exercice

1.   Synthèse de l’image idéale

Calculer les coordonnées des projecBons des 4 points dans le plan image sachant que la matrice homogène de transformaBon entre la caméra et l’objet est égale à :

2.   Retrouver M _co avec POSIT

Retrouver M

à parBr de l’image idéale synthéBsée en 1.

M

=

1 0 0 0.15 0 0 − 1 0.1 0 1 0 0.3 0 0 0 1

⎛

⎝

⎜ ⎜

⎜ ⎜

⎞

⎠

⎟ ⎟

⎟ ⎟

EsFmaFon de pose

POSIT : exercice

1.   Synthèse de l’image idéale

2.  ApplicaBon de POSIT

EsFmaFon de pose

POSIT : exercice

  IniBalisaBon de ε _i à 0

  ItéraBon 1 :

EsFmaFon de pose

POSIT : exercice

  ItéraBon 1 :

EsFmaFon de pose

POSIT : exercice

  ItéraBon 2 :

o

I =

4.32 ⋅ 10

− 5.85 ⋅ 10

− 3.97 ⋅ 10

⎡

⎣

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥

o

J =

3.97 ⋅ 10

− 3.9 ⋅ 10

− 3.333 ⋅ 10

⎡

⎣

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥

o

i =

0.996

− 1.35 ⋅ 10

− 9.14 ⋅ 10

⎡

⎣

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥

o

j =

0.118

− 1.16 ⋅ 10

− 0.993

⎡

⎣

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥

o

k =

1.123 ⋅ 10

0.998

− 0.997 ⋅ 10

⎡

⎣

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥

Z

= 0.26

ε

= 4.74 ⋅ 10

ε

= 0.377

ε

= − 3.84 ⋅ 10

EsFmaFon de pose

POSIT : exercice

  ItéraBon 5 :

o

I =

3.33 ⋅ 10

− 2.56 ⋅ 10

2.11 ⋅ 10

⎡

⎣

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥

o

J =

− 2.17 ⋅ 10

− 1.7 ⋅ 10

− 3.333 ⋅ 10

⎡

⎣

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥

o

i =

0.99997

− 7.68 ⋅ 10

6.33 ⋅ 10

⎡

⎣

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥

o

j =

− 6.5 ⋅ 10

− 5.11 ⋅ 10

− 0.99999

⎡

⎣

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥

o

k =

1.123 ⋅ 10

0.998

− 0.997 ⋅ 10

⎡

⎣

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥

Z

= 0.3002

ε

= 2.56 ⋅ 10

ε

= 0.333

ε

= − 1.7 ⋅ 10

EsFmaFon de pose

POSIT : exercice

  Arrêt à l’itéraBon 5 :

  On retrouve à 10 ^-4 près les termes de M _co qui nous avait servie à calculer l’image idéale de l’objet.

T

= x

Z

f = 5 ⋅ 10

⋅ 0.3002

0.01 = 0.1501 T

= y

Z

f = 0.1001 T

= Z

= 0.3002 R

=

0.99997 − 7.68 ⋅ 10

6.33 ⋅ 10

− 6.5 ⋅ 10

− 5.11 ⋅ 10

− 0.99999 7.69 ⋅ 10

0.99995 − 5.12 ⋅ 10

⎛

⎝

⎜ ⎜

⎜

⎞

⎠

⎟ ⎟

⎟

EsFmaFon de pose

Pose relaFve

  EsBmer la pose relaBve entre 2 images :

◦  Tâche de suivi de cible

◦  Faible erreur de suivi

◦  p

pose désirée

◦   p pose courante

◦   Trouver e = p - p

p

p

R

e

R

R

EsFmaFon de pose

Pose relaFve

  Soit :   Avec :

  Soit :   Avec :

F ! = L

C

F ! vitesse des primitives dans l'image

L

matrice d'interaction (Jacobien image) C

torseur cinématique de R

!

e = L

C

L le Jacobien de la pose

EsFmaFon de pose

Pose relaFve

  On obBent :

  ApproximaBon au 1 ^er ordre :   Avec :

!

e = L

L

F ! avec L

= ( ) L

L

^L

e ≈ L

L

( F − F

) ^{pour F − F}

^petit.

F

les primitives en p

F les primitives en p

EsFmaFon de pose

Pose relaFve

  Si les primiBves sont les coordonnées x _i y _i de n points (pix) :

L

=

− G

Z

0 x

Z

x

y

G

− G

+ x

G

y

G

G

0 − G

Z

y

Z

G

+ y

G

− x

y

G

− x

G

G

! ! ! ! ! !

− G

Z

0 x

Z

x

y

G

− G

+ x

G

y

G

G

0 − G

Z

y

Z

G

+ y

G

− x

y

G

− x

G

G

⎛

⎝

⎜ ⎜

⎜ ⎜

⎜ ⎜

⎜ ⎜

⎜ ⎜

⎜ ⎜

⎜

⎞

⎠

⎟ ⎟

⎟ ⎟

⎟ ⎟

⎟ ⎟

⎟ ⎟

⎟ ⎟

⎟

F =

x

y

! x

y

⎡

⎣

⎢ ⎢

⎢ ⎢

⎢ ⎢

⎢

⎤

⎦

⎥ ⎥

⎥ ⎥

⎥ ⎥

⎥

EsFmaFon de pose

Pose relaFve

  Si les angles sont exprimés avec la représentaBon angle/

axe :

L

= R

0 0 L

⎛

⎝ ⎜

⎜

⎞

⎠ ⎟

⎟ avec e = t

θ u

⎡

⎣ ⎢

⎢

⎤

⎦ ⎥

⎥

L

= I

− θ

2 ⎡⎣ ⎤⎦ u

+ 1 − sinc θ sinc

θ

2

⎛

⎝

⎜ ⎜

⎜

⎞

⎠

⎟ ⎟

⎟

⎡⎣ ⎤⎦ u

2

et ⎡⎣ ⎤⎦ u

=

0 − u

u

u

0 − u

− u

u

0

⎛

⎝

⎜ ⎜

⎜⎜

⎞

⎠

⎟ ⎟

⎟⎟

EsFmaFon de pose

Pose relaFve

  Dans ce cas :   D’où :

◦  Adapté au suivi de cible car e est maintenu peBt par l’asservissement visuel.

◦   Rapide car la pseudo-inverse peut être calculée au préalable.

◦   Imprécis quand e augmente.

L

⎯ →

⎯⎯

I

e ≈ L

( F − F

) ^{pour F − F}

^petit.

EsFmaFon de pose

Pose relaFve : exemple

  Soit :

  Qui correspond à :

  On obBent alors:

t

= ⎡ 0.1 0 0

⎣ ⎤

⎦

T

, θ = 0.1 rad et u = ⎡ 1 0 0

⎣ ⎤

⎦

T

F

= ⎡ 500 333 833 333 375 250 500 0

⎣ ⎤

⎦

T

F = ⎡ 173.3 448.7 519.9 448.7 128.9 359.3 167.5 100.3

⎣ ⎤

⎦

T

L

( F − F

) = 0.099 0.002 0.002 0.102 0.986 0.06 ⎡ − 0.15

⎣ ⎤

⎦

⎡

⎣ ⎢

⎢

⎤

⎦ ⎥

⎥

T

EsFmaFon de pose

UFlisaFon de marqueurs lumineux : exemple Pro file d e l a su rf ace d e l a scè ne Faisceau LASER

Plan image d

z f

x

x

On a donc : x

= f x

z = f d

z d'où : z = f d x

Il est possible de connaître la profondeur de la scène.

EsFmaFon de POSE

UFlisaFon d’OpenCV

  FoncBon solvePnP :

◦  Paramètres :

◦  Coordonnées des points de l’objet

◦  Coordonnées des points dans l’image

◦  Paramètres intrinsèques de la caméra

◦  Algorithme uBlisé (3 possibilités)

◦  Valeurs retournées :

◦  Vecteur de translaBon

◦  Vecteur de rotaBon en uBlisant la représentaBon angle / axe