librealsense: Pyrealsense2: Question about 3D reconstruction problem

Here is my version of the realsense code for anyone who would need it:

import pyrealsense2 as rs

# Pointcloud persistency in case of dropped frames
pc = rs.pointcloud()
points = rs.points()

# Create a pipeline
pipeline = rs.pipeline()

# Create a config and configure the pipeline to stream
config = rs.config()

# This is the minimal recommended resolution for D435
config.enable_stream(rs.stream.depth,  848, 480, rs.format.z16, 90)
config.enable_stream(rs.stream.color, 848, 480, rs.format.bgr8, 30)

# Start streaming
profile = pipeline.start(config)

# Getting the depth sensor's depth scale
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()

# Create an align object
align_to = rs.stream.depth

align = rs.align(align_to)
depth_sensor = profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()

try:
    while True:
        # Get frameset of color and depth
        frames = pipeline.wait_for_frames()

        # Align the depth frame to color frame
        aligned_frames = align.process(frames)

        # Get aligned frames
        depth_frame = aligned_frames.get_depth_frame()
        color_frame = aligned_frames.get_color_frame()
        depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics

        # Tell pointcloud object to map to this color frame
        pc.map_to(color_frame)

        # Generate the pointcloud and texture mappings
        points = pc.calculate(depth_frame)

        # Validate that both frames are valid
        if not depth_frame or not color_frame:
            continue

        depth_image = np.asanyarray(depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())

@ravan786 Sorry, I do not have experience with such an approach or making a point cloud created from multiple frames. I can imagine that it will be necessary to identify the points in a common reference frame so what I would do is to tie points from multiple color+depth frame pairs to one frame of reference (e.g. world) and then present them as one point cloud in that frame.

Hi @ljc19800331 ,

1: Is there a better way for to get textured 3D pointcloud in pyrealsense?

tl;dr: You should use points.get_texture_coordinates():

pc = rs.pointcloud()
pc.map_to(color)
points = pc.calculate(depth)
vtx = np.asanyarray(points.get_vertices())
tex = np.asanyarray(points.get_texture_coordinates())   # <----------- Like this

In details: Please note that pc.map_to(color) does not change the color image, it takes the color and maps it to the point cloud as its texture, using the calibration data of the streams that created these images (which you are lacking in your code, thus have misalignment).

After calling pc.map_to(color) you should call points.get_texture_coordinates() which returns a buffer protocol object that can be put into a numpy array. This array should be in the same size as points and its ith element contains the texture coordinates ([u,v]) of the ith element in the vertices array. The texture coordinates are the coordinate (valued in the range [0-1]) of the pixel in the color image you used in map_to.

1. Is there a direct way I can get the extrinsic parameters for the camera?

Yes 😃 I assume you want to de/project by yourself, so you will also need the intrinsics parameters and the depth scale. The SDK maps RealSense device in the following hierarchy:

Device
 |___ Sensor 1
         |___ Stream 1  # Stream Profile 1, one of video/motion/pose stream
         |___ ...
         |___ Stream N # Stream Profile N, one of video/motion/pose stream
 |___ ...
 |___ Sensor M

Each stream profile is either a video, motion, or pose stream (at the moment). Video streams profiles provide intrinsics of cameras and motion stream profiles provide motion device intrinsics. Inside the library we keep the extrinsic data between the streams, and you can ask each stream to get its transformation (extrinsics) to another stream.

Here’s how to get this data in python and also some additional data, and finally mapping a depth pixel to a color one:

import pyrealsense2 as rs
pipeline = rs.pipeline()
pipe_profile = pipeline.start()
frames = pipeline.wait_for_frames()
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()

# Intrinsics & Extrinsics
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
depth_to_color_extrin = depth_frame.profile.get_extrinsics_to(color_frame.profile)

# Depth scale - units of the values inside a depth frame, i.e how to convert the value to units of 1 meter
depth_sensor = pipe_profile.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()

# Map depth to color
depth_pixel = [240, 320]   # Random pixel
depth_point = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel, depth_scale)
color_point = rs.rs2_transform_point_to_point(depth_to_color_extrin, depth_point)
color_pixel = rs.rs2_project_point_to_pixel(color_intrin, color_point)
pipeline.stop()