Display CompressedDepth Image Python cv2
Folks,
What is the correct way to subscribe to a CompressDepth (32FC1, plus, no regular depth image available) image and display (e.g., cv2.imshow) in Python? I can do this easily in roscpp, but I get an empty array rospy, even if I use something like:
depth_image = cv_bridge.CvBridge().compressed_imgmsg_to_cv2(msg)
Or
depth_image = np.array(cv_bridge.CvBridge().compressed_imgmsg_to_cv2(msg))
And even:
np_arr = np.fromstring(msg.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_UNCHANGED)
Nothing works
Asked by Pototo on 2016-12-09 17:06:47 UTC
Answers
The right way to decode compressedDepth is to first remove the header from the raw data and then convert the remaining data.
This is documented in image_transport_plugins/compressed_depth_image_transport/src/codec.cpp. On my machine the header size is 12 bytes. This might however be different on other architectures since the size of an enum is not defined.
The following python code snippet exports compressed 16UC1 and 32FC1 depth images as png file:
# 'msg' as type CompressedImage
depth_fmt, compr_type = msg.format.split(';')
# remove white space
depth_fmt = depth_fmt.strip()
compr_type = compr_type.strip()
if compr_type != "compressedDepth":
raise Exception("Compression type is not 'compressedDepth'."
"You probably subscribed to the wrong topic.")
# remove header from raw data
depth_header_size = 12
raw_data = msg.data[depth_header_size:]
depth_img_raw = cv2.imdecode(np.fromstring(raw_data, np.uint8), cv2.CV_LOAD_IMAGE_UNCHANGED)
if depth_img_raw is None:
# probably wrong header size
raise Exception("Could not decode compressed depth image."
"You may need to change 'depth_header_size'!")
if depth_fmt == "16UC1":
# write raw image data
cv2.imwrite(os.path.join(path_depth, "depth_" + str(msg.header.stamp) + ".png"), depth_img_raw)
elif depth_fmt == "32FC1":
raw_header = msg.data[:depth_header_size]
# header: int, float, float
[compfmt, depthQuantA, depthQuantB] = struct.unpack('iff', raw_header)
depth_img_scaled = depthQuantA / (depth_img_raw.astype(np.float32)-depthQuantB)
# filter max values
depth_img_scaled[depth_img_raw==0] = 0
# depth_img_scaled provides distance in meters as f32
# for storing it as png, we need to convert it to 16UC1 again (depth in mm)
depth_img_mm = (depth_img_scaled*1000).astype(np.uint16)
cv2.imwrite(os.path.join(path_depth, "depth_" + str(msg.header.stamp) + ".png"), depth_img_mm)
else:
raise Exception("Decoding of '" + depth_fmt + "' is not implemented!")
Asked by Christian Rauch on 2017-07-16 08:54:25 UTC
Comments
Encoding "32FC1; compressedDepth png"
in python: https://answers.ros.org/question/407659/how-to-generate-format-32fc1-compresseddepth-png-image-in-python-from-float-array/
Asked by lucasw on 2023-05-30 07:58:04 UTC
Comments