depthz: quantized-by-default qpred codec, fpred fallback, SIMD, benchmark

Real full-precision stereo depth (e.g. ZED 32FC1) has more distinct float
patterns than the 16-bit dictionary admits, so dpred always fell back to
plain zstd on raw floats. Two new vendored methods fix that:

- fpred (lossless): dictionary-free MED prediction on float total-order
  integers, used automatically when the dictionary overflows. Byte-
  compatible with the standalone depth_codec library.
- qpred (lossy, NEW DEFAULT at 0.1 mm): uniform quantization with a
  configurable step, MED prediction on the code plane, zigzag + LEB128
  varint residuals (level count not limited to 16 bits), zstd. Valid
  pixels reconstruct within +/- step/2; invalid pixels (NaN/inf/<=0) and
  off-grid depths decode as NaN per REP 118. The step is stored
  uncompressed in the payload and exposed via BlobHeader, so blobs stay
  fully self-describing from the header alone.

New publisher parameter <base_topic>.depthz.quantization (mm, default 0.1,
0 = bit-exact lossless; ignored for 16UC1 which stays lossless). The
CompressedImage format string advertises the lossiness.

The qpred hot paths are SIMD-optimized with runtime AVX2 dispatch
(quantize, residuals, code->float; scalar and AVX2 encode emit
bit-identical blobs, and quantization is FPU-rounding-mode independent),
plus a varint fast path and the same wavefront reconstruction the 16UC1
decoder uses.

Codex review fixes: off-grid depths (v/step beyond ~2^31) are treated as
invalid instead of silently saturating with unbounded error; the AVX2
code->float conversion multiplies in double so a blob decodes identically
with or without AVX2.

Adds benchmark/: a black-box perf tool driving any image_transport plugin
(depthz, compressedDepth, ...) end to end over real frames extracted from
MCAP bags, with error-bound verification and perf-record integration.
Off by default (-DDEPTHZ_BUILD_BENCHMARK=ON).

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

Author: facontidavide

README.md

@@ -57,7 +57,7 @@ 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 losslessly (value dictionary + 2D prediction + zstd).
+- [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.
 

depthz_image_transport/CMakeLists.txt

@@ -80,4 +80,21 @@ if(BUILD_TESTING)
   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

@@ -1,16 +1,45 @@
 # depthz_image_transport
 
-Lossless `image_transport` plugin for depth images (`32FC1` and `16UC1`).
+`image_transport` plugin for depth images (`32FC1` and `16UC1`).
 
-Compared to `compressedDepth` (PNG), `depthz` compresses better, encodes
-roughly 10× faster, and is truly lossless: `compressedDepth` quantizes
-32FC1 to 16 bits before the PNG stage, `depthz` reproduces the input
-bit-exactly (NaN included). The codec is vendored from
+**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. Blobs produced by this plugin decode
-cleanly with that standalone library, but this plugin only vendors its
-"dpred"/"dpred16" methods (a subset of the standalone library's full method
-list), so blobs encoded elsewhere with a different method are rejected.
+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
 
@@ -39,7 +68,18 @@ An existing stream can be converted with
 bag recording: record `<base_topic>/depthz` instead of
 `<base_topic>/compressedDepth`.
 
-Parameters: `<base_topic>.depthz.zstd_level` (1–3, default 1).
+## 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/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.