How to get depth value from rostopic subscriber

[xczzz]

My camera is D455. I tried to get depth from rostopic. Here is my code. Firstly I subscribe rostopic to get aligned depth.
ros::Subscriber aligned_depth_sub = nh.subscribe("/camera/aligned_depth_to_color/image_raw", 1, &depth_callback);
Then, I change the msg into cv::Mat in callback function like this.
Mat depth_image = Mat::zeros(Size(848, 480), CV_16UC1); void depth_callback(const sensor_msgs::ImageConstPtr &msg) { depth_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1)->image; imshow("Depth_Image", depth_image); waitKey(1); }
Finally I get the depth value of a specific pixel which is (pixel_x, pixel_y).
float real_z = depth_scale * depth_image.at<uint16_t>(pixel_x, pixel_y);
But the value is absolutely wrong. So I wonder how can I get the correct value when I subscribing rostopic.

[MartyG-RealSense]

Hi @xczzz A script provided in #807 resembles your method (subscribing to /camera/aligned_depth_to_color/image_raw and finding the depth at a particular pixel xy).

Within that discussion, Doronhi the RealSense ROS wrapper developer also suggests the Python-based ROS wrapper example script show_center_depth.py as a possible approach to obtaining the depth value of a pixel at #1524 (comment)

[xczzz]

Hi @MartyG-RealSense I found the problem. It should be depth_image.at<uint16_t>(pixel_y, pixel_x), not depth_image.at<uint16_t>(pixel_x, pixel_y).

[MartyG-RealSense]

Great to hear that you found a solution! Thanks very much for sharing it with the RealSense ROS community :)

[xczzz]

Thank you. There is another question about depth image. How can I get colorized depth image with realsense ros?

[xczzz]

@MartyG-RealSense I can get colorized depth image when using roslaunch with "filter:=colorizer". But then I can not get depth value from the black and white depth image with "depth_image.at<uint16_t>(pixel_y, pixel_x)". I hope to get a colorized depth image and depth value at the same. How can I do that?

[MartyG-RealSense]

The documentation on the front page of the RealSense ROS wrapper states that "colorizer will color the depth image. On the depth topic an RGB image will be published, instead of the 16bit depth values". I researched the subject extensively but was not able to find a workaround, unfortunately.

[xczzz]

It's alright. I will continue to study it. Thank you again.

[xczzz]

Besides, I wonder if there is any way to convert the sensor/Image from ros topic into rs::frameset? If have, I think I can use API of librealsense2 to do my work.

[MartyG-RealSense]

A rosbag-reading 'parser' tool called RealSenseHelper that a RealSense user comes to mind as an example of reading frames from topics. It can be downloaded as a zip file called 'RealSenseHelperzip' at the bottom of the opening comment of IntelRealSense/librealsense#2215

In regard to live retrieval of data from the ROS wrapper as rs2::frame, #1258 may be a helpful reference.

[xczzz]

@MartyG-RealSense I think rosbag is not suitable for my work. So I wonder how to get a specific point's 3D coordinate from pointcloud corresponding to a pixel in color image.

[MartyG-RealSense]

If it is not compulsory for you to generate a pointcloud, just get the xyz of a specific color image pixel coordinate, and it is also not compulsory to use ROS then you could consider converting a specific color pixel to a depth pixel with the SDK instruction rs2_project_color_pixel_to_depth_pixel

IntelRealSense/librealsense#5603 (comment)

If you need to use ROS then #277 (comment) may be helpful.

[xczzz]

@MartyG-RealSense Thank you very much. It is very helpful to me. But can you supply an example code about how to get the specific index in pointcloud corresponding to a pixel in color image please? I am not sure about it. (x, y) is the pixel. And the index in pointcloud is (x+y*image_width)? Is it right?

[MartyG-RealSense]

(x,y) refers to pixel coordinates, yes.

If your preference is to use Python in combination with ROS, the tutorial in the link below may fit your requirements. It uses librealsense's rs2_deproject_pixel_to_point instruction for converting a 2D pixel into a 3D point in combination with ROS's sensor_msgs/CameraInfo

https://medium.com/@yasuhirachiba/converting-2d-image-coordinates-to-3d-coordinates-using-ros-intel-realsense-d435-kinect-88621e8e733a

Alternatively, if you wanted a purely Python script for aligning depth to color and then obtaining 3D points with rs2_deproject_pixel_to_point then IntelRealSense/librealsense#7581 may be of interest.

[MartyG-RealSense]

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

[MartyG-RealSense]

Case closed due to no further comments received.

[JohannaPrinz]

@xczzz do you know if there is a similar solution in python for that?
I also using D455 and need the depth data from a specific (x,y) coordinate.

Would be fantastic if anybody could help me with that =)
Thanks a lot

[MartyG-RealSense]

Hi @JohannaPrinz If you are only using Python (not ROS) then the SDK instruction rs2_project_color_pixel_to_depth_pixel will convert a specific color pixel to a depth pixel to obtain Z-depth for that XY coordinate, as described in the Python script at IntelRealSense/librealsense#5603 (comment)

If you wanted to launch a Python node script from ROS then the script show_center_depth.py is a good reference.

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