|
| 1 | +#!/usr/bin/env python3 |
| 2 | + |
| 3 | +import cv2 |
| 4 | +import depthai as dai |
| 5 | +import numpy as np |
| 6 | + |
| 7 | +withDepth = True |
| 8 | + |
| 9 | +outputDepth = False |
| 10 | +outputRectified = True |
| 11 | +lrcheck = True |
| 12 | +extended = False |
| 13 | +subpixel = False |
| 14 | + |
| 15 | +# Create pipeline |
| 16 | +pipeline = dai.Pipeline() |
| 17 | + |
| 18 | +# Define sources and outputs |
| 19 | +monoLeft = pipeline.createMonoCamera() |
| 20 | +monoRight = pipeline.createMonoCamera() |
| 21 | +if withDepth: |
| 22 | + stereo = pipeline.createStereoDepth() |
| 23 | + |
| 24 | +xoutLeft = pipeline.createXLinkOut() |
| 25 | +xoutRight = pipeline.createXLinkOut() |
| 26 | +xoutDisp = pipeline.createXLinkOut() |
| 27 | +xoutDepth = pipeline.createXLinkOut() |
| 28 | +xoutRectifL = pipeline.createXLinkOut() |
| 29 | +xoutRectifR = pipeline.createXLinkOut() |
| 30 | + |
| 31 | +# XLinkOut |
| 32 | +xoutLeft.setStreamName("left") |
| 33 | +xoutRight.setStreamName("right") |
| 34 | +if withDepth: |
| 35 | + xoutDisp.setStreamName("disparity") |
| 36 | + xoutDepth.setStreamName("depth") |
| 37 | + xoutRectifL.setStreamName("rectified_left") |
| 38 | + xoutRectifR.setStreamName("rectified_right") |
| 39 | + |
| 40 | +# Properties |
| 41 | +monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) |
| 42 | +monoLeft.setBoardSocket(dai.CameraBoardSocket.LEFT) |
| 43 | +monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P) |
| 44 | +monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT) |
| 45 | + |
| 46 | +if withDepth: |
| 47 | + # StereoDepth |
| 48 | + stereo.setConfidenceThreshold(200) |
| 49 | + stereo.setRectifyEdgeFillColor(0) # black, to better see the cutout |
| 50 | + # stereo.setInputResolution(1280, 720) |
| 51 | + stereo.setMedianFilter(dai.StereoDepthProperties.MedianFilter.MEDIAN_OFF) |
| 52 | + stereo.setLeftRightCheck(lrcheck) |
| 53 | + stereo.setExtendedDisparity(extended) |
| 54 | + stereo.setSubpixel(subpixel) |
| 55 | + |
| 56 | + # Linking |
| 57 | + monoLeft.out.link(stereo.left) |
| 58 | + monoRight.out.link(stereo.right) |
| 59 | + |
| 60 | + stereo.syncedLeft.link(xoutLeft.input) |
| 61 | + stereo.syncedRight.link(xoutRight.input) |
| 62 | + stereo.disparity.link(xoutDisp.input) |
| 63 | + |
| 64 | + if outputRectified: |
| 65 | + stereo.rectifiedLeft.link(xoutRectifL.input) |
| 66 | + stereo.rectifiedRight.link(xoutRectifR.input) |
| 67 | + |
| 68 | + if outputDepth: |
| 69 | + stereo.depth.link(xoutDepth.input) |
| 70 | + |
| 71 | +else: |
| 72 | + # Link plugins CAM . XLINK |
| 73 | + monoLeft.out.link(xoutLeft.input) |
| 74 | + monoRight.out.link(xoutRight.input) |
| 75 | + |
| 76 | +# Connect to device and start pipeline |
| 77 | +with dai.Device(pipeline) as device: |
| 78 | + |
| 79 | + leftQueue = device.getOutputQueue(name="left", maxSize=8, blocking=False) |
| 80 | + rightQueue = device.getOutputQueue(name="right", maxSize=8, blocking=False) |
| 81 | + if (withDepth): |
| 82 | + dispQueue = device.getOutputQueue(name="disparity", maxSize=8, blocking=False) |
| 83 | + depthQueue = device.getOutputQueue(name="depth", maxSize=8, blocking=False) |
| 84 | + rectifLeftQueue = device.getOutputQueue(name="rectified_left", maxSize=8, blocking=False) |
| 85 | + rectifRightQueue = device.getOutputQueue(name="rectified_right", maxSize=8, blocking=False) |
| 86 | + |
| 87 | + # Disparity range is used for normalization |
| 88 | + disparityMultiplier = 255 / stereo.getMaxDisparity() |
| 89 | + |
| 90 | + while True: |
| 91 | + left = leftQueue.get() |
| 92 | + cv2.imshow("left", left.getFrame()) |
| 93 | + right = rightQueue.get() |
| 94 | + cv2.imshow("right", right.getFrame()) |
| 95 | + |
| 96 | + if withDepth: |
| 97 | + disparity = dispQueue.get() |
| 98 | + disp = disparity.getCvFrame() |
| 99 | + disp = (disp*disparityMultiplier).astype(np.uint8) # Extended disparity range |
| 100 | + cv2.imshow("disparity", disp) |
| 101 | + disp = cv2.applyColorMap(disp, cv2.COLORMAP_JET) |
| 102 | + cv2.imshow("disparity_color", disp) |
| 103 | + |
| 104 | + if outputDepth: |
| 105 | + depth = depthQueue.get() |
| 106 | + cv2.imshow("depth", depth.getCvFrame().astype(np.uint16)) |
| 107 | + |
| 108 | + if outputRectified: |
| 109 | + rectifL = rectifLeftQueue.get() |
| 110 | + cv2.imshow("rectified_left", rectifL.getFrame()) |
| 111 | + |
| 112 | + rectifR = rectifRightQueue.get() |
| 113 | + cv2.imshow("rectified_right", rectifR.getFrame()) |
| 114 | + |
| 115 | + if cv2.waitKey(1) == ord('q'): |
| 116 | + break |
0 commit comments