@jakaskerl
My last question for now is in regard of the camera intrinsic and extrinsic values. I know that extrinsic values are retrieved from calibration and they do not affect the focal length or FOV. More so its location and orientation. In contrast of the intrinsic values which can be retrieved by a python script that you guys have created. The intrinsic parameters of a camera depend on how it captures the images. Parameters such as focal length, aperture, field-of-view, resolution, etc govern the intrinsic matrix of a camera model. 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?

I assume tof-rgb alignment is not affected by the implementation of the values however I am unsure that these values do play an affect on how the camera captures point cloud or capture depth.

Python script from here:
For testing purposes I have added my intrinsic values and and pinhole intrinsic in bold to see if my point cloud would look any different.

import depthai as dai

import numpy as np

import cv2

import time

from datetime import timedelta

import datetime

import os

import sys

try:

import open3d as o3d

except ImportError:

sys.exit("Critical dependency missing: Open3D. Please install it using the command: '{} -m pip install open3d' and then rerun the script.".format(sys.executable))

FPS = 30

RGB_SOCKET = dai.CameraBoardSocket.CAM_C

TOF_SOCKET = dai.CameraBoardSocket.CAM_A

ALIGN_SOCKET = RGB_SOCKET

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)

pointcloud = pipeline.create(dai.node.PointCloud)

# Camera intrinsic parameters (ensure I am using the correct calibration values)

fx = 494.35192765 # Update with my calibrated value

fy = 499.48351759 # Update with my calibrated value

cx = 321.84779556 # Update with my calibrated value

cy = 218.30442303 # Update with my calibrated value

intrinsic = o3d.camera.PinholeCameraIntrinsic(width=640, height=480, fx=fx, fy=fy, cx=cx, cy=cy)

# ToF settings

camTof.setFps(FPS)

camTof.setImageOrientation(dai.CameraImageOrientation.ROTATE_180_DEG)

camTof.setBoardSocket(TOF_SOCKET)

tofConfig = tof.initialConfig.get()

# choose a median filter or use none - using the median filter improves the pointcloud but causes discretization of the data

tofConfig.median = dai.MedianFilter.KERNEL_7x7

# tofConfig.median = dai.MedianFilter.KERNEL_5x5

# tofConfig.median = dai.MedianFilter.KERNEL_7x7

tof.initialConfig.set(tofConfig)

# rgb settings

camRgb.setBoardSocket(RGB_SOCKET)

camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_800_P)

camRgb.setFps(FPS)

camRgb.setIspScale(3,4)

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"])

align.outputAligned.link(pointcloud.inputDepth)

sync.inputs["rgb"].setBlocking(False)

camRgb.isp.link(align.inputAlignTo)

pointcloud.outputPointCloud.link(sync.inputs["pcl"])

sync.out.link(out.input)

out.setStreamName("out")

def colorizeDepth(frameDepth):

invalidMask = frameDepth == 0

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)

    logDepth = np.clip(logDepth, logMinDepth, logMaxDepth)

    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)

    depthFrameColor[invalidMask] = 0

except IndexError:

    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):

global depthWeight

global rgbWeight

rgbWeight = float(percentRgb) / 100.0

depthWeight = 1.0 - rgbWeight

with dai.Device(pipeline) as device:

isRunning = True

q = device.getOutputQueue(name="out", maxSize=4, blocking=False)

vis = o3d.visualization.VisualizerWithKeyCallback()

vis.create_window()

pcd = o3d.geometry.PointCloud()

coordinateFrame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1000, origin=[0,0,0])

vis.add_geometry(coordinateFrame)

first = True

view_control = vis.get_view_control()

while isRunning:

    inMessage = q.get()

    inColor = inMessage["rgb"]

    inPointCloud = inMessage["pcl"]

    cvColorFrame = inColor.getCvFrame()

    cvRGBFrame = cv2.cvtColor(cvColorFrame, cv2.COLOR_BGR2RGB)

    cv2.imshow("color", cvColorFrame)

    key = cv2.waitKey(1)

    if key == ord('q'):

        break

    if key == ord('c'):

        print("saving...")

        current_time = datetime.datetime.now()

        formatted_time = current_time.strftime("%Y_%m_%d_%H_%M_%S")

        new_output = formatted_time

        os.mkdir(new_output)

        o3d.io.write_point_cloud(os.path.join(new_output, "tof_pointcloud.ply"), pcd)

        cv2.imwrite(os.path.join(new_output, "Image_of_material.png"), cvColorFrame)

        print(f"RGB point cloud saved to folder {new_output}")

    if inPointCloud:

        points = inPointCloud.getPoints().astype(np.float64)

        points[:, 1] = -points[:, 1]  # Invert Y axis

        pcd.points = o3d.utility.Vector3dVector(points)

        colors = (cvRGBFrame.reshape(-1, 3) / 255.0).astype(np.float64)

        pcd.colors = o3d.utility.Vector3dVector(colors)

        if first:

            vis.add_geometry(pcd)

            first = False

        else:

            vis.update_geometry(pcd)

    vis.poll_events()

    vis.update_renderer()

vis.destroy_window()

    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

      jakaskerl

      Nice, thank you for the update. 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? I've tried stereo depth to generate point cloud but it gave me similar results in regard of misaligned point cloud, but I will try again.

      I believe that you placed a stereo depth point cloud in one the discussion threads that I have created. It does works however it generates the point cloud in a narrow shape. So I am still reading documentation on the different settings and parameters that are appropriate for me use case. Most of my objects will be 20cm to 50 cm away from the camera.

        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

            jakaskerl
            So with the file that you sent me, I was able to run it. And it successfully displayed the tof and rgb color camera window. I then processed to apply a method for me to save both rgb-color camera and tof depth. Below are the examples. there is still misalignment(which i assume is expected right now), but it does seem to pick up objects that are behind my purple bottle. Along with the space between the back wall and the front of the cardboard box

              gdeanrexroth The correct angle displays perfect alignment, but as a i rotate the .ply file you can see the gaps and holes. Even with pre and post processing filtering, it winds up the same. Better but still misaligned. I can and will continue my project however it heavily relies on displaying align material and parts point cloud. That's a huge part of the project with the camera. The expected results should look like the ToF Demo video.

              @jakaskerl Here is somewhat of a better example.

                @jakaskerl
                More examples of the new script point cloud, some improvements are noticeable. But from the side again everything is not fully aligned.

                @jakaskerl
                With the pictures I attached above somewhat explains my side of the project as well. Without the fully, formed point cloud my team and I will not be able to view our material/parts in a 3d(point cloud) format. This results are with me undistorting the RGB camera. 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.

                  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

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

                    I doubt that this is my issue but can incorrect calibration of misalignment? I have tried undistorting but it seemed to not work. I am new to the computer vision world so I am unaware of few terms. This is my result of using undistorting

                      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

                        jakaskerl
                        Yes I did use the image aligned node within my script. I used the tof-rgb alignment script that you provided, I added a key function that will allow me to capture and save the tof-rgb depth as a ply file. There are other ways that I can approach this however, I simply wanted to see how the rgb would look.
                        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

                        # Define intrinsic parameters

                        RGB_INTRINSICS = np.array([

                        [494.3519287109375, 0.0, 321.8478088378906],
                        
                        [0.0, 499.4835205078125, 258.3044128417969],
                        
                        [0.0, 0.0, 1.0]

                        ])

                        TOF_INTRINSICS = np.array([

                        [842.6837768554688, 0.0, 673.1340942382812],
                        
                        [0.0, 851.867431640625, 412.4818115234375],
                        
                        [0.0, 0.0, 1.0]

                        ])

                        # Define camera resolutions

                        RGB_WIDTH, RGB_HEIGHT = 640, 480

                        TOF_WIDTH, TOF_HEIGHT = 1280, 800

                        class FPSCounter:

                        def __init__(self):
                        
                            self.frameTimes = []
                        
                        def tick(self):
                        
                            # record the current time for the FPS calculation
                        
                            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 based on the recorded frame times
                        
                            return (len(self.frameTimes) - 1) / (self.frameTimes[-1] - self.frameTimes[0])

                        def save_ply(rgb_image, depth_image, filename):

                        # Get dimensions of the RGB image
                        
                        height, width, _ = rgb_image.shape
                        
                        # Save as PLY file
                        
                        with open(filename, 'w') as ply_file:
                        
                            #write ply header
                        
                            ply_file.write('ply\\n')
                        
                            ply_file.write('format ascii 1.0\\n')
                        
                            ply_file.write(f'element vertex {height \* width}\\n')
                        
                            ply_file.write('property float x\\n')
                        
                            ply_file.write('property float y\\n')
                        
                            ply_file.write('property float z\\n')
                        
                            ply_file.write('property uchar red\\n')
                        
                            ply_file.write('property uchar green\\n')
                        
                            ply_file.write('property uchar blue\\n')
                        
                            ply_file.write('end_header\\n')
                        
                            
                        
                            # Convert depth image to point cloud using ToF intrinsics
                        
                            fx = TOF_INTRINSICS[0, 0]
                        
                            fy = TOF_INTRINSICS[1, 1]
                        
                            cx = TOF_INTRINSICS[0, 2]
                        
                            cy = TOF_INTRINSICS[1, 2]
                        
                            
                        
                            for v in range(height):
                        
                                for u in range(width):
                        
                                    z = depth_image[v, u]  # Depth value
                        
                                    if z > 0:  # Ignore zero 
                        
                                        # calculate 3d coordinates
                        
                                        x = (u - cx) \* z / fx
                        
                                        y = (v - cy) \* z / fy
                        
                                        r, g, b = rgb_image[v, u]  # RGB color
                        
                                        #write the vertex data to PLY file
                        
                                        ply_file.write(f'{x} {y} {z} {r} {g} {b}\\n')

                        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, 1)

                        # defines image sizes

                        depthSize = (TOF_WIDTH, TOF_HEIGHT)

                        rgbSize = camRgb.getIspSize()

                        # set output stream name

                        out.setStreamName("out")

                        # configure synchronization threshold

                        sync.setSyncThreshold(timedelta(seconds=0.5 / FPS))

                        # Linking

                        camRgb.isp.link(sync.inputs["rgb"]) #link RGB camera output to sync node

                        camTof.raw.link(tof.input) #link TOF camera raw output to TOF node

                        tof.depth.link(align.input) #link TOF depth output to align node

                        align.outputAligned.link(sync.inputs["depth_aligned"]) #link aligned depth to sync node

                        sync.inputs["rgb"].setBlocking(False) #set rgb input as non-blocking

                        camRgb.isp.link(align.inputAlignTo) #link sync output to XLinkOut node

                        sync.out.link(out.input)

                        def colorizeDepth(frameDepth):

                        invalidMask = frameDepth == 0
                        
                        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)
                        
                            logDepth = np.clip(logDepth, logMinDepth, logMaxDepth)
                        
                            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)
                        
                            depthFrameColor[invalidMask] = 0
                        
                        except IndexError:
                        
                            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):

                        global depthWeight
                        
                        global rgbWeight
                        
                        rgbWeight = float(percentRgb) / 100.0
                        
                        depthWeight = 1.0 - rgbWeight

                        # Connect to device and start pipeline

                        remapping = True

                        save_ply_flag = False

                        with dai.Device(pipeline) as device:

                        queue = device.getOutputQueue("out", 8, False)
                        
                        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))
                        
                            # Use the predefined intrinsics if available, otherwise use the calibration data
                        
                            if TOF_INTRINSICS is not None:
                        
                                M1 = TOF_INTRINSICS
                        
                            if RGB_INTRINSICS is not None:
                        
                                M2 = RGB_INTRINSICS
                        
                            try:
                        
                                T = np.array(calibData.getCameraTranslationVector(ALIGN_SOCKET, RGB_SOCKET, False)) \* 10
                        
                            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
                        
                            
                        
                            if frameDepth is not None:
                        
                                rgbFrame = frameRgb.getCvFrame()
                        
                                alignedDepthColorized = colorizeDepth(frameDepth.getFrame())
                        
                                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"):
                        
                                    remapping = not remapping
                        
                                    print(f"Remap turned {'ON' if remapping else 'OFF'}.")
                        
                                elif key == ord('s'):
                        
                                    save_ply_flag = 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)
                        
                            if save_ply_flag:
                        
                                rgb_frame = cv2.cvtColor(rgbFrame, cv2.COLOR_BGR2RGB)
                        
                                save_ply(rgb_frame, frameDepth.getFrame(), 'george_rgb1.ply')
                        
                                print("PLY file saved as 'george_rgb.ply'")
                        
                                save_ply_flag = False
                        
                            if key == ord("q"):
                        
                                break

                        cv2.destroyAllWindows()

                        I ran this script :https://docs.luxonis.com/software/depthai/examples/calibration_reader/
                        Here are the results from it, however if its extrinsic. Then I may have to recalibrate the camera with the method you recommended to me :

                        RGB Camera Default intrinsics...

                        [[494.3519287109375, 0.0, 321.8478088378906], [0.0, 499.4835205078125, 258.3044128417969], [0.0, 0.0, 1.0]]

                        640

                        480

                        RGB Camera Default intrinsics...

                        [[494.3519287109375, 0.0, 321.8478088378906], [0.0, 499.4835205078125, 258.3044128417969], [0.0, 0.0, 1.0]]

                        640

                        480

                        RGB Camera resized intrinsics... 3840 x 2160

                        [[2.96611157e+03 0.00000000e+00 1.93108691e+03]

                        [0.00000000e+00 2.99690112e+03 1.18982642e+03]

                        [0.00000000e+00 0.00000000e+00 1.00000000e+00]]

                        RGB Camera resized intrinsics... 4056 x 3040

                        [[3.13295532e+03 0.00000000e+00 2.03971057e+03]

                        [0.00000000e+00 3.16547681e+03 1.63600427e+03]

                        [0.00000000e+00 0.00000000e+00 1.00000000e+00]]

                        LEFT Camera Default intrinsics...

                        [[842.6837768554688, 0.0, 673.1340942382812], [0.0, 851.867431640625, 412.4818115234375], [0.0, 0.0, 1.0]]

                        1280

                        800

                        LEFT Camera resized intrinsics... 1280 x 720

                        [[842.68377686 0. 673.13409424]

                        [ 0. 851.86743164 372.48181152]

                        [ 0. 0. 1. ]]

                        RIGHT Camera resized intrinsics... 1280 x 720

                        [[836.24615479 0. 656.42828369]

                        [ 0. 845.62658691 399.05911255]

                        [ 0. 0. 1. ]]

                        LEFT Distortion Coefficients...

                        k1: -9.116254806518555

                        k2: 262.5550842285156

                        p1: 0.007134947460144758

                        p2: -0.0009857615223154426

                        k3: 1347.274169921875

                        k4: -9.195904731750488

                        k5: 260.98687744140625

                        k6: 1308.9786376953125

                        s1: 0.0

                        s2: 0.0

                        s3: 0.0

                        s4: 0.0

                        τx: 0.0

                        τy: 0.0

                        RIGHT Distortion Coefficients...

                        k1: -5.861973762512207

                        k2: 5.3061065673828125

                        p1: 0.005871884059160948

                        p2: 0.00142634566873312

                        k3: 88.46317291259766

                        k4: -6.072614669799805

                        k5: 7.037742614746094

                        k6: 83.25321960449219

                        s1: 0.0

                        s2: 0.0

                        s3: 0.0

                        s4: 0.0

                        τx: 0.0

                        τy: 0.0

                        RGB FOV 71.86000061035156, Mono FOV 71.86000061035156

                        LEFT Camera stereo rectification matrix...

                        [[ 9.94211665e-01 8.86633717e-03 -1.81476751e+01]

                        [-4.90145546e-03 9.94452611e-01 2.86947005e+01]

                        [ 2.85166444e-06 4.52051103e-06 9.96386328e-01]]

                        RIGHT Camera stereo rectification matrix...

                        [[ 1.00186534e+00 8.93177200e-03 -6.82440986e+00]

                        [-4.93918803e-03 1.00179181e+00 -7.21055399e-01]

                        [ 2.87361723e-06 4.55387305e-06 9.96286100e-01]]

                        Transformation matrix of where left Camera is W.R.T right Camera's optical center

                        [[ 9.99597728e-01 -7.52320397e-04 -2.83513945e-02 -3.98795390e+00]

                        [ 5.31902653e-04 9.99969602e-01 -7.78123224e-03 -2.61692833e-02]

                        [ 2.83563845e-02 7.76302209e-03 9.99567747e-01 -1.03633568e-01]

                        [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]

                        Transformation matrix of where left Camera is W.R.T RGB Camera's optical center

                        [[ 9.99843597e-01 -8.23507272e-03 1.56521089e-02 -7.51402760e+00]

                        [ 8.25367495e-03 9.99965310e-01 -1.12427305e-03 -1.49354547e-01]

                        [-1.56423096e-02 1.25328498e-03 9.99876857e-01 4.59043831e-01]

                        [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]

                        @jakaskerl

                        Screenshots of the same point cloud but from different angles. This is using the updated script that you gave me, along with the using the image align node.

                          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

                            jakaskerl
                            This is the error I ran into:
                            line 219, in <module>

                            **pipeline = create_pipeline(ALIGN_SOCKET)**
                            
                                       **^^^^^^^^^^^^^^^^^^^^^^^^^^^^^**

                            File "s:\DEPT\SVM4\Shared\Crossfunctional_Work\Projects\DepthCameras\LuxonisDepthAI\test_run\jimmy.py", line 61, in create_pipeline

                            **tof.setNumShaves(4)**
                            
                            **^^^^^^^^^^^^^^^^**

                            AttributeError: 'depthai.node.ToF' object has no attribute 'setNumShaves'