# RGB-ToF Align This example demonstrates how to align depth information from a ToF (Time-of-Flight) sensor to an RGB camera. This setup is useful for applications requiring the overlay or comparison of depth and color data. An OpenCV window is created to display the blended image of the RGB and aligned depth data. Trackbars are provided to adjust the blending ratio. ## Demo ## Setup Please run the [install script](https://github.com/luxonis/depthai-python/blob/main/examples/install_requirements.py) to download all required dependencies. Please note that this script must be ran from git context, so you have to download the [depthai-python](https://github.com/luxonis/depthai-python) repository first and then run the script ```bash git clone https://github.com/luxonis/depthai-python.git cd depthai-python/examples python3 install_requirements.py ``` For additional information, please follow the [installation guide](https://docs.luxonis.com/software/depthai/manual-install.md). ## Source code #### Python ```python #!/usr/bin/env python3 import numpy as np import cv2 import depthai as dai import time from datetime import timedelta # This example is intended to run unchanged on an OAK-D-SR-PoE camera FPS = 30.0 RGB_SOCKET = dai.CameraBoardSocket.CAM_C TOF_SOCKET = dai.CameraBoardSocket.CAM_A ALIGN_SOCKET = RGB_SOCKET class FPSCounter: def __init__(self): self.frameTimes = [] def tick(self): now = time.time() self.frameTimes.append(now) self.frameTimes = self.frameTimes[-100:] def getFps(self): if len(self.frameTimes) <= 1: return 0 # Calculate the FPS return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0]) ISP_SCALE = 2 device = dai.Device() calibrationHandler = device.readCalibration() 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) camTof = pipeline.create(dai.node.Camera) sync = pipeline.create(dai.node.Sync) align = pipeline.create(dai.node.ImageAlign) out = pipeline.create(dai.node.XLinkOut) # ToF settings camTof.setFps(FPS) camTof.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG) camTof.setBoardSocket(TOF_SOCKET) # rgb settings camRgb.setBoardSocket(RGB_SOCKET) camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P) camRgb.setFps(FPS) camRgb.setIspScale(1, ISP_SCALE) out.setStreamName("out") sync.setSyncThreshold(timedelta(seconds=0.5 / FPS)) # Linking camRgb.isp.link(sync.inputs["rgb"]) camTof.raw.link(tof.input) tof.depth.link(align.input) align.outputAligned.link(sync.inputs["depth_aligned"]) sync.inputs["rgb"].setBlocking(False) camRgb.isp.link(align.inputAlignTo) sync.out.link(out.input) def colorizeDepth(frameDepth): invalidMask = frameDepth == 0 # Log the depth, minDepth and maxDepth try: minDepth = np.percentile(frameDepth[frameDepth != 0], 3) maxDepth = np.percentile(frameDepth[frameDepth != 0], 95) logDepth = np.log(frameDepth, where=frameDepth != 0) logMinDepth = np.log(minDepth) logMaxDepth = np.log(maxDepth) np.nan_to_num(logDepth, copy=False, nan=logMinDepth) # Clip the values to be in the 0-255 range logDepth = np.clip(logDepth, logMinDepth, logMaxDepth) # Interpolate only valid logDepth values, setting the rest based on the mask depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)) depthFrameColor = np.nan_to_num(depthFrameColor) depthFrameColor = depthFrameColor.astype(np.uint8) depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET) # Set invalid depth pixels to black depthFrameColor[invalidMask] = 0 except IndexError: # Frame is likely empty depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8) except Exception as e: raise e return depthFrameColor rgbWeight = 0.4 depthWeight = 0.6 def updateBlendWeights(percentRgb): """ 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(percentRgb) / 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 rgbDepthWindowName = "rgb-depth" cv2.namedWindow(rgbDepthWindowName) cv2.createTrackbar( "RGB Weight %", rgbDepthWindowName, int(rgbWeight * 100), 100, updateBlendWeights, ) fpsCounter = FPSCounter() while True: messageGroup = queue.get() fpsCounter.tick() assert isinstance(messageGroup, dai.MessageGroup) frameRgb = messageGroup["rgb"] assert isinstance(frameRgb, dai.ImgFrame) frameDepth = messageGroup["depth_aligned"] assert isinstance(frameDepth, dai.ImgFrame) sizeRgb = frameRgb.getData().size sizeDepth = frameDepth.getData().size # 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), np.array(rgbDistortion), ) # Colorize the aligned depth alignedDepthColorized = colorizeDepth(frameDepth.getFrame()) # Resize depth to match the rgb frame cv2.putText( alignedDepthColorized, f"FPS: {fpsCounter.getFps():.2f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2, ) cv2.imshow("depth", alignedDepthColorized) blended = cv2.addWeighted( cvFrameUndistorted, rgbWeight, alignedDepthColorized, depthWeight, 0 ) cv2.imshow(rgbDepthWindowName, blended) key = cv2.waitKey(1) if key == ord("q"): break ``` #### C++ ```cpp #include #include "depthai/depthai.hpp" constexpr auto FPS = 30.0; constexpr auto RGB_SOCKET = dai::CameraBoardSocket::CAM_B; constexpr auto TOF_SOCKET = dai::CameraBoardSocket::CAM_A; constexpr auto ALIGN_SOCKET = RGB_SOCKET; class FPSCounter { public: void tick() { auto now = std::chrono::steady_clock::now(); frameTimes.push(now); if(frameTimes.size() > 100) { frameTimes.pop(); } } double getFps() { if(frameTimes.size() <= 1) return 0; auto duration = std::chrono::duration_cast(frameTimes.back() - frameTimes.front()).count(); return (frameTimes.size() - 1) * 1000.0 / duration; } private: std::queue frameTimes; }; cv::Mat colorizeDepth(const cv::Mat& frameDepth) { cv::Mat invalidMask = (frameDepth == 0); cv::Mat depthFrameColor; try { double minDepth = 0.0; double maxDepth = 0.0; cv::minMaxIdx(frameDepth, &minDepth, &maxDepth, nullptr, nullptr, ~invalidMask); if(minDepth == maxDepth) { depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); return depthFrameColor; } cv::Mat logDepth; frameDepth.convertTo(logDepth, CV_32F); cv::log(logDepth, logDepth); logDepth.setTo(log(minDepth), invalidMask); cv::normalize(logDepth, logDepth, 0, 255, cv::NORM_MINMAX, CV_8UC1); cv::applyColorMap(logDepth, depthFrameColor, cv::COLORMAP_JET); depthFrameColor.setTo(cv::Scalar(0, 0, 0), invalidMask); } catch(const std::exception& e) { depthFrameColor = cv::Mat::zeros(frameDepth.size(), CV_8UC3); } return depthFrameColor; } double rgbWeight = 0.4; double depthWeight = 0.6; void updateBlendWeights(int percentRgb, void*) { rgbWeight = static_cast(percentRgb) / 100.0; depthWeight = 1.0 - rgbWeight; } int main() { dai::Pipeline pipeline; auto camRgb = pipeline.create(); auto camTof = pipeline.create(); auto tof = pipeline.create(); auto sync = pipeline.create(); auto align = pipeline.create(); auto xout = pipeline.create(); camTof->setFps(FPS); camTof->setImageOrientation(dai::CameraImageOrientation::ROTATE_180_DEG); camTof->setBoardSocket(TOF_SOCKET); camRgb->setBoardSocket(RGB_SOCKET); camRgb->setResolution(dai::ColorCameraProperties::SensorResolution::THE_800_P); camRgb->setFps(FPS); camRgb->setIspScale(1, 2); xout->setStreamName("out"); sync->setSyncThreshold(std::chrono::milliseconds(static_cast(1000 / FPS))); camRgb->isp.link(sync->inputs["rgb"]); camTof->raw.link(tof->input); tof->depth.link(align->input); align->outputAligned.link(sync->inputs["depth_aligned"]); camRgb->isp.link(align->inputAlignTo); sync->out.link(xout->input); dai::Device device(pipeline); auto queue = device.getOutputQueue("out", 8, false); FPSCounter fpsCounter; std::string rgbDepthWindowName = "rgb-depth"; cv::namedWindow(rgbDepthWindowName); cv::createTrackbar("RGB Weight %", rgbDepthWindowName, nullptr, 100, updateBlendWeights); while(true) { auto messageGroup = queue->get(); fpsCounter.tick(); auto frameRgb = messageGroup->get("rgb"); auto frameDepth = messageGroup->get("depth_aligned"); if(frameRgb && frameDepth) { auto cvFrame = frameRgb->getCvFrame(); auto alignedDepthColorized = colorizeDepth(frameDepth->getFrame()); cv::putText(alignedDepthColorized, "FPS: " + std::to_string(fpsCounter.getFps()), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2); cv::imshow("depth", alignedDepthColorized); cv::Mat blended; cv::addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0, blended); cv::imshow(rgbDepthWindowName, blended); } int key = cv::waitKey(1); if(key == 'q' || key == 'Q') { break; } } return 0; } ``` ## Pipeline ### examples/tof_align.pipeline.json ```json { "pipeline": { "connections": [ { "node1Id": 0, "node1Output": "isp", "node1OutputGroup": "", "node2Id": 3, "node2Input": "rgb", "node2InputGroup": "inputs" }, { "node1Id": 4, "node1Output": "outputAligned", "node1OutputGroup": "", "node2Id": 3, "node2Input": "depth_aligned", "node2InputGroup": "inputs" }, { "node1Id": 2, "node1Output": "raw", "node1OutputGroup": "", "node2Id": 1, "node2Input": "input", "node2InputGroup": "" }, { "node1Id": 1, "node1Output": "depth", "node1OutputGroup": "", "node2Id": 4, "node2Input": "input", "node2InputGroup": "" }, { "node1Id": 0, "node1Output": "isp", "node1OutputGroup": "", "node2Id": 4, "node2Input": "inputAlignTo", "node2InputGroup": "" }, { "node1Id": 3, "node1Output": "out", "node1OutputGroup": "", "node2Id": 5, "node2Input": "in", "node2InputGroup": "" } ], "globalProperties": { "calibData": null, "cameraTuningBlobSize": null, "cameraTuningBlobUri": "", "leonCssFrequencyHz": 700000000.0, "leonMssFrequencyHz": 700000000.0, "pipelineName": null, "pipelineVersion": null, "sippBufferSize": 18432, "sippDmaBufferSize": 16384, "xlinkChunkSize": -1 }, "nodes": [ [ 0, { "id": 0, "ioInfo": [ [ [ "", "inputConfig" ], { "blocking": false, "group": "", "id": 1, "name": "inputConfig", "queueSize": 8, "type": 3, "waitForMessage": false } ], [ [ "", "raw" ], { "blocking": false, "group": "", "id": 6, "name": "raw", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "still" ], { "blocking": false, "group": "", "id": 7, "name": "still", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "inputControl" ], { "blocking": true, "group": "", "id": 2, "name": "inputControl", "queueSize": 8, "type": 3, "waitForMessage": false } ], [ [ "", "video" ], { "blocking": false, "group": "", "id": 3, "name": "video", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "isp" ], { "blocking": false, "group": "", "id": 4, "name": "isp", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "preview" ], { "blocking": false, "group": "", "id": 5, "name": "preview", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "frameEvent" ], { "blocking": false, "group": "", "id": 8, "name": "frameEvent", "queueSize": 8, "type": 0, "waitForMessage": false } ] ], "name": "ColorCamera", "properties": { "boardSocket": 2, "cameraName": "", "colorOrder": 0, "fp16": false, "fps": 30.0, "imageOrientation": -1, "initialControl": { "aeLockMode": false, "aeMaxExposureTimeUs": 0, "aeRegion": { "height": 0, "priority": 0, "width": 0, "x": 0, "y": 0 }, "afRegion": { "height": 0, "priority": 0, "width": 0, "x": 0, "y": 0 }, "antiBandingMode": 0, "autoFocusMode": 3, "awbLockMode": false, "awbMode": 0, "brightness": 0, "captureIntent": 0, "chromaDenoise": 0, "cmdMask": 0, "contrast": 0, "controlMode": 0, "effectMode": 0, "expCompensation": 0, "expManual": { "exposureTimeUs": 0, "frameDurationUs": 0, "sensitivityIso": 0 }, "frameSyncMode": 0, "lensPosAutoInfinity": 0, "lensPosAutoMacro": 0, "lensPosition": 0, "lensPositionRaw": 0.0, "lowPowerNumFramesBurst": 0, "lowPowerNumFramesDiscard": 0, "lumaDenoise": 0, "saturation": 0, "sceneMode": 0, "sharpness": 0, "strobeConfig": { "activeLevel": 0, "enable": 0, "gpioNumber": 0 }, "strobeTimings": { "durationUs": 0, "exposureBeginOffsetUs": 0, "exposureEndOffsetUs": 0 }, "wbColorTemp": 0 }, "interleaved": true, "isp3aFps": 0, "ispScale": { "horizDenominator": 2, "horizNumerator": 1, "vertDenominator": 2, "vertNumerator": 1 }, "numFramesPoolIsp": 3, "numFramesPoolPreview": 4, "numFramesPoolRaw": 3, "numFramesPoolStill": 4, "numFramesPoolVideo": 4, "previewHeight": 300, "previewKeepAspectRatio": true, "previewWidth": 300, "rawPacked": null, "resolution": 5, "sensorCropX": -1.0, "sensorCropY": -1.0, "stillHeight": -1, "stillWidth": -1, "videoHeight": -1, "videoWidth": -1 } } ], [ 1, { "id": 1, "ioInfo": [ [ [ "", "inputConfig" ], { "blocking": false, "group": "", "id": 9, "name": "inputConfig", "queueSize": 4, "type": 3, "waitForMessage": false } ], [ [ "", "input" ], { "blocking": true, "group": "", "id": 10, "name": "input", "queueSize": 8, "type": 3, "waitForMessage": false } ], [ [ "", "intensity" ], { "blocking": false, "group": "", "id": 11, "name": "intensity", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "amplitude" ], { "blocking": false, "group": "", "id": 12, "name": "amplitude", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "depth" ], { "blocking": false, "group": "", "id": 13, "name": "depth", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "phase" ], { "blocking": false, "group": "", "id": 14, "name": "phase", "queueSize": 8, "type": 0, "waitForMessage": false } ] ], "name": "ToF", "properties": { "initialConfig": { "enableBurstMode": false, "enableDistortionCorrection": true, "enableFPPNCorrection": null, "enableOpticalCorrection": null, "enablePhaseShuffleTemporalFilter": true, "enablePhaseUnwrapping": null, "enableTemperatureCorrection": null, "enableWiggleCorrection": null, "median": 0, "phaseUnwrapErrorThreshold": 100, "phaseUnwrappingLevel": 4 }, "numFramesPool": 4, "numShaves": 1, "warpHwIds": [] } } ], [ 2, { "id": 2, "ioInfo": [ [ [ "", "inputConfig" ], { "blocking": false, "group": "", "id": 15, "name": "inputConfig", "queueSize": 8, "type": 3, "waitForMessage": false } ], [ [ "", "raw" ], { "blocking": false, "group": "", "id": 20, "name": "raw", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "still" ], { "blocking": false, "group": "", "id": 21, "name": "still", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "inputControl" ], { "blocking": true, "group": "", "id": 16, "name": "inputControl", "queueSize": 8, "type": 3, "waitForMessage": false } ], [ [ "", "video" ], { "blocking": false, "group": "", "id": 17, "name": "video", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "isp" ], { "blocking": false, "group": "", "id": 18, "name": "isp", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "preview" ], { "blocking": false, "group": "", "id": 19, "name": "preview", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "frameEvent" ], { "blocking": false, "group": "", "id": 22, "name": "frameEvent", "queueSize": 8, "type": 0, "waitForMessage": false } ] ], "name": "Camera", "properties": { "boardSocket": 0, "calibAlpha": null, "cameraName": "", "colorOrder": 0, "fp16": false, "fps": 30.0, "imageOrientation": 3, "initialControl": { "aeLockMode": false, "aeMaxExposureTimeUs": 0, "aeRegion": { "height": 0, "priority": 0, "width": 0, "x": 0, "y": 0 }, "afRegion": { "height": 0, "priority": 0, "width": 0, "x": 0, "y": 0 }, "antiBandingMode": 0, "autoFocusMode": 3, "awbLockMode": false, "awbMode": 0, "brightness": 0, "captureIntent": 0, "chromaDenoise": 0, "cmdMask": 0, "contrast": 0, "controlMode": 0, "effectMode": 0, "expCompensation": 0, "expManual": { "exposureTimeUs": 0, "frameDurationUs": 0, "sensitivityIso": 0 }, "frameSyncMode": 0, "lensPosAutoInfinity": 0, "lensPosAutoMacro": 0, "lensPosition": 0, "lensPositionRaw": 0.0, "lowPowerNumFramesBurst": 0, "lowPowerNumFramesDiscard": 0, "lumaDenoise": 0, "saturation": 0, "sceneMode": 0, "sharpness": 0, "strobeConfig": { "activeLevel": 0, "enable": 0, "gpioNumber": 0 }, "strobeTimings": { "durationUs": 0, "exposureBeginOffsetUs": 0, "exposureEndOffsetUs": 0 }, "wbColorTemp": 0 }, "interleaved": true, "isp3aFps": 0, "ispScale": { "horizDenominator": 0, "horizNumerator": 0, "vertDenominator": 0, "vertNumerator": 0 }, "numFramesPoolIsp": 3, "numFramesPoolPreview": 4, "numFramesPoolRaw": 3, "numFramesPoolStill": 4, "numFramesPoolVideo": 4, "previewHeight": 300, "previewKeepAspectRatio": true, "previewWidth": 300, "rawPacked": null, "resolutionHeight": -1, "resolutionWidth": -1, "sensorCropX": -1.0, "sensorCropY": -1.0, "sensorType": -1, "stillHeight": -1, "stillWidth": -1, "videoHeight": -1, "videoWidth": -1, "warpMeshHeight": 0, "warpMeshSource": -1, "warpMeshStepHeight": 32, "warpMeshStepWidth": 32, "warpMeshUri": "", "warpMeshWidth": 0 } } ], [ 3, { "id": 3, "ioInfo": [ [ [ "inputs", "rgb" ], { "blocking": false, "group": "inputs", "id": 23, "name": "rgb", "queueSize": 8, "type": 3, "waitForMessage": false } ], [ [ "inputs", "depth_aligned" ], { "blocking": true, "group": "inputs", "id": 24, "name": "depth_aligned", "queueSize": 8, "type": 3, "waitForMessage": false } ], [ [ "", "out" ], { "blocking": false, "group": "", "id": 25, "name": "out", "queueSize": 8, "type": 0, "waitForMessage": false } ] ], "name": "Sync", "properties": { "syncAttempts": -1, "syncThresholdNs": 16667000 } } ], [ 4, { "id": 4, "ioInfo": [ [ [ "", "inputAlignTo" ], { "blocking": false, "group": "", "id": 26, "name": "inputAlignTo", "queueSize": 1, "type": 3, "waitForMessage": true } ], [ [ "", "inputConfig" ], { "blocking": false, "group": "", "id": 27, "name": "inputConfig", "queueSize": 4, "type": 3, "waitForMessage": false } ], [ [ "", "input" ], { "blocking": false, "group": "", "id": 28, "name": "input", "queueSize": 4, "type": 3, "waitForMessage": true } ], [ [ "", "outputAligned" ], { "blocking": false, "group": "", "id": 29, "name": "outputAligned", "queueSize": 8, "type": 0, "waitForMessage": false } ], [ [ "", "passthroughInput" ], { "blocking": false, "group": "", "id": 30, "name": "passthroughInput", "queueSize": 8, "type": 0, "waitForMessage": false } ] ], "name": "ImageAlign", "properties": { "alignHeight": 0, "alignWidth": 0, "initialConfig": { "staticDepthPlane": 0 }, "interpolation": -1, "numFramesPool": 4, "numShaves": 2, "outKeepAspectRatio": true, "warpHwIds": [] } } ], [ 5, { "id": 5, "ioInfo": [ [ [ "", "in" ], { "blocking": true, "group": "", "id": 31, "name": "in", "queueSize": 8, "type": 3, "waitForMessage": true } ] ], "name": "XLinkOut", "properties": { "maxFpsLimit": -1.0, "metadataOnly": false, "streamName": "out" } } ] ] } } ``` ### Need assistance? Head over to [Discussion Forum](https://discuss.luxonis.com/) for technical support or any other questions you might have.