Get best of the week

Mesurer la distance aux objets à l'aide de RealSense D435

La tâche de détecter des objets dans l'image est aujourd'hui l'un des leaders dans le domaine de la vision industrielle. Son essence n'est pas seulement de classer l'objet dans l'image, mais aussi d'indiquer son emplacement exact.

Les résultats de la détection d'objets peuvent être complétés par des informations sur la distance de cet objet. Le problème de mesure de distance peut être résolu en utilisant la caméra de profondeur Intel RealSense D435 , qui mesure la profondeur à chaque point.

Dans cet article, nous allons résoudre le problème de la mesure de la distance à un objet en temps réel en utilisant la bibliothèque OpenCV et la technologie RealSense.

image

Exigences


Caméra RealSense

Pour mesurer la distance, nous avons besoin d'une caméra de profondeur RealSense D435 . Pour cet exemple, il n'est pas nécessaire d' installer le SDK RealSense .

Bibliothèque Pyrealsense2

Tout le code, pour faciliter la démonstration, nous allons l'écrire en Python 3.7 , nous avons donc besoin du paquet pyrealsense2 .

pip install pyrealsense2

Bibliothèque OpenCV

De plus, nous avons besoin de la bibliothèque OpenCV (toute version à partir de 3.4 convient). Le manuel d'installation de la bibliothèque peut être consulté sur le site officiel d' OpenCV .

Avec OpenCV , une autre bibliothèque nécessaire sera installée - numpy .

Détection d'objets


La première étape consiste à sélectionner des objets dans la vidéo. Pour détecter des objets en temps réel, les modèles dits à un étage (par exemple, Retina Net, SSD, YOLO ), qui gagnent en vitesse de fonctionnement, sont parfaits .

Pour simplifier l'expérience, nous prendrons le modèle formé SSDLite Mobilenet v2 . À la place, tout autre modèle peut être utilisé qui renvoie les coordonnées de l'objet sous une forme ou une autre.
Vous pouvez télécharger l'archive avec les fichiers de modèle dans le référentiel TensorFlow officiel .

L'archive contient un graphique de calcul figé au format binaire .pb, ainsi que diverses configurations. Mais pour utiliser le modèle dans OpenCV, vous devez également avoir un graphique de calculs au format texte .pbtxt . Il n'est pas dans l'archive, il doit donc être généré manuellement.

Copiez deux scripts du référentiel OpenCV :

  1. github.com/opencv/opencv/blob/master/samples/dnn/tf_text_graph_ssd.py
  2. github.com/opencv/opencv/blob/master/samples/dnn/tf_text_graph_common.py

Ensuite, exécutez la commande suivante, en spécifiant les chemins nécessaires:

python tf_text_graph_ssd.py --input /path/to/model.pb --config /path/to/example.config --output /path/to/graph.pbtxt

La sortie est un fichier graph.pbtxt .

Nous mettons les fichiers de modèle existants dans le dossier du projet. Dans mon cas, c'est le dossier models / .

Développement


Maintenant, tout est prêt pour le développement. Commençons à écrire du code.

Nous importons les bibliothèques nécessaires et faisons un stub sur la méthode draw_predictions () , dont nous aurons besoin plus tard:

import pyrealsense2 as rs
import numpy as np
import cv2 as cv

def draw_predictions(pred_img, color_img, depth_image):
    pass

Créez les objets principaux:

pipeline = rs.pipeline() # <-  pipeline      
config = rs.config() # <-      
colorizer = rs.colorizer() # <-      

spatial = rs.spatial_filter()
spatial.set_option(rs.option.holes_fill, 3)

Nous initialisons le modèle SSD . Ici, le fichier .pbtxt que nous avons généré ci - dessus nous est utile :

#  
model = cv.dnn.readNetFromTensorflow(
    "models/ssdlite_mobilenet_v2_coco_2018_05_09/frozen_inference_graph.pb", 
    "models/ssdlite_mobilenet_v2_coco_2018_05_09/graph.pbtxt"
)

Nous commençons les flux de caméras:

#  2 : RGB  depth
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
profile = pipeline.start(config)

notez que nous capturons le flux de couleurs au format BGR , car c'est le format qui utilise OpenCV par défaut.

Nous commençons un cycle dans lequel nous allons séquentiellement capturer et traiter les trames:

cv.namedWindow("RealSense object detection", cv.WINDOW_AUTOSIZE)
try:
    while True:
        #     ""  ""
        frames = pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()
        if not depth_frame or not color_frame:
            continue

Pour un traitement ultérieur des trames, nous les représentons sous la forme de tableaux numpy :

        #    numpy-
        depth_image = np.asanyarray(depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())

Nous utilisons le modèle ssd pour détecter:

        #  
        model.setInput(cv.dnn.blobFromImage(color_image, size=(300, 300), swapRB=True, crop=False))
        pred = model.forward()
        draw_predictions(pred, color_image, depth_image)

Nous dessinons le résultat dans la fenêtre:

	#     
        colorized_depth = cv.applyColorMap(cv.convertScaleAbs(depth_image, alpha=0.03), cv.COLORMAP_JET)
        #    
        images = np.hstack((color_image, colorized_depth))
        cv.imshow("RealSense object detection", images)

Nous écrivons la condition pour sortir de la boucle:

	#    'ESC'  'q'
        key = cv.waitKey(1) & 0xFF
        if (key == 27) or (key == ord('q')):
            cv.destroyWindow("RealSense object detection")
            break

Fermez le flux:

finally:
    pipeline.stop()

Et la dernière. Nous remplissons la méthode de rendu draw_predictions () que nous avons créée au tout début. Dans ce document, nous considérerons la distance aux objets. J'ai décidé de calculer la distance comme suit: prendre la moyenne de tous les points de distance dans le cadre de l'objet trouvé:

def draw_predictions(pred_img, color_img, depth_image):
    for detection in pred_img[0,0,:,:]:
        score = float(detection[2])
        #            50%
        if score > 0.5:
            left = detection[3] * color_img.shape[1]
            top = detection[4] * color_img.shape[0]
            right = detection[5] * color_img.shape[1]
            bottom = detection[6] * color_img.shape[0]
            cv.rectangle(color_img, (int(left), int(top)), (int(right), int(bottom)), (210, 230, 23), 2)
            
            #    
            depth = depth_image[int(left):int(right), int(top):int(bottom)].astype(float)
            depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
            depth = depth * depth_scale
            dist,_,_,_ = cv.mean(depth)
            dist = round(dist, 1)
            cv.putText(color_img, "dist: "+str(dist)+"m", (int(left), int(top)-5), cv.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 1)

image

Code complet
import pyrealsense2 as rs
import numpy as np
import cv2 as cv

def draw_predictions(pred_img, color_img, depth_image): # <-        
    for detection in pred_img[0,0,:,:]:
        score = float(detection[2])
        #            50%
        if score > 0.5:
            left = detection[3] * color_img.shape[1]
            top = detection[4] * color_img.shape[0]
            right = detection[5] * color_img.shape[1]
            bottom = detection[6] * color_img.shape[0]
            cv.rectangle(color_img, (int(left), int(top)), (int(right), int(bottom)), (210, 230, 23), 2)
            
            #    
            depth = depth_image[int(left):int(right), int(top):int(bottom)].astype(float)
            depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
            depth = depth * depth_scale
            dist,_,_,_ = cv.mean(depth)
            dist = round(dist, 1)
            cv.putText(color_img, "dist: "+str(dist)+"m", (int(left), int(top)-5), cv.FONT_HERSHEY_PLAIN, 1, (0, 255, 0), 1)


pipeline = rs.pipeline() # <-  pipeline      
config = rs.config() # <-      
colorizer = rs.colorizer() # <-      

#  
model = cv.dnn.readNetFromTensorflow(
    "models/ssdlite_mobilenet_v2_coco_2018_05_09/frozen_inference_graph.pb", 
    "models/ssdlite_mobilenet_v2_coco_2018_05_09/graph.pbtxt"
)

#  2 : RGB  depth
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
profile = pipeline.start(config)

cv.namedWindow("RealSense object detection", cv.WINDOW_AUTOSIZE)
try:
    while True:
        #     ""  ""
        frames = pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()
        if not depth_frame or not color_frame:
            continue

        #    numpy-
        depth_image = np.asanyarray(depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())
        
        #  
        model.setInput(cv.dnn.blobFromImage(color_image, size=(300, 300), swapRB=True, crop=False))
        pred = model.forward()
        draw_predictions(pred, color_image, depth_image)

        #     
        colorized_depth = cv.applyColorMap(cv.convertScaleAbs(depth_image, alpha=0.03), cv.COLORMAP_JET)
        #    
        images = np.hstack((color_image, colorized_depth))
        cv.imshow("RealSense object detection", images)
        
        #    'ESC'  'q'
        key = cv.waitKey(1) & 0xFF
        if (key == 27) or (key == ord('q')):
            cv.destroyWindow("RealSense object detection")
            break

finally:
    pipeline.stop()


Conclusion


Ainsi, nous avons un script simple pour estimer la distance à l'objet sélectionné. L'algorithme peut faire une erreur d'environ 50 cm, mais dans l'ensemble, il fonctionne bien à moins de quatre mètres. Pour augmenter la précision de la mesure, vous pouvez appliquer des filtres supplémentaires intégrés dans pyrealsense2 à la carte de profondeur pour augmenter la qualité de l'image, ou modifier l'algorithme de calcul de la profondeur lui-même (par exemple, calculer une moyenne pondérée ou mesurer la distance dans une zone centrale). Comme vous pouvez le voir, la tâche n'est pas difficile, mais intéressante.

Source: https://habr.com/ru/post/undefined/

More articles:


All Articles