Introduction a la Recherche en Laboratoire

Developpement d’un robot golfeur :Vision, detection et integration 3D

Auteur:Hugo Mathias2A MMIS

Tuteur:Gautier Stauffer

Fevrier - Mai 2016

Contents

1 Introduction 21.1 Quelques notions de golf . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2 Le projet . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.3 Etat de l’art . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2 Detection de la balle 32.1 Principe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1.1 Couleur . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.1.2 Contours . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.2 Algorithme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

3 Utilisation de ces resultats : 73.1 Quelques mots sur ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.2 Que faire des coordonnees de la balle? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83.3 Et pour le trou ? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

4 Resultats 11

5 Conclusion 145.1 Resume . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145.2 Discussion et perspective . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

1

1 Introduction

1.1 Quelques notions de golf

Une partie de golf est generalement constituee de 18 trous, eux-memes pouvant etre decoupes en plusieursparties.Il y a tout d’abord le jeu long, qui consiste a se rapprocher le plus possible du trou en faisant decoller laballe ; puis le putting, une fois le green (surface d’herbe rase qui entoure le trou) atteint, qui se rapprochealors du mini-golf : il faut simplement faire rouler la balle jusqu’au trou.Bien evidemment, ce probleme est loin d’etre evident car le green n’est jamais totalement plat et comportele plus souvent des reliefs.C’est a ce probleme de putting que nous allons nous interesser ici.

1.2 Le projet

Nous considerons donc un ”robot golfeur”, i.e un dispositif pouvant se deplacer et tirer et possedant unecamera ; et le but du projet est de lui permettre de rentrer la balle dans le trou en faisant le moins de couppossible.Ceci est un travail complexe car en entree le robot possede uniquement une ”vision” du green, et, a contrariod’un joueur, il lui est impossible d’estimer comment tirer a partir de cette simple vision.Il va donc falloir lui donner une representation 3D du green et le programmer pour qu’il sache quelle forceet angle de tir donner a la balle.Le projet est donc subdivise en 3 parties :

• Alexandre Caulier s’occupe de la partie motricite du robot, i.e lui permettre de se deplacer sur le greenpour recuperer les informations demandees puis jusqu’a la balle pour effectivement putter.

• Chew Sink Yeow s’occupe de la partie representation en 3D : il utilise le deplacement de la camera durobot pour construire en temps reel un green en 3D a l’aide d’un nuage de marqueurs, grace a ORBSLAM.

• Je m’occupe de la partie detection de la balle et du trou et integration a la map 3D. En effet, la creationdu green 3D a l’aide des marqueurs ne permettant pas de differencier la balle et le trou, il faut lesdetecter d’un autre cote a l’aide des images en 2D capturees par le robot ; puis indiquer au robot aquels (ensembles de) marqueurs correspondent ces elements particuliers du green.J’ai donc ete souvent amene a travailler en contact avec Chew, notamment pour utiliser les donneesqu’il acquerissait, comme explique en detail plus loin.

1.3 Etat de l’art

Ce projet de detection d’un objet est un projet qui s’etend a des domaines bien plus large que le golf, tresclassique en robotique. On pourra noter par exemple des matchs de football entre robots avec une balleorange.De nombreuses recherches sont effectuees a ce sujet afin de trouver la methode la plus adequate pour lesuivi / la detection d’objets. Quelques-unes de ces methodes sont expliquees dans le document de HaythemGhazouani, Suivi d’objets base forme et couleur pour la navigation robotique en temps reel. Nous verronspar la suite quelle methode nous avons adopte.

2

2 Detection de la balle

Apres l’etude de des differents types de solution existants, nous avons choisi la detection par la couleur enconsiderant une balle orange : en effet le green etant completement vert, le orange se detache tres largementdu reste de l’image ce qui rend la detection facile.Cette technique est largement utilisee dans le domaine de detection aujourd’hui, que ce soit par exemple dansdes travaux avec une Kinect (Detecting objects using color and depth segmentation with Kinect sensor, 2012),avec des robots (Appearance-Based Obstacle Detection with Monocular Color Vision, 2000),... et presentel’avantage d’etre a la fois simple dans son implementation et tres efficace dans notre cas d’etude.

2.1 Principe

On considere ici une image de la camera a un instant donne.

2.1.1 Couleur

La premiere chose a faire est de convertir l’image du systeme RGB actuel en systeme HSV (Hue SaturationValue), H representant la couleur, S la quantite de blanc dans l’image et V celle de noir.Ce systeme de couleur en coordonnees cylindriques, contrairement au cube RGB, a l’avantage de mieuxcorrespondre a la perception humaine des couleurs et donc d’etre plus intuitif.

Il est tres utilise dans les domaines de vision / detection car le fait que chacune des coordonnees RBG soitliee a la quantite de lumiere sur l’objet rend la detection plus difficile.Le passage au systeme HSV se fait selon le principe d’une projetcion du cube RGB incline selon l’axe (blanc- noir) sur le plan chromatique, ce qui donne un hexagone.

3

On obtient ensuite les formules suivantes, avec C le chroma egal a max(R,G,B)−min(R,G,B) :

H = 60◦ ×

(G−BC

)mod 6 si max = R(

B −RC

)+ 2 si max = G(

R−GC

)+ 4 si max = B

V = max(R,G,B)

S =C

V(0 si C vaut 0)

(2)

Un exemple avec l’image de la balle orange : on trouve une valeur en (R, G, B) d’environ (0.9, 0.35, 0.18)au centre de la balle d’ou avec les formules et C = 0.72:

H = 60◦ ×(

0.35− 0.18

0.72

)mod 6 = 7.3

V = R = 0.9

S =0.72

0.9= 0.80

La couleur (7.3, 204, 230) fait donc partie de l’intervalle observe en HSV.

Ceci etant fait, on peut appliquer un masque sur l’image avec l’intervalle de valeurs trouve (dans notrecas avec le orange) puis un ”et” bit a bit avec l’image originale ce qui va nous donner une image noirepartout sauf la ou on a detecte du orange.

Afin de se debarasser d’un eventuel bruit parasite et de trous dans l’objet on applique successivement un”opening” et un ”closing” sur l’image resultat (erosion dilatation / dilatation erosion).L’erosion permet de supprimer le bruit (mais risque aussi ”d’eroder” l’objet principal), la dilatation sert are-epaissir l’objet.

4

Ci dessous un exemple sur une image quelconque, et avec la balle :

Figure 1: Opening Figure 2: Closing

Figure 3: Opening Figure 4: Closing

2.1.2 Contours

A ce niveau la on a donc un premier resulat visuel, mais peu exploitable en dehors. L’interet est doncd’utiliser cette detection pour recuperer d’avantage d’informations.

On applique donc une detection des contours sur l’image resultat passee en noir et blanc (concretementil s’agit de detecter les zones blanches par rapport au noir), ces derniers etant stockees dabs un tableaucontenant chaque pixels du contour, puis l’on recherche le ”minimum enclosing circle”, le plus petit cerclecontenant entierement l’objet delimite par les contours trouves precedemment.

5

L’interet d’utiliser ce cercle plutot que de simplement chercher le centre du contour trouve est double :

• Premierement il nous donne acces instantanement au rayon du cercle, ce qui va nous servir par la suite.

• Et deuxiemement, il est possible, pour peu que la balle ne soit pas eclairee uniformement, que le contourne forme pas un cercle complet mais soit en partie ”plat”.Le calcul du centre serait alors fausse, ce qui ne sera pas le cas avec le cercle englobant.

2.2 Algorithme

On utilise pour implementer cette detection les fonctions de la bibliotheque Open CV.L’algorithme ci-dessous permet une detection en temps reel a partir d’une camera.

cap = cv2.VideoCapture(0)

while(1):

# Take each frame

_, frame = cap.read()

# Convert BGR to HSV

hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

# define range of orange color in HSV

lower_orange = np.array([2,50,100])

upper_orange = np.array([15,240,240])

# kernel for opening / closing

kernel = np.ones((5,5),np.uint8)

# Mask

mask = cv2.inRange(hsv, lower_orange, upper_orange)

6

output = cv2.bitwise_and(frame, frame, mask = mask)

# Opening / Closing

opening = cv2.morphologyEx(output, cv2.MORPH_OPEN, kernel)

closing = cv2.morphologyEx(opening, cv2.MORPH_CLOSE, kernel)

# Searching for contours

image = cv2.cvtColor(closing, cv2.COLOR_RGB2GRAY)

thresh, im_bw = cv2.threshold(image, 45, 255, 0)

contours, hierarchy = cv2.findContours(im_bw, 1, 2)

if not contours:

print ’pas de balle dans cette image’

else:

cnt = contours[0]

(x,y),radius = cv2.minEnclosingCircle(cnt)

center = (int(x),int(y))

radius = int(radius)

cv2.circle(frame,center,radius,(0,255,0),2)

print center

print radius

#show the images

cv2.imshow("frame", frame)

cv2.imshow("output", output)

k = cv2.waitKey(5) & 0xFF

if k == 27:

break

cv2.destroyAllWindows()

3 Utilisation de ces resultats :

3.1 Quelques mots sur ROS

ROS (Robot Operating System) est une bibliotheque permettant de publier des donnees facilement.ROS fonctionne selon le principe des noeuds et topics : un noeud contient des informations et un topic relieun noeud a un autre pour eventuellement traiter ces informations.Plus precisement, on a d’un cote un programme informatique qui calcule certaines donnees (dans notre casles coordonnees des marqueurs par exemple). On peut alors stocker ces donnees dans un ficher.bag qui peutetre ”re-ecoute” par la suite.D’un autre cote on a un programme appele ”listener” qui va ecouter ces enregistrements (qui constituerontdonc un noeud) et recuperer certaines donnees.

7

3.2 Que faire des coordonnees de la balle?

Une fois ces coordonnees recuperees, le but est de les lier au nuage de points 3D recupere grace a ORBSLAM.Pour ce faire, plusieurs etapes :

• Tout d’abord, il faut recuperer les parametres suivants, qui ont ete calcule lors de la calibration originalede la camera : les matrices de calibration K, de rectification R et de projection P ainsi que les coefficientsde distorsion (d1, d2, t1, t2, d3).Ces differents parametres permettent de projeter a tout moment les marqueurs de l’espace 3D sur laframe capturee actuellement par la camera, dans le repere de la frame d’origine, comme le montrele schema suivant :

– On commence par rectifier X a l’aide de la matrice R et des coefficients t1 et t2.

– On projette X sur la frame grace a P.

– On regle la distorsion eventuelle en multipliant chaque coordonnee de X par le di correspondant.

– et enfin on multiplie par K.

Figure 5: Un exemple de parametres obtenus

8

• Mais nous voulons ces coordonnees dans le repere de la frame ou l’on projette (puisque celles du centrede la balle sont dans ce repere la).

• Nous allons donc se ramener a ce repere la grace a une transformation dans l’espace, stockee dans lefichier tf.En effet ce fichier contient, pour chaque mouvement de camera, 2 transformations (translation +rotation) qui permettent d’effectuer le changement de repere dans l’espace (on se translate jusqu’a lanouvelle origine puis on tourne les axes).

• La translation est representee par un vecteur a 3 coordonnees (Tx, Ty, Tz), et la rotation par un quater-nion (x, y, z, w). (equivalent a une matrice de rotation, voir ci-dessous).

• Pour calculer la transformation on passe en coordonnees homogenes (i.e avec une quatrieme coor-donnee). Notre vecteur de translation devient donc

T =

1 0 0 Tx0 1 0 Ty0 0 1 Tz0 0 0 1

notre quaternion

R =

w2 + x2 − y2 − z2 2xy − 2wz 2xz + 2wy 0

2xy + 2wz w2 − x2 + y2 − z2 2yz − 2wx 02xz − 2wy 2yz + 2wx w2 − x2 − y2 + z2 0

0 0 0 1

et notre marqueur M = (X,Y, Z, 1).

L’interet d’utiliser ce systeme de coordonnees est de pouvoir appliquer ces transformations facilement,en calculant simplement (TR)−1 ×M .

• Pourquoi l’inverse ? : car lorsqu’on applique le produit (TR) a un point on a les nouvelles coordonneesde ce point, toujours dans notre repere d’origine.Or de notre cote nous possedons deja les coordonnees dans le repere d’origine donc en appliquantla transformee inverse on obtient de nouvelles coordonnees qui correspondent exactement aux coor-donnees du point donne initialement mais dans le nouveau repere.

La question de projeter ou non l’ensemble des marqueurs se pose ici. En effet, il se peut que le robot ait dejaparcouru beaucoup de terrain et ait donc reconstitue une bonne partie du green, y compris des endroits quine se projeteront pas sur la frame traitee.Une possibilite serait donc de calculer un cube de projection en dehors duquel on ne considererait pas lesmarqueurs. Neanmoins calculer les coordonnees de ce cube s’avere fastidieux et complique pour un gain enefficacite assez negligeable au vu du nombre total de marqueurs. Nous avons donc fait le choix de projeterl’ensemble des marqueurs.

On projette donc nos marqueurs sur la frame et on compare les coordonnees de chacun d’eux avec cellede la balle.Possedant centre et rayon de la balle, on peut determiner pour chaque marqueur si il appartient a la balle.Des que l’on a trouve un marqueur qui correspond, on recupere ses coordonnees 3D et comme on sait qu’ilest necessairement a la surface de la balle on peut trouver les coordonnees du centre de la balle dans l’espace3D grace a la methode suivante :

9

Soient (X,Y, Z) les coordonnees du marqueur trouvees, (x, y) les coordonnes de ce marqueur dans la frame,(xc, yc) celles du centre de la balle dans la frame, r le rayon sur l’image et R le rayon de la balle dans la map3D (que l’on peut retrouver a l’aide du rayon reel de la balle, suppose connu).

Notons α = Rr et d la distance du marqueur au centre sur la frame =

√(xc − x)2 + (yc − y)2

On se place maintenant dans l’espace 3D : la distance dans le plan (x, y) du marqueur est centre est,au rapport des rayons pres, la meme que celle en 2D.Notons D cette distance, on a D = αd.

D’autre part on a bien sur(X −Xc)

2 + (Y − Yc)2 + (Z − Zc)2 = R2

soit(Z − Zc) =

√R2 −D2 =

√R2 − α2d2

donc

Zc = Z −R√

1− d2

r2

et de la meme maniere on a :

(X −Xc) = α(x− xc) et (Y − Yc) = α(y − yc)

soitXc = X − α(x− xc) et Yc = Y − α(y − yc)

On a donc les coordonnees du centre de notre balle dans l’espace de nos marqueurs.Ne reste plus qu’a le publier dans ROS pour qu’il soit integre comme information a la map 3D.

On a donc l’algorithme suivant, en pseudo langage, et avec les memes notations que precedemment :

pour chaque frame

T : la translation, R : la rotation

detection de la balle

si pas de balle

on passe a la frame suivante

sinon

soit (x_c, y_c, r) le resultat

pour chaque marqueur M (X, Y, Z)

calcul de m (x,y,z) = (RT)^-1 x M

si (x - x_c)^2 + (y - y_c)^2 < r^2

on sort de la boucle

fin si

fin pour

(X_c, Y_c, Z_c) = (X - (r/R)(x - x_c), Y - (r/R)(y - y_c), Z - R(sqrt(1 - (d/r)^2)))

transmission de l’information

fin si

fin pour

10

3.3 Et pour le trou ?

Le principe reste globalement le meme, a la difference pres que le trou va etre vu par la camera comme uneellipse de couleur.Il y a donc neanmoins quelques changements a effectuer dans l’algorithme :

• changement de valeur pour la couleur (le trou etant generalement blanc ou noir)

• on s’interesse a la plus petite ellipse entourant le contour

• Quelques changements dans le calcul du centre du trou.

4 Resultats

Suite a un probleme de compatibilite entre les versions de ROS sur 2 ordinateurs differents que nous n’avonspas reussi a resoudre dans les temps, nous n’avons pas pu exploiter les marqueurs calcules par Chew commenous voulions le faire au depart.

Pour toutefois verifier les calculs mis en jeu dans notre methode nous avons donc mene l’experimentation apartir de marqueurs purement ”inventes”.

Il faut garder a l’esprit que cette section est purement la pour illustrer quelques techniques un peu plusprecisement avec de vraies valeurs ; un certain nombre de procedures expliquees precedemment ne pourrontdonc pas etre appliquees ici du a l’absence de certaines donnees.

Voici les resultats :

• Donnees de depart : on se place dans un cube centre en 0 et de taille (10m× 10m× 10m), on obtient,a l’aide du script basique suivant :

from random import uniform

for i in range(100):

print (uniform(-5,5), uniform(-5,5), uniform(-5,5))

11

les marqueurs suivants :

• Considerons que le marqueur surligne est sur la surface de la balle. On suppose que la balle fait 3 cmde rayon et qu’elle est centree en (−2, 18, 0.93,−0.52).

• Considerons d’autre part que la frame a pour coin inferieur gauche le point (−3, 0, 5) et qu’elle mesure3 metres par 2.

• Enfin considerons que la camera est placee 3 metres derriere la frame en son centre (donc en (−1.5, 1,−8))et qu’elle tourne ensuite de 45◦ autour de l’axe des y et se deplace de 0.5 m selon l’axe des x et 0.5 mselon l’axe des z.

12

On a donc

T =

1 0 0 0.50 1 0 00 0 1 0.50 0 0 1

et (avec la formule classique avec les cos / sin ici) :

R =

√22 0

√22 0

0 1 0 0

−√22 0

√22 0

0 0 0 1

d’ou

(TR)−1 =

√22 0

√22 −1

0 1 0 0

−√22 0

√22 −1

0 0 0 1

−1

=

√22 0 −

√22 0

0 1 0 0√22 0

√22 −

√22

0 0 0 1

• On peut donc calculer les nouvelles coordonnees de la camera en multipliant par (TR), on trouve

(5.096, 1, 7.217)

• Pour des raisons de simplicite ici (et n’ayant tout simplement pas acces aux parametres de calibragede la camera fictive), nous allons projeter de maniere basique sur le plan (i.e sans tenir compte d’uneeventuelle distorsion)On cherche donc l’intersection de la droite passant par (−2.16, 0.94,−0.5) et (5.096, 1, 7.217) avec leplan x+ z = 8.071 (on trouve l’equation du plan a partir d’un point et de sa normale (−1, 0,−1)).

• Soit (x0, y0, z0) ce point, et U le vecteur directeur de la droite, de coordonnees (7, 256, 0.06, 7.717)

On a alors le systeme suivant :

x0 = 5.096 + 7.256k

y0 = 1 + 0.06k

z0 = 7.217 + 7.717k

En reportant dans l’equation du plan on trouve k = −0.283 d’ou :

x0 = 3.043

y0 = 0.983

z0 = 5.033

• On veut maintenant recuperer ces coordonnees mais dans le nouveau repere : on applique (TR)−1 eton trouve (−1.421, 0.983, 5, 003)

• On se place desormais sur le frame 2D capturee par la camera post - rotation.Dans l’ideal on a trouve les coordonnees de la balle 2D dans cette frame avec la detection. Pourl’exemple ici, ne pouvant pas appliquer nos techniques de detection vues precedemment, nous allonsobtenir artificiellement centre et rayon par des calculs classiques.

• De la meme maniere que precedemment on projette donc le centre de la balle sur la frame, on trouvecomme coordonnees : (−1, 424, 0.985, 5, 003)

• Pour le rayon on applique le theoreme de Thales : la camera est a d = 3 m de la frame et a D = ||U || =10, 593 m donc le rayon de la balle sur la frame est r = R× d

D = 0.849cm.

• On suppose que l’on a projete chacun des marqueurs et qu’on arrive a celui qui nous interesse.On calcule donc

√(−1.424 + 1.421)2 + (0.985− 0.983)2 = 0.361 cm donc le marqueur est bien sur la

balle.

13

• Derniere chose : on veut retrouver les coordonnees du centre.On a, d’apres les formules :

Xc = −2.16− 10.593

3(−1.424 + 1.421) = −2.171

Yc = 0.94− 10.593

3(0.985− 0.983) = 0.933

Zc = −0.50− 3

√1−

(0.00361

0.0085

)2

= −0.525

On constate donc qu’on ne retombe pas tout a fait sur nos pieds, probablement a cause des nombreuseserreurs d’arrondi effectuees au cours des calculs.

5 Conclusion

5.1 Resume

Ce projet de recherche s’est donc articule en 2 parties :Une premiere partie de recherche de methodes pour detecter la balle avec un travail principalement docu-mentaire de comparaison et de reflexion sur les techniques possibles.Puis une partie plus technique avec l’apprentissage de ROS et la mise en oeuvre de methodes pour lier ladetection avec les resultats 3D obtenus par ailleurs.

5.2 Discussion et perspective

N’ayant pas pu tester l’ensemble des techniques mises en oeuvre ici avec les valeurs reelles, suite au problemede compatibilite evoque plus haut et a l’absence de test effectif sur un green avec le robot, il est difficile dejuger precisement l’approche envisagee.Toutefois, des remarques :

• Tout d’abord, la detection par couleur s’est revelee etre une bonne approche : les experiences meneesa partir de photos se sont revelees concluantes.

• Pour ce qui est de la projection des marqueurs, on a vu qu’il y a quand meme des multiplications dematrice en jeu : reduire le nombre de marqueurs a projeter pourrait donc etre interessant, une solutionpourrait etre par recherche dichotomique autour de l’axe de la camera : on convergerait ainsi vers lapyramide du ”viewing frustrum”.

• Une autre piste d’amelioration pourrait etre de gerer la presence de 2 balles sur le green, une au robotet l’autre a un autre joueur : comment differencier les 2 si elles sont de la meme couleur, quellesinformations lui donner, peut-on appliquer les memes methodes...

14