Stereo Depth

View as Markdown

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