Hello,

setDepthAlign method can align depth to a camera, but is there a way to align RGB camera to a mono camera?

    John194
    It's not integrated in API, but you can do alignment by getting RGB and mono camera intrinsics and extrinsics out of calibration (EEPROM) and the performing the transformations.

    Thoughts?

        jakaskerl

        I tried the RGB getMesh code and modified it to take in the rotation from extrinsics. It did not help my use case:

        I have a 3D point (obtained from the stereo pair's rectified images by calculating the disparity and such) that I'm trying to project back into the RGB camera.

        The results are almost successful but there is a noticeable offset which is different for each of my two cameras. For one camera, I have to multiply the intrinsic's x principal point by 1.03, for another - x by 1.08, y by 1.03. I thought the calibration was incorrect but the depth alignment that I get from the camera is accurate.

        I tried various modifications to my process but I cannot get this dialed in correctly.

          4 days later

          Hi @John194

          import numpy as np
import cv2
import depthai as dai
from projector_3d import PointCloudVisualizer

FPS = 10

RGB_SOCKET = dai.CameraBoardSocket.RGB
LEFT_SOCKET = dai.CameraBoardSocket.LEFT
RIGHT_SOCKET = dai.CameraBoardSocket.RIGHT

COLOR_RESOLUTION = dai.ColorCameraProperties.SensorResolution.THE_1080_P
LEFT_RIGHT_RESOLUTION = dai.MonoCameraProperties.SensorResolution.THE_800_P
depthSize = (1280, 800)
rgbSize = (1280, 800)

# Used for colorizing the depth map
MIN_DEPTH = 500  # mm
MAX_DEPTH = 10000  # mm

device = dai.Device()


def get_calibration(device, LEFT_SOCKET, RGB_SOCKET, size = (1280,800)):
    calibData = device.readCalibration2()
    M1 = np.array(calibData.getCameraIntrinsics(LEFT_SOCKET, *depthSize))
    D1 = np.array(calibData.getDistortionCoefficients(LEFT_SOCKET))
    M2 = np.array(calibData.getCameraIntrinsics(RGB_SOCKET, *rgbSize))
    D2 = np.array(calibData.getDistortionCoefficients(RGB_SOCKET))

    T = (
        np.array(calibData.getCameraTranslationVector(LEFT_SOCKET, RGB_SOCKET, False))
    )  # to mm for matching the depth
    R = np.array(calibData.getCameraExtrinsics(LEFT_SOCKET, RGB_SOCKET, False))[
        0:3, 0:3
    ]
    return M1, D1, M2, D2 ,T ,R

def align_images(left, right, M1, D1, M2, D2, T, R, size = (1280,800)):
    R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(M1, D1, M2, D2, size, R, T)

    # Compute the undistortion and rectification transformation map
    left = cv2.resize(left, size)
    right = cv2.resize(right, size)
    map1x, map1y = cv2.initUndistortRectifyMap(M1, D1, R1, P1, size, cv2.CV_32FC1)
    map2x, map2y = cv2.initUndistortRectifyMap(M2, D2, R2, P2, size, cv2.CV_32FC1)
    # Remap the images
    rectified1 = cv2.remap(left, map1x, map1y, cv2.INTER_LINEAR)
    rectified2 = cv2.remap(right, map2x, map2y, cv2.INTER_LINEAR)
    return rectified1, rectified2



pipeline = dai.Pipeline()

# Define sources and outputs
camRgb = pipeline.create(dai.node.ColorCamera)
left = pipeline.create(dai.node.MonoCamera)
right = pipeline.create(dai.node.MonoCamera)
stereo = pipeline.create(dai.node.StereoDepth)
sync = pipeline.create(dai.node.Sync)
out = pipeline.create(dai.node.XLinkOut)

left.setResolution(LEFT_RIGHT_RESOLUTION)
left.setBoardSocket(LEFT_SOCKET)
left.setFps(FPS)

right.setResolution(LEFT_RIGHT_RESOLUTION)
right.setBoardSocket(RIGHT_SOCKET)
right.setFps(FPS)

camRgb.setBoardSocket(RGB_SOCKET)
camRgb.setResolution(COLOR_RESOLUTION)
camRgb.setFps(FPS)

stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
stereo.setExtendedDisparity(True)
stereo.setLeftRightCheck(True)
stereo.setDepthAlign(LEFT_SOCKET)
stereo.enableDistortionCorrection(True)
stereo.setDepthAlignmentUseSpecTranslation(False)
stereo.setRectificationUseSpecTranslation(False)
stereo.setFocalLengthFromCalibration(True)
stereo.setDisparityToDepthUseSpecTranslation(False)

out.setStreamName("out")

# Linking
camRgb.isp.link(sync.inputs["rgb"])
left.out.link(stereo.left)
right.out.link(stereo.right)
left.out.link(sync.inputs["left"])
right.out.link(sync.inputs["right"])
stereo.depth.link(sync.inputs["depth"])
sync.out.link(out.input)


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


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


# Connect to device and start pipeline
with device:
    device.startPipeline(pipeline)
    queue = device.getOutputQueue("out", 8, False)

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

    cv2.namedWindow(rgb_depth_window_name)
    cv2.createTrackbar(
        "RGB Weight %",
        rgb_depth_window_name,
        int(rgbWeight * 100),
        100,
        updateBlendWeights,
    )

    M1, D1, M2, D2, T, R = get_calibration(device, LEFT_SOCKET, RGB_SOCKET, depthSize)
    while True:
        messageGroup: dai.MessageGroup = queue.get()
        frameRgb: dai.ImgFrame = messageGroup["rgb"]
        frameleft: dai.ImgFrame = messageGroup["left"]
        frameDepth: dai.ImgFrame = messageGroup["depth"]
        # Blend when both received
        if frameRgb is not None and frameDepth is not None and frameleft is not None:
            frameRgb = frameRgb.getCvFrame()
            frameleft = frameleft.getCvFrame()

            rectified1, rectified2 = align_images(frameleft, frameRgb, M1, D1, M2, D2, T, R)
            rectified2 = cv2.cvtColor(rectified2, cv2.COLOR_BGR2GRAY)
            # Save or display the rectified images
            cv2.imshow('rectified1.jpg', rectified1)
            cv2.imshow('rectified2.jpg', rectified2)

            #alignedDepthColorized = cv2.cvtColor(alignedDepthColorized, cv2.COLOR_BGR2GRAY) 
            blended = cv2.addWeighted(rectified1, rgbWeight, rectified2, depthWeight, 0)
            cv2.imshow(rgb_depth_window_name, blended)

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

            jakaskerl

            Hi, thanks for the script, here is the result I got:

            I tried to make the images align by changing the translation vector but it has no effect. T is completely ignored for some reason.

            Also, I want to clarify what I want to achieve with my MRE:

            The white box is what I get, the red box is what I'm trying to get.

            Maybe adapting your script and translating the 3D point by the baseline should return what I need. I'll try to play with it later.

              Hi @John194
              A limitation of the script is that it doesn't work well at close distances. Just keep that in mind.

              Thanks,
              Jaka

                Hi @John194
                There will be an alignment node that aims to fix these issues in the next depthai release in the upcoming days.

                Thanks,
                Jaka