ROS CompressedDepth to numpy (or cv2)

Question

Folks,

I am using this link as starting point to convert my CompressedDepth (image of type: "32FC1; compressedDepth," in meters) image to OpenCV frames:

Python CompressedImage Subscriber Publisher

I get an empty data when I try to print, or I get a NonType when I see the result of my array, etc.

What is the right way to convert a compressedDepth image? Republishing is not gonna work do to wifi/router bandwidth and speed constraints.

Line 43 changed to "/camera/depth_registered/image_raw/compressedDepth;" Line 57 changed to "image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_GRAYSCALE);" Other than data, eliminated the "feature" and "republishing part." Everything else is the same thing. — Pototo, Dec 10 '16 at 02:35

score 0 · Answer 1 · answered Jul 16 '17 at 13:31

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

ROS CompressedDepth to numpy (or cv2)

1 Answers1