# 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.py --camera left
    python tof_align.py --camera right
"""

import argparse
from datetime import timedelta

import cv2
import depthai as dai

FPS = 30.0
CAMERA_SIZE = (640, 400)

MIN_DEPTH = 100.0
MAX_DEPTH = 7000.0

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 = dai.utility.colorizeDepthFrame(frameDepth, MIN_DEPTH, MAX_DEPTH, useLog=True).getCvFrame()
            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 <argparse/argparse.hpp>
#include <chrono>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <string>

#include "depthai/depthai.hpp"

constexpr float FPS = 30.0f;
const cv::Size CAMERA_SIZE(640, 400);

constexpr float MIN_DEPTH = 100.0f;
constexpr float MAX_DEPTH = 7000.0f;
float rgbWeight = 0.5f;
float depthWeight = 0.5f;

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

int main(int argc, char** argv) {
    argparse::ArgumentParser program("tof_align");
    program.add_description("Align ToF depth over left or right camera and show a blended overlay.");
    program.add_argument("--camera")
        .default_value(std::string("left"))
        .choices("left", "right")
        .help("Camera to align depth onto: left=CAM_B, right=CAM_C (default: left)");

    try {
        program.parse_args(argc, argv);
    } catch(const std::runtime_error& err) {
        std::cerr << err.what() << '\n';
        std::cerr << program;
        return EXIT_FAILURE;
    }

    const std::string cameraArg = program.get<std::string>("--camera");
    const dai::CameraBoardSocket alignSocket = (cameraArg == "right") ? dai::CameraBoardSocket::CAM_C : dai::CameraBoardSocket::CAM_B;
    std::cout << "Aligning ToF depth over " << cameraArg << " camera\n";

    dai::Pipeline pipeline;

    auto tof = pipeline.create<dai::node::ToF>();
    tof->build(dai::CameraBoardSocket::AUTO, dai::ToFConfig::Profile::MID_RANGE, FPS);

    auto cam = pipeline.create<dai::node::Camera>()->build(alignSocket);
    auto camOut = cam->requestOutput(std::make_pair(CAMERA_SIZE.width, CAMERA_SIZE.height), std::nullopt, dai::ImgResizeMode::CROP, FPS, true);

    auto align = pipeline.create<dai::node::ImageAlign>();
    align->setRunOnHost(true);
    tof->depth.link(align->input);
    camOut->link(align->inputAlignTo);

    auto sync = pipeline.create<dai::node::Sync>();
    sync->setSyncThreshold(std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(0.5 / FPS)));
    sync->setRunOnHost(true);
    camOut->link(sync->inputs["rgb"]);
    align->outputAligned.link(sync->inputs["depth_aligned"]);
    sync->inputs["rgb"].setBlocking(false);

    auto syncQueue = sync->out.createOutputQueue();

    const std::string windowBlend = "tof-overlay-" + cameraArg;
    const std::string windowDepth = "depth-aligned";

    pipeline.start();
    cv::namedWindow(windowBlend);
    cv::namedWindow(windowDepth);
    cv::createTrackbar("RGB Weight %", windowBlend, nullptr, 100, updateBlendWeights);
    cv::setTrackbarPos("RGB Weight %", windowBlend, static_cast<int>(rgbWeight * 100));

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

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

        cv::Mat cvFrame = frameRgb->getCvFrame();
        if(cvFrame.channels() == 1) {
            cv::cvtColor(cvFrame, cvFrame, cv::COLOR_GRAY2BGR);
        }

        cv::Mat depthColorized = dai::utility::colorizeDepthFrame(*frameDepth, MIN_DEPTH, MAX_DEPTH, cv::COLORMAP_JET, true).getCvFrame();
        if(depthColorized.size() != cvFrame.size()) {
            cv::resize(depthColorized, depthColorized, cvFrame.size());
        }

        cv::imshow(windowDepth, depthColorized);

        cv::Mat blended;
        cv::addWeighted(cvFrame, rgbWeight, depthColorized, depthWeight, 0, blended);
        cv::imshow(windowBlend, blended);

        if(cv::waitKey(1) == 'q') {
            break;
        }
    }

    return 0;
}
```

### Need assistance?

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