Pyrealsense2: Question about 3D reconstruction problem

[ljc19800331]

Issue Description

Hello, I am recently using pyrealsense2 for 3D reconstruction based on SR300 and I got the following questions:

1: Is there a better way for to get textured 3D pointcloud in pyrealsense?
My method is to first get the 3D point cloud coordinates and the corresponding color image, and map these two information together to 3D textured information ( So far I am doing this step in MATLAB). However, I got the result that these two information are not correctly aligned together ( It might have some problems with my code).

Code in Python:

pc = rs.pointcloud()
points = rs.points
frames = pipeline.wait_for_frames()
depth = frames.get_depth_frame()
color = frames.get_color_frame()
pc.map_to(color)
points = pc.calculate(depth)
color_img = np.asanyarray(color.get_data())
depth_img = np.asanyarray(depth.get_data())
vtx = np.asanyarray(points.get_vertices())
pt_vtx = np.zeros( (len(vtx), 3) , float )
for i in range(len(vtx)):
     pt_vtx[i][0] = np.float(vtx[i][0])
     pt_vtx[i][1] = np.float(vtx[i][1])
     pt_vtx[i][2] = np.float(vtx[i][2])

Code in MATLAB:
The result I input for the MATLAB is the color_img and vtx.

pc = readNPY('data.npy');
img = imread('img_color.png');
color = double(reshape(im2double(imrotate(img, 90)), [480*640 3]));
pcshow( [ pc(:,1),pc(:,2),pc(:,3) ] , color);

But the image is not correctly aligned up with the point cloud (Would the problem be the difference between left image and right image? ).

The final result I want to get is similar to the result in this demo:
https://github.com/IntelRealSense/librealsense/tree/master/examples/pointcloud

2. Is there a direct way I can get the extrinsic parameters for the camera?
I want to get the extrinsic parameters for the camera relative to the target object in the image. However, I have no idea how to input the variables for the function to get the corresponding parameters:

rs.extrinsics.translation
rs.extrinsics.rotation
rotation = property(lambda self: object(), lambda self, v: None, lambda self: None)

Sorry for the long questions and I appreciate the helps.

[zivsha]

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.

2. 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()

[ljc19800331]

Hello @zivsha , thanks for the reply. I can successfully get the texture point cloud. However, it seems like the mapping the depth information to the 3D point cloud vertices.
The result is like this:

But my goal is to get the color map point cloud (sorry for the mistake before), like this:

The problem of the alignment is that I only map the color image to the point cloud based on the index from 1 to 300700 (480*640), which is wrong.

For the first problem, I might be confused about texture point cloud before. My goal is actually to map the color image to the point cloud, similar to the demo here:

https://github.com/IntelRealSense/librealsense/tree/master/examples/pointcloud

Is there any way to do it in python? I see the demo code in c++. My new code is shown in the following:

pc = rs.pointcloud()
frames = pipeline.wait_for_frames()
depth = frames.get_depth_frame()
color = frames.get_color_frame()
img_color = np.asanyarray(color.get_data())
img_depth = np.asanyarray(depth.get_data())
pc.map_to(color)
points = pc.calculate(depth)
vtx = np.asanyarray(points.get_vertices())
tex = np.asanyarray(points.get_texture_coordinates())

npy_vtx = np.zeros((len(vtx), 3), float)
for i in range(len(vtx)):
    npy_vtx[i][0] = np.float(vtx[i][0])
    npy_vtx[i][1] = np.float(vtx[i][1])
    npy_vtx[i][2] = np.float(vtx[i][2])

npy_tex = np.zeros((len(tex), 3), float)
for i in range(len(tex)):
    npy_tex[i][0] = np.float(tex[i][0])
    npy_tex[i][1] = np.float(tex[i][1])

For the second problem, I got the result after using your code to map depth pixel to color pixel.

depth_pixel = [240, 320]
depth_point = [-1.91091912711272e-05, 1.9768735000980087e-05, 0.00012498664727900177]
color_point = [0.025680750608444214, -0.0007127835415303707, 0.004205822013318539]
color_pixel = [4096.9208984375, 132.96957397460938]

I am confused about the 4096 value here. I think the color pixel coordinate should be something like [200, 400] within the range of 480 * 640 (I set this config at the beginning).

Thanks again for the answers.

[zivsha]

Hi,
I'm not sure if I understand your question, also I'm not sure what you are doing in your code...
To get a pointcloud with the real color of each point, you should do the same as the C++ example, which is using the texture coordinates from the points. These coordinates will indicate per point, the (u,v) coordinates in the color image, so you should be able to calculate the color pixel from them along with the color image resolution (which you can query from the frame).
I don't understand how you created the first image where it looks like a weird heat map, so if you could explain better what you are doing is what you are getting that would be great.

Regarding your second question, I'm not sure how you got this result, but please note that you should only transform points with depth value > 0, maybe that's the issue

[ljc19800331]

Thanks for the reply @zivsha ,

I think the reason is that I am not quite understand how to use the texture coordinate information (u,v) with range in [0,1]. I don't know how to map (u,v) back to the color image.

The problem might be that to find the mapping function that can perform. Given an color image,

1. A mapping function that takes a 3D point to texture coordinates (pyrealsense has done it)
   f(x, y, z) returns texture (u, v)
2. A sampling/lookup function which takes (u, v) coordinates and returns a color
   texture (u, v) returns RGB (r, g, b, a)

I know how to do it in c++ with the example shown previously. But I am not sure if pyrealsense has built in functions that can solve this problem. The second one is my main problem and I don't know how to convert (u,v) in range [0,1] to a color pixel value.

Thanks again for the patience and reply.

[zivsha]

The mapping of (u,v) to a color pixel is:
For {u/v} : [0-1] --> [0-{width-1/height-1}], Meaning that
(0,0) is mapped to (0,0)
(0,1) is mapped to (0,height-1)
(1,0) is mapped to (width-1,0)
(1,1) is mapped to (width-1,height-1)

We do not provide a function that takes texture (u, v) and returns RGB (r, g, b, a), mainly because mostly rendering libraries take texture parameters and will handle it on their own. But you can do the mapping yourself if you wish by converting texture to pixel using the above explanation.

[nathanlem1]

Is there any way to render (visualize) the pointcloud obtained from Intel RealSense API in Python? There are examples for rendering in C++ but not in Python. I hope you may give me a hint here. Thanks

[zivsha]

Unfortunately I don't know any, but a quick web search for "python point cloud" provides a few ways to do that...

[ljc19800331]

@zivsha Thanks for your reply before.
Recently when I want to write the function that takes texture (u, v) and returns RGB (r, g, b, a). However, when I get the data, I got the following questions:

My code is like that:
vtx = np.asanyarray(points.get_vertices())
tex = np.asanyarray(points.get_texture_coordinates())
np.savetxt("vtx.csv", vtx, delimiter=",")
np.savetxt("tex.csv", vtx, delimiter=",")
cv2.imwrite('color.png', color_image)

1. The texture coordinates is 3 dimension with (tx, ty). The vertice coordinate is (vx, vy, vz). The image coordinate is (u,v) and its shape is (480,640,3). Based on your previous comment:
   (0,0) is mapped to (0,0)
   (0,1) is mapped to (0,height-1)
   (1,0) is mapped to (width-1,0)
   (1,1) is mapped to (width-1,height-1)

Does this comment mean that all the texture coordinates have to be scaled to the specific index in the color image? For example:

1. if (u,v) = (0.5, 0.5) then it is mapped to ( (width-1)/2, (heigth-1)/2 ) = (639/2, 479/2)?
2. if (u,v) = (-0.1, -0.3) then how to map this to the index in the color image? (it is less than 0)
3. if (u,v) = (0.25, 0.45) then it is scaled to (639/4, 479*0.45) ?
4. if (u,v) = (0.5, 1.15) then how to map the data which is larger then 1?
5. Since the texture coordinate is 307200 * 2 (480*640). Is the index in the texture coordinate corresponds to the index in the vertice point cloud (vertice is 307200 * 3) ?

Thank you very much.

[alt01]

is there a way to align color to depth?

[svarnypetr]

For anyone following this with the D435 camera, you need to feed the deprojection the depth and not just the depth scale.

[svarnypetr]

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

[eyildiz-ugoe]

@svarnypetr Thanks for the effort, do you also have a snippet for rgb2cloud mapping in which we could obtain a 3D position of a 2D pixel? And why should the resolution be 848x480? I'm trying to get this working with the max. resolution and the resulting pixels are just way off. Is there a bug, or?

[svarnypetr]

@eyildiz-ugoe Sorry, don't have. But for getting the 3D coordinate you use deprojection:
rs.rs2_deproject_pixel_to_point(depth_intrin, [y, x], depth)
Where the depth is the value at x, y coordinates in the depth image.

What do you mean by way off? The 3D x, y won't be the same as the pixel x,y.

Higher resolution is possible, I use this for performance purposes due to my hardware. AFAIK the maximal resolution for D435 is 1280x720.

[eyildiz-ugoe]

@svarnypetr Way off as in a mapped coordinate like color_pixel: [55854.78125, 143.9385223388672] is pretty ridiculous, as x cannot be that high.

What I am trying to do is to map 2D pixel to 3D point and vice versa, and that does not seem to be working with Realsense. I've already created another issue to address this #4613

[svarnypetr]

@eyildiz-ugoe In my experience it shouldn't be a problem with Realsense.

[riteshkumar300]

Hi,
In the above approach you are making point cloud from a single color and depth frame(you are taking single frame from live stream and generating PC on it). Can you please suggest a way for making a complete point cloud of an object using multiple frames ?
I tried it using open3d reconstruction system but the texture is of very poor quality and appears to be blurred. Can anyone suggest anything to improve this texture?

Any help would be really appreciated...:)

[svarnypetr]

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

[riteshkumar300]

@svarnypetr any idea or code for tying points from multiple frame pairs?

[svarnypetr]

@ravan786 No, sorry.