Hi @John194
import numpy as np
import cv2
import depthai as dai
from projector_3d import PointCloudVisualizer
FPS = 10
RGB_SOCKET = dai.CameraBoardSocket.RGB
LEFT_SOCKET = dai.CameraBoardSocket.LEFT
RIGHT_SOCKET = dai.CameraBoardSocket.RIGHT
COLOR_RESOLUTION = dai.ColorCameraProperties.SensorResolution.THE_1080_P
LEFT_RIGHT_RESOLUTION = dai.MonoCameraProperties.SensorResolution.THE_800_P
depthSize = (1280, 800)
rgbSize = (1280, 800)
# Used for colorizing the depth map
MIN_DEPTH = 500 # mm
MAX_DEPTH = 10000 # mm
device = dai.Device()
def get_calibration(device, LEFT_SOCKET, RGB_SOCKET, size = (1280,800)):
calibData = device.readCalibration2()
M1 = np.array(calibData.getCameraIntrinsics(LEFT_SOCKET, *depthSize))
D1 = np.array(calibData.getDistortionCoefficients(LEFT_SOCKET))
M2 = np.array(calibData.getCameraIntrinsics(RGB_SOCKET, *rgbSize))
D2 = np.array(calibData.getDistortionCoefficients(RGB_SOCKET))
T = (
np.array(calibData.getCameraTranslationVector(LEFT_SOCKET, RGB_SOCKET, False))
) # to mm for matching the depth
R = np.array(calibData.getCameraExtrinsics(LEFT_SOCKET, RGB_SOCKET, False))[
0:3, 0:3
]
return M1, D1, M2, D2 ,T ,R
def align_images(left, right, M1, D1, M2, D2, T, R, size = (1280,800)):
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(M1, D1, M2, D2, size, R, T)
# Compute the undistortion and rectification transformation map
left = cv2.resize(left, size)
right = cv2.resize(right, size)
map1x, map1y = cv2.initUndistortRectifyMap(M1, D1, R1, P1, size, cv2.CV_32FC1)
map2x, map2y = cv2.initUndistortRectifyMap(M2, D2, R2, P2, size, cv2.CV_32FC1)
# Remap the images
rectified1 = cv2.remap(left, map1x, map1y, cv2.INTER_LINEAR)
rectified2 = cv2.remap(right, map2x, map2y, cv2.INTER_LINEAR)
return rectified1, rectified2
pipeline = dai.Pipeline()
# Define sources and outputs
camRgb = pipeline.create(dai.node.ColorCamera)
left = pipeline.create(dai.node.MonoCamera)
right = pipeline.create(dai.node.MonoCamera)
stereo = pipeline.create(dai.node.StereoDepth)
sync = pipeline.create(dai.node.Sync)
out = pipeline.create(dai.node.XLinkOut)
left.setResolution(LEFT_RIGHT_RESOLUTION)
left.setBoardSocket(LEFT_SOCKET)
left.setFps(FPS)
right.setResolution(LEFT_RIGHT_RESOLUTION)
right.setBoardSocket(RIGHT_SOCKET)
right.setFps(FPS)
camRgb.setBoardSocket(RGB_SOCKET)
camRgb.setResolution(COLOR_RESOLUTION)
camRgb.setFps(FPS)
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
stereo.setExtendedDisparity(True)
stereo.setLeftRightCheck(True)
stereo.setDepthAlign(LEFT_SOCKET)
stereo.enableDistortionCorrection(True)
stereo.setDepthAlignmentUseSpecTranslation(False)
stereo.setRectificationUseSpecTranslation(False)
stereo.setFocalLengthFromCalibration(True)
stereo.setDisparityToDepthUseSpecTranslation(False)
out.setStreamName("out")
# Linking
camRgb.isp.link(sync.inputs["rgb"])
left.out.link(stereo.left)
right.out.link(stereo.right)
left.out.link(sync.inputs["left"])
right.out.link(sync.inputs["right"])
stereo.depth.link(sync.inputs["depth"])
sync.out.link(out.input)
def colorizeDepth(frameDepth, minDepth=MIN_DEPTH, maxDepth=MAX_DEPTH):
invalidMask = frameDepth == 0
# Log the depth, minDepth and maxDepth
logDepth = np.log(frameDepth, where=frameDepth != 0)
logMinDepth = np.log(minDepth)
logMaxDepth = np.log(maxDepth)
depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)).astype(
np.uint8
)
depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET)
# Set invalid depth pixels to black
depthFrameColor[invalidMask] = 0
return depthFrameColor
rgbWeight = 0.4
depthWeight = 0.6
def updateBlendWeights(percent_rgb):
"""
Update the rgb and depth weights used to blend depth/rgb image
@param[in] percent_rgb The rgb weight expressed as a percentage (0..100)
"""
global depthWeight
global rgbWeight
rgbWeight = float(percent_rgb) / 100.0
depthWeight = 1.0 - rgbWeight
# Connect to device and start pipeline
with device:
device.startPipeline(pipeline)
queue = device.getOutputQueue("out", 8, False)
# Configure windows; trackbar adjusts blending ratio of rgb/depth
rgb_depth_window_name = "rgb-depth"
cv2.namedWindow(rgb_depth_window_name)
cv2.createTrackbar(
"RGB Weight %",
rgb_depth_window_name,
int(rgbWeight * 100),
100,
updateBlendWeights,
)
M1, D1, M2, D2, T, R = get_calibration(device, LEFT_SOCKET, RGB_SOCKET, depthSize)
while True:
messageGroup: dai.MessageGroup = queue.get()
frameRgb: dai.ImgFrame = messageGroup["rgb"]
frameleft: dai.ImgFrame = messageGroup["left"]
frameDepth: dai.ImgFrame = messageGroup["depth"]
# Blend when both received
if frameRgb is not None and frameDepth is not None and frameleft is not None:
frameRgb = frameRgb.getCvFrame()
frameleft = frameleft.getCvFrame()
rectified1, rectified2 = align_images(frameleft, frameRgb, M1, D1, M2, D2, T, R)
rectified2 = cv2.cvtColor(rectified2, cv2.COLOR_BGR2GRAY)
# Save or display the rectified images
cv2.imshow('rectified1.jpg', rectified1)
cv2.imshow('rectified2.jpg', rectified2)
#alignedDepthColorized = cv2.cvtColor(alignedDepthColorized, cv2.COLOR_BGR2GRAY)
blended = cv2.addWeighted(rectified1, rgbWeight, rectified2, depthWeight, 0)
cv2.imshow(rgb_depth_window_name, blended)
key = cv2.waitKey(1)
if key == ord("q"):
break
