ROS CompressedDepth к Numpy (или CV2)

Question

Folks,

Я использую эту ссылку в качестве отправной точки для преобразования моего CompressedDepth (изображения типа: "32FC1; compressedDepth" в метрах) изображение OpenCV кадров:

Python CompressedImage Subscriber Publisher

я получаю пустые данные, когда я пытаюсь напечатать, или я получаю нетиповой, когда я вижу результат моего массива и т.д.

Что такое правильный способ преобразования compressedDep го изображения? Перепубликация не собирается работать с пропускной способностью Wi-Fi/маршрутизатора и ограничениями скорости.

Answer 1

Правильный способ декодирования compressedDepth - это сначала удалить заголовок из необработанных данных и затем преобразовать оставшиеся данные.

Это задокументировано в image_transport_plugins/compressed_depth_image_transport/src/codec.cpp. На моей машине размер заголовка составляет 12 байтов. Однако это может быть другим в других архитектурах, поскольку размер перечисления не определен.

Следующие экспорта питон фрагмент кода сжатые 16UC1 и 32FC1 глубины изображения, как PNG файла:

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