Add depthz_image_transport: fast depth compression (32FC1 and 16UC1)

image_transport plugin for depth images, vendoring the depth codec from
facontidavide/depth_image_compression.

By default 32FC1 is quantized to a configurable uniform grid
(<base_topic>.depthz.quantization, default 0.1 mm): every decoded pixel
is within +/- half a step of the input, invalid pixels (NaN/inf/<= 0)
are preserved and decode as NaN per REP 118, and the "qpred" payload
(MED prediction on the quantization codes, zigzag + LEB128 varint
residuals, zstd) has no 16-bit level limit, so arbitrarily fine steps
and long ranges are supported. Setting quantization to 0 selects the
bit-exact lossless mode: a value dictionary + 2D prediction ("dpred")
for images with up to 65536 distinct values, and a dictionary-free
float-total-order predictor ("fpred") beyond that. 16UC1 input is
always compressed losslessly ("dpred16").

On real full-precision stereo depth the default mode compresses better
than compressedDepth (PNG) while encoding and decoding several times
faster, with a much finer, explicitly bounded quantization error.

Implementation notes:
- Blobs are fully self-describing: method, dimensions, pixel format and
  quantization step are readable from the header without decompressing.
- Hot paths use runtime-dispatched AVX2 kernels next to scalar paths
  that emit bit-identical blobs (and are independent of the FPU
  rounding mode); decoding reconstructs along a wavefront.
- Steady-state encode/decode performs no heap allocation (thread-local
  scratch, bounded-by-header zstd decompression, per-thread contexts).
- Hostile/corrupt blobs are rejected before any allocation sized from
  untrusted fields; degenerate image sizes are handled; the test suite
  covers round trips, error bounds, and adversarial payloads under
  ASan/UBSan.
- benchmark/ contains an optional black-box perf tool (off by default,
  -DDEPTHZ_BUILD_BENCHMARK=ON) that drives any image_transport plugin
  end to end over frames extracted from MCAP bags.

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>

Author: facontidavide

README.md

@@ -57,6 +57,8 @@ Currently provided are:
 
 - [compressed_image_transport](https://github.com/ros-perception/image_transport_plugins/tree/rolling/compressed_image_transport)
 
+- [depthz_image_transport](https://github.com/ros-perception/image_transport_plugins/tree/rolling/depthz_image_transport) - A library compressing 32FC1 and 16UC1 depth images (2D prediction + zstd). By default 32FC1 is quantized to a configurable 0.1 mm grid; a bit-exact lossless mode is available (`quantization: 0.0`), and 16UC1 is always lossless.
+
 - [zstd_image_transport](https://github.com/ros-perception/image_transport_plugins/tree/rolling/zstd_image_transport) - A libraory using ZSTD to compress the pointclouds.
 
 - [theora_image_transport](https://github.com/ros-perception/image_transport_plugins/tree/master/theora_image_transport) - A library using theora to compress the pointclouds.

depthz_image_transport/CMakeLists.txt

@@ -0,0 +1,100 @@
+cmake_minimum_required(VERSION 3.20)
+
+project(depthz_image_transport)
+
+# The codec is performance-critical: never build it unoptimized by default.
+if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
+  set(CMAKE_BUILD_TYPE RelWithDebInfo)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# zstd: use the CMake config package if the distro ships one, otherwise the
+# plain library. Prefer the shared target: distro static libzstd.a is not
+# built with -fPIC and cannot be linked into this shared plugin library.
+find_package(zstd CONFIG QUIET)
+if(TARGET zstd::libzstd)
+  set(ZSTD_DEPENDENCY zstd::libzstd)
+elseif(TARGET zstd::libzstd_shared)
+  set(ZSTD_DEPENDENCY zstd::libzstd_shared)
+elseif(TARGET zstd::libzstd_static)
+  set(ZSTD_DEPENDENCY zstd::libzstd_static)
+else()
+  find_path(ZSTD_INCLUDE_DIR zstd.h REQUIRED)
+  find_library(ZSTD_LIBRARY NAMES zstd REQUIRED)
+  set(ZSTD_DEPENDENCY ${ZSTD_LIBRARY})
+  include_directories(${ZSTD_INCLUDE_DIR})
+endif()
+
+include_directories(include)
+
+add_library(
+  ${PROJECT_NAME} SHARED
+  src/depth_codec.cpp
+  src/depthz_publisher.cpp
+  src/depthz_subscriber.cpp
+  src/manifest.cpp
+)
+
+# The vendored depth codec uses C++20 (std::countr_zero, std::endian).
+target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_20)
+target_link_libraries(${PROJECT_NAME}
+  ${ZSTD_DEPENDENCY}
+  image_transport::image_transport
+  rclcpp::rclcpp
+  pluginlib::pluginlib
+  ${sensor_msgs_TARGETS}
+)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# This is a pluginlib-only package: DepthzPublisher/DepthzSubscriber are
+# loaded dynamically by class name (see manifest.cpp), never included or
+# linked against directly by downstream packages. include/ is therefore not
+# installed -- consistent with the sibling compressed_image_transport and
+# zstd_image_transport plugin packages, which likewise export nothing. (The
+# headers stay in-source for this package's own build/tests, which include
+# them via the include_directories(include) above.)
+pluginlib_export_plugin_description_file(image_transport depthz_plugins.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(test_depth_codec test/test_depth_codec.cpp)
+  target_compile_features(test_depth_codec PRIVATE cxx_std_20)
+  target_include_directories(test_depth_codec PRIVATE src)
+  target_link_libraries(test_depth_codec ${PROJECT_NAME})
+endif()
+
+# Local perf-profiling tool, not part of the package's public surface or its
+# CI: exercises DepthzPublisher/DepthzSubscriber end to end through the
+# public image_transport API. Off by default; see benchmark/README.md.
+option(DEPTHZ_BUILD_BENCHMARK "Build the depthz_image_transport perf benchmark tool" OFF)
+if(DEPTHZ_BUILD_BENCHMARK)
+  add_executable(benchmark_depthz benchmark/benchmark_depthz.cpp)
+  target_compile_features(benchmark_depthz PRIVATE cxx_std_20)
+  target_link_libraries(benchmark_depthz
+    image_transport::image_transport
+    rclcpp::rclcpp
+    ${sensor_msgs_TARGETS}
+  )
+  install(TARGETS benchmark_depthz
+    RUNTIME DESTINATION lib/${PROJECT_NAME}
+  )
+endif()
+
+ament_package()

depthz_image_transport/README.md

@@ -0,0 +1,85 @@
+# depthz_image_transport
+
+`image_transport` plugin for depth images (`32FC1` and `16UC1`).
+
+**By default the 32FC1 transport is LOSSY**: depth is quantized to a uniform
+0.1 mm grid before compression, so every decoded pixel is within ±0.05 mm of
+the input — far below the noise floor of any real depth camera. Set the
+`quantization` parameter to `0.0` for bit-exact lossless mode (NaN payloads
+included), or to a larger step for more compression. `16UC1` input (already
+integer millimeters) is always compressed losslessly.
+
+The codec is vendored from
+[facontidavide/depth_image_compression](https://github.com/facontidavide/depth_image_compression),
+where the algorithm is documented. Method by input and configuration:
+
+| input | `quantization` | blob method | guarantee |
+|---|---|---|---|
+| 32FC1 | `> 0` (default 0.1 mm) | `qpred` | ± step/2 per valid pixel, invalid (NaN/inf/≤0) → NaN |
+| 32FC1 | `0.0`, ≤ 65536 distinct values | `dpred` | bit-exact |
+| 32FC1 | `0.0`, > 65536 distinct values | `fpred` | bit-exact |
+| 16UC1 | (ignored) | `dpred16` | bit-exact |
+
+Lossless `dpred`/`fpred`/`dpred16` blobs are interchangeable with the
+standalone library in both directions; `qpred` originates in this plugin
+(standalone releases predating it reject it, as this plugin rejects the
+standalone library's other methods). Every blob is self-describing:
+dimensions, pixel format and quantization step are readable from its header
+without decompressing anything.
+
+## Performance
+
+On real full-precision stereo depth (the hardest input: nearly every pixel
+carries a unique float bit pattern), the default quantized mode compresses
+substantially better than `compressedDepth` while encoding and decoding
+several times faster — and with a much finer, explicitly bounded
+quantization error than `compressedDepth`'s 16-bit inverse-depth stage.
+Coarser steps trade precision for ratio; the lossless mode compresses the
+least, since it must reproduce the sensor's mantissa noise bit-exactly.
+Compression is data-dependent, so measure on your own streams: the
+`benchmark/` directory contains a tool that runs any recorded MCAP depth
+topic through the real publisher/subscriber plugins and reports ratio,
+throughput, and error-bound verification (see `benchmark/README.md`).
+
+## Usage
+
+Subscribers select the transport with the standard `image_transport`
+parameter (in rviz2: the *Transport Hint* dropdown of the Image/Camera
+display):
+
+```bash
+ros2 run my_pkg my_depth_consumer --ros-args -p image_transport:=depthz
+```
+
+Publishers advertise all installed transports, so `<base_topic>/depthz`
+appears automatically. To publish only selected transports (saving encoder
+CPU), use the publishing node's `enable_pub_plugins` parameter:
+
+```yaml
+/camera_node:
+  ros__parameters:
+    depth.image_rect.enable_pub_plugins:
+      - image_transport/raw
+      - image_transport/depthz
+```
+
+An existing stream can be converted with
+`ros2 run image_transport republish` (`out_transport:=depthz`), e.g. for
+bag recording: record `<base_topic>/depthz` instead of
+`<base_topic>/compressedDepth`.
+
+## Parameters
+
+| parameter | default | meaning |
+|---|---|---|
+| `<base_topic>.depthz.quantization` | `0.1` | 32FC1 quantization step in **millimeters**; decoded depth is within ± half this step. `0.0` = bit-exact lossless. Ignored for 16UC1. |
+| `<base_topic>.depthz.zstd_level` | `1` | zstd level of the entropy stage (1–3); higher is slower with slightly better ratio. |
+
+The published `CompressedImage.format` string advertises the lossiness
+(e.g. `32FC1; depthz; lossy 0.100mm`), so bag consumers can tell without
+decoding.
+
+Only `32FC1` and `16UC1` encodings are accepted; other encodings are
+declined with an error log (use `compressed` or `zstd` for color images).
+
+See `benchmark/README.md` for the tooling that produced the numbers above.

depthz_image_transport/benchmark/.gitignore

@@ -0,0 +1,4 @@
+data/
+__pycache__/
+colcon_ws/
+results/

depthz_image_transport/benchmark/README.md

@@ -0,0 +1,110 @@
+# depthz_image_transport perf benchmark
+
+Local tooling for profiling `DepthzPublisher`/`DepthzSubscriber` with `perf`,
+using real depth data. Not part of the package's public surface or CI --
+opt in with `-DDEPTHZ_BUILD_BENCHMARK=ON`.
+
+`benchmark_depthz` talks to the plugin exclusively through the public
+`image_transport`/`rclcpp` API (`ImageTransport::advertise`,
+`image_transport::create_subscription(..., "depthz", ...)`), the same way
+any real node would. Profiling this binary profiles exactly what a deployed
+publisher or subscriber node spends its time on -- pluginlib loading
+included -- not just the vendored codec's hot loop in isolation.
+
+## 1. Get real depth frames
+
+`extract_frames.py` pulls raw `sensor_msgs/Image` frames off an MCAP topic
+into a compact `.dzbm` file (see the docstring for the format):
+
+```sh
+python3 extract_frames.py \
+  --mcap ~/ws_eternal/src/Harvesting/harvest_bringup/test/test_data/rosbags/van_noord_tomato_20250919_181407/20250919_181407_0.mcap \
+  --topic /zed_wrist/zed_node/depth/depth_registered \
+  --out data/wrist_1920x1200.dzbm
+```
+
+Needs `pip install mcap mcap-ros2-support`. That bag has two raw 32FC1
+streams from the ZED stereo cameras worth benchmarking:
+
+| topic | resolution | frames |
+|---|---|---|
+| `/zed_base/zed_node/depth/depth_registered` | 960x600 | 10 |
+| `/zed_wrist/zed_node/depth/depth_registered` | 1920x1200 | 10 |
+
+Point `--mcap`/`--topic` at any other bag with a raw (uncompressed) 32FC1 or
+16UC1 `sensor_msgs/Image` topic -- it doesn't have to be ZED/depth-camera
+specific.
+
+## 2. Build
+
+```sh
+colcon build --packages-select depthz_image_transport \
+  --cmake-args -DDEPTHZ_BUILD_BENCHMARK=ON -DCMAKE_BUILD_TYPE=RelWithDebInfo
+source install/setup.bash
+```
+
+`RelWithDebInfo` keeps optimizations on (required -- see the package's own
+`CMakeLists.txt` comment) while still emitting frame pointers/debug info for
+`perf record -g` to unwind.
+
+## 3. Run
+
+```sh
+benchmark_depthz --frames data/wrist_1920x1200.dzbm --mode roundtrip --iterations 300
+```
+
+`--mode`:
+- `roundtrip` (default): a real `DepthzPublisher` encodes each frame, a real
+  `DepthzSubscriber` decodes it. End-to-end throughput and compression
+  ratio.
+- `encode`: only the publisher plugin's encode path runs (the "subscriber"
+  is a bare passthrough callback, kept alive only because
+  `image_transport::Publisher` publishes on demand and needs to see a real
+  subscriber count -- see its class doc).
+- `decode`: all frames are pre-encoded once (untimed) by a real
+  `DepthzPublisher`, then the timed loop republishes those captured blobs
+  directly onto the internal `<topic>/depthz` topic with a plain
+  `rclcpp::Publisher`, bypassing the publisher plugin entirely, while a real
+  `DepthzSubscriber` decodes them. This isolates decode cost from encode
+  cost.
+
+Other flags: `--transport <name>` (benchmark any installed image_transport
+plugin against the same frames, e.g. `compressedDepth`), `--iterations N`
+(loop the dataset N times), `--warmup N` (untimed iterations first, so
+`thread_local` scratch buffers reach their steady-state size before the
+clock starts), `--zstd-level 1-3`, `--quantization MM` (depthz quantization
+step in millimeters; the benchmark defaults to `0` = lossless, deliberately
+overriding the plugin's own lossy 0.1 mm default so the bit-exact verify
+stays meaningful — pass `--quantization 0.1` to measure the plugin's actual
+default behavior), `--no-verify` (skip verification entirely; use before
+profiling so the comparison doesn't show up as noise in the flamegraph),
+`--qos-depth N`.
+
+Output reports wall time, fps, MB/s (raw), compression ratio, and (unless
+`--no-verify`) a verification against the source frames: bit-exact for
+lossless depthz, the documented ± step/2 error bound plus NaN preservation
+for quantized depthz, and informational-only for other transports.
+
+## 4. Profile with perf
+
+```sh
+perf stat -d -- benchmark_depthz --frames data/wrist_1920x1200.dzbm --mode encode --no-verify
+perf record -g --call-graph dwarf -o encode.perf.data -- \
+  benchmark_depthz --frames data/wrist_1920x1200.dzbm --mode encode --no-verify
+perf report -i encode.perf.data
+```
+
+`run_perf.sh` automates all of the above (build, extract both ZED streams,
+run `perf stat` + `perf record` for all 3 modes x 2 resolutions) and drops
+results under `results/`:
+
+```sh
+./run_perf.sh
+```
+
+Because the benchmark drives a `SingleThreadedExecutor` by hand (publish,
+then `spin_some` until the callback fires, repeat) rather than a background
+spin thread, encode and decode samples land on the same call stack you'd
+expect from the source -- `dpred_encode`/`build_value_dict`/`zstd_append`
+for encode, `dpred_decode`/`predict_unpack`/`ZSTD_decompressDCtx` for
+decode -- with no cross-thread noise to untangle.