# Neural Depth Align

Demonstrates aligning [NeuralDepth](https://docs.luxonis.com/software-v3/depthai/depthai-components/nodes/neural_depth.md) output
to an RGB camera using the [ImageAlign](https://docs.luxonis.com/software-v3/depthai/depthai-components/nodes/image_align.md)
node.

## Pipeline

### examples/neural_depth_align.pipeline.json

```json
{"pipeline": {"connections": [{"node1Id": 9, "node1Output": "outputAligned", "node1OutputGroup": "", "node2Id": 8, "node2Input": "depth_aligned", "node2InputGroup": "inputs"}, {"node1Id": 3, "node1Output": "depth", "node1OutputGroup": "", "node2Id": 9, "node2Input": "input", "node2InputGroup": ""}, {"node1Id": 4, "node1Output": "out", "node1OutputGroup": "", "node2Id": 5, "node2Input": "input", "node2InputGroup": ""}, {"node1Id": 5, "node1Output": "right", "node1OutputGroup": "outputs", "node2Id": 6, "node2Input": "input2", "node2InputGroup": ""}, {"node1Id": 5, "node1Output": "left", "node1OutputGroup": "outputs", "node2Id": 6, "node2Input": "input1", "node2InputGroup": ""}, {"node1Id": 6, "node1Output": "output2", "node1OutputGroup": "", "node2Id": 7, "node2Input": "right", "node2InputGroup": "inputs"}, {"node1Id": 6, "node1Output": "output1", "node1OutputGroup": "", "node2Id": 7, "node2Input": "left", "node2InputGroup": "inputs"}, {"node1Id": 6, "node1Output": "output2", "node1OutputGroup": "", "node2Id": 3, "node2Input": "rightFrameInternal", "node2InputGroup": ""}, {"node1Id": 6, "node1Output": "output1", "node1OutputGroup": "", "node2Id": 3, "node2Input": "leftFrameInternal", "node2InputGroup": ""}, {"node1Id": 7, "node1Output": "out", "node1OutputGroup": "", "node2Id": 3, "node2Input": "nnDataInput", "node2InputGroup": ""}, {"node1Id": 2, "node1Output": "0", "node1OutputGroup": "dynamicOutputs", "node2Id": 4, "node2Input": "right", "node2InputGroup": "inputs"}, {"node1Id": 1, "node1Output": "0", "node1OutputGroup": "dynamicOutputs", "node2Id": 4, "node2Input": "left", "node2InputGroup": "inputs"}, {"node1Id": 0, "node1Output": "0", "node1OutputGroup": "dynamicOutputs", "node2Id": 8, "node2Input": "rgb", "node2InputGroup": "inputs"}, {"node1Id": 0, "node1Output": "0", "node1OutputGroup": "dynamicOutputs", "node2Id": 9, "node2Input": "inputAlignTo", "node2InputGroup": ""}], "globalProperties": {"calibData": null, "cameraTuningBlobSize": null, "cameraTuningBlobUri": "", "eepromId": 0, "leonCssFrequencyHz": 700000000.0, "leonMssFrequencyHz": 700000000.0, "pipelineName": null, "pipelineVersion": null, "sippBufferSize": 18432, "sippDmaBufferSize": 16384, "xlinkChunkSize": -1}, "nodes": [[9, {"alias": "", "id": 9, "ioInfo": [[["", "outputAligned"], {"blocking": false, "group": "", "id": 43, "name": "outputAligned", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "passthroughInput"], {"blocking": false, "group": "", "id": 44, "name": "passthroughInput", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "inputAlignTo"], {"blocking": false, "group": "", "id": 42, "name": "inputAlignTo", "queueSize": 1, "type": 3, "waitForMessage": true}], [["", "input"], {"blocking": false, "group": "", "id": 41, "name": "input", "queueSize": 4, "type": 3, "waitForMessage": false}], [["", "inputConfig"], {"blocking": false, "group": "", "id": 40, "name": "inputConfig", "queueSize": 4, "type": 3, "waitForMessage": false}]], "logLevel": 3, "name": "ImageAlign", "parentId": -1, "properties": {"alignHeight": 0, "alignWidth": 0, "initialConfig": {"staticDepthPlane": 0}, "interpolation": -1, "numFramesPool": 4, "numShaves": 2, "outKeepAspectRatio": true, "warpHwIds": []}}], [8, {"alias": "", "id": 8, "ioInfo": [[["", "out"], {"blocking": false, "group": "", "id": 39, "name": "out", "queueSize": 8, "type": 0, "waitForMessage": false}], [["inputs", "rgb"], {"blocking": false, "group": "inputs", "id": 38, "name": "rgb", "queueSize": 10, "type": 3, "waitForMessage": false}], [["inputs", "depth_aligned"], {"blocking": false, "group": "inputs", "id": 37, "name": "depth_aligned", "queueSize": 10, "type": 3, "waitForMessage": false}]], "logLevel": 3, "name": "Sync", "parentId": -1, "properties": {"syncAttempts": -1, "syncThresholdNs": 50000000}}], [7, {"alias": "neuralNetwork", "id": 7, "ioInfo": [[["", "passthrough"], {"blocking": false, "group": "", "id": 36, "name": "passthrough", "queueSize": 8, "type": 0, "waitForMessage": false}], [["inputs", "left"], {"blocking": false, "group": "inputs", "id": 34, "name": "left", "queueSize": 1, "type": 3, "waitForMessage": true}], [["", "out"], {"blocking": false, "group": "", "id": 35, "name": "out", "queueSize": 8, "type": 0, "waitForMessage": false}], [["inputs", "right"], {"blocking": false, "group": "inputs", "id": 33, "name": "right", "queueSize": 1, "type": 3, "waitForMessage": true}], [["", "in"], {"blocking": true, "group": "", "id": 32, "name": "in", "queueSize": 3, "type": 3, "waitForMessage": true}]], "logLevel": 3, "name": "NeuralNetwork", "parentId": 3, "properties": {"backend": "", "backendProperties": {}, "blobSize": null, "blobUri": "", "deviceModel": 0, "modelSource": 0, "modelUri": "", "numFrames": 8, "numNCEPerThread": 0, "numShavesPerThread": 0, "numThreads": 0}}], [6, {"alias": "rectification", "id": 6, "ioInfo": [[["", "output2"], {"blocking": false, "group": "", "id": 31, "name": "output2", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "passthrough2"], {"blocking": false, "group": "", "id": 29, "name": "passthrough2", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "output1"], {"blocking": false, "group": "", "id": 30, "name": "output1", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "input2"], {"blocking": false, "group": "", "id": 27, "name": "input2", "queueSize": 4, "type": 3, "waitForMessage": false}], [["", "passthrough1"], {"blocking": false, "group": "", "id": 28, "name": "passthrough1", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "input1"], {"blocking": false, "group": "", "id": 26, "name": "input1", "queueSize": 4, "type": 3, "waitForMessage": false}]], "logLevel": 3, "name": "Rectification", "parentId": 3, "properties": {"enableRectification": true, "outputHeight": 480, "outputWidth": 768}}], [5, {"alias": "messageDemux", "id": 5, "ioInfo": [[["outputs", "right"], {"blocking": false, "group": "outputs", "id": 24, "name": "right", "queueSize": 8, "type": 0, "waitForMessage": false}], [["outputs", "left"], {"blocking": false, "group": "outputs", "id": 25, "name": "left", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "input"], {"blocking": true, "group": "", "id": 23, "name": "input", "queueSize": 3, "type": 3, "waitForMessage": false}]], "logLevel": 3, "name": "MessageDemux", "parentId": 3, "properties": {"dummy": 0}}], [4, {"alias": "sync", "id": 4, "ioInfo": [[["", "out"], {"blocking": false, "group": "", "id": 22, "name": "out", "queueSize": 8, "type": 0, "waitForMessage": false}], [["inputs", "left"], {"blocking": false, "group": "inputs", "id": 21, "name": "left", "queueSize": 10, "type": 3, "waitForMessage": false}], [["inputs", "right"], {"blocking": false, "group": "inputs", "id": 20, "name": "right", "queueSize": 10, "type": 3, "waitForMessage": false}]], "logLevel": 3, "name": "Sync", "parentId": 3, "properties": {"syncAttempts": -1, "syncThresholdNs": 10000000}}], [3, {"alias": "", "id": 3, "ioInfo": [[["", "confidence"], {"blocking": false, "group": "", "id": 19, "name": "confidence", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "edge"], {"blocking": false, "group": "", "id": 18, "name": "edge", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "depth"], {"blocking": false, "group": "", "id": 17, "name": "depth", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "disparity"], {"blocking": false, "group": "", "id": 16, "name": "disparity", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "rightFrameInternal"], {"blocking": false, "group": "", "id": 15, "name": "rightFrameInternal", "queueSize": 1, "type": 3, "waitForMessage": false}], [["", "leftFrameInternal"], {"blocking": false, "group": "", "id": 14, "name": "leftFrameInternal", "queueSize": 1, "type": 3, "waitForMessage": false}], [["", "nnDataInput"], {"blocking": true, "group": "", "id": 13, "name": "nnDataInput", "queueSize": 5, "type": 3, "waitForMessage": false}], [["", "inputConfig"], {"blocking": true, "group": "", "id": 12, "name": "inputConfig", "queueSize": 5, "type": 3, "waitForMessage": false}]], "logLevel": 3, "name": "NeuralDepth", "parentId": -1, "properties": {"initialConfig": {"algorithmControl": {"customDepthUnitMultiplier": 1000.0, "depthUnit": 2}, "postProcessing": {"confidenceThreshold": 125, "edgeThreshold": 10, "temporalFilter": {"alpha": 0.4000000059604645, "delta": 3, "enable": false, "persistencyMode": 3}}}}}], [2, {"alias": "", "id": 2, "ioInfo": [[["dynamicOutputs", "0"], {"blocking": false, "group": "dynamicOutputs", "id": 11, "name": "0", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "raw"], {"blocking": false, "group": "", "id": 10, "name": "raw", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "mockIsp"], {"blocking": true, "group": "", "id": 9, "name": "mockIsp", "queueSize": 8, "type": 3, "waitForMessage": false}], [["", "inputControl"], {"blocking": true, "group": "", "id": 8, "name": "inputControl", "queueSize": 3, "type": 3, "waitForMessage": false}]], "logLevel": 3, "name": "Camera", "parentId": -1, "properties": {"boardSocket": 2, "cameraName": "", "fps": -1.0, "imageOrientation": -1, "initialControl": {"aeLockMode": false, "aeMaxExposureTimeUs": 1832433472, "aeRegion": {"height": 0, "priority": 385, "width": 0, "x": 160, "y": 0}, "afRegion": {"height": 27960, "priority": 24702, "width": 10736, "x": 0, "y": 0}, "antiBandingMode": 56, "autoFocusMode": 3, "awbLockMode": false, "awbMode": 144, "brightness": 0, "captureIntent": 109, "chromaDenoise": 0, "cmdMask": 0, "contrast": 15, "controlMode": 126, "effectMode": 96, "enableHdr": false, "expCompensation": 0, "expManual": {"exposureTimeUs": 3692785835, "frameDurationUs": 0, "sensitivityIso": 0}, "frameSyncMode": 0, "lensPosAutoInfinity": 0, "lensPosAutoMacro": 17, "lensPosition": 0, "lensPositionRaw": 0.0, "lowPowerNumFramesBurst": 97, "lowPowerNumFramesDiscard": 100, "lumaDenoise": 0, "miscControls": [], "saturation": 0, "sceneMode": 97, "sharpness": 0, "strobeConfig": {"activeLevel": 0, "enable": 0, "gpioNumber": 0}, "strobeTimings": {"durationUs": 0, "exposureBeginOffsetUs": 0, "exposureEndOffsetUs": 0}, "wbColorTemp": 0}, "isp3aFps": 0, "maxSizePoolIsp": 10485760, "maxSizePoolOutputs": null, "maxSizePoolRaw": 10485760, "mockIspHeight": -1, "mockIspWidth": -1, "numFramesPoolIsp": 3, "numFramesPoolOutputs": null, "numFramesPoolPreview": 4, "numFramesPoolRaw": 3, "numFramesPoolStill": 4, "numFramesPoolVideo": 4, "outputRequests": [{"enableUndistortion": null, "fps": {"value": {"index": 0, "value": 10.0}}, "resizeMode": 0, "size": {"value": {"index": 0, "value": [1280, 800]}}, "type": null}], "resolutionHeight": -1, "resolutionWidth": -1, "sensorType": -1}}], [1, {"alias": "", "id": 1, "ioInfo": [[["dynamicOutputs", "0"], {"blocking": false, "group": "dynamicOutputs", "id": 7, "name": "0", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "raw"], {"blocking": false, "group": "", "id": 6, "name": "raw", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "mockIsp"], {"blocking": true, "group": "", "id": 5, "name": "mockIsp", "queueSize": 8, "type": 3, "waitForMessage": false}], [["", "inputControl"], {"blocking": true, "group": "", "id": 4, "name": "inputControl", "queueSize": 3, "type": 3, "waitForMessage": false}]], "logLevel": 3, "name": "Camera", "parentId": -1, "properties": {"boardSocket": 1, "cameraName": "", "fps": -1.0, "imageOrientation": -1, "initialControl": {"aeLockMode": false, "aeMaxExposureTimeUs": 1832429432, "aeRegion": {"height": 47121, "priority": 705, "width": 30756, "x": 13023, "y": 20498}, "afRegion": {"height": 27960, "priority": 24702, "width": 40896, "x": 0, "y": 0}, "antiBandingMode": 0, "autoFocusMode": 3, "awbLockMode": false, "awbMode": 32, "brightness": 0, "captureIntent": 139, "chromaDenoise": 0, "cmdMask": 0, "contrast": 0, "controlMode": 215, "effectMode": 119, "enableHdr": false, "expCompensation": 0, "expManual": {"exposureTimeUs": 2720963007, "frameDurationUs": 0, "sensitivityIso": 0}, "frameSyncMode": 0, "lensPosAutoInfinity": 0, "lensPosAutoMacro": 240, "lensPosition": 0, "lensPositionRaw": 0.0, "lowPowerNumFramesBurst": 64, "lowPowerNumFramesDiscard": 0, "lumaDenoise": 0, "miscControls": [], "saturation": 0, "sceneMode": 59, "sharpness": 0, "strobeConfig": {"activeLevel": 0, "enable": 0, "gpioNumber": 0}, "strobeTimings": {"durationUs": 24702, "exposureBeginOffsetUs": 0, "exposureEndOffsetUs": 1832429432}, "wbColorTemp": 0}, "isp3aFps": 0, "maxSizePoolIsp": 10485760, "maxSizePoolOutputs": null, "maxSizePoolRaw": 10485760, "mockIspHeight": -1, "mockIspWidth": -1, "numFramesPoolIsp": 3, "numFramesPoolOutputs": null, "numFramesPoolPreview": 4, "numFramesPoolRaw": 3, "numFramesPoolStill": 4, "numFramesPoolVideo": 4, "outputRequests": [{"enableUndistortion": null, "fps": {"value": {"index": 0, "value": 10.0}}, "resizeMode": 0, "size": {"value": {"index": 0, "value": [1280, 800]}}, "type": null}], "resolutionHeight": -1, "resolutionWidth": -1, "sensorType": -1}}], [0, {"alias": "", "id": 0, "ioInfo": [[["dynamicOutputs", "0"], {"blocking": false, "group": "dynamicOutputs", "id": 3, "name": "0", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "raw"], {"blocking": false, "group": "", "id": 2, "name": "raw", "queueSize": 8, "type": 0, "waitForMessage": false}], [["", "mockIsp"], {"blocking": true, "group": "", "id": 1, "name": "mockIsp", "queueSize": 8, "type": 3, "waitForMessage": false}], [["", "inputControl"], {"blocking": true, "group": "", "id": 0, "name": "inputControl", "queueSize": 3, "type": 3, "waitForMessage": false}]], "logLevel": 3, "name": "Camera", "parentId": -1, "properties": {"boardSocket": 0, "cameraName": "", "fps": -1.0, "imageOrientation": -1, "initialControl": {"aeLockMode": false, "aeMaxExposureTimeUs": 3395871759, "aeRegion": {"height": 23539, "priority": 1680057611, "width": 27103, "x": 57443, "y": 14509}, "afRegion": {"height": 8229, "priority": 143477788, "width": 58441, "x": 63661, "y": 34201}, "antiBandingMode": 159, "autoFocusMode": 3, "awbLockMode": false, "awbMode": 16, "brightness": -56, "captureIntent": 16, "chromaDenoise": 0, "cmdMask": 0, "contrast": -118, "controlMode": 46, "effectMode": 31, "enableHdr": false, "expCompensation": -85, "expManual": {"exposureTimeUs": 4008679, "frameDurationUs": 2196813195, "sensitivityIso": 238377071}, "frameSyncMode": 243, "lensPosAutoInfinity": 238, "lensPosAutoMacro": 27, "lensPosition": 0, "lensPositionRaw": 0.0, "lowPowerNumFramesBurst": 66, "lowPowerNumFramesDiscard": 21, "lumaDenoise": 0, "miscControls": [], "saturation": 68, "sceneMode": 246, "sharpness": 141, "strobeConfig": {"activeLevel": 31, "enable": 70, "gpioNumber": -56}, "strobeTimings": {"durationUs": 1359893532, "exposureBeginOffsetUs": -1803871563, "exposureEndOffsetUs": 115413366}, "wbColorTemp": 44429}, "isp3aFps": 0, "maxSizePoolIsp": 10485760, "maxSizePoolOutputs": null, "maxSizePoolRaw": 10485760, "mockIspHeight": -1, "mockIspWidth": -1, "numFramesPoolIsp": 3, "numFramesPoolOutputs": null, "numFramesPoolPreview": 4, "numFramesPoolRaw": 3, "numFramesPoolStill": 4, "numFramesPoolVideo": 4, "outputRequests": [{"enableUndistortion": true, "fps": {"value": {"index": 0, "value": 10.0}}, "resizeMode": 0, "size": {"value": {"index": 0, "value": [1280, 960]}}, "type": null}], "resolutionHeight": -1, "resolutionWidth": -1, "sensorType": -1}}]]}}
```

## Source code

#### Python

```python
#!/usr/bin/env python3

import numpy as np
import cv2
import depthai as dai
import time
from datetime import timedelta
FPS = 25

RGB_SOCKET = dai.CameraBoardSocket.CAM_A
LEFT_SOCKET = dai.CameraBoardSocket.CAM_B
RIGHT_SOCKET = dai.CameraBoardSocket.CAM_C

class FPSCounter:
    def __init__(self):
        self.frameTimes = []

    def tick(self):
        now = time.time()
        self.frameTimes.append(now)
        self.frameTimes = self.frameTimes[-10:]

    def getFps(self):
        if len(self.frameTimes) <= 1:
            return 0
        return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0])

pipeline = dai.Pipeline()

platform = pipeline.getDefaultDevice().getPlatform()

# Define sources and outputs
camRgb = pipeline.create(dai.node.Camera).build(RGB_SOCKET)
left = pipeline.create(dai.node.Camera).build(LEFT_SOCKET)
right = pipeline.create(dai.node.Camera).build(RIGHT_SOCKET)
stereo = pipeline.create(dai.node.NeuralDepth)
sync = pipeline.create(dai.node.Sync)
align = pipeline.create(dai.node.ImageAlign)

sync.setSyncThreshold(timedelta(seconds=1/(2*FPS)))

rgbOut = camRgb.requestOutput(size = (1280, 960), fps = FPS, enableUndistortion=True)
leftOut = left.requestFullResolutionOutput(fps = FPS)
rightOut = right.requestFullResolutionOutput(fps = FPS)
stereo.build(leftOut, rightOut, dai.DeviceModelZoo.NEURAL_DEPTH_LARGE)
# Linking
stereo.depth.link(align.input)
rgbOut.link(align.inputAlignTo)
rgbOut.link(sync.inputs["rgb"])
align.outputAligned.link(sync.inputs["depth_aligned"])

queue = sync.out.createOutputQueue()

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 pipeline:
    pipeline.start()

    # Configure windows; trackbar adjusts blending ratio of rgb/depth
    windowName = "rgb-depth"

    # Set the window to be resizable and the initial size
    cv2.namedWindow(windowName, cv2.WINDOW_NORMAL)
    cv2.resizeWindow(windowName, 1280, 720)
    cv2.createTrackbar(
        "RGB Weight %",
        windowName,
        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)

        # Blend when both received
        if frameDepth is not None:
            cvFrame = frameRgb.getCvFrame()
            # Colorize the aligned depth
            alignedDepthColorized = colorizeDepth(frameDepth.getFrame())
            # Resize depth to match the rgb frame
            cv2.imshow("Depth aligned", alignedDepthColorized)

            if len(cvFrame.shape) == 2:
                cvFrameUndistorted = cv2.cvtColor(cvFrame, cv2.COLOR_GRAY2BGR)
            # print("RGB Size:", cvFrame.shape)
            # print("Depth Size:", alignedDepthColorized.shape)
            blended = cv2.addWeighted(
                cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0
            )
            cv2.putText(
                blended,
                f"FPS: {fpsCounter.getFps():.2f}",
                (10, 30),
                cv2.FONT_HERSHEY_SIMPLEX,
                1,
                (255, 255, 255),
                2,
            )
            cv2.imshow(windowName, blended)

        key = cv2.waitKey(1)
        if key == ord("q"):
            break
```

#### C++

```cpp
#include <algorithm>
#include <chrono>
#include <cmath>
#include <deque>
#include <opencv2/opencv.hpp>
#include <optional>
#include <string>
#include <vector>

#include "depthai/capabilities/ImgFrameCapability.hpp"
#include "depthai/depthai.hpp"

// Define constants from the Python script
constexpr float FPS = 25.0f;
const dai::CameraBoardSocket RGB_SOCKET = dai::CameraBoardSocket::CAM_A;
const dai::CameraBoardSocket LEFT_SOCKET = dai::CameraBoardSocket::CAM_B;
const dai::CameraBoardSocket RIGHT_SOCKET = dai::CameraBoardSocket::CAM_C;

// FPS Counter class to calculate and display frames per second
class FPSCounter {
   public:
    void tick() {
        auto now = std::chrono::steady_clock::now();
        frameTimes.push_back(now);
        // Keep the last 10 timestamps, same as the Python version
        if(frameTimes.size() > 10) {
            frameTimes.pop_front();
        }
    }

    double getFps() const {
        if(frameTimes.size() <= 1) {
            return 0.0;
        }
        auto duration = std::chrono::duration_cast<std::chrono::duration<double>>(frameTimes.back() - frameTimes.front()).count();
        return (static_cast<double>(frameTimes.size()) - 1.0) / duration;
    }

   private:
    std::deque<std::chrono::steady_clock::time_point> frameTimes;
};

// Function to colorize a depth frame for visualization
cv::Mat colorizeDepth(const cv::Mat& frameDepth) {
    if(frameDepth.empty() || frameDepth.channels() != 1) {
        return cv::Mat::zeros(frameDepth.size(), CV_8UC3);
    }

    cv::Mat depth32f;
    frameDepth.convertTo(depth32f, CV_32F);

    const cv::Mat nonZeroMask = depth32f != 0.0f;
    const int nz = cv::countNonZero(nonZeroMask);
    if(nz == 0) {
        return cv::Mat::zeros(frameDepth.size(), CV_8UC3);
    }

    // Extract non-zero depth values to calculate percentiles
    std::vector<float> values;
    values.reserve(nz);
    for(int r = 0; r < depth32f.rows; ++r) {
        const float* d = depth32f.ptr<float>(r);
        const uchar* m = nonZeroMask.ptr<uchar>(r);
        for(int c = 0; c < depth32f.cols; ++c) {
            if(m[c]) {
                values.push_back(d[c]);
            }
        }
    }

    std::sort(values.begin(), values.end());

    // Lambda to calculate percentile
    auto pct = [&](double p) {
        if(values.empty()) return 0.0f;
        size_t idx = static_cast<size_t>(std::round((p / 100.0) * (values.size() - 1)));
        return values[idx];
    };

    const float minDepth = pct(3.0);
    const float maxDepth = pct(95.0);

    // Apply logarithmic scaling
    cv::Mat logDepth;
    depth32f.copyTo(logDepth);
    logDepth.setTo(minDepth, ~nonZeroMask);  // Replace zeros to avoid log(0)
    cv::log(logDepth, logDepth);

    const float logMinDepth = std::log(minDepth);
    const float logMaxDepth = std::log(maxDepth);

    // Clip and linearly scale to the [0, 255] range
    cv::min(logDepth, logMaxDepth, logDepth);
    cv::max(logDepth, logMinDepth, logDepth);
    if(logMaxDepth > logMinDepth) {
        logDepth = (logDepth - logMinDepth) * (255.0f / (logMaxDepth - logMinDepth));
    }

    cv::Mat depth8U;
    logDepth.convertTo(depth8U, CV_8U);

    // Apply color map and set invalid pixels to black
    cv::Mat depthFrameColor;
    cv::applyColorMap(depth8U, depthFrameColor, cv::COLORMAP_JET);
    depthFrameColor.setTo(cv::Scalar::all(0), ~nonZeroMask);

    return depthFrameColor;
}

// Global variables for blending weights, controlled by the trackbar
float rgbWeight = 0.4f;
float depthWeight = 0.6f;

// Callback function for the OpenCV trackbar
void updateBlendWeights(int percentRgb, void*) {
    rgbWeight = static_cast<float>(percentRgb) / 100.0f;
    depthWeight = 1.0f - rgbWeight;
}

int main() {
    // Create the DepthAI pipeline
    dai::Pipeline pipeline;

    // --- Define pipeline nodes ---
    auto camRgb = pipeline.create<dai::node::Camera>();
    camRgb->build(RGB_SOCKET);

    auto left = pipeline.create<dai::node::Camera>();
    left->build(LEFT_SOCKET);

    auto right = pipeline.create<dai::node::Camera>();
    right->build(RIGHT_SOCKET);

    auto stereo = pipeline.create<dai::node::NeuralDepth>();
    auto sync = pipeline.create<dai::node::Sync>();
    auto align = pipeline.create<dai::node::ImageAlign>();

    // --- Configure nodes ---
    sync->setSyncThreshold(std::chrono::milliseconds(static_cast<long long>(1000.0 / (2.0 * FPS))));

    // auto* rgbOut = camRgb->requestOutput(std::make_pair(1280, 960), std::nullopt, std::nullopt, FPS, true);
    auto* rgbOut = camRgb->requestOutput(std::make_pair(1280, 960), std::nullopt, dai::ImgResizeMode::CROP, FPS, true);
    auto* leftOut = left->requestFullResolutionOutput(std::nullopt, FPS);
    auto* rightOut = right->requestFullResolutionOutput(std::nullopt, FPS);

    stereo->build(*leftOut, *rightOut, dai::DeviceModelZoo::NEURAL_DEPTH_LARGE);

    // --- Link pipeline nodes ---
    stereo->depth.link(align->input);
    rgbOut->link(align->inputAlignTo);
    rgbOut->link(sync->inputs["rgb"]);
    align->outputAligned.link(sync->inputs["depth_aligned"]);

    // Create an output queue for the synchronized frames
    auto queue = sync->out.createOutputQueue();

    // Start the pipeline
    pipeline.start();

    // --- Setup OpenCV windows and trackbar ---
    const std::string windowName = "rgb-depth";
    cv::namedWindow(windowName, cv::WINDOW_NORMAL);
    cv::resizeWindow(windowName, 1280, 720);
    cv::createTrackbar("RGB Weight %", windowName, nullptr, 100, updateBlendWeights);
    cv::setTrackbarPos("RGB Weight %", windowName, static_cast<int>(rgbWeight * 100));

    FPSCounter fpsCounter;

    while(true) {
        // Get the synchronized message group from the queue
        auto messageGroup = queue->get<dai::MessageGroup>();
        fpsCounter.tick();

        auto frameRgb = messageGroup->get<dai::ImgFrame>("rgb");
        auto frameDepth = messageGroup->get<dai::ImgFrame>("depth_aligned");

        if(frameRgb && frameDepth) {
            cv::Mat cvFrame = frameRgb->getCvFrame();

            // Colorize the aligned depth frame for visualization
            cv::Mat alignedDepthColorized = colorizeDepth(frameDepth->getFrame());
            cv::imshow("Depth aligned", alignedDepthColorized);

            // Blend the RGB and colorized depth frames
            cv::Mat blended;
            cv::addWeighted(cvFrame, rgbWeight, alignedDepthColorized, depthWeight, 0, blended);

            // Add FPS text to the blended image
            char fpsStr[20];
            snprintf(fpsStr, sizeof(fpsStr), "FPS: %.2f", fpsCounter.getFps());
            cv::putText(blended, fpsStr, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2);

            // Show the final blended result
            cv::imshow(windowName, blended);
        }

        // Check for 'q' key press to exit
        int key = cv::waitKey(1);
        if(key == 'q' || key == 27) {  // 'q' or ESC
            break;
        }
    }

    return 0;
}
```

### Need assistance?

Head over to [Discussion Forum](https://discuss.luxonis.com/) for technical support or any other questions you might have.