Point cloud generation from depth images

[Geada8]

---

Required Info
Camera Model	D455
Firmware Version	5.16.0.1
Operating System & Version	Win 11
Platform	PC
SDK Version	2.56.1
Language	Python 3.11

Issue Description

I'm trying to create a point cloud from depth images captured by a realsense camera, but the results are not very good.

Code

The code I'm using is the following:

import numpy as np
import cv2
import open3d as o3d

file_path = '...'
depth_image = cv2.imread(file_path, cv2.IMREAD_UNCHANGED)

if depth_image is None:
print(f"Error: Could not load depth image from {file_path}")
exit()

fx = 392.542 # Focal length x
fy = 392.542 # Focal length y
ppx = 323.578 # Principal point x
ppy = 240.324 # Principal point y

height, width = depth_image.shape

print("Depth min:", np.min(depth_image))
print("Depth max:", np.max(depth_image))

z = depth_image.astype(float) / 1000.0 # Convert from mm to meters, adjust if needed

x, y = np.meshgrid(np.arange(width), np.arange(height))

x_3d = (x - ppx) * z / fx
y_3d = (y - ppy) * z / fy
z_3d = z

points_3d = np.stack((x_3d, y_3d, z_3d), axis=-1).reshape(-1, 3)

points_3d = points_3d[z_3d.reshape(-1) > 0]

print(f"Total valid points: {points_3d.shape[0]}")

pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points_3d)

o3d.io.write_point_cloud("pointcloud.ply", pcd)
print("Point cloud saved to pointcloud.ply")

o3d.visualization.draw_geometries([pcd], window_name="Point Cloud Visualization")

Results

Some examples of RGB images, depth and the corresponding point cloud :

Set 1

Set 2

Set 3

Set 4

Do you have any suggestions for what I can do to improve the results?

[MartyG-RealSense]

Hi @Geada8 As your script is apparently not using pyrealsense2 (the RealSense Python wrapper), my first suspicion would be that the camera's infrared emitter component is not enabled. When the emitter is on, it projects light and an invisible pattern of infrared dots that helps to illuminate dimly lit scenes and analyze plain surfaces with no texture (like the walls and doors in your images) for depth information.

Below is an example of pyrealsense2 code that enables the emitter component.

import pyrealsense2 as rs
pipeline = rs.pipeline()
config = rs.config()
pipeline_profile = pipeline.start(config)
device = pipeline_profile.get_device()
depth_sensor = device.query_sensors()[0]
if depth_sensor.supports(rs.option.emitter_enabled):
depth_sensor.set_option(rs.option.emitter_enabled, 1)

isl-org/Open3D#473 (comment) has an example of an Open3D pointcloud script that utilizes pyrealsense2. It would be necessary to install the RealSense pyrealsense2 wrapper to use this script if you have not done so already though.

[Geada8]

Unfortunately, the infrared emitter was already on and made no improvements.

That code you mentioned uses older versions of Open3D, so even after some changes and help from ChatGPT, I wasn't able to make it work

This part never worked:

pcd = create_point_cloud_from_rgbd_image(rgbd, pinhole_camera_intrinsic)
pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
pointcloud.points = pcd.points
pointcloud.colors = pcd.colors

The code I used when I started this issue works but with bad results so I tried adapting the one you mentioned to mix some things that I add that worked, but the problem kept happening and I wasn't able to create any point cloud.

That adapted code probably is useless, but after many debugging steps, this is what I have:

import pyrealsense2 as rs
import numpy as np
import cv2
import os
import open3d as o3d

# Create a pipeline
pipeline = rs.pipeline()

# Create a config object
config = rs.config()

# Enable the streams from the RealSense camera
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

# Start the pipeline
pipeline.start(config)

# Enable infrared emitter
depth_sensor = pipeline.get_active_profile().get_device().first_depth_sensor()
depth_sensor.set_option(rs.option.emitter_enabled, 1)  # 1 = emitter on, 0 = emitter off

# Create a directory to store the images
output_dir = r'---'
if not os.path.exists(output_dir):
    os.makedirs(output_dir)

frame_count = 0  # Start from zero
images_saved = False  # Flag to track if images have been saved

depth_image = None
color_image = None

try:
    # Display a window for the user
    cv2.namedWindow('RealSense Depth')
    cv2.namedWindow('RealSense Color')

    while True:
        # Wait for a coherent pair of frames: depth and color
        frames = pipeline.wait_for_frames()

        # Get the depth frame and color frame
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()

        if depth_frame and color_frame:
            # Convert images to numpy arrays
            depth_image = np.asanyarray(depth_frame.get_data())
            color_image = np.asanyarray(color_frame.get_data())

            # Display both the depth and color frames
            depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
            cv2.imshow('RealSense Depth', depth_colormap)
            cv2.imshow('RealSense Color', color_image)

            # Check for key press
            key = cv2.waitKey(1)  # Wait for 1 ms to allow image display to update
            if key == 32 and not images_saved:  # Space bar pressed and images not saved yet
                # Save the color and depth images
                color_image_path = os.path.join(output_dir, f'color_image_{frame_count}.png')
                depth_image_path = os.path.join(output_dir, f'depth_image_{frame_count}.png')

                cv2.imwrite(color_image_path, color_image)
                cv2.imwrite(depth_image_path, depth_image)

                print(f'Saved color and depth images: {color_image_path}, {depth_image_path}')
                images_saved = True  # Set flag to indicate images have been saved

            # Exit if 'q' is pressed
            elif key == ord('q'):
                print("Exiting...")
                break

finally:
    # Stop the pipeline
    pipeline.stop()
    cv2.destroyAllWindows()

# Proceed with point cloud generation after closing the preview windows
if depth_image is not None and color_image is not None:
    print("Depth and color images captured successfully.")
    print(f"Depth image shape: {depth_image.shape}, Color image shape: {color_image.shape}")

    # Normalize depth image: Convert from uint16 to float32 in meters
    depth_image_float = depth_image.astype(np.float32) / 1000.0  # Convert from mm to meters

    # Print depth statistics for debugging
    print(f"Depth min: {np.min(depth_image_float)}, Depth max: {np.max(depth_image_float)}")

    # Create RGBD image from color and normalized depth
    img_depth = o3d.geometry.Image(depth_image_float)
    img_color = o3d.geometry.Image(color_image)

    rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(img_color, img_depth, convert_rgb_to_intensity=False)

    # Check if RGBD image is created
    if rgbd is None:
        print("Failed to create RGBD image.")
        exit()

    # Use the fixed camera intrinsics
    fx = 392.542   # Focal length x
    fy = 392.542   # Focal length y
    ppx = 323.578  # Principal point x
    ppy = 240.324  # Principal point y

    # Retrieve image dimensions
    height, width = depth_image.shape

    pinhole_camera_intrinsic = o3d.camera.PinholeCameraIntrinsic(
        width, height, fx, fy, ppx, ppy
    )

    # Create point cloud from RGBD image and camera intrinsics
    pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, pinhole_camera_intrinsic)

    # Print the number of points in the point cloud
    num_points = len(pcd.points)
    print(f"Number of points in the point cloud: {num_points}")

    # Check if point cloud contains points
    if num_points == 0:
        print("Point cloud creation failed. No points were generated.")
    else:
        # Transform the point cloud to adjust the coordinate system if necessary
        pcd.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])

        # Save the point cloud to a file
        o3d.io.write_point_cloud("pointcloud.ply", pcd)
        print("Point cloud saved to pointcloud.ply")

        # Visualize the point cloud
        o3d.visualization.draw_geometries([pcd], window_name="Point Cloud Visualization")

Do you have any more suggestions? What can I do to improve my original results?

[MartyG-RealSense]

There are few available references regarding creating a pointcloud with Open3D using a RealSense camera, unfortunately.

#12090 is another reference that you could look at.

[MartyG-RealSense]

Hi @Geada8 Do you require further assistance with this case, please? Thanks!

[Geada8]

I do not think I can improve the results this way any further, I am currently trying to implement point cloud registration with icp to remove some noise this way alongside the point cloud registration. If you have some code on that it would be apreciated, otherwise I'm good.

[MartyG-RealSense]

In regard to using Open3D and ICP with a RealSense camera, isl-org/Open3D#362 should be a good entry point into the subject., with Python code at isl-org/Open3D#362 (comment)

[MartyG-RealSense]

Hi @Geada8 Were the links in the comment above helpful to you, please?

[MartyG-RealSense]

Hi @Geada8 Do you require further assistance with this case, please? Thanks!

[MartyG-RealSense]

Case closed due to no further comments received.