Point Cloud Alignment

jakaskerl Likely with depthai V3

Ok, and any timeline on the release of this version ?

I'm a little confused. If there is a bug in the ImageAlign node in firmware, how did you get such a perfectly aligned point cloud here ?

gdeanrexroth

gdeanrexroth Overall until you guys fix the tof-rgb alignment and release the depthai V3. I can assume that I will not be able to make any more progress on my current project utilizing the ToF senor to generate point cloud. Until the Luxonis team fixes this issue, correct?

The new script I sent aims to fix this issue. We will probably just change the example (not sure what the current idea is for depthai). The only thing different seems to be the RGB undistortion. You can continue to develop the project just make sure you undistort the RGB camera.

apirrone I'm a little confused. If there is a bug in the ImageAlign node in firmware, how did you get such a perfectly aligned point cloud here ?

This was done here iirc.

Thanks,
Jaka

gdeanrexroth The correct angle displays perfect alignment, but as a i rotate the .ply file you can see the gaps and holes.

That is expected and is a result of occlusion.. This can't be fixed at all since information is missing.

gdeanrexroth So I am still unsure if I can make much more progress on my side since the issue is being fixed internally by you and your team.

The only fix on our part is include the RGB undistortion that was missed in the original release..

Thanks,
Jaka

jakaskerl

Undistorting the rgb doesn't help for me

Without rgb undistort :

With rgb undistort:

It's probably because I'm using fisheye lenses.
Using cv2.fisheye.initUndistortRectifyMap(...) doesn't help

apirrone
Yes, most likely. The image is still not undistorted properly. Not sure if this was even tested.

cc @CenekAlbl

Thanks,
Jaka

gdeanrexroth With undistorting not working properly are there any steps you recommend us to take? Or do we have to wait until depthai v3?

Should be working properly, just on on fisheye lenses.

gdeanrexroth This is my result of using undistorting

Did you use the ImageAlign node to align the tof and RGB? The RGB looks ok at first glance, so either extrinsics/intrinsics are incorrect or there is a mistake in aligning.

Thanks,
Jaka

gdeanrexroth
Try this:

import os
import time
import json
import cv2
import depthai as dai
import numpy as np
from datetime import timedelta
print(dai.__version__)
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_v != i):
                print(f'v -> {v} and i -> {i}') # This should never be printed
            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 create_pipeline(ALIGN_SOCKET):
    pipeline = dai.Pipeline()

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

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

    # Input for ToF configuration
    xinTofConfig = pipeline.create(dai.node.XLinkIn)
    xinTofConfig.setStreamName("tofConfig")
    xinTofConfig.out.link(tof.inputConfig)

    # 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 ColorCamera nodes for stereo pair
    colorLeft = pipeline.create(dai.node.ColorCamera)
    colorRight = pipeline.create(dai.node.ColorCamera)


    # Configure ColorCameras
    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)
    colorLeft.setColorOrder(dai.ColorCameraProperties.ColorOrder.RGB)
    colorRight.setColorOrder(dai.ColorCameraProperties.ColorOrder.RGB)
    #colorLeft.setImageOrientation(dai.CameraImageOrientation.NORMAL)
    #colorRight.setImageOrientation(dai.CameraImageOrientation.NORMAL)

    #colorLeft.setIspScale(2, 2)  # Corrected line
    #colorRight.setIspScale(2, 1)  # Corrected line


    # Create StereoDepth node
    stereo = pipeline.create(dai.node.StereoDepth)
    stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
    stereo.initialConfig.setMedianFilter(dai.MedianFilter.MEDIAN_OFF)
    stereo.setLeftRightCheck(True)
    stereo.setExtendedDisparity(False)
    stereo.setSubpixel(False)
    stereo.setDepthAlign(ALIGN_SOCKET)

    # Link the RAW outputs of the ColorCameras to the StereoDepth node
    colorLeft.isp.link(stereo.left)
    colorRight.isp.link(stereo.right)


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

    # Link outputs to Sync node with specified input names
    tof.depth.link(sync.inputs["depth_tof"])
    stereo.depth.link(sync.inputs["depth_stereo"])
    stereo.rectifiedLeft.link(sync.inputs["left_img"])
    stereo.rectifiedRight.link(sync.inputs["right_img"])
    colorLeft.isp.link(sync.inputs["rgb_img"])  # Corrected line

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

    return pipeline

def get_calib(RGB_SOCKET, ALIGN_SOCKET, depthSize, rgbSize):
    calibData = device.readCalibration2()
    M1 = np.array(calibData.getCameraIntrinsics(ALIGN_SOCKET, *depthSize))
    D1 = np.array(calibData.getDistortionCoefficients(ALIGN_SOCKET))
    M2 = np.array(calibData.getCameraIntrinsics(RGB_SOCKET, *rgbSize))
    D2 = np.array(calibData.getDistortionCoefficients(RGB_SOCKET))
    try:
        T = (
            np.array(calibData.getCameraTranslationVector(ALIGN_SOCKET, RGB_SOCKET, False))
            * 10
        )  # to mm for matching the depth
    except:
        T = np.array([0.0, 0.0, 0.001])
    try:
        R = np.array(calibData.getCameraExtrinsics(ALIGN_SOCKET, RGB_SOCKET, False))[
        0:3, 0:3
        ]
    except:
        R = np.eye(3)
    TARGET_MATRIX = M1
    lensPosition = calibData.getLensPosition(RGB_SOCKET)
    return M1, D1, M2, D2, T, R, TARGET_MATRIX

def getAlignedDepth(frameDepth, M1, D1, M2, D2, T, R, TARGET_MATRIX, depthSize,rgbSize):
    R1, R2, _, _, _, _, _ = cv2.stereoRectify(M1, D1, M2, D2, (100, 100), R, T)  # The (100,100) doesn't matter as it is not used for calculating the rotation matrices
    leftMapX, leftMapY = cv2.initUndistortRectifyMap(M1, None, R1, TARGET_MATRIX, depthSize, cv2.CV_32FC1)
    depthRect = cv2.remap(frameDepth, leftMapX, leftMapY, cv2.INTER_NEAREST)
    newR = np.dot(R2, np.dot(R, R1.T))  # Should be very close to identity
    newT = np.dot(R2, T)
    combinedExtrinsics = np.eye(4)
    combinedExtrinsics[0:3, 0:3] = newR
    combinedExtrinsics[0:3, 3] = newT
    depthAligned = reprojection(depthRect, TARGET_MATRIX, combinedExtrinsics, TARGET_MATRIX)
    # Rotate the depth to the RGB frame
    R_back = R2.T
    mapX, mapY = cv2.initUndistortRectifyMap(TARGET_MATRIX, None, R_back, M2, rgbSize, cv2.CV_32FC1)
    outputAligned = cv2.remap(depthAligned, mapX, mapY, cv2.INTER_NEAREST)
    return outputAligned

MIN_DEPTH = 500  # mm
MAX_DEPTH = 10000  # mm
def colorizeDepth(frameDepth, minDepth=MIN_DEPTH, maxDepth=MAX_DEPTH):
    invalidMask = frameDepth == 0
    # Log the depth, minDepth and maxDepth
    logDepth = np.log(frameDepth, where=frameDepth != 0)
    logMinDepth = np.log(minDepth)
    logMaxDepth = np.log(maxDepth)
    depthFrameColor = np.interp(logDepth, (logMinDepth, logMaxDepth), (0, 255)).astype(
        np.uint8
    )
    depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_JET)
    # Set invalid depth pixels to black
    depthFrameColor[invalidMask] = 0
    return depthFrameColor

RGB_SOCKET = dai.CameraBoardSocket.RGB
TOF_SOCKET = dai.CameraBoardSocket.CAM_A
LEFT_SOCKET = dai.CameraBoardSocket.LEFT
RIGHT_SOCKET = dai.CameraBoardSocket.RIGHT
ALIGN_SOCKET = RIGHT_SOCKET

COLOR_RESOLUTION = dai.ColorCameraProperties.SensorResolution.THE_1080_P
LEFT_RIGHT_RESOLUTION = dai.MonoCameraProperties.SensorResolution.THE_800_P
toFSize =  (640, 480)
rgbSize = (1280, 800)
rgbWeight = 0.4
depthWeight = 0.6

def updateBlendWeights(percent_rgb):
    """
    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(percent_rgb) / 100.0
    depthWeight = 1.0 - rgbWeight

if __name__ == '__main__':
    pipeline = create_pipeline(ALIGN_SOCKET)
    rgb_depth_window_name = "rgb-depth"

    with dai.Device(pipeline) as device:
        cv2.namedWindow(rgb_depth_window_name)
        cv2.createTrackbar(
        "RGB Weight %",
        rgb_depth_window_name,
        int(rgbWeight * 100),
        100,
        updateBlendWeights,
        )
        # Create output queue
        q_sync = device.getOutputQueue(name="sync_out", maxSize=4, blocking=False)
        try:
            M1, D1, M2, D2, T, R, TARGET_MATRIX = get_calib(RGB_SOCKET, ALIGN_SOCKET, toFSize, rgbSize)
        except:
            raise
        # Read calibration data
        calibration_handler = device.readCalibration()
        camera_names = {
            dai.CameraBoardSocket.CAM_B: 'cam_b',
            dai.CameraBoardSocket.CAM_C: 'cam_c',
            dai.CameraBoardSocket.CAM_A: 'tof'
        }

        # Create timestamped directory
        timestamp = time.strftime("%Y%m%d_%H%M%S")
        save_dir = os.path.join("data", timestamp)
        os.makedirs(save_dir, exist_ok=True)

        extrinsics_coeffs_tof_cam_b = calibration_handler.getCameraExtrinsics(dai.CameraBoardSocket.CAM_A, dai.CameraBoardSocket.CAM_B)
        extrinsics_coeffs_cam_b_tof = calibration_handler.getCameraExtrinsics(dai.CameraBoardSocket.CAM_B, dai.CameraBoardSocket.CAM_A)
        extrinsics_coeffs_tof_cam_c = calibration_handler.getCameraExtrinsics(dai.CameraBoardSocket.CAM_A, dai.CameraBoardSocket.CAM_C)

        # Save calibration data into the folder
        calib_data = {}
        for camera_socket, camera_name in camera_names.items():
            intrinsics = calibration_handler.getCameraIntrinsics(camera_socket)
            dist_coeffs = calibration_handler.getDistortionCoefficients(camera_socket)
            calib_data[camera_name] = {
                'intrinsics': intrinsics,
                'distortion_coefficients': dist_coeffs
            }

        calib_data['extrinsics'] = {
            'tof_cam_b': extrinsics_coeffs_tof_cam_b,
            'cam_b_tof': extrinsics_coeffs_cam_b_tof,
            'tof_cam_c': extrinsics_coeffs_tof_cam_c

        }
        calibration_file = os.path.join(save_dir, "calibration.json")
        with open(calibration_file, 'w') as f:
            json.dump(calib_data, f, indent=4)

        frame_counter = 0
        while True:
            # Get synchronized messages
            msgGrp = q_sync.get()

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

            if len(frames) == 5:
                # Process the frames
                depth_tof_frame = frames['depth_tof']
                depth_stereo_frame = frames['depth_stereo']
                left_frame = frames['left_img']
                right_frame = frames['right_img']
                rgb_frame = frames['rgb_img']

                # Save the frames
                depth_tof_filename = os.path.join(save_dir, f"depth_tof_{frame_counter:06d}.npy")
                depth_stereo_filename = os.path.join(save_dir, f"depth_stereo_{frame_counter:06d}.npy")
                left_filename = os.path.join(save_dir, f"left_img_{frame_counter:06d}.png")
                right_filename = os.path.join(save_dir, f"right_img_{frame_counter:06d}.png")
                rgb_filename = os.path.join(save_dir, f"rgb_img_{frame_counter:06d}.png")

                np.save(depth_tof_filename, depth_tof_frame)
                np.save(depth_stereo_filename, depth_stereo_frame)
                cv2.imwrite(left_filename, left_frame)
                cv2.imwrite(right_filename, right_frame)
                cv2.imwrite(rgb_filename, rgb_frame)

                # Optional: Display the images and depth maps
                # Normalize and colorize depth maps for visualization
                depth_tof_display = cv2.normalize(depth_tof_frame, None, 0, 255, cv2.NORM_MINMAX)
                depth_tof_display = np.uint8(depth_tof_display)
                depth_tof_display = cv2.applyColorMap(depth_tof_display, cv2.COLORMAP_JET)
                cv2.imshow("Depth ToF", depth_tof_display)

                depth_stereo_display = cv2.normalize(depth_stereo_frame, None, 0, 255, cv2.NORM_MINMAX)
                depth_stereo_display = np.uint8(depth_stereo_display)
                depth_stereo_display = cv2.applyColorMap(depth_stereo_display, cv2.COLORMAP_JET)
                cv2.imshow("Depth Stereo", depth_stereo_display)

                cv2.imshow("Left Image", left_frame)
                cv2.imshow("Right Image", right_frame)
                cv2.imshow("RGB Image", rgb_frame)
                h, w = depth_stereo_frame.shape[:2]
                M1_r, D1_r, M2_r, D2_r, T_r, R_r, TARGET_MATRIX_r = get_calib(RIGHT_SOCKET, TOF_SOCKET, toFSize, (w, h))
                alignedFrame = depth_stereo_frame
                alignedDepth = getAlignedDepth(depth_tof_frame, M1_r, D1_r, M2_r, D2_r, T_r, R_r, TARGET_MATRIX_r, toFSize,(w, h))
                frame_counter += 1

                alignedDepthColorized = colorizeDepth(alignedDepth)
                alignedFrame = colorizeDepth(alignedFrame)
                #mapX, mapY = cv2.initUndistortRectifyMap(
                #    M2_r, D2_r, None, M2_r, (w,h), cv2.CV_32FC1
                #)
                #alignedFrame = cv2.remap(alignedFrame, mapX, mapY, cv2.INTER_LINEAR)
                cv2.imshow("Aligned Image", alignedFrame)
                cv2.imshow("Aligned depth Image", alignedDepthColorized)
                #cv2.waitKey(0)

                blended = cv2.addWeighted(alignedFrame, rgbWeight, alignedDepthColorized, depthWeight, 0)
                cv2.imshow(rgb_depth_window_name, blended)
                # Exit condition
                if cv2.waitKey(1) == ord('q'):
                    break

        device.close()
        print('Data collection complete.')

The script aims to align TOF to depth.

LMK if it works. If it doesn't, either intrinsics or extrinsics are bad.

Thanks,
Jaka

gdeanrexroth
Update depthai version