Perception avec OpenCV

Le domaine de “Computer Vision” (CV, ou vision par ordinateur) est une branche de l’intelligence artificielle, qui traite des techniques permettant d’extraire des informations de “haut niveau” utiles à partir d’images. Donc ce domaine développé depuis les années 60, on retrouve généralement des techniques provenant des mathématiques, du traitement d’images, des neurosciences, de l’apprentissage artificiel… Nous allons ici effleurer ce domaine en nous familiarisant avec OpenCV.

1. Introduction à OpenCV

OpenCV est une bibliothèque logicielle qui est devenue le “standard” du domaine. Cette bibliothèque fournit un énorme ensemble de fonctionnalités et d’algorithmes à la pointe de l’état de l’art. Entre autres sont disponibles :

2. Ouverture d’une image

img

roi=img[140:225, 210:310]
cv.imshow("Mon image", roi) #on donne un nom unique à chaque fenêtre
cv.waitKey(0) #permet d'attendre à a l'infini
cv.imwrite("roi.png", roi)

3. Seuil sur la couleur

Nous avons vu que les images sont généralement représentés dans l’espace BGR, ce qui est cohérent avec le fonctionnement du pixel de l’écran (et du capteur), mais moins évidant lorsque l’on souhaite travailler sur les couleurs. Comment par exemple définir le volume 3D dans l’espace BGR représentant le “rose”? C’est pourquoi pour traiter la couleur, il est recommandé de convertir l’encodage de l’image dans un autre espace. L’espace le plus couramment utilisé est le HSV (Hue, Saturation, Value ou Teinte, Saturation, Valeur).

img_HSV = cv.cvtColor(img, cv.COLOR_BGR2HSV)

On notera que l’espace HSV est encodé avec H dans [0, 179], S dans [0,255] et V dans [0,255]

img_seuil = cv.inRange(img_HSV, (MIN_H, MIN_S, MIN_V), (MAX_H, MAX_S, MAX_V)

Le résultat de la fonction de seuil inRange est une image binaire

4. Détection des cubes

Nous sommes maintenant capable de sélectionner des pixels en fonction de leur couleur, il nous faut encore “regrouper” ces informations afin de détecter et reconnaître les cubes.

contours, hierarchy = cv.findContours(
   img_seuil, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)

contours est une liste contenant tous les contours trouvés hierarchy contient les informations sur la hiérarchie des contours (les contours à l’intérieur des contours)

def trouver_centroid(cnt):
    M = cv.moments(cnt)
    if M['m00'] > 0.0:
       cx = int(M['m10']/M['m00'])
       cy = int(M['m01']/M['m00'])
       return (x, y)
    else:
       return (0, 0)

Nous pouvons ensuite utiliser la position obtenue pour écrire un texte :

cv.putText(img, 'cube', (x, y), cv.FONT_HERSHEY_SIMPLEX, 1,(255, 255, 255),1, cv.LINE_AA)

img

5. Intégration avec ROS

Nous allons maintenant intégrer cette détection de cube coloré à ROS en lisant l’image de la caméra de Ergo Jr simulée par Gazebo.


import rospy
from sensor_msgs.msg import Image
from std_srvs.srv import Trigger, TriggerResponse
from cv_bridge import CvBridge
import cv2 as cv
import numpy as np


class NodeVision(object):
    def __init__(self):
     # Params
     self.image = None
     self.debug_img = None
     self.br = CvBridge() #pour la conversion entre les imags OpenCV et les images ROS
     # Node cycle rate (in Hz).
     self.loop_rate = rospy.Rate(10)

     # Pour publier des images pour le debuggage
     self.img_pub = rospy.Publisher(
         '/ergo_jr/camera_ergo/debug_img', Image, queue_size=1)

     # Pour récupérer les images du robot simulé
     rospy.Subscriber(
         '/ergo_jr/camera_ergo/image_raw', Image, self.callback)

     # Créaction d'un service (on utilise le srv standard Trigger)
     self.service_vision = rospy.Service(
         '/ergo_jr/cube_detection', Trigger, self.handle_cube)

    def trouver_cube(self,img):
     raise NotImplementedError("Complétez la partie 2.4 avant d'exécuter")
     # ICI le traitement OpenCV

     # retour du résultat
     resp = TriggerResponse()
     # Si pas de cube
     # resp.success = False
     # Sinon
     # resp.success = True
     # resp.message="COULEUR"
     return resp

    def handle_cube(self, req):
     #Méthode callback qui sera éxécutée à chaque appel du service

     # retour du résultat
     resp = TriggerResponse()
     resp.success = False

     # uniquement si l'image existe
     if self.image is not None:
         imgtmp = self.image.copy()
         # on appelle la méthode de traitement d'image
         resp = self.trouver_cube(imgtmp)

     return resp

    def callback(self, msg):
     #méthode callback qui sera éxécutée à chaque reception d'un message
     self.image = self.br.imgmsg_to_cv2(msg, "bgr8") #On converti l'image ROS en une image OpenCV

    def start(self):
     rospy.loginfo("Démarage du node vision")

     while not rospy.is_shutdown():

         if self.image is not None:

      # éventuellement, publication d'une image de débug, ici une copie de l'image d'origine
      self.debug_img = self.image.copy()
      self.img_pub.publish(
          self.br.cv2_to_imgmsg(self.debug_img, "bgr8")) #On converti l'image OpenCV en une image ROS

         self.loop_rate.sleep()


if __name__ == '__main__':
    rospy.init_node("Vision")
    vision = NodeVision()
    vision.start()