# ToF Align

This example aligns ToF depth onto a left or right mono camera (selectable via `--camera left`/`--camera right`) using the
[ImageAlign](https://docs.luxonis.com/software-v3/depthai/depthai-components/nodes/image_align.md) node, and displays a blended
overlay with an adjustable RGB/depth mix trackbar.

## Demo

This example requires the DepthAI v3 API, see [installation instructions](https://docs.luxonis.com/software-v3/depthai.md).

## Source code

#### Python

```python
#!/usr/bin/env python3
"""Align ToF depth over left or right camera and show a blended overlay.

Usage:
    python tof_align_overlay.py --camera left   # align over CAM_B (default)
    python tof_align_overlay.py --camera right  # align over CAM_C
"""

import argparse
from datetime import timedelta

import cv2
import depthai as dai
import numpy as np

FPS = 10.0
CAMERA_SIZE = (640, 400)

# show depth in range 0.1m - 7m
MIN_DEPTH = 100
MAX_DEPTH = 7000

def colorizeDepth(frameDepth: np.ndarray, minDepth: float, maxDepth: float) -> np.ndarray:
    invalidMask = frameDepth == 0
    try:
        logDepth = np.log(frameDepth.astype(np.float32) + 1e-6)
        logDepth[invalidMask] = 0.0
        logDepth = np.clip(logDepth, np.log(minDepth + 1e-6), np.log(maxDepth + 1e-6))
        depthFrameColor = np.interp(logDepth, (logDepth[~invalidMask].min(), logDepth[~invalidMask].max()), (0, 255))
        depthFrameColor = depthFrameColor.astype(np.uint8)
        depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET)
        depthFrameColor[invalidMask] = 0
    except (IndexError, ValueError):
        depthFrameColor = np.zeros((frameDepth.shape[0], frameDepth.shape[1], 3), dtype=np.uint8)
    return depthFrameColor

rgbWeight = 0.5
depthWeight = 0.5

def updateBlendWeights(percentRgb):
    global rgbWeight, depthWeight
    rgbWeight = float(percentRgb) / 100.0
    depthWeight = 1.0 - rgbWeight

def main():
    parser = argparse.ArgumentParser(description="ToF depth overlay on left or right camera")
    parser.add_argument(
        "--camera",
        choices=["left", "right"],
        default="left",
        help="Camera to align depth onto: left=CAM_B, right=CAM_C (default: left)",
    )
    args = parser.parse_args()

    pipeline = dai.Pipeline()

    camera_sockets = {
        "left": dai.CameraBoardSocket.CAM_B,
        "right": dai.CameraBoardSocket.CAM_C,
    }

    align_socket = camera_sockets[args.camera]
    print(f"Aligning ToF depth over {args.camera} camera ({align_socket})")

    tof = pipeline.create(dai.node.ToF)
    tof.build(
        boardSocket=dai.CameraBoardSocket.AUTO,
        profile=dai.ToFConfig.Profile.MID_RANGE,
        fps=FPS,
    )

    cam = pipeline.create(dai.node.Camera).build(align_socket)
    camOut = cam.requestOutput(CAMERA_SIZE, enableUndistortion=True, fps=FPS)

    align = pipeline.create(dai.node.ImageAlign)
    align.setRunOnHost(True)
    tof.depth.link(align.input)
    camOut.link(align.inputAlignTo)

    sync = pipeline.create(dai.node.Sync)
    sync.setSyncThreshold(timedelta(seconds=0.5 / FPS))
    sync.setRunOnHost(True)
    camOut.link(sync.inputs["rgb"])
    align.outputAligned.link(sync.inputs["depth_aligned"])
    sync.inputs["rgb"].setBlocking(False)

    syncQueue = sync.out.createOutputQueue()

    window_blend = f"tof-overlay-{args.camera}"
    window_depth = "depth-aligned"

    with pipeline as p:
        p.start()
        cv2.namedWindow(window_blend)
        cv2.namedWindow(window_depth)
        cv2.createTrackbar("RGB Weight %", window_blend, int(rgbWeight * 100), 100, updateBlendWeights)

        while p.isRunning():
            msgGroup = syncQueue.get()
            assert isinstance(msgGroup, dai.MessageGroup)

            frameRgb = msgGroup["rgb"]
            frameDepth = msgGroup["depth_aligned"]

            cvFrame = frameRgb.getCvFrame()
            if len(cvFrame.shape) == 2:
                cvFrame = cv2.cvtColor(cvFrame, cv2.COLOR_GRAY2BGR)

            depthColorized = colorizeDepth(frameDepth.getFrame(), MIN_DEPTH, MAX_DEPTH)
            if depthColorized.shape[:2] != cvFrame.shape[:2]:
                depthColorized = cv2.resize(
                    depthColorized, (cvFrame.shape[1], cvFrame.shape[0])
                )

            cv2.imshow(window_depth, depthColorized)

            blended = cv2.addWeighted(cvFrame, rgbWeight, depthColorized, depthWeight, 0)
            cv2.imshow(window_blend, blended)

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

if __name__ == "__main__":
    main()
```

#### C++

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

#include "depthai/depthai.hpp"

// Constants from the Python script
constexpr float FPS = 30.0f;
const dai::CameraBoardSocket RGB_SOCKET = dai::CameraBoardSocket::CAM_C;
const dai::CameraBoardSocket TOF_SOCKET = dai::CameraBoardSocket::CAM_A;
const cv::Size SIZE(640, 400);

// FPSCounter class, similar to the one in the Python script
class FPSCounter {
   public:
    void tick() {
        auto now = std::chrono::steady_clock::now();
        frameTimes.push_back(now);
        // Keep the last 100 timestamps, similar to the Python example
        while(frameTimes.size() > 100) {
            frameTimes.pop_front();
        }
    }

    double getFps() {
        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;
};

cv::Mat colorizeDepth(const cv::Mat& frameDepth) {
    // -----------------------------------------------------------------------
    // 1.  Basic checks & convert to CV_32F
    // -----------------------------------------------------------------------
    if(frameDepth.empty() || frameDepth.channels() != 1) return cv::Mat::zeros(frameDepth.size(), CV_8UC3);

    cv::Mat depth32f;
    frameDepth.convertTo(depth32f, CV_32F);  // safe for any input type

    // -----------------------------------------------------------------------
    // 2.  Build mask of valid (non-zero) pixels
    // -----------------------------------------------------------------------
    const cv::Mat nonZeroMask = depth32f != 0.0f;
    const int nz = cv::countNonZero(nonZeroMask);
    if(nz == 0) return cv::Mat::zeros(frameDepth.size(), CV_8UC3);

    // -----------------------------------------------------------------------
    // 3.  3 % / 95 % percentiles (identical to Python version)
    // -----------------------------------------------------------------------
    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());
    auto pct = [&](double p) {
        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);

    // -----------------------------------------------------------------------
    // 4.  Logarithm (zeros replaced by minDepth to avoid -inf)
    // -----------------------------------------------------------------------
    cv::Mat logDepth;
    depth32f.copyTo(logDepth);
    logDepth.setTo(minDepth, ~nonZeroMask);  // overwrite zeros
    cv::log(logDepth, logDepth);

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

    // -----------------------------------------------------------------------
    // 5.  Clip & linearly scale to [0,255]  (same as np.interp)
    // -----------------------------------------------------------------------
    cv::min(logDepth, logMaxDepth, logDepth);
    cv::max(logDepth, logMinDepth, logDepth);
    logDepth = (logDepth - logMinDepth) * (255.0f / (logMaxDepth - logMinDepth));

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

    // -----------------------------------------------------------------------
    // 6.  Colour map + 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
float rgbWeight = 0.4f;
float depthWeight = 0.6f;

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

int main() {
    dai::Pipeline pipeline;

    // Define sources and outputs
    auto camRgb = pipeline.create<dai::node::Camera>();
    auto tof = pipeline.create<dai::node::ToF>();
    auto sync = pipeline.create<dai::node::Sync>();
    auto align = pipeline.create<dai::node::ImageAlign>();
    align->setRunOnHost(true);

    camRgb->build(RGB_SOCKET);
    const auto profile = dai::ToFConfig::Profile::MID_RANGE;
    tof->build(TOF_SOCKET, profile, FPS);

    // Set sync threshold
    sync->setSyncThreshold(std::chrono::milliseconds(static_cast<uint32_t>(500 / FPS)));
    sync->setRunOnHost(true);

    // Linking
    auto cameraOutput = camRgb->requestOutput(std::make_pair(SIZE.width, SIZE.height), std::nullopt, dai::ImgResizeMode::CROP, FPS, true);

    cameraOutput->link(sync->inputs["rgb"]);
    tof->depth.link(align->input);
    align->outputAligned.link(sync->inputs["depth_aligned"]);
    sync->inputs["rgb"].setBlocking(false);
    cameraOutput->link(align->inputAlignTo);
    auto syncQueue = sync->out.createOutputQueue();

    auto confFilter = pipeline.create<dai::node::ToFDepthConfidenceFilter>();
    tof->depth.link(confFilter->depth);
    tof->amplitude.link(confFilter->amplitude);
    confFilter->setRunOnHost(true);

    auto filteredDepthQ = confFilter->filteredDepth.createOutputQueue();

    // Start the pipeline
    pipeline.start();

    // Configure windows and trackbar
    const std::string rgbDepthWindowName = "rgb-depth";
    cv::namedWindow(rgbDepthWindowName);
    cv::createTrackbar("RGB Weight %", rgbDepthWindowName, nullptr, 100, updateBlendWeights);
    cv::setTrackbarPos("RGB Weight %", rgbDepthWindowName, static_cast<int>(rgbWeight * 100));

    FPSCounter fpsCounter;

    while(true) {
        auto messageGroup = syncQueue->get<dai::MessageGroup>();
        if(messageGroup == nullptr) continue;

        fpsCounter.tick();

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

        if(filteredDepthMsg) {
            cv::Mat filteredDepthMat = filteredDepthMsg->getCvFrame();
            // Display filtered depth map
            cv::imshow("Filtered Depth", colorizeDepth(filteredDepthMat));
        }

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

            // Add FPS text to the depth frame
            std::string fpsText = "FPS: " + std::to_string(fpsCounter.getFps());
            cv::putText(alignedDepthColorized, fpsText, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2);
            cv::imshow("depth", alignedDepthColorized);

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

        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.