How to send out 32bit ToF Depthmap through spout

BonkoKaradjov
Should be good:

import os
import time
import cv2
import depthai as dai
import numpy as np
from datetime import timedelta

def create_pipeline():
    pipeline = dai.Pipeline()

    # Create ToF node
    tof = pipeline.create(dai.node.ToF)
    tof.setNumShaves(4)

    # Configure ToF
    tofConfig = tof.initialConfig.get()
    tofConfig.enableFPPNCorrection = True
    tofConfig.enableOpticalCorrection = True
    tofConfig.enableWiggleCorrection = True
    tofConfig.enableTemperatureCorrection = True
    tofConfig.phaseUnwrappingLevel = 4
    tof.initialConfig.set(tofConfig)

    # Create ToF camera node
    cam_tof = pipeline.create(dai.node.Camera)
    cam_tof.setFps(20)
    cam_tof.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG)
    cam_tof.setBoardSocket(dai.CameraBoardSocket.CAM_A)
    cam_tof.raw.link(tof.input)

    # Create RGB camera nodes
    colorLeft = pipeline.create(dai.node.ColorCamera)
    colorRight = pipeline.create(dai.node.ColorCamera)

    # Configure RGB cameras
    colorLeft.setBoardSocket(dai.CameraBoardSocket.CAM_B)
    colorRight.setBoardSocket(dai.CameraBoardSocket.CAM_C)
    colorLeft.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P)
    colorRight.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P)
    colorLeft.setFps(20)
    colorRight.setFps(20)
    colorLeft.setInterleaved(False)
    colorRight.setInterleaved(False)

    # Create Sync node
    sync = pipeline.create(dai.node.Sync)
    sync.setSyncThreshold(timedelta(milliseconds=50))

    # Link outputs to Sync
    tof.depth.link(sync.inputs["tof"])
    colorLeft.isp.link(sync.inputs["left"])
    colorRight.isp.link(sync.inputs["right"])

    # Create output
    xout = pipeline.create(dai.node.XLinkOut)
    xout.setStreamName("sync_out")
    sync.out.link(xout.input)

    return pipeline

def colorize_depth_for_display(depth_frame):
    depth_normalized = cv2.normalize(depth_frame, None, 0, 255, cv2.NORM_MINMAX)
    depth_colored = cv2.applyColorMap(np.uint8(depth_normalized), cv2.COLORMAP_JET)
    return depth_colored

if __name__ == '__main__':
    pipeline = create_pipeline()

    with dai.Device(pipeline) as device:
        q_sync = device.getOutputQueue(name="sync_out", maxSize=4, blocking=False)
        frame_counter = 0
        save_dir = None

        while True:
            msgGrp = q_sync.get()
            
            frames = {}
            for name, msg in msgGrp:
                frames[name] = msg.getCvFrame()

            if len(frames) == 3:  # We expect 3 frames (ToF, left RGB, right RGB)
                # Get raw frames
                tof_depth = frames['tof']
                left_rgb = frames['left']
                right_rgb = frames['right']

                # Display frames (colorize depth only for display)
                cv2.imshow("ToF Depth", colorize_depth_for_display(tof_depth))
                cv2.imshow("Left RGB", left_rgb)
                cv2.imshow("Right RGB", right_rgb)

                key = cv2.waitKey(1)
                if key == ord('q'):
                    break
                elif key == ord('c'):
                    # Create new directory for saving if it doesn't exist
                    if save_dir is None:
                        timestamp = time.strftime("%Y%m%d_%H%M%S")
                        save_dir = os.path.join("data", timestamp)
                        os.makedirs(save_dir, exist_ok=True)
                        print(f"Created directory: {save_dir}")

                    # Save raw frames
                    tof_filename = os.path.join(save_dir, f"tof_{frame_counter:06d}.npy")
                    left_filename = os.path.join(save_dir, f"left_{frame_counter:06d}.png")
                    right_filename = os.path.join(save_dir, f"right_{frame_counter:06d}.png")

                    # Save ToF as raw numpy array, RGB images as PNG
                    np.save(tof_filename, tof_depth)
                    cv2.imwrite(left_filename, left_rgb)
                    cv2.imwrite(right_filename, right_rgb)
                    
                    print(f"Saved frame {frame_counter}")
                    frame_counter += 1

    cv2.destroyAllWindows()
    print("Program finished")

Thanks,
Jaka

Hi BonkoKaradjov
Team has had a chance to look at your scripts.
Issue was that your calibration was done using rotated TOF frame not RGB, so the alignment has to also be done for rotated TOF and not rotated RGB frame, otherwise the rot matrix is incorrect.

Code for properly aligning the streams:

import os 
import pathlib
import numpy as np
import cv2
import depthai as dai
from numba import jit, prange

@jit(nopython=True, parallel=True)
def reprojection(depth_image, depth_camera_intrinsics, camera_extrinsics, color_camera_intrinsics, depth_image_show = None):
    height = len(depth_image)
    width = len(depth_image[0])
    if depth_image_show is not None:
        image = np.zeros((height, width), np.uint8)
    else:
        image = np.zeros((height, width), np.uint16)
    if(camera_extrinsics[0][3] > 0):
        sign = 1
    else:
        sign = -1
    for i in prange(0, height):
        for j in prange(0, width):
            if sign == 1:
                # Reverse the order of the pixels
                j = width - j - 1
            d = depth_image[i][j]
            if(d == 0):
                continue
            # Convert pixel to 3d point
            x = (j - depth_camera_intrinsics[0][2]) * d / depth_camera_intrinsics[0][0]
            y = (i - depth_camera_intrinsics[1][2]) * d / depth_camera_intrinsics[1][1]
            z = d

            # Move the point to the camera frame
            x1 = camera_extrinsics[0][0] * x + camera_extrinsics[0][1] * y + camera_extrinsics[0][2] * z + camera_extrinsics[0][3]
            y1 = camera_extrinsics[1][0] * x + camera_extrinsics[1][1] * y + camera_extrinsics[1][2] * z + camera_extrinsics[1][3]
            z1 = camera_extrinsics[2][0] * x + camera_extrinsics[2][1] * y + camera_extrinsics[2][2] * z + camera_extrinsics[2][3]

            u = color_camera_intrinsics[0][0] * (x1  / z1) + color_camera_intrinsics[0][2]
            v = color_camera_intrinsics[1][1] * (y1  / z1) + color_camera_intrinsics[1][2]
            int_u = round(u)
            int_v = round(v)
            if int_u >= 0 and int_u < (len(image[0]) - 1) and int_v >= 0 and int_v < len(image):
                if depth_image_show is not None:
                    image[int_v][int_u] = depth_image_show[i][j][0]
                    image[int_v][int_u + sign] = depth_image_show[i][j][0]
                else:
                    image[int_v][int_u] = z1
                    image[int_v][int_u + sign] = z1
    return image

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

def get_Calibration_Data(calibData: dai.CalibrationHandler, TOF_SOCKET, ALIGN_SOCKET, depthSize = (640, 480), rgbSize = (1920, 1080)):
    M1 = np.array(calibData.getCameraIntrinsics(TOF_SOCKET, *depthSize))
    D1 = np.array(calibData.getDistortionCoefficients(TOF_SOCKET))
    M2 = np.array(calibData.getCameraIntrinsics(ALIGN_SOCKET, *rgbSize))
    D2 = np.array(calibData.getDistortionCoefficients(ALIGN_SOCKET))
    T = (
        np.array(calibData.getCameraTranslationVector(TOF_SOCKET, ALIGN_SOCKET, False))
        * 10
    )  # to mm for matching the depth
    R = np.array(calibData.getCameraExtrinsics(TOF_SOCKET, ALIGN_SOCKET, False))[
        0:3, 0:3
    ]
    TARGET_MATRIX = M1
    return M1, D1, M2, D2, TARGET_MATRIX, R, T

def getAlignedDepth(frameDepth, TARGET_MATRIX, M2, R, T, rgbSize = (1920, 1080)):
    R_180 = np.array([[-1., 0., 0.],
                      [0., -1., 0.],
                      [0., 0., 1.]])
    combinedExtrinsics = np.eye(4)
    combinedExtrinsics[0:3, 0:3] =  R
    combinedExtrinsics[0:3, 3] =  T
    depthAligned = reprojection(frameDepth, TARGET_MATRIX, combinedExtrinsics, TARGET_MATRIX)
    mapX, mapY = cv2.initUndistortRectifyMap(TARGET_MATRIX, None, np.eye(3), M2, rgbSize, cv2.CV_32FC1)
    outputAligned = cv2.remap(depthAligned, mapX, mapY, cv2.INTER_NEAREST)
    return outputAligned
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("-path", help="Path to the dataset, were all files are.")
args = parser.parse_args()

### Write down, which sockets you wanna align to ####
ALIGN_SOCKET = dai.CameraBoardSocket.CAM_C
TOF_SOCKET = dai.CameraBoardSocket.CAM_A

path = args.path
calibData = dai.CalibrationHandler(path + "/calib.json")
for files in os.listdir(path):
    ### LOADING ALL images which are used ###
    if files.split(".")[-1] == "json":
        continue
    index = files.split(".")[0].split("_")[1]
    depth = np.load(path + f"/tof_{index}.npy")
    left = cv2.imread(path + f"/left_{index}.png")
    right = cv2.imread(path + f"/right_{index}.png")


    ### LOGIC ####
    # Logic is following:

    # Since device was calibrated with the rotated depth, the depth frame MUST be rotated before aligned
    depth = cv2.rotate(depth, cv2.ROTATE_180)

    # From data you have, get all needed matrices
    M1, D1, M2, D2, TARGET_MATRIX, R, T = get_Calibration_Data(calibData, TOF_SOCKET, ALIGN_SOCKET, (depth.shape[1], depth.shape[0]), (right.shape[1], right.shape[0]))

    #Align rotated depth to the rotated RGB image
    alignedDepth = getAlignedDepth(depth, TARGET_MATRIX, M2, R, T, (right.shape[1], right.shape[0]))

    #Rotate back both depth and RGB
    right = cv2.rotate(right, cv2.ROTATE_180)
    alignedDepth = cv2.rotate(alignedDepth, cv2.ROTATE_180)


    blended = cv2.addWeighted(right, 0.5, colorizeDepth(alignedDepth), 0.5, 0)
    cv2.imshow("Blended", blended)
    cv2.waitKey(0)

Thanks,
Jaka

BonkoKaradjov
Why are you trying to recalibrate the device? Everything should be calibrated already.

Thanks,
Jaka

BonkoKaradjov
Camera should already calibrated, you shouldn't need further calibration.

You can use your script, but make sure to use alignment from https://discuss.luxonis.com/d/5692-how-to-send-out-32bit-tof-depthmap-through-spout/11 in order to achieve proper alignment.

LMK what the main issue is with NDIlib if you run into any.

Thanks,
Jaka