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

1#include <opencv2/opencv.hpp>
2
3#include "depthai/depthai.hpp"
4
5int main() {
6    dai::Pipeline pipeline;
7
8    auto monoLeft = pipeline.create<dai::node::Camera>()->build(dai::CameraBoardSocket::CAM_B);
9    auto monoRight = pipeline.create<dai::node::Camera>()->build(dai::CameraBoardSocket::CAM_C);
10
11    auto stereo = pipeline.create<dai::node::StereoDepth>();
12
13    auto monoLeftOut = monoLeft->requestFullResolutionOutput();
14    auto monoRightOut = monoRight->requestFullResolutionOutput();
15
16    monoLeftOut->link(stereo->left);
17    monoRightOut->link(stereo->right);
18
19    stereo->setRectification(true);
20    stereo->setExtendedDisparity(true);
21    stereo->setLeftRightCheck(true);
22
23    auto disparityQueue = stereo->disparity.createOutputQueue();
24
25    double maxDisparity = 1.0;
26    pipeline.start();
27    while(true) {
28        auto disparity = disparityQueue->get<dai::ImgFrame>();
29        cv::Mat npDisparity = disparity->getFrame();
30
31        double minVal, curMax;
32        cv::minMaxLoc(npDisparity, &minVal, &curMax);
33        maxDisparity = std::max(maxDisparity, curMax);
34
35        // Normalize the disparity image to an 8-bit scale.
36        cv::Mat normalized;
37        npDisparity.convertTo(normalized, CV_8UC1, 255.0 / maxDisparity);
38
39        cv::Mat colorizedDisparity;
40        cv::applyColorMap(normalized, colorizedDisparity, cv::COLORMAP_JET);
41
42        // Set pixels with zero disparity to black.
43        colorizedDisparity.setTo(cv::Scalar(0, 0, 0), normalized == 0);
44
45        cv::imshow("disparity", colorizedDisparity);
46
47        int key = cv::waitKey(1);
48        if(key == 'q') {
49            break;
50        }
51    }
52
53    pipeline.stop();
54    return 0;
55}