Data hosted with ♥ by Pastebin.com - Download Raw - See Original
  1. from MultiMsgSyncV2 import TwoStageHostSeqSync
  2. import blobconverter
  3. import cv2
  4. import depthai as dai
  5. import numpy as np
  6. import datetime
  7. import argparse
  8. import json
  9. import sys
  10. from imutils.video import FPS
  11.  
  12. emotionsclass = ['neutral', 'happy', 'sad', 'surprise', 'anger']
  13.  
  14. fps = FPS().start()
  15.  
  16. def frame_norm(frame, bbox):
  17.     normVals = np.full(len(bbox), frame.shape[0])
  18.     normVals[::2] = frame.shape[1]
  19.     return (np.clip(np.array(bbox), 0, 1) * normVals).astype(int)
  20.  
  21. def create_pipeline(stereo):
  22.     pipeline = dai.Pipeline()
  23.  
  24.     cam = pipeline.create(dai.node.ColorCamera)
  25.     cam.setPreviewSize(640, 400)
  26.     cam.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
  27.     cam.setInterleaved(False)
  28.     cam.setBoardSocket(dai.CameraBoardSocket.RGB)
  29.  
  30.     manipRgb = pipeline.create(dai.node.ImageManip)
  31.     rgbRr = dai.RotatedRect()
  32.     rgbRr.center.x, rgbRr.center.y = cam.getPreviewWidth() // 2, cam.getPreviewHeight() // 2
  33.     rgbRr.size.width, rgbRr.size.height = cam.getPreviewHeight(), cam.getPreviewWidth()
  34.     rgbRr.angle = -90
  35.     manipRgb.initialConfig.setCropRotatedRect(rgbRr, False)
  36.  
  37.     manipRgbOut = pipeline.create(dai.node.XLinkOut)
  38.     manipRgbOut.setStreamName("color")
  39.     manipRgb.out.link(manipRgbOut.input)
  40.     cam.preview.link(manipRgb.inputImage)
  41.  
  42.     # ImageManip will resize the frame before sending it to the Face detection NN node
  43.     face_det_manip = pipeline.create(dai.node.ImageManip)
  44.     face_det_manip.initialConfig.setResize(300, 300)
  45.     face_det_manip.initialConfig.setFrameType(dai.RawImgFrame.Type.RGB888p)
  46.     rrFD = dai.RotatedRect()
  47.     rrFD.center.x, rrFD.center.y = cam.getPreviewWidth() // 2, cam.getPreviewHeight() // 2
  48.     rrFD.size.width, rrFD.size.height = cam.getPreviewHeight(), cam.getPreviewWidth()
  49.     rrFD.angle = -90
  50.     face_det_manip.initialConfig.setCropRotatedRect(rrFD, False)
  51.     cam.preview.link(face_det_manip.inputImage)
  52.  
  53.     if stereo:
  54.         monoLeft = pipeline.create(dai.node.MonoCamera)
  55.         monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
  56.         monoLeft.setBoardSocket(dai.CameraBoardSocket.LEFT)
  57.  
  58.         monoRight = pipeline.create(dai.node.MonoCamera)
  59.         monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
  60.         monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT)
  61.  
  62.         stereo = pipeline.create(dai.node.StereoDepth)
  63.         stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
  64.         stereo.setLeftRightCheck(True)
  65.         stereo.setDepthAlign(dai.CameraBoardSocket.RGB)
  66.         stereo.setOutputSize(monoLeft.getResolutionWidth(), monoLeft.getResolutionHeight())
  67.         stereo.setExtendedDisparity(True)
  68.         stereo.setSubpixel(False)
  69.  
  70.         manipLeft = pipeline.create(dai.node.ImageManip)
  71.         rrL = dai.RotatedRect()
  72.         rrL.center.x, rrL.center.y = monoLeft.getResolutionWidth() // 2, monoLeft.getResolutionHeight() // 2
  73.         rrL.size.width, rrL.size.height = monoLeft.getResolutionHeight(), monoLeft.getResolutionWidth()
  74.         rrL.angle = -90
  75.         manipLeft.initialConfig.setCropRotatedRect(rrL, False)
  76.         monoLeft.out.link(manipLeft.inputImage)
  77.  
  78.         manipRight = pipeline.create(dai.node.ImageManip)
  79.         rrR = dai.RotatedRect()
  80.         rrR.center.x, rrR.center.y = monoRight.getResolutionWidth() // 2, monoRight.getResolutionHeight() // 2
  81.         rrR.size.width, rrR.size.height = monoRight.getResolutionHeight(), monoRight.getResolutionWidth()
  82.         rrR.angle = -90
  83.         manipRight.initialConfig.setCropRotatedRect(rrR, False)
  84.         monoRight.out.link(manipRight.inputImage)
  85.  
  86.         monoLeft.out.link(stereo.left)
  87.         monoRight.out.link(stereo.right)
  88.  
  89.         # Spatial Detection network if OAK-D
  90.         face_det_nn = pipeline.create(dai.node.MobileNetSpatialDetectionNetwork)
  91.         face_det_nn.input.setBlocking(False)
  92.         face_det_nn.setBoundingBoxScaleFactor(0.8)
  93.         face_det_nn.setDepthLowerThreshold(100)
  94.         face_det_nn.setDepthUpperThreshold(5000)
  95.         stereo.depth.link(face_det_nn.inputDepth)
  96.     else: # Detection network if OAK-1
  97.         face_det_nn = pipeline.create(dai.node.MobileNetDetectionNetwork)
  98.  
  99.     face_det_nn.setConfidenceThreshold(0.5)
  100.     face_det_nn.setBlobPath(blobconverter.from_zoo(name="face-detection-retail-0004", shaves=6))
  101.     face_det_manip.out.link(face_det_nn.input)
  102.  
  103.     # Send face detections to the host (for bounding boxes)
  104.     face_det_xout = pipeline.create(dai.node.XLinkOut)
  105.     face_det_xout.setStreamName("detection")
  106.     face_det_nn.out.link(face_det_xout.input)
  107.  
  108.     # Script node will take the output from the face detection NN as an input and set ImageManipConfig
  109.     # to the 'recognition_manip' to crop the initial frame
  110.     image_manip_script = pipeline.create(dai.node.Script)
  111.     face_det_nn.out.link(image_manip_script.inputs['face_det_in'])
  112.  
  113.     # Only send metadata, we are only interested in timestamp, so we can sync
  114.     # depth frames with NN output
  115.     face_det_nn.passthrough.link(image_manip_script.inputs['passthrough'])
  116.  
  117.     image_manip_script.setScript("""
  118.    l = [] # List of images
  119.    # So the correct frame will be the first in the list
  120.    # For this experiment this function is redundant, since everything
  121.    # runs in blocking mode, so no frames will get lost
  122.    def get_latest_frame(seq):
  123.        global l
  124.        for i, frame in enumerate(l):
  125.            #node.io['manip_frame'].send(frame)
  126.            if seq == frame.getSequenceNum():
  127.                # node.warn(f"List len {len(l)} Frame with same seq num: {i},seq {seq}")
  128.                l = l[i:]
  129.                break
  130.        return l[0]
  131.  
  132.    def correct_bb(bb):
  133.        if bb.xmin < 0: bb.xmin = 0.001
  134.        if bb.ymin < 0: bb.ymin = 0.001
  135.        if bb.xmax > 1: bb.xmax = 0.999
  136.        if bb.ymax > 1: bb.ymax = 0.999
  137.        return bb
  138.    while True:
  139.        preview = node.io['preview'].tryGet()
  140.        if preview is not None:
  141.            # node.warn(f"New frame {preview.getSequenceNum()}")
  142.            l.append(preview)
  143.  
  144.        face_dets = node.io['face_det_in'].tryGet()
  145.        # node.warn(f"Faces detected: {len(face_dets)}")
  146.        if face_dets is not None:
  147.            passthrough = node.io['passthrough'].get()
  148.            seq = passthrough.getSequenceNum()
  149.            # node.warn(f"New detection {seq}")
  150.            if len(l) == 0:
  151.                continue
  152.            img = get_latest_frame(seq)
  153.  
  154.            for i, det in enumerate(face_dets.detections):
  155.                cfg = ImageManipConfig()
  156.                correct_bb(det)
  157.                cfg.setCropRect(det.xmin, det.ymin, det.xmax, det.ymax)
  158.                # node.warn(f"Sending {i + 1}. det. Seq {seq}. Det {det.xmin}, {det.ymin}, {det.xmax}, {det.ymax}")
  159.                cfg.setResize(62, 62)
  160.                cfg.setKeepAspectRatio(False)
  161.                node.io['manip_cfg'].send(cfg)
  162.                node.io['manip_img'].send(img)
  163.  
  164.                cfg2 = ImageManipConfig()
  165.                cfg2.setCropRect(det.xmin, det.ymin, det.xmax, det.ymax)
  166.                # node.warn(f"Sending {i + 1}. det. Seq {seq}. Det {det.xmin}, {det.ymin}, {det.xmax}, {det.ymax}")
  167.                cfg2.setResize(64, 64)
  168.                cfg2.setKeepAspectRatio(False)
  169.                node.io['emanip_cfg'].send(cfg2)
  170.                node.io['emanip_img'].send(img)
  171.    """)
  172.     #cam.preview.link(image_manip_script.inputs['preview'])
  173.     manipRgb.out.link(image_manip_script.inputs['preview'])
  174.  
  175.     recognition_manip = pipeline.create(dai.node.ImageManip)
  176.     recognition_manip.initialConfig.setResize(62, 62)
  177.     #recognition_manip.setWaitForConfigInput(True)
  178.     image_manip_script.outputs['manip_cfg'].link(recognition_manip.inputConfig)
  179.     image_manip_script.outputs['manip_img'].link(recognition_manip.inputImage)
  180.  
  181.     # face_cropped_xout = pipeline.create(dai.node.XLinkOut)
  182.     # face_cropped_xout.setStreamName("face_cropped")
  183.     # recognition_manip.out.link(face_cropped_xout.input)
  184.  
  185.     # frame_xout = pipeline.create(dai.node.XLinkOut)
  186.     # frame_xout.setStreamName("frame_xout")
  187.     # image_manip_script.outputs['manip_frame'].link(frame_xout.input)
  188.  
  189.     # Second stange recognition NN
  190.     recognition_nn = pipeline.create(dai.node.NeuralNetwork)
  191.     recognition_nn.setBlobPath(blobconverter.from_zoo(name="age-gender-recognition-retail-0013", shaves=6))
  192.     recognition_manip.out.link(recognition_nn.input)
  193.  
  194.     recognition_xout = pipeline.create(dai.node.XLinkOut)
  195.     recognition_xout.setStreamName("recognition")
  196.     recognition_nn.out.link(recognition_xout.input)
  197.  
  198.     # third stange expression NN
  199.     expression_manip = pipeline.create(dai.node.ImageManip)
  200.     expression_manip.initialConfig.setResize(64, 64)
  201.     #expression_manip.setWaitForConfigInput(True)
  202.     image_manip_script.outputs['emanip_cfg'].link(expression_manip.inputConfig)
  203.     image_manip_script.outputs['emanip_img'].link(expression_manip.inputImage)
  204.  
  205.     expression_nn = pipeline.create(dai.node.NeuralNetwork)
  206.     expression_nn.setBlobPath(blobconverter.from_zoo(name="emotions-recognition-retail-0003", shaves=6))
  207.     expression_manip.out.link(expression_nn.input)
  208.  
  209.     expression_xout = pipeline.create(dai.node.XLinkOut)
  210.     expression_xout.setStreamName("emotions")
  211.     expression_nn.out.link(expression_xout.input)
  212.  
  213.     return pipeline
  214.  
  215. with dai.Device() as device:
  216.     stereo = 1 < len(device.getConnectedCameras())
  217.     device.startPipeline(create_pipeline(stereo))
  218.     # face_cropped_q = device.getOutputQueue("face_cropped", 4, False)
  219.     # frame_xout_q = device.getOutputQueue("frame_xout", 4, False)
  220.  
  221.     sync = TwoStageHostSeqSync()
  222.     queues = {}
  223.     # Create output queues
  224.     for name in ["color", "detection", "recognition", "emotions"]:
  225.         queues[name] = device.getOutputQueue(name)
  226.  
  227.     while True:
  228.         try:
  229.             for name, q in queues.items():
  230.                 # Add all msgs (color frames, object detections and recognitions) to the Sync class.
  231.                 if q.has():
  232.                     sync.add_msg(q.get(), name)
  233.  
  234.             # face_cropped_in = face_cropped_q.tryGet()
  235.             # if face_cropped_in is not None:
  236.             #     cv2.imshow("cropped", face_cropped_in.getCvFrame())
  237.  
  238.             # frame_in = frame_xout_q.tryGet()
  239.             # if frame_in is not None:
  240.             #     cv2.imshow("frame on host", frame_in.getCvFrame())
  241.            
  242.             msgs = sync.get_msgs()
  243.             resultList={"age":[],"gen_der":[],"dis_tance":[],"expression":[],"dtstamp":[]}
  244.  
  245.             if msgs is not None:
  246.                
  247.                 frame = msgs["color"].getCvFrame()
  248.                 detections = msgs["detection"].detections
  249.                 recognitions = msgs["recognition"]
  250.                 expressions = msgs["emotions"]
  251.  
  252.                 for i, detection in enumerate(detections):
  253.                     bbox = frame_norm(frame, (detection.xmin, detection.ymin, detection.xmax, detection.ymax))
  254.  
  255.                     # Decoding of recognition results
  256.                     age = 0
  257.                     gender_str = ""
  258.                     try:
  259.                         rec = recognitions[i]
  260.                         age = int(float(np.squeeze(np.array(rec.getLayerFp16('age_conv3')))) * 100) + 10
  261.                         gender = np.squeeze(np.array(rec.getLayerFp16('prob')))
  262.                         gender_str = "female" if gender[0] > gender[1] else "male"
  263.                     except:
  264.                         pass
  265.  
  266.                     emotion_name = ""
  267.                     try:
  268.                         exp = expressions[i]
  269.                         emotion_results = np.array(exp.getFirstLayerFp16())
  270.                         emotion_name = emotionsclass[np.argmax(emotion_results)]
  271.                     except:
  272.                         pass
  273.  
  274.                     Distance = 0
  275.  
  276.                     det_frame = frame[bbox[1]:bbox[3], bbox[0]:bbox[2]]
  277.                     fh, fw, fc = det_frame.shape
  278.                     frame_h, frame_w, frame_c = frame.shape
  279.  
  280.                     # Create blur mask around the face
  281.                     mask = np.zeros((frame_h, frame_w), np.uint8)
  282.                     polygon = cv2.ellipse2Poly((bbox[0] + int(fw /2), bbox[1] + int(fh/2)), (int(fw /2), int(fh/2)), 0,0,360,delta=1)
  283.                     cv2.fillConvexPoly(mask, polygon, 255)
  284.                     frame_copy = frame.copy()
  285.                     frame_copy = cv2.blur(frame_copy, (80, 80))
  286.                     face_extracted = cv2.bitwise_and(frame_copy, frame_copy, mask=mask)
  287.                     background_mask = cv2.bitwise_not(mask)
  288.                     background = cv2.bitwise_and(frame, frame, mask=background_mask)
  289.                     # Blur the face
  290.                     frame = cv2.add(background, face_extracted)
  291.                    
  292.                     #cv2.rectangle(frame, (bbox[0], bbox[1]), (bbox[2], bbox[3]), (10, 245, 10), 2)
  293.                     draw_border(frame,(bbox[0],bbox[1]), (bbox[2],bbox[3]), (0 ,255 , 255), 2, 5, 15)
  294.                     y = ((bbox[1] + bbox[3]) // 2) - 30
  295.                     cv2.putText(frame, str(age), (bbox[0], y), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 255), 2)
  296.                     cv2.putText(frame, gender_str, (bbox[0], y + 30), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 255), 2)
  297.                     cv2.putText(frame, emotion_name, (bbox[0], y + 60), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 255), 2)
  298.                     if stereo:
  299.                         # You could also get detection.spatialCoordinates.x and detection.spatialCoordinates.y coordinates
  300.                         coords = "{:.2f} m".format(detection.spatialCoordinates.z/1000)
  301.                         Distance = detection.spatialCoordinates.z/1000
  302.                         cv2.putText(frame, coords, (bbox[0], y + 90), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 2)
  303.  
  304.                 cv2.imshow("Camera", frame)
  305.                
  306.                 fps.update()
  307.  
  308.             if cv2.waitKey(1) == ord('q'):
  309.                 break
  310.  
  311.         except:
  312.             pass
  313.  
  314. fps.stop()
  315. print("FPS: {:.2f}".format(fps.fps()))
  316. cv2.destroyAllWindows()