Get best of the week

Medición de distancia a objetos con RealSense D435

La tarea de detectar objetos en la imagen hoy es una de las principales en el campo de la visión artificial. Su esencia no es solo clasificar el objeto en la imagen, sino también indicar su ubicación exacta.

Los resultados de la detección de objetos se pueden complementar con información sobre qué tan lejos se encuentra este objeto. El problema de medición de distancia se puede resolver con la cámara de profundidad Intel RealSense D435 , que mide la profundidad en cada punto.

En este artículo, resolveremos el problema de medir la distancia a un objeto en tiempo real usando la biblioteca OpenCV y la tecnología RealSense.

imagen

Requisitos


Cámara RealSense

Para medir la distancia, necesitamos una cámara de profundidad RealSense D435 . Para este ejemplo, no es necesario instalar RealSense SDK .

Biblioteca Pyrealsense2

Todo el código, para facilitar la demostración, lo escribiremos en Python 3.7 , por lo que necesitamos el paquete pyrealsense2 .

pip install pyrealsense2

Biblioteca OpenCV

Además, necesitamos la biblioteca OpenCV (cualquier versión a partir de 3.4 es adecuada). El manual de instalación de la biblioteca se puede ver en el sitio web oficial de OpenCV .

Junto con OpenCV , se instalará una biblioteca más necesaria: numpy .

Detección de objetos


El primer paso es seleccionar objetos en el video. Para detectar objetos en tiempo real, los llamados modelos de una etapa (por ejemplo, Retina Net, SSD, YOLO ), que ganan en velocidad de operación, son perfectos .

Para simplificar el experimento, tomaremos el modelo entrenado SSDLite Mobilenet v2 . En cambio, se puede utilizar cualquier otro modelo que devuelva las coordenadas del objeto de una forma u otra.
Puede descargar el archivo con los archivos del modelo en el repositorio oficial de TensorFlow .

El archivo contiene un gráfico de cálculo congelado en formato binario .pb, así como varias configuraciones. Pero para usar el modelo en OpenCV, también debe tener un gráfico de cálculos en el formato de texto .pbtxt . No está en el archivo, por lo que debe generarse manualmente.

Copie dos scripts del repositorio de 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

Luego, ejecute el siguiente comando, especificando las rutas necesarias:

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

El resultado es un archivo graph.pbtxt .

Ponemos los archivos de modelo existentes en la carpeta del proyecto. En mi caso, este es el modelo / carpeta .

Desarrollo


Ahora todo está listo para el desarrollo. Comencemos a escribir código.

Importamos las bibliotecas necesarias y creamos un stub en el método draw_predictions () , que necesitaremos más adelante:

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

def draw_predictions(pred_img, color_img, depth_image):
    pass

Crea los objetos principales:

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

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

Inicializamos el modelo ssd . Aquí, el archivo .pbtxt que generamos anteriormente es útil para nosotros :

#  
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"
)

Comenzamos transmisiones de cámara:

#  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)

tenga en cuenta que capturamos la secuencia de color en formato BGR , ya que este es el formato que usa OpenCV de forma predeterminada.

Comenzamos un ciclo en el que capturaremos secuencialmente y procesaremos marcos:

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

Para el procesamiento posterior de marcos, los representamos en forma de matrices numpy :

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

Utilizamos el modelo ssd para detectar:

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

Dibujamos el resultado en la ventana:

	#     
        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)

Escribimos la condición para salir del bucle:

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

Cierra la secuencia:

finally:
    pipeline.stop()

Y el último. Completamos el método de representación draw_predictions () que creamos al principio. En él, consideraremos la distancia a los objetos. Decidí calcular la distancia de la siguiente manera: tomar el promedio de todos los puntos de distancia en el marco del objeto encontrado:

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)

imagen

Código completo
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()


Conclusión


Por lo tanto, tenemos un script simple para estimar la distancia al objeto seleccionado. El algoritmo puede cometer un error de unos 50 cm, pero en general funciona bien dentro de los cuatro metros. Para aumentar la precisión de la medición, puede aplicar filtros adicionales incrustados en pyrealsense2 al mapa de profundidad para aumentar la calidad de la imagen, o modificar el algoritmo de cálculo de profundidad en sí (por ejemplo, calcular un promedio ponderado o medir la distancia en un área central). Como puede ver, la tarea no es difícil, pero sí interesante.

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

More articles:


All Articles