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
22disparityQueue = stereo.disparity.createOutputQueue()
23
24colorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET)
25colorMap[0] = [0, 0, 0]  # to make zero-disparity pixels black
26
27with pipeline:
28    pipeline.start()
29    maxDisparity = 1
30    while pipeline.isRunning():
31        disparity = disparityQueue.get()
32        assert isinstance(disparity, dai.ImgFrame)
33        npDisparity = disparity.getFrame()
34        maxDisparity = max(maxDisparity, np.max(npDisparity))
35        colorizedDisparity = cv2.applyColorMap(((npDisparity / maxDisparity) * 255).astype(np.uint8), colorMap)
36        cv2.imshow("disparity", colorizedDisparity)
37        key = cv2.waitKey(1)
38        if key == ord('q'):
39            pipeline.stop()
40            break