Stereo Depth Calibration Update

1#!/usr/bin/env python3
2
3import cv2
4import depthai as dai
5import numpy as np
6
7device = dai.Device()
8calibration = device.readCalibration()
9pipeline = dai.Pipeline(device)
10monoLeft = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B)
11monoRight = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_C)
12stereo = pipeline.create(dai.node.StereoDepth)
13
14# Linking
15monoLeftOut = monoLeft.requestFullResolutionOutput(type=dai.ImgFrame.Type.NV12)
16monoRightOut = monoRight.requestFullResolutionOutput(type=dai.ImgFrame.Type.NV12)
17monoLeftOut.link(stereo.left)
18monoRightOut.link(stereo.right)
19
20stereo.setRectification(True)
21stereo.setExtendedDisparity(True)
22stereo.setLeftRightCheck(True)
23
24rectifiedLeftQueue = stereo.rectifiedLeft.createOutputQueue()
25rectifiedRightQueue = stereo.rectifiedRight.createOutputQueue()
26disparityQueue = stereo.disparity.createOutputQueue()
27
28colorMap = cv2.applyColorMap(np.arange(256, dtype=np.uint8), cv2.COLORMAP_JET)
29colorMap[0] = [0, 0, 0]  # to make zero-disparity pixels black
30
31with pipeline:
32    pipeline.start()
33    maxDisparity = 1
34    while pipeline.isRunning():
35        leftRectified = rectifiedLeftQueue.get()
36        rightRectified = rectifiedRightQueue.get()
37        disparity = disparityQueue.get()
38        assert isinstance(leftRectified, dai.ImgFrame)
39        assert isinstance(rightRectified, dai.ImgFrame)
40        assert isinstance(disparity, dai.ImgFrame)
41        cv2.imshow("left", leftRectified.getCvFrame())
42        cv2.imshow("right", rightRectified.getCvFrame())
43        npDisparity = disparity.getFrame()
44        maxDisparity = max(maxDisparity, np.max(npDisparity))
45        colorizedDisparity = cv2.applyColorMap(((npDisparity / maxDisparity) * 255).astype(np.uint8), colorMap)
46        cv2.imshow("disparity", colorizedDisparity)
47        key = cv2.waitKey(1)
48        if key == ord('q'):
49            pipeline.stop()
50            break
51        elif key == ord('u'):
52            randomDistortionCoeffs = np.random.rand(14)
53            calibration.setDistortionCoefficients(dai.CameraBoardSocket.CAM_B, randomDistortionCoeffs)
54            calibration.setDistortionCoefficients(dai.CameraBoardSocket.CAM_C, randomDistortionCoeffs)
55            try:
56                device.setCalibration(calibration)
57            except:
58                print("Failed to update calibration!")
59            try:
60                updatedCalib = device.getCalibration()
61                distortionCoeffs = updatedCalib.getDistortionCoefficients(dai.CameraBoardSocket.CAM_C)
62                print("Updated distortion coefficients: ", distortionCoeffs)
63            except:
64                pass