Inaccurate mapping of 2D pixel to 3D world coordinate

[Silvuurleaf]

|---------------------------------|------------------------------------------- |
| Camera Model | D435 |
| Firmware Version | (Open RealSense Viewer --> Click info) |
| Operating System & Version | Windows 10 |
| Platform | PC |
| SDK Version | pyrealsense2|
| Language | python 3.6 |
| Segment | Robotics |

I'm trying to find the 3D world coordinates of a point in my image (X, Y, Z) relative to my camera by deprojecting the pixel from 2D to 3D. However when I use the command

rs.rs2_deproject_pixel_to_point(intrinsics, pixel, depth_of_pixel)

the X,Y distance is off by 3-4". I think this is due to the camera intrinsic matrix being given for an image that is 1280 x 720, while I'm doing image analysis on images of size 640 x 480. However, I don't know how to get the intrinsics of my camera for images of this size.

I'm collecting the intrinsics here

self.profile = self.pipe.start()
      for x in range(40):  
            self.pipe.wait_for_frames()

profile = self.profile.get_stream(rs.stream.depth) 
self.RSintrinsics = profile.as_video_stream_profile().get_intrinsics()
frameset = self.pipe.wait_for_frames()  
self.pipe.stop()

Then I align the depth and color image frames.

align = rs.align(rs.stream.color)
frameset = align.process(frameset)

From here I find a pixel that I want in world coordinates relative to my camera and deproject it using intel RealSense SDK.

rs.rs2_deproject_pixel_to_point(self.RSintrinsics, pixel_XY, depth_of_pixel)

Below is the image I'm using to check my deprojection. I'm using an aruco marker and simply finding it's center and how far away it is relative to my camera in X,Y,Z coordinates.

The red dot in the center of the blue square is the center of the aruco marker. The other red dot represents the cener of the camera.

The intrinisics that I get from SDK is the following.

width: 1280, height: 720, ppx: 639.221, ppy: 359.779, fx: 643.132, fy: 643.132, model: Brown Conrady, coeffs: [0, 0, 0, 0, 0]

Depth is: 1.278

I deproject the pixel in two ways manually using the code I found here: https://pterneas.com/2018/10/10/intel-realsense-coordinate-mapping/

And the results I get are close, but are still off. The below results are measured in meters.

deprojected point: [0.025393313942056888, -0.2559040390922383, 1.2780001163482666]

The equivalent measurements in inches are as follows. I'm measuring these relative to the red dot that represents the center of the camera.

Xdistance_measured: 1"
Xactual =~ 1"
Ydistance_measured: 10"
Yactual =~ 8.5"

Then the realsense deprojection is completely off.

[-0.622418224811554, -0.4089147448539734, 1.2780001163482666]

How can I fix these issues?

[Silvuurleaf]

Just an update the measured xdistance and measured ydistance are both off. The x distance is off by about an inch and the ydistance is off by about 2"

[ev-mp]

@Silvuurleaf, the way the deprojection is done is not correct - and the missing part imho, could be due to misinterpretation of the transformation imposed upon Depth frame when "aligning" to another stream.
The "depth aligned to ..." is roughly* equivalent to "recapturing" the scene with a depth sensor which has the properties of the sensor to which it is being "aligned" - the sensor's position orientation, optical distortions and sensor size (of the RGB sensor in the above case).
*In most cases the actual coverage is less than 100% due to differences in sensors' FOV and their corresponding position.

The code snippet utilizes the depth intrinsic, and tries to apply it on "aligned" depth frame, which has the intrinsic of the RGB frame, which leads to errors in projection:

profile = self.profile.get_stream(rs.stream.depth)
self.RSintrinsics = profile.as_video_stream_profile().get_intrinsics()
align = rs.align(rs.stream.color)
frameset = align.process(frameset)
rs.rs2_deproject_pixel_to_point(self.RSintrinsicsAlignedDepthIntrinsic, pixel_XY, depth_of_pixel)

To avoid bookkeeping the of the stream to which depth has been aligned to, I'd recommend to extract the new intrinsic immediately after acquiring the "aligned depth" frame:

...
aligned_frames= align.process(frameset)
#Get aligned frames
aligned_depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
#Validate aligned depth
if not aligned_depth_frame:
continue
#Grab new intrinsics
aligned_depth_intrinsics = rs.video_stream_profile(aligned_depth_frame.profile).get_intrinsics())
...

[ev-mp]

@Silvuurleaf , did you manage to resolve the discrepancies ?

[Silvuurleaf]

I'm sorry I didn't see that I had a response. Actually, I implemented another solution. I used the RGB's FOV and depth of a particular pixel to find a correlation between (u,v) image pixels in world coordinates relative to the center of the RGB camera.

That seems to work okay for now. I'll revisit this thread if I need a more precise solution.

[ev-mp]

Sure, feel free to reopen if you'll need more assistance.

[npd26]

@Silvuurleaf is it possible like this,

I'm sorry I didn't see that I had a response. Actually, I implemented another solution. I used the RGB's FOV and depth of a particular pixel to find a correlation between (u,v) image pixels in world coordinates relative to the center of the RGB camera.

That seems to work okay for now. I'll revisit this thread if I need a more precise solution.

I will find contour center(one image point (u,v)) with OpenCV and then using realsense deproject_pixel_to_point to get its actual (x,y,z) value w.r.t camera frame? can you guide me liitle bit?

thanks.

[eyildiz-ugoe]

I've the same problem, on top of that, I've used the code provided by the support, yet, the mapping is still not correct @ev-mp . I am using a D435.

import pyrealsense2 as rs
import numpy as np

config = rs.config()
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
pipeline = rs.pipeline()
pipe_profile = pipeline.start(config)
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)
color_to_depth_extrin = color_frame.profile.get_extrinsics_to(depth_frame.profile)
print("\n Depth intrinsics: " + str(depth_intrin))
print("\n Color intrinsics: " + str(color_intrin))
print("\n Depth to color extrinsics: " + str(depth_to_color_extrin))

# 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()
print("\n\t depth_scale: " + str(depth_scale))
depth_image = np.asanyarray(depth_frame.get_data())
depth_pixel = [200, 200] # Random pixel
depth_value = depth_image[200][200]*depth_scale
print("\n\t depth_pixel@" + str(depth_pixel) + " value: " + str(depth_value) + " meter")

# From pixel to 3D point
depth_point = rs.rs2_deproject_pixel_to_point(depth_intrin, depth_pixel, depth_value)
print("\n\t 3D depth_point: " + str(depth_point))

# From 3D depth point to 3D color point
color_point = rs.rs2_transform_point_to_point(depth_to_color_extrin, depth_point)
print("\n\t 3D color_point: " + str(color_point))

# From color point to 2D color pixel
color_pixel = rs.rs2_project_point_to_pixel(color_intrin, color_point)
print("\n\t color_pixel: " + str(color_pixel))

Depth intrinsics: width: 1280, height: 720, ppx: 643.548, ppy: 367.861, fx: 652.776, fy: 652.776, model: Brown Conrady, coeffs: [0, 0, 0, 0, 0]

 Color intrinsics: width: 1280, height: 720, ppx: 640.522, ppy: 360.351, fx: 926.419, fy: 927.058, model: Inverse Brown Conrady, coeffs: [0, 0, 0, 0, 0]

 Depth to color extrinsics: rotation: [0.999905, 0.0111846, -0.0081191, -0.0111741, 0.999937, 0.00133797, 0.00813355, -0.00124712, 0.999966]
translation: [0.0147091, -5.76127e-05, 0.000246798]

	 depth_scale: 0.0010000000475

	 depth_pixel@[200, 200] value: 0.0 meter

	 3D depth_point: [-0.0, -0.0, 0.0]

	 3D color_point: [0.01470907311886549, -5.761265492765233e-05, 0.00024679797934368253]

	 color_pixel: [55854.78125, 143.9385223388672]

This part color_pixel: [55854.78125, 143.9385223388672] is obviously wrong, since X cannot be so.

[dpbnasika]

Is there one solution to find the correct 3d point with respect to camera in real world and it’s corresponding 2d pixel ?. Especially with ROS or ROS2 wrapper, kindly help. I always get this offset and I tried all the ways that has been suggested in the forums and all of them are scattered here and there and , if there is no ready solution available , how does the process looks like.

Best

[MartyG-RealSense]

A ROS 'node script' (a Python pyrealsense2 script launched from ROS) provided by the ROS1 and ROS2 wrappers called show_center_depth.py can be used to obtain the real-world distance value in meters of the coordinate at the center of the camera's view.

ROS1
https://github.com/IntelRealSense/realsense-ros/blob/ros1-legacy/realsense2_camera/scripts/show_center_depth.py

ROS2
https://github.com/IntelRealSense/realsense-ros/blob/ros2-development/realsense2_camera/scripts/show_center_depth.py

In both versions of the script it does so using the RealSense SDK instruction rs2_deproject_pixel_to_point (which you referred to in your opening comment in this discussion), which converts a 2D pixel coordinate into a 3D world coordinate.

After performing the conversion from 2D pixel to 3D world point, it is also possible to convert a 3D point coordinate back to a 2D pixel coordinate with rs2_project_point_to_pixel, though the show_center_depth.py script does not demonstrate this.

---

rs2_deproject_pixel_to_point and rs2_project_point_to_pixel can also be used in SDK program scripts outside of ROS. When depth and RGB color is being used, the typical approach in such scripts is to first perform depth to color alignment and then secondly use rs2_deproject_pixel_to_point to convert a 2D XY image pixel coordinate to a 3D XYZ point coordinate in the real-world space.

The pyrealsense2 example program opencv_pointcloud_viewer.py may be a useful reference. It uses map.to and pc.calculate to generate a pointcloud where RGB and depth are mapped together. This approach is slightly more accurate than using align_to. You can then store the 3D XYZ world coordinate in vertices and individually retrieve the X, Y and Z values of those vertices.

https://github.com/IntelRealSense/librealsense/blob/master/wrappers/python/examples/opencv_pointcloud_viewer.py#L305-L311

[FeiSong123]

Hello guys ! I am new here in python realsense, I want to get a color 3d point from a color 2d point and my depthImage have been aligned to color, can you help me please

[MartyG-RealSense]

Hi @FeiSong123 Instead of aligning the entire image, you can use rs2_project_color_pixel_to_depth_pixel to convert a single specific coordinate from a 2D color pixel to a 3D depth pixel. An example of use of this instruction in Python is at #5603 (comment)