import cv2
import depthai as dai
from depthai_sdk.fps import FPSHandler
# Create pipeline
pipeline = dai.Pipeline()
pipeline.setXLinkChunkSize(0)
# Define sources and outputs
camRgb = pipeline.create(dai.node.ColorCamera)
monoLeft = pipeline.create(dai.node.MonoCamera)
monoRight = pipeline.create(dai.node.MonoCamera)
stereo = pipeline.create(dai.node.StereoDepth)
xoutRgb = pipeline.create(dai.node.XLinkOut)
xoutDepth = pipeline.create(dai.node.XLinkOut)
xoutRgb.setStreamName("rgb")
xoutDepth.setStreamName("depth")
# mono and rgb properties
set_fps = 30
camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_12_MP)
camRgb.setFps(set_fps)
monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
monoLeft.setBoardSocket(dai.CameraBoardSocket.LEFT)
monoLeft.setFps(set_fps)
monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT)
monoRight.setFps(set_fps)
# stereoConfig
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
stereo.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7)
stereoConfig = stereo.initialConfig.get()
stereoConfig.postProcessing.speckleFilter.enable = True
stereoConfig.postProcessing.speckleFilter.speckleRange = 50
stereoConfig.postProcessing.spatialFilter.enable = True
stereoConfig.postProcessing.spatialFilter.holeFillingRadius = 2
stereoConfig.postProcessing.spatialFilter.numIterations = 1
stereoConfig.postProcessing.thresholdFilter.minRange = 150
stereoConfig.postProcessing.thresholdFilter.maxRange = 1000
stereoConfig.postProcessing.decimationFilter.decimationFactor = 2
stereo.initialConfig.set(stereoConfig)
stereo.setLeftRightCheck(True)
stereo.setExtendedDisparity(True)
# Align depth map to rgb
stereo.setDepthAlign(dai.CameraBoardSocket.RGB)
stereo.setOutputSize(800, 599)
# Linking
monoLeft.out.link(stereo.left)
monoRight.out.link(stereo.right)
camRgb.isp.link(xoutRgb.input)
stereo.depth.link(xoutDepth.input)
with dai.Device(version=dai.OpenVINO.Version.VERSION_2021_4, maxUsbSpeed=dai.UsbSpeed.SUPER_PLUS) as device:
# try:
# calibData = device.readCalibration2()
# lensPosition = calibData.getLensPosition(dai.CameraBoardSocket.RGB)
# if lensPosition:
# camRgb.initialControl.setManualFocus(lensPosition)
# except:
# raise
print('USB SPEED', device.getUsbSpeed())
device.startPipeline(pipeline)
rgbQ = device.getOutputQueue('rgb', maxSize=4, blocking=False)
depthQ = device.getOutputQueue('depth', maxSize=4, blocking=False)
fps = FPSHandler()
fps2 = FPSHandler()
synced_msgs = None
while True:
if rgbQ.has():
rgbQ.get()
fps.nextIter()
print('RGB FPS', fps.fps())
if depthQ.has():
depthQ.get()
fps2.nextIter()
print('DEPTH FPS', fps2.fps())
if cv2.waitKey(1) == ord('q'):
break