Point Cloud Alignment

jakaskerl

Yes I am using open3d settings to align my point cloud. Here is an example of how I am applying it.
import open3d as o3d

import numpy as np

import copy

def draw_registration_result(source, target, transformation):

source_temp = copy.deepcopy(source)

target_temp = copy.deepcopy(target)

source_temp.paint_uniform_color([1, 0.706, 0])  # Yellow

target_temp.paint_uniform_color([0, 0.651, 0.929])  # Blue

source_temp.transform(transformation)

o3d.visualization.draw_geometries([source_temp, target_temp],

                                  zoom=0.4559,

                                  front=[0.6452, -0.3036, -0.7011],

                                  lookat=[1.9892, 2.0208, 1.8945],

                                  up=[-0.2779, -0.9482, 0.1556])

# Load your point clouds

source = o3d.io.read_point_cloud(r'file_path_to_pcd_1.pcd')

target = o3d.io.read_pointcloud(r'file_path_to_pcd_2.pcd')

# Visualize the initial alignment

print("Initial alignment")

draw_registration_result(source, target, np.identity(4))

# Perform ICP registration

threshold = 0.07 # Distance threshold, you may need to adjust this

trans_init = np.identity(4) # Initial transformation

reg_p2p = o3d.pipelines.registration.registration_icp(

source, target, threshold, trans_init,

o3d.pipelines.registration.TransformationEstimationPointToPoint())

print("Point-to-point ICP registration result:")

print(reg_p2p)

print("Transformation is:")

print(reg_p2p.transformation)

# Visualize the result after ICP

print("ICP result")

draw_registration_result(source, target, reg_p2p.transformation)

**I am using open3d with global and icp registration as well. I ran the both of my pcds in meshlab as well but the result was the same. Most of the items I am capturing do have a glare or shine in it. Based off some research I see that glare, plastic and some items that are reflective may conflict with my alignment of my point cloud. Correct me if I am wrong.

Thanks **

gdeanrexroth
for example this a screenshot of a point cloud that i cropped with using open3d. The unaligned points/voxels are evident in the screenshot. ICP and Global both struggle to refine or align two pcds because of the unalignment(my assumption). From above/straight view, everything looks on the point cloud. However as we see in the other screenshot, the sides are wrapped with the appropriate skin of the bottle.

Again I have two scripts that does the work here. I have a script that captures the tof depth and raw color feed from the camera that does some calculations and convert the images into a .pcd or .ply file. Then I have the second script that is reading in the file path of both point clouds to do pre processing work along with alignment.

jakaskerl
Yes, non reflective objects such as the cardboard box comes out pretty well.

See the example above. Still a little bit off but it is formed and also defined. Most of all the points are aligned, except for a few. I am unable to wrap the bottle, so i went with the next available thing to me.

Now that I know about the issues that reflective objects/surfaces cause. Most of the material/objects will have a glare or some sort of reflection. Any suggestions on how I can bypass this. I will continue to use refinement and other alignment methods on non reflective objects to see if it combines accurately.

jakaskerl
Thank you for response. With that being said, I assume the Luxonis team will work on solving this issue. But as for me, my project with the camera ends this month. I am sure that whenever this issue is fixed by you guys, you will update us. Does this affect the alignment issues that I have going on right now? Can I also assume that ICP and Global registration would not work in my case due to the material I capturing or it will work with some issues?

@jakaskerl
Stereo depth was not better. ToF did improve once I removed extra lighting that was exposed to camera. I know that documentation mentioned some of the issues that both stereo and depth may face. A control environment in door may be the short time solution for me. As you stated that you guys will be testing out alignment issues and etc. So i should expect some misalignment still but as of present day, it is expected to be that way correct(even if the object is not reflective, shiny or transparent)?

@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()

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.