saving and working with depth images in python

[waspinator]

Using a realsense D435i I'm capturing data using ROS into bag files with the standard topics. In particular I have

• /aligned_depth_to_color/image_raw (sensor_msgs/Image)
• /color/image_raw (sensor_msgs/Image)
• /extrinsics/depth_to_color (realsense2_camera/Extrinsics)

My understanding is that color images can be saved in whatever format is convenient (jpeg, png, ...) but that depth should be saved as .raw. I'm not sure how to save, open, or interpret .raw files.

I'm reading the messages using rosbag and saving them to file using opencv.

import cv2
import rosbag
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

bridge = CvBridge()

bag = rosbag.Bag('my_ros_bag_file.bag', "r")
count = 0
for topic, msg, timestamp in bag.read_messages(topics=['/aligned_depth_to_color/image_raw', '/color/image_raw']):
    if topic == '/aligned_depth_to_color/image_raw':
        depth_img = bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")
        cv2.imwrite('depth_{}.raw'.format(count), depth_img)

    if topic == '/color/image_raw':
        color_img = bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
        cv2.imwrite('depth_{}.jpg'.format(count), color_img)

    count += 1

but opencv doesn't seem to understand what I mean by "raw": could not find a writer for the specified extension in function imwrite. Saving as 'png' produces a result that seems to match distance in mm, but from reading other issues (IntelRealSense/librealsense#4427) it seems that doesn't save the correct depth data.

Once I have a method of saving into the "raw" format, how would I read it? For example, I would read a regular png file with presumably wrong depth data using img = cv2.imread('depth.png', cv2.IMREAD_UNCHANGED) which would produce a numpy array for the depth values. How would I read a raw file to get a numpy array of depth values?

Once I have my data saved to file, my end goal is to measure the distance to an object, it's width and its height. I'm able to extract the pixel locations of the bottom, top, left, and right locations, but I would have to convert those to real-world coordinates in order to measure the distances. I probably should try using get_distance and rs2_deproject_pixel_to_point(). Which intrinsics do I need (/aligned_depth_to_color/camera_info?) and how would I convert them from sensor_msgs/camera_info to something pyrealsense2 could work with?

For example, ROS reports the following values in /aligned_depth_to_color/camera_info

Width: 1920
Height: 1080
Distortion model: 'plumb_bob'

Rectification matrix (is this a virtual monocular camera?)
---
     [ 1  0  0]
 R = [ 0  1  0]
     [ 0  0  1]

Intrinsic camera matrix
---
     [fx  0 cx]   [1391     0  970]
 K = [ 0 fy cy] = [   0  1392  538]
     [ 0  0  1]   [   0     0    1]

Projection matrix
---
     [fx'  0  cx' Tx]   [1391     0  970    0]
 P = [ 0  fy' cy' Ty] = [   0  1392  538    0]
     [ 0   0   1   0]   [   0     0    1    0]

where (fx, fy) are focal lengths and (cx, cy) is the principal point

These seem to line up nicely with the attributes of pyrealsense2.intrinsics, but it's missing Distortion coefficients (coeffs)

So I could do something like this:

import cv2
import pyrealsense2 as rs

#  probably wrong method of storing/reading depth
depth = cv2.imread('frame000010_depth.png', cv2.IMREAD_UNCHANGED)

intrinsics = rs.intrinsics()
intrinsics.coeffs = [0, 0, 0, 0, 0] # assuming that 0s is okay
intrinsics.model = rs.distortion.brown_conrady # picked a random one
intrinsics.height = 1080
intrinsics.width = 1920
intrinsics.fx = 1391.1939697265625
intrinsics.fy = 1392.2294921875
intrinsics.ppx = 970.15185546875
intrinsics.ppy = 538.706787109375

x = 1500
y = 1000
z = depth[y, x] # let's say 500
rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], z) # or is it [y, x]? both seem to work even though one should be out of range

[190.42929077148438, 165.66708374023438, 500.0]

[10.727527618408203, 345.2351989746094, 500.0] # if x and y are flipped

Are there examples of how I would use ROS topics or/and saved color and depth files (as opposed to a live video stream) to measure dimensions?

Thanks

[RealSenseSupport]

Your code is correct.

The depth image is single channel 16-bit image.

In the first snippet, the shape of depth_img is (1080, 1920). the dtype of depth_img is uint16. and it's a landscape image.
And the 1500th pixel in the 1000th row in the depth_image is depth_img[1000, 1500]

in the second snippet, the variable z should be depth_img[1000, 1500]
in this function, x is the column number. y is the rows count. the code should be
rs.rs2_deproject_pixel_to_point(intrinsics, [1500, 1000], z)
the origin of the point is the camera.

rs2_deproject_pixel_to_point is written in
https://github.com/IntelRealSense/librealsense/blob/master/include/librealsense2/rsutil.h#L83

[RealSenseSupport]

@waspinator Did you get it through? Any other questions about this?

[waspinator]

yes, saving as png works well enough.

[karlita101]

Is there another way of saving depth images as png with rosbag? ( Not familiar with it at all and looking for something user/beginner friendly)
Thank you