From e6f60aaf758a507b82f346575d600a0fda3d3c06 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Mon, 18 Nov 2024 17:54:37 +0200 Subject: [PATCH 1/3] Fix ImageAlign examples --- examples/ImageAlign/depth_align.py | 2 +- examples/ImageAlign/image_align.py | 25 +++++++++++++++++++++---- examples/ImageAlign/thermal_align.py | 21 +++++++++++++++++++-- examples/ImageAlign/tof_align.py | 23 ++++++++++++++++++++--- 4 files changed, 61 insertions(+), 10 deletions(-) diff --git a/examples/ImageAlign/depth_align.py b/examples/ImageAlign/depth_align.py index dddfab913..b73ec10f3 100755 --- a/examples/ImageAlign/depth_align.py +++ b/examples/ImageAlign/depth_align.py @@ -172,7 +172,7 @@ def updateBlendWeights(percentRgb): cv2.imshow("Depth aligned", alignedDepthColorized) blended = cv2.addWeighted( - cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0 + cvFrameUndistorted, rgbWeight, alignedDepthColorized, depthWeight, 0 ) cv2.putText( blended, diff --git a/examples/ImageAlign/image_align.py b/examples/ImageAlign/image_align.py index c3c08ad0f..0afcdcc9a 100755 --- a/examples/ImageAlign/image_align.py +++ b/examples/ImageAlign/image_align.py @@ -3,6 +3,7 @@ import cv2 import depthai as dai from datetime import timedelta +import numpy as np # This is an interactive example that shows how two frame sources without depth information. FPS = 30.0 @@ -14,7 +15,17 @@ COLOR_RESOLUTION = dai.ColorCameraProperties.SensorResolution.THE_1080_P LEFT_RIGHT_RESOLUTION = dai.MonoCameraProperties.SensorResolution.THE_720_P +ISP_SCALE = 3 + device = dai.Device() + +calibrationHandler = device.readCalibration() +rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(1920 / ISP_SCALE), int(1080 / ISP_SCALE)) +rgbDistortion = calibrationHandler.getDistortionCoefficients(RGB_SOCKET) +distortionModel = calibrationHandler.getDistortionModel(RGB_SOCKET) +if distortionModel != dai.CameraModel.Perspective: + raise RuntimeError("Unsupported distortion model for RGB camera. This example supports only Perspective model.") + pipeline = dai.Pipeline() # Define sources and outputs @@ -32,7 +43,7 @@ camRgb.setBoardSocket(RGB_SOCKET) camRgb.setResolution(COLOR_RESOLUTION) camRgb.setFps(FPS) -camRgb.setIspScale(1, 3) +camRgb.setIspScale(1, ISP_SCALE) out.setStreamName("out") @@ -109,12 +120,18 @@ def updateDepthPlane(depth): # Colorize the aligned depth leftCv = leftAligned.getCvFrame() + cvFrameUndistorted = cv2.undistort( + frameRgbCv, + np.array(rgbIntrinsics), + np.array(rgbDistortion), + ) + if len(leftCv.shape) == 2: leftCv = cv2.cvtColor(leftCv, cv2.COLOR_GRAY2BGR) - if leftCv.shape != frameRgbCv.shape: - leftCv = cv2.resize(leftCv, (frameRgbCv.shape[1], frameRgbCv.shape[0])) + if leftCv.shape != cvFrameUndistorted.shape: + leftCv = cv2.resize(leftCv, (cvFrameUndistorted.shape[1], cvFrameUndistorted.shape[0])) - blended = cv2.addWeighted(frameRgbCv, rgbWeight, leftCv, leftWeight, 0) + blended = cv2.addWeighted(cvFrameUndistorted, rgbWeight, leftCv, leftWeight, 0) cv2.imshow(windowName, blended) key = cv2.waitKey(1) diff --git a/examples/ImageAlign/thermal_align.py b/examples/ImageAlign/thermal_align.py index f4523e424..9cdc8659e 100755 --- a/examples/ImageAlign/thermal_align.py +++ b/examples/ImageAlign/thermal_align.py @@ -40,6 +40,17 @@ def getFps(self): raise RuntimeError("No thermal camera found!") +ISP_SCALE = 3 + +device = dai.Device() + +calibrationHandler = device.readCalibration() +rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(1920 / ISP_SCALE), int(1080 / ISP_SCALE)) +rgbDistortion = calibrationHandler.getDistortionCoefficients(RGB_SOCKET) +distortionModel = calibrationHandler.getDistortionModel(RGB_SOCKET) +if distortionModel != dai.CameraModel.Perspective: + raise RuntimeError("Unsupported distortion model for RGB camera. This example supports only Perspective model.") + pipeline = dai.Pipeline() # Define sources and outputs @@ -57,7 +68,7 @@ def getFps(self): camRgb.setBoardSocket(RGB_SOCKET) camRgb.setResolution(COLOR_RESOLUTION) camRgb.setFps(FPS) -camRgb.setIspScale(1,3) +camRgb.setIspScale(1,ISP_SCALE) out.setStreamName("out") @@ -132,6 +143,12 @@ def updateDepthPlane(depth): frameRgbCv = frameRgb.getCvFrame() fpsCounter.tick() + cvFrameUndistorted = cv2.undistort( + frameRgbCv, + np.array(rgbIntrinsics), + np.array(rgbDistortion), + ) + # Colorize the aligned depth thermalFrame = thermalAligned.getCvFrame().astype(np.float32) # Create a mask for nan values @@ -143,7 +160,7 @@ def updateDepthPlane(depth): # Apply the mask back with black pixels (0) colormappedFrame[mask] = 0 - blended = cv2.addWeighted(frameRgbCv, rgbWeight, colormappedFrame, thermalWeight, 0) + blended = cv2.addWeighted(cvFrameUndistorted, rgbWeight, colormappedFrame, thermalWeight, 0) cv2.putText( blended, diff --git a/examples/ImageAlign/tof_align.py b/examples/ImageAlign/tof_align.py index d9ec664ac..ea312741e 100755 --- a/examples/ImageAlign/tof_align.py +++ b/examples/ImageAlign/tof_align.py @@ -29,8 +29,19 @@ def getFps(self): return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) +ISP_SCALE = 2 + +device = dai.Device() + +calibrationHandler = device.readCalibration() +rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(1920 / ISP_SCALE), int(1080 / ISP_SCALE)) +rgbDistortion = calibrationHandler.getDistortionCoefficients(RGB_SOCKET) +distortionModel = calibrationHandler.getDistortionModel(RGB_SOCKET) +if distortionModel != dai.CameraModel.Perspective: + raise RuntimeError("Unsupported distortion model for RGB camera. This example supports only Perspective model.") pipeline = dai.Pipeline() + # Define sources and outputs camRgb = pipeline.create(dai.node.ColorCamera) tof = pipeline.create(dai.node.ToF) @@ -48,7 +59,7 @@ def getFps(self): camRgb.setBoardSocket(RGB_SOCKET) camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) camRgb.setFps(FPS) -camRgb.setIspScale(1, 2) +camRgb.setIspScale(1, ISP_SCALE) out.setStreamName("out") @@ -109,7 +120,8 @@ def updateBlendWeights(percentRgb): # Connect to device and start pipeline -with dai.Device(pipeline) as device: +with device: + device.startPipeline(pipeline) queue = device.getOutputQueue("out", 8, False) # Configure windows; trackbar adjusts blending ratio of rgb/depth @@ -138,6 +150,11 @@ def updateBlendWeights(percentRgb): # Blend when both received if frameDepth is not None: cvFrame = frameRgb.getCvFrame() + cvFrameUndistorted = cv2.undistort( + cvFrame, + np.array(rgbIntrinsics), + np.array(rgbDistortion), + ) # Colorize the aligned depth alignedDepthColorized = colorizeDepth(frameDepth.getFrame()) # Resize depth to match the rgb frame @@ -153,7 +170,7 @@ def updateBlendWeights(percentRgb): cv2.imshow("depth", alignedDepthColorized) blended = cv2.addWeighted( - cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0 + cvFrameUndistorted, rgbWeight, alignedDepthColorized, depthWeight, 0 ) cv2.imshow(rgbDepthWindowName, blended) From e33723bf09421ea04bdb90201f4575d77fc14643 Mon Sep 17 00:00:00 2001 From: SzabolcsGergely Date: Mon, 18 Nov 2024 18:01:31 +0200 Subject: [PATCH 2/3] Obtain intrinsics dynamically --- examples/ImageAlign/depth_align.py | 3 ++- examples/ImageAlign/image_align.py | 3 ++- examples/ImageAlign/thermal_align.py | 3 ++- examples/ImageAlign/tof_align.py | 2 +- 4 files changed, 7 insertions(+), 4 deletions(-) diff --git a/examples/ImageAlign/depth_align.py b/examples/ImageAlign/depth_align.py index b73ec10f3..df773e6c5 100755 --- a/examples/ImageAlign/depth_align.py +++ b/examples/ImageAlign/depth_align.py @@ -34,7 +34,6 @@ def getFps(self): device = dai.Device() calibrationHandler = device.readCalibration() -rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(1920 / ISP_SCALE), int(1080 / ISP_SCALE)) rgbDistortion = calibrationHandler.getDistortionCoefficients(RGB_SOCKET) distortionModel = calibrationHandler.getDistortionModel(RGB_SOCKET) if distortionModel != dai.CameraModel.Perspective: @@ -160,6 +159,8 @@ def updateBlendWeights(percentRgb): if frameDepth is not None: cvFrame = frameRgb.getCvFrame() + rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(cvFrame.shape[1]), int(cvFrame.shape[0])) + # Undistort the rgb frame cvFrameUndistorted = cv2.undistort( cvFrame, diff --git a/examples/ImageAlign/image_align.py b/examples/ImageAlign/image_align.py index 0afcdcc9a..4e555f205 100755 --- a/examples/ImageAlign/image_align.py +++ b/examples/ImageAlign/image_align.py @@ -20,7 +20,6 @@ device = dai.Device() calibrationHandler = device.readCalibration() -rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(1920 / ISP_SCALE), int(1080 / ISP_SCALE)) rgbDistortion = calibrationHandler.getDistortionCoefficients(RGB_SOCKET) distortionModel = calibrationHandler.getDistortionModel(RGB_SOCKET) if distortionModel != dai.CameraModel.Perspective: @@ -120,6 +119,8 @@ def updateDepthPlane(depth): # Colorize the aligned depth leftCv = leftAligned.getCvFrame() + rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(frameRgbCv.shape[1]), int(frameRgbCv.shape[0])) + cvFrameUndistorted = cv2.undistort( frameRgbCv, np.array(rgbIntrinsics), diff --git a/examples/ImageAlign/thermal_align.py b/examples/ImageAlign/thermal_align.py index 9cdc8659e..c25baa25f 100755 --- a/examples/ImageAlign/thermal_align.py +++ b/examples/ImageAlign/thermal_align.py @@ -45,7 +45,6 @@ def getFps(self): device = dai.Device() calibrationHandler = device.readCalibration() -rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(1920 / ISP_SCALE), int(1080 / ISP_SCALE)) rgbDistortion = calibrationHandler.getDistortionCoefficients(RGB_SOCKET) distortionModel = calibrationHandler.getDistortionModel(RGB_SOCKET) if distortionModel != dai.CameraModel.Perspective: @@ -143,6 +142,8 @@ def updateDepthPlane(depth): frameRgbCv = frameRgb.getCvFrame() fpsCounter.tick() + rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(frameRgbCv.shape[1]), int(frameRgbCv.shape[0])) + cvFrameUndistorted = cv2.undistort( frameRgbCv, np.array(rgbIntrinsics), diff --git a/examples/ImageAlign/tof_align.py b/examples/ImageAlign/tof_align.py index ea312741e..070a37d78 100755 --- a/examples/ImageAlign/tof_align.py +++ b/examples/ImageAlign/tof_align.py @@ -34,7 +34,6 @@ def getFps(self): device = dai.Device() calibrationHandler = device.readCalibration() -rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(1920 / ISP_SCALE), int(1080 / ISP_SCALE)) rgbDistortion = calibrationHandler.getDistortionCoefficients(RGB_SOCKET) distortionModel = calibrationHandler.getDistortionModel(RGB_SOCKET) if distortionModel != dai.CameraModel.Perspective: @@ -150,6 +149,7 @@ def updateBlendWeights(percentRgb): # Blend when both received if frameDepth is not None: cvFrame = frameRgb.getCvFrame() + rgbIntrinsics = calibrationHandler.getCameraIntrinsics(RGB_SOCKET, int(cvFrame.shape[1]), int(cvFrame.shape[0])) cvFrameUndistorted = cv2.undistort( cvFrame, np.array(rgbIntrinsics), From b21407233a4e8e2ba5ba7e142c0746677bdf36a8 Mon Sep 17 00:00:00 2001 From: Matevz Morato Date: Mon, 18 Nov 2024 19:11:55 +0100 Subject: [PATCH 3/3] Fix the thermal_align.py example --- examples/ImageAlign/thermal_align.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/examples/ImageAlign/thermal_align.py b/examples/ImageAlign/thermal_align.py index c25baa25f..3e2673dc2 100755 --- a/examples/ImageAlign/thermal_align.py +++ b/examples/ImageAlign/thermal_align.py @@ -42,8 +42,6 @@ def getFps(self): ISP_SCALE = 3 -device = dai.Device() - calibrationHandler = device.readCalibration() rgbDistortion = calibrationHandler.getDistortionCoefficients(RGB_SOCKET) distortionModel = calibrationHandler.getDistortionModel(RGB_SOCKET)