Zed-ros-wrapper: Depth in meters

[Mostafa-Mansour]

Working with ROS, I can get only the depth map (depths scaled to the range from 0 to 255). Even when I subscribe to "/depth/depth_registered" topic, I still get the scaled depth. How can I get the true depths in meters (or mm) inside ROS?.

[Myzhar]

The "/depth/depth_registered" topic is by default a 32 bit floating point image and each pixel is the depth value in meters.
You can get the value in millimeters setting the parameter "openni_depth_mode" to 1, the image will be converted to 16 bit unsigned, like required by OpenNI specifications

[Mostafa-Mansour]

Thanks for your reply.

However, when I subscribe to " /depth/depth_registered" topic or when I write a rostopic echo command on the terminal, I get a matrix of integer values between 0-255. It means that what I get is the scaled depth map not the depth values themselves.

[ravikt]

Hi @Mostafa-Mansour, Convert the "depth/depth_registered" from ros message format to opencv image format using cv bridge. Republish the opencv depth image. You will get the depth in meters for all the pixels.

[Mostafa-Mansour]

Thanks @ravikt , I got it.

[Mostafa-Mansour]

hi @ravikt , I converted it to opencv image and I can get the depth for each individual pixel but when I want to publish it again, I need to convert it sensor_msgs::Image data type and I lose the information again. So, how can I republish it without losing the information? should I publish it as an image or should I publish it as an array using other datatypes?

[Myzhar]

How are you reading the pixel value?
The Image message has an "uint8_t" vector for data (http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Image.html) you must convert it to float:

float* dataPtr = (float*)(&msg.data[0]);

[Mostafa-Mansour]

I converted it to float, then read it.

// using this piece of code
cv::Mat cv_image=cv_ptr->image; // cv_ptr is the pointer you get from cv_bridge
cv::Scalar intensity=cv_image.at (i,j); // i,j is our pixel
double pixel_depth=intensity.val[0]; // pixel depth in meter (or mm depending in your configuration )

I have a matrix of these pixel values and I want to publish it .

[Myzhar]

You cannot publish a cv::Mat directly. You must copy the data to the data field of sensor_msg::Image. Doing it pixel by pixel is not efficient.

[Mostafa-Mansour]

I understand that it will not be efficient. But if I copy the data to the data field of sensor_msg::Image, I will get a grey scale pixels not depths. Am I right?

[Myzhar]

No, if you set "encoding" and "steps" value correctly you will get floating point depths in meters

[Mostafa-Mansour]

Ok. Could you please tell me which "encodings" and "step" I should use and which file I should edit for that?

[Myzhar]

This function can help you to better understand:
https://github.com/stereolabs/zed-ros-wrapper/blob/master/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp#L629

and this is how to set it to float32 data type:

https://github.com/stereolabs/zed-ros-wrapper/blob/master/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp#L887

[ravikt]

@Mostafa-Mansour, You can convert it to numpy message or range message.

[Myzhar]

@Mostafa-Mansour let us know if you solved your problems and we can close this issue

[Mostafa-Mansour]

Thanks for your help. I will try to solve it later. I will close the issue and if I face any troubles later, I will write you again.