Point Cloud Alignment

gdeanrexroth
Some efforts were done to improve TOF-RBG alignment.

Script below has some additional undistortion of the RGB, the results were allegedly better. It seems to be an issue of alignment node in FW.

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

# This example is intended to run unchanged on an OAK-D-SR-PoE camera
FPS = 30.0

RGB_SOCKET = dai.CameraBoardSocket.CAM_C
TOF_SOCKET = dai.CameraBoardSocket.CAM_A
ALIGN_SOCKET = RGB_SOCKET

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

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

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


pipeline = dai.Pipeline()
# Define sources and outputs
camRgb = pipeline.create(dai.node.ColorCamera)
tof = pipeline.create(dai.node.ToF)
camTof = pipeline.create(dai.node.Camera)
sync = pipeline.create(dai.node.Sync)
align = pipeline.create(dai.node.ImageAlign)
out = pipeline.create(dai.node.XLinkOut)

# ToF settings
camTof.setFps(FPS)
camTof.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG)
camTof.setBoardSocket(TOF_SOCKET)

# rgb settings
camRgb.setBoardSocket(RGB_SOCKET)
camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P)
camRgb.setFps(FPS)
camRgb.setIspScale(1, 2)

depthSize = (1280,800) #PLEASE SET TO BE SIZE OF THE TOF STREAM
rgbSize = camRgb.getIspSize()

out.setStreamName("out")

sync.setSyncThreshold(timedelta(seconds=0.5 / FPS))
rgbSize = camRgb.getIspSize()

# Linking
camRgb.isp.link(sync.inputs["rgb"])
camTof.raw.link(tof.input)
tof.depth.link(align.input)
align.outputAligned.link(sync.inputs["depth_aligned"])
sync.inputs["rgb"].setBlocking(False)
camRgb.isp.link(align.inputAlignTo)
sync.out.link(out.input)

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
remapping = True
with dai.Device(pipeline) as device:
    queue = device.getOutputQueue("out", 8, False)

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

    cv2.namedWindow(rgbDepthWindowName)
    cv2.createTrackbar(
        "RGB Weight %",
        rgbDepthWindowName,
        int(rgbWeight * 100),
        100,
        updateBlendWeights,
    )
    try:
        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 RuntimeError:
            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 RuntimeError:
            R = np.eye(3)
        TARGET_MATRIX = M1

        lensPosition = calibData.getLensPosition(RGB_SOCKET)
    except:
        raise
    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)

        sizeRgb = frameRgb.getData().size
        sizeDepth = frameDepth.getData().size
        # Blend when both received
        if frameDepth is not None:
            rgbFrame = frameRgb.getCvFrame()
            # Colorize the aligned depth
            alignedDepthColorized = colorizeDepth(frameDepth.getFrame())
            # Resize depth to match the rgb frame
            cv2.putText(
                alignedDepthColorized,
                f"FPS: {fpsCounter.getFps():.2f}",
                (10, 30),
                cv2.FONT_HERSHEY_SIMPLEX,
                1,
                (255, 255, 255),
                2,
            )
            cv2.imshow("depth", alignedDepthColorized)
            key = cv2.waitKey(1)
            if key == ord("m"):
                if remapping:
                    print("Remap turned OFF.")
                    remapping = False
                else:
                    print("Remap turned ON.")
                    remapping = True

            if remapping:
                mapX, mapY = cv2.initUndistortRectifyMap(
                    M2, D2, None, M2, rgbSize, cv2.CV_32FC1
                )
                rgbFrame = cv2.remap(rgbFrame, mapX, mapY, cv2.INTER_LINEAR)

            blended = cv2.addWeighted(
                rgbFrame, rgbWeight, alignedDepthColorized, depthWeight, 0
            )
            cv2.imshow(rgbDepthWindowName, blended)

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

Thanks,
Jaka

gdeanrexroth
FW side changes are needed so the script will likely stay the same.

Thanks,
Jaka

jakaskerl when can we expect the firmware corrections ?

apirrone Likely with depthai V3

gdeanrexroth Yup.

gdeanrexroth Does the intrinsic or extrinsic value need to be in the script that captures the point cloud(pcd or ply file)? Or does it need to be implemented into the script that does the icp or global registration?

In the pointcloud generation. The idea is to create a colorized pointcloud -- depth needs to be aligned to RGB. This can only be done by knowing the extrinsics from RGB to stereo, and intrinsics of rgb and the rectified stereo frames.

Thanks,
Jaka

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