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!")
