DepthAI OAK pointcloud

pointcloud.py

#!/usr/bin/env python3

import cv2
import numpy as np
import depthai as dai

out_depth      = False  # Disparity by default
out_rectified  = True   # Output and display rectified streams
lrcheck  = True   # Better handling for occlusions
extended = False  # Closer-in minimum depth, disparity range is doubled
subpixel = True   # Better accuracy for longer distance, fractional disparity 32-levels
# Options: MEDIAN_OFF, KERNEL_3x3, KERNEL_5x5, KERNEL_7x7
median   = dai.StereoDepthProperties.MedianFilter.KERNEL_7x7

print("StereoDepth config options:")
print("    Left-Right check:  ", lrcheck)
print("    Extended disparity:", extended)
print("    Subpixel:          ", subpixel)
print("    Median filtering:  ", median)

pipeline = dai.Pipeline()

monoLeft = pipeline.create(dai.node.MonoCamera)
monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
monoLeft.setBoardSocket(dai.CameraBoardSocket.LEFT)

monoRight = pipeline.create(dai.node.MonoCamera)
monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT)

stereo = pipeline.createStereoDepth()
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
stereo.initialConfig.setMedianFilter(median)
stereo.setLeftRightCheck(lrcheck)
stereo.setExtendedDisparity(extended)
stereo.setSubpixel(subpixel)
monoLeft.out.link(stereo.left)
monoRight.out.link(stereo.right)

xout_depth = pipeline.createXLinkOut()
xout_depth.setStreamName('depth')
stereo.depth.link(xout_depth.input)

xout_disparity = pipeline.createXLinkOut()
xout_disparity.setStreamName('disparity')
# stereo.disparity.link(xout_disparity.input)

xout_rectif_right = pipeline.createXLinkOut()
xout_rectif_right.setStreamName('rectified_right')
stereo.rectifiedRight.link(xout_rectif_right.input)

with dai.Device(pipeline) as device:
    qDepth = device.getOutputQueue("depth")
    qDisp = device.getOutputQueue("disparity")
    qRec = device.getOutputQueue("rectified_right")

    try:
        from projector_3d import PointCloudVisualizer
    except ImportError as e:
        raise ImportError(f"\033[1;5;31mError occured when importing PCL projector: {e}. Try disabling the point cloud \033[0m ")
    calibData = device.readCalibration()
    right_intrinsic = np.array(calibData.getCameraIntrinsics(dai.CameraBoardSocket.RIGHT, 640, 400))
    pcl_converter = PointCloudVisualizer(right_intrinsic, 640, 400)

    while True:
        depth = qDepth.get().getFrame()
        last_rectif_right = qRec.get().getCvFrame()

        pcl_converter.rgbd_to_projection(depth, last_rectif_right, False)
        pcl_converter.visualize_pcd()

        if cv2.waitKey(1) == ord('q'):
            break