@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'

                jakaskerl
                Thank you for your response. I updated depthai within my conda environment, i was on version 2.25.1.0 and now I am at 2.28.0.0. I then proceed to run the code that you gave me and it successfully ran. However it was extremely lagging, I can only assume that the behavior occurred due to the multiple streams that were outputed…left,right image, rgb, depth tof, stereo and etc.

                This is the results of what were outputted. Is this expected? Or do I still have to recalibrate my camera? In my opinion the rgb depth seems to improved once again, but since the multiple streams caused lag. I was unable to fully conclude much. Is it possible for me to edit this script to capture the rgb-depth ,stereo depth and tof depth and save them as .ply files to view them as a point cloud? I proved the real image so you can understand the environment that I am in and also see the objects that I am capturing. If I have said anything that is incorrect, please correct me 🙂.

                  @jakaskerl
                  One last thing here is the data from the calibration.json, which was created from the script along with the .npy files. With the new script that you gave me, do I have to add in my intrinsic and extrinsic parameters as I did before in a format like this? I asked a similar question before and I remember you saying that I should add it to the/a script that captures the pcd. However this files generates .npy files for stereo,tof,left,right and rgb. I am able to convert that .npy file into a point cloud, the results will be pasted at the bottom.
                  :
                  "

                  RGB_INTRINSICS = np.array([

                  [836.2461547851562, 0.0, 656.4282836914062],

                  [0.0, 845.6265869140625, 439.0591125488281],

                  [0.0, 0.0, 1.0]
                  I add the above parameters to a both the script you gave me and a script that convers .npy and depth png to a point cloud.

                  ])

                  **
                  TOF_INTRINSICS = np.array([    [494.3519287109375, 0.0, 321.8478088378906],    [0.0, 499.4835205078125, 258.3044128417969],    [0.0, 0.0, 1.0]])**

                  "
                  {

                  "cam_b": {

                  "intrinsics": [

                  [

                  842.6837768554688,

                  0.0,

                  673.1340942382812

                  ],

                  [

                  0.0,

                  851.867431640625,

                  412.4818115234375

                  ],

                  [

                  0.0,

                  0.0,

                  1.0

                  ]

                  ],

                  "distortion_coefficients": [

                  -9.116254806518555,

                  262.5550842285156,

                  0.007134947460144758,

                  -0.0009857615223154426,

                  1347.274169921875,

                  -9.195904731750488,

                  260.98687744140625,

                  1308.9786376953125,

                  0.0,

                  0.0,

                  0.0,

                  0.0,

                  0.0,

                  0.0

                  ]

                  },

                  "cam_c": {

                  "intrinsics": [

                  [

                  836.2461547851562,

                  0.0,

                  656.4282836914062

                  ],

                  [

                  0.0,

                  845.6265869140625,

                  439.0591125488281

                  ],

                  [

                  0.0,

                  0.0,

                  1.0

                  ]

                  ],

                  "distortion_coefficients": [

                  -5.861973762512207,

                  5.3061065673828125,

                  0.005871884059160948,

                  0.00142634566873312,

                  88.46317291259766,

                  -6.072614669799805,

                  7.037742614746094,

                  83.25321960449219,

                  0.0,

                  0.0,

                  0.0,

                  0.0,

                  0.0,

                  0.0

                  ]

                  },

                  "tof": {

                  "intrinsics": [

                  [

                  494.3519287109375,

                  0.0,

                  321.8478088378906

                  ],

                  [

                  0.0,

                  499.4835205078125,

                  258.3044128417969

                  ],

                  [

                  0.0,

                  0.0,

                  1.0

                  ]

                  ],

                  "distortion_coefficients": [

                  -8.41947078704834,

                  64.94961547851562,

                  0.006374209653586149,

                  0.0031096169259399176,

                  -82.0173110961914,

                  -8.491059303283691,

                  65.28081512451172,

                  -83.71215057373047,

                  0.0,

                  0.0,

                  0.0,

                  0.0,

                  0.0,

                  0.0

                  ]

                  },

                  "extrinsics": {

                  "tof_cam_b": [

                  [

                  0.9998435974121094,

                  0.008253674954175949,

                  -0.015642309561371803,

                  7.521265506744385

                  ],

                  [

                  -0.00823507271707058,

                  0.9999653100967407,

                  0.0012532849796116352,

                  0.08689548820257187

                  ],

                  [

                  0.015652108937501907,

                  -0.0011242730543017387,

                  0.999876856803894,

                  -0.34154483675956726

                  ],

                  [

                  0.0,

                  0.0,

                  0.0,

                  1.0

                  ]

                  ],

                  "cam_b_tof": [

                  [

                  0.9998435974121094,

                  -0.00823507271707058,

                  0.015652108937501907,

                  -7.5140275955200195

                  ],

                  [

                  0.008253674954175949,

                  0.9999653100967407,

                  -0.0011242730543017387,

                  -0.14935454726219177

                  ],

                  [

                  -0.015642309561371803,

                  0.0012532849796116352,

                  0.999876856803894,

                  0.45904383063316345

                  ],

                  [

                  0.0,

                  0.0,

                  0.0,

                  1.0

                  ]

                  ],

                  "tof_cam_c": [

                  [

                  0.9990038275718689,

                  0.007529935333877802,

                  -0.04398486390709877,

                  3.5399038791656494

                  ],

                  [

                  -0.007824795320630074,

                  0.9999480247497559,

                  -0.006535347551107407,

                  0.06738178431987762

                  ],

                  [

                  0.04393336549401283,

                  0.006873009726405144,

                  0.9990108013153076,

                  -0.23108027875423431

                  ],

                  [

                  0.0,

                  0.0,

                  0.0,

                  1.0

                  ]

                  ]

                  }

                  }

                  The image above is the result one of the .npy tof files that were created.

                    gdeanrexroth
                    Alignment looks good! The multiple streams are the cause of lag (displaying them doesn't help either). No need to recalibrate 🙂.

                    gdeanrexroth With the new script that you gave me, do I have to add in my intrinsic and extrinsic parameters as I did before in a format like this? I asked a similar question before and I remember you saying that I should add it to the/a script that captures the pcd.

                    The script that creates the pointcloud. If you wish to convert the depth to pointcloud, you need intrinsics. You can also just use the pointcloud node and this will all be done on device.

                    The layering your image shows, is because your depth is limited to 95 disparity values. If you turn on subpixel, this should be much better since you now have 754(don't quote me on the value) values which results in smoother transition.

                    Thanks,
                    Jaka

                      jakaskerl
                      Ahh okay, i misunderstood. I will try both methods of adding the node or just converting depth to point cloud. The JSON created a the calibration file after the code you gave me ran. Do I take the intrinsic parameters from there or do I have use the parameters from the calibration.py 🙂 ? Below is an example script that I am using to display my depth color image and my .npy file which then converts the depth image into a point cloud. I am still unfamiliar with .npy files, is it neccesary for me to include into this script:

                      import matplotlib.image as mpimg

                      import re

                      import open3d as o3d

                      import numpy as np

                      import matplotlib.pyplot as plt

                      # Define camera intrinsic parameters for both RGB and TOF

                      RGB_INTRINSICS = np.array([

                      [842.6837768554688, 0.0, 673.1340942382812],

                      [0.0, 851.867431640625, 412.4818115234375],

                      [0.0, 0.0, 1.0]])

                      TOF_INTRINSICS = np.array([

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

                      ])

                      # This is a special function used for reading NYU pgm format

                      # as it is written in big endian byte order.

                      def read_nyu_pgm(filename, byteorder='>'):

                      with open(filename, 'rb') as f:
                      
                          buffer = f.read()
                      
                      try:
                      
                          header, width, height, maxval = re.search(
                      
                              b"(^P5\\s(?:\\s\*#.\*[\\r\\n])\*"
                      
                              b"(\\d+)\\s(?:\\s\*#.\*[\\r\\n])\*"
                      
                              b"(\\d+)\\s(?:\\s\*#.\*[\\r\\n])\*"
                      
                              b"(\\d+)\\s(?:\\s\*#.\*[\\r\\n]\\s)\*)", buffer).groups()
                      
                      except AttributeError:
                      
                          raise ValueError("Not a raw PGM file: '%s'" % filename)
                      
                      img = np.frombuffer(buffer,
                      
                                          dtype=byteorder + 'u2',
                      
                                          count=int(width) \* int(height),
                      
                                          offset=len(header)).reshape((int(height), int(width)))
                      
                      img_out = img.astype('u2')
                      
                      return img_out

                      print("Read dataset")

                      # Use your specified paths for RGB and depth images

                      color_raw = o3d.io.read_image(r"S:\DEPT\SVM4\Shared\Crossfunctional_Work\Projects\DepthCameras\LuxonisDepthAI\test_run\data\20241023_132728\rgb_img_000005.png")

                      depth_array = np.load(r"S:\DEPT\SVM4\Shared\Crossfunctional_Work\Projects\DepthCameras\LuxonisDepthAI\test_run\data\20241023_132728\depth_stereo_000004.npy")

                      # Convert numpy depth array to Open3D image

                      depth_raw = o3d.geometry.Image(depth_array.astype(np.float32))

                      color = o3d.geometry.Image(np.asarray(color_raw))

                      # Create an Open3D RGBD image using your data

                      rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(

                      color_raw, depth_raw, convert_rgb_to_intensity=False)

                      print(rgbd_image)

                      plt.subplot(1, 2, 1)

                      plt.title('RGB image')

                      plt.imshow(rgbd_image.color)

                      plt.subplot(1, 2, 2)

                      plt.title('Depth image')

                      plt.imshow(rgbd_image.depth)

                      plt.show()

                      # Create point cloud from RGBD image using custom camera intrinsics

                      intrinsic = o3d.camera.PinholeCameraIntrinsic(

                      width=1280,
                      
                      height=800,
                      
                      fx=RGB_INTRINSICS[0, 0],
                      
                      fy=RGB_INTRINSICS[1, 1],
                      
                      cx=RGB_INTRINSICS[0, 2],
                      
                      cy=RGB_INTRINSICS[1, 2]

                      )

                      pcd = o3d.geometry.PointCloud.create_from_rgbd_image(

                      rgbd_image,
                      
                      intrinsic)

                      # Flip it, otherwise the pointcloud will be upside down

                      pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])

                      o3d.visualization.draw_geometries([pcd])

                      Outcome of script that CREATES the point cloud. And yes this before me trying the point cloud node. Intrinsincs are in but I am unsure if they are placed in the right area or format. Thank you for your reply 🙂 :

                      10/25/2024 8:17AM
                      this is my attempt to insert the point cloud while subpixel is set to True.

                      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(True) # this was False at first but Jaka recommended True to get smoother point cloud
                      
                      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)
                      
                      pc_node = pipeline.create(dai.node.PointCloud)
                      
                      stereo.depth.link(pc_node.depth)
                      
                      pc_out = pipeline.create(dai.node.XLinkOut)
                      
                      pc_out.setStreamName("pc_out")
                      
                      pc_node.out.link(pc_out.input)
                      
                      # 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

                        gdeanrexroth Do I take the intrinsic parameters from there or do I have use the parameters from the calibration.py 🙂 ?

                        Take intrinsics from eeprom calibration.

                        Thanks,
                        Jaka

                          jakaskerl
                          Okay I have been using the those numbers within the file that creates the point cloud. I am still trying to use ICP to stitch the point clouds. My results vary but overall are still giving the similar results.

                          I am using a simple script that just displays the .ply file which allow me to crop them. But here is the version of the script that i am using.
                          import numpy as np

                          import open3d as o3d

                          def demo_crop_geometry():

                          print("Demo for manual geometry cropping")
                          
                          print("1) Press 'Y' twice to align geometry with negative direction of y-axis")
                          
                          print("2) Press 'K' to lock screen and to switch to selection mode")
                          
                          print("3) Drag for rectangle selection,")
                          
                          print("   or use ctrl + left click for polygon selection")
                          
                          print("4) Press 'C' to get a selected geometry")
                          
                          print("5) Press 'S' to save the selected geometry")
                          
                          print("6) Press 'F' to switch to freeview mode")
                          
                          
                          
                          # Load a single point cloud file
                          
                          # pcd = o3d.io.read_point_cloud(r'S:\\DEPT\\SVM4\\Shared\\Crossfunctional_Work\\Projects\\DepthCameras\\LuxonisDepthAI\\test_run\\Image and PointCloud of Material For AutoStore\\capture_1_2024_10_21_12_06_01\\tof_pointcloud_1.ply')
                          
                          # pcd = o3d.io.read_point_cloud(r"S:\\DEPT\\SVM4\\Shared\\Crossfunctional_Work\\Projects\\DepthCameras\\LuxonisDepthAI\\test_run\\GT_CUP\\MANUALLY_MODIFIED\\tof_pointcloud_5.ply")
                          
                          pcd = o3d.io.read_point_cloud(r'S:\\DEPT\\SVM4\\Shared\\Crossfunctional_Work\\Projects\\DepthCameras\\LuxonisDepthAI\\test_run\\GT_CUP\\MANUALLY_MODIFIED\\cropped_cup_5.ply')
                          
                          # Visualize and edit the point cloud
                          
                          o3d.visualization.draw_geometries_with_editing([pcd])

                          if name == "main":

                          demo_crop_geometry()

                          @jakaskerl
                          overall for my project this is the ending result my team and I are looking for.

                          I took this example point cloud from the ICP registration website. But we are still struggling to get a result that resembles that. Even at the bare minimum state I am struggling to display/ distribute a point cloud that resembles the object that is in front of the camera.

                          A minor issue that I am experiencing is with the numba library. Every so often I will quit the code so i can view the png's and .npy files. I will run the code again and it will display this error message
                          "2.27.0.0

                          Traceback (most recent call last):

                          File "s:\DEPT\SVM4\Shared\Crossfunctional_Work\Projects\DepthCameras\LuxonisDepthAI\test_run\tree_s.py", line 9, in <module>

                          from numba import jit, prange

                          ModuleNotFoundError: No module named 'numba'"
                          But whenever it does work, it displays the current depthai that I have installed which is 2.28.0.0 and successfully runs the code. I am sure that this conflicting issue is on my end. But I am bringing it to your attention.

                          EDIT
                          My last struggle is adding the point cloud node to the script that you gave me. I followed the instructions that was listed on the website however I am struggling to enable it within the script correctly.

                          Here is another point cloud displayed in meshlab application: