Stereo Depth

1#!/usr/bin/env python3
2
3import cv2
4import depthai as dai
5import numpy as np
6
7pipeline = dai.Pipeline()
8monoLeft = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B)
9monoRight = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_C)
10stereo = pipeline.create(dai.node.StereoDepth)
11
12# Linking
13monoLeftOut = monoLeft.requestFullResolutionOutput()
14monoRightOut = monoRight.requestFullResolutionOutput()
15monoLeftOut.link(stereo.left)
16monoRightOut.link(stereo.right)
17
18stereo.setRectification(True)
19stereo.setExtendedDisparity(True)
20stereo.setLeftRightCheck(True)
21
22syncedLeftQueue = stereo.syncedLeft.createOutputQueue()
23syncedRightQueue = stereo.syncedRight.createOutputQueue()
24disparityQueue = stereo.disparity.createOutputQueue()
25
26colorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET)
27colorMap[0] = [0, 0, 0]  # to make zero-disparity pixels black
28
29with pipeline:
30    pipeline.start()
31    maxDisparity = 1
32    while pipeline.isRunning():
33        leftSynced = syncedLeftQueue.get()
34        rightSynced = syncedRightQueue.get()
35        disparity = disparityQueue.get()
36        assert isinstance(leftSynced, dai.ImgFrame)
37        assert isinstance(rightSynced, dai.ImgFrame)
38        assert isinstance(disparity, dai.ImgFrame)
39        cv2.imshow("left", leftSynced.getCvFrame())
40        cv2.imshow("right", rightSynced.getCvFrame())
41        npDisparity = disparity.getFrame()
42        maxDisparity = max(maxDisparity, np.max(npDisparity))
43        colorizedDisparity = cv2.applyColorMap(((npDisparity / maxDisparity) * 255).astype(np.uint8), colorMap)
44        cv2.imshow("disparity", colorizedDisparity)
45        key = cv2.waitKey(1)
46        if key == ord('q'):
47            pipeline.stop()
48            break