From 681f6511085cb80b60dab97ce00ccb2e67d33cb4 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 6 Jul 2026 15:08:36 +0200 Subject: [PATCH 1/2] 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 (.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 --- README.md | 2 + depthz_image_transport/CMakeLists.txt | 100 ++ depthz_image_transport/README.md | 85 ++ depthz_image_transport/benchmark/.gitignore | 4 + depthz_image_transport/benchmark/README.md | 110 ++ .../benchmark/benchmark_depthz.cpp | 535 +++++++ .../benchmark/extract_frames.py | 128 ++ depthz_image_transport/benchmark/run_perf.sh | 97 ++ depthz_image_transport/depthz_plugins.xml | 13 + .../depthz_publisher.hpp | 79 + .../depthz_subscriber.hpp | 67 + depthz_image_transport/package.xml | 32 + depthz_image_transport/src/depth_codec.cpp | 1285 +++++++++++++++++ depthz_image_transport/src/depth_codec.hpp | 136 ++ .../src/depthz_publisher.cpp | 189 +++ .../src/depthz_subscriber.cpp | 85 ++ depthz_image_transport/src/manifest.cpp | 41 + .../test/test_depth_codec.cpp | 558 +++++++ image_transport_plugins/package.xml | 1 + 19 files changed, 3547 insertions(+) create mode 100644 depthz_image_transport/CMakeLists.txt create mode 100644 depthz_image_transport/README.md create mode 100644 depthz_image_transport/benchmark/.gitignore create mode 100644 depthz_image_transport/benchmark/README.md create mode 100644 depthz_image_transport/benchmark/benchmark_depthz.cpp create mode 100755 depthz_image_transport/benchmark/extract_frames.py create mode 100755 depthz_image_transport/benchmark/run_perf.sh create mode 100644 depthz_image_transport/depthz_plugins.xml create mode 100644 depthz_image_transport/include/depthz_image_transport/depthz_publisher.hpp create mode 100644 depthz_image_transport/include/depthz_image_transport/depthz_subscriber.hpp create mode 100644 depthz_image_transport/package.xml create mode 100644 depthz_image_transport/src/depth_codec.cpp create mode 100644 depthz_image_transport/src/depth_codec.hpp create mode 100644 depthz_image_transport/src/depthz_publisher.cpp create mode 100644 depthz_image_transport/src/depthz_subscriber.cpp create mode 100644 depthz_image_transport/src/manifest.cpp create mode 100644 depthz_image_transport/test/test_depth_codec.cpp diff --git a/README.md b/README.md index 8081fc3..74d0afb 100644 --- a/README.md +++ b/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. diff --git a/depthz_image_transport/CMakeLists.txt b/depthz_image_transport/CMakeLists.txt new file mode 100644 index 0000000..3343c3a --- /dev/null +++ b/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() diff --git a/depthz_image_transport/README.md b/depthz_image_transport/README.md new file mode 100644 index 0000000..4e83697 --- /dev/null +++ b/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 `/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 `/depthz` instead of +`/compressedDepth`. + +## Parameters + +| parameter | default | meaning | +|---|---|---| +| `.depthz.quantization` | `0.1` | 32FC1 quantization step in **millimeters**; decoded depth is within ± half this step. `0.0` = bit-exact lossless. Ignored for 16UC1. | +| `.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. diff --git a/depthz_image_transport/benchmark/.gitignore b/depthz_image_transport/benchmark/.gitignore new file mode 100644 index 0000000..19f77f0 --- /dev/null +++ b/depthz_image_transport/benchmark/.gitignore @@ -0,0 +1,4 @@ +data/ +__pycache__/ +colcon_ws/ +results/ diff --git a/depthz_image_transport/benchmark/README.md b/depthz_image_transport/benchmark/README.md new file mode 100644 index 0000000..ad81806 --- /dev/null +++ b/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 `/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 ` (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. diff --git a/depthz_image_transport/benchmark/benchmark_depthz.cpp b/depthz_image_transport/benchmark/benchmark_depthz.cpp new file mode 100644 index 0000000..f9043f4 --- /dev/null +++ b/depthz_image_transport/benchmark/benchmark_depthz.cpp @@ -0,0 +1,535 @@ +// Copyright (c) 2026, Davide Faconti +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Black-box perf benchmark for depthz_image_transport, and a like-for-like +// baseline tool for any other image_transport plugin (--transport). This +// talks to the publisher/subscriber plugins exclusively through the public +// image_transport/rclcpp API -- the same way any real node would -- so +// "perf record" on this binary profiles exactly what a deployed publisher +// or subscriber node would spend its time on, pluginlib loading included. +// +// Three modes, selected with --mode: +// roundtrip (default): a real publisher plugin encodes each frame and a +// real subscriber plugin decodes it, end to end. +// encode: only the publisher plugin's encode path runs. The "subscriber" +// is a bare passthrough callback (so image_transport still sees a +// subscriber and does not skip the on-demand encode), but no decoding +// happens. +// decode: frames are pre-encoded once (untimed) by a real publisher +// plugin, then the timed loop republishes those captured blobs directly +// onto the transport's internal sub-topic with a plain +// rclcpp::Publisher, bypassing the publisher plugin entirely, while a +// real subscriber plugin decodes them. +// +// Usage: +// benchmark_depthz --frames data/wrist_1920x1200.dzbm --mode roundtrip +// --iterations 200 --zstd-level 1 +// benchmark_depthz --frames data/wrist_1920x1200.dzbm --transport compressedDepth +// --mode roundtrip --iterations 200 +// +// See run_perf.sh for how to wrap this in `perf record`/`perf stat`. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace +{ + +using sensor_msgs::msg::CompressedImage; +using sensor_msgs::msg::Image; + +struct Options +{ + std::string frames_path; + std::string mode = "roundtrip"; + std::string transport = "depthz"; + int iterations = 200; + int warmup = 5; + int zstd_level = 1; + // 0 = lossless (the plugin's own default is lossy 0.1 mm; see print_usage). + double quantization_mm = 0.0; + int qos_depth = 10; + bool verify = true; + std::chrono::milliseconds drain_timeout{5000}; +}; + +void print_usage(const char * argv0) +{ + std::cerr << + "Usage: " << argv0 << " --frames [--mode roundtrip|encode|decode]\n" + " [--transport depthz|compressedDepth|...] [--iterations N] [--warmup N]\n" + " [--zstd-level 1-3] [--quantization MM] [--qos-depth N] [--no-verify]\n" + "\n" + "--transport selects any installed image_transport publisher/subscriber plugin\n" + "by its registered transport name, not just depthz -- e.g. --transport\n" + "compressedDepth benchmarks the PNG-based transport against the same frames,\n" + "for a like-for-like comparison. --zstd-level and --quantization only apply to\n" + "depthz; --quantization is the step in millimeters (default 0 = lossless here,\n" + "overriding the plugin's own lossy 0.1 mm default so the bit-exact verify\n" + "stays meaningful). With a nonzero step, verify checks the +/- step/2 error\n" + "bound and NaN preservation instead of bit-exactness.\n"; +} + +Options parse_args(int argc, char ** argv) +{ + Options opt; + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + auto next = [&]() -> std::string { + if (i + 1 >= argc) { + print_usage(argv[0]); + std::exit(2); + } + return argv[++i]; + }; + if (arg == "--frames") { + opt.frames_path = next(); + } else if (arg == "--mode") { + opt.mode = next(); + } else if (arg == "--transport") { + opt.transport = next(); + } else if (arg == "--iterations") { + opt.iterations = std::stoi(next()); + } else if (arg == "--warmup") { + opt.warmup = std::stoi(next()); + } else if (arg == "--zstd-level") { + opt.zstd_level = std::stoi(next()); + } else if (arg == "--quantization") { + opt.quantization_mm = std::stod(next()); + } else if (arg == "--qos-depth") { + opt.qos_depth = std::stoi(next()); + } else if (arg == "--no-verify") { + opt.verify = false; + } else if (arg == "--help" || arg == "-h") { + print_usage(argv[0]); + std::exit(0); + } else { + std::cerr << "unknown argument: " << arg << "\n"; + print_usage(argv[0]); + std::exit(2); + } + } + if (opt.frames_path.empty()) { + std::cerr << "error: --frames is required\n"; + print_usage(argv[0]); + std::exit(2); + } + if (opt.mode != "roundtrip" && opt.mode != "encode" && opt.mode != "decode") { + std::cerr << "error: --mode must be roundtrip, encode, or decode\n"; + std::exit(2); + } + return opt; +} + +// ---- .dzbm loader (see extract_frames.py for the format) ------------------- + +struct RawFrame +{ + std::string encoding; + uint32_t width = 0; + uint32_t height = 0; + std::vector data; +}; + +template +T read_pod(std::ifstream & f) +{ + T v{}; + f.read(reinterpret_cast(&v), sizeof(T)); + if (!f) { + throw std::runtime_error("benchmark_depthz: truncated .dzbm file"); + } + return v; +} + +std::vector load_dzbm(const std::string & path) +{ + std::ifstream f(path, std::ios::binary); + if (!f) { + throw std::runtime_error("benchmark_depthz: cannot open " + path); + } + char magic[4]; + f.read(magic, 4); + if (!f || std::memcmp(magic, "DZBM", 4) != 0) { + throw std::runtime_error("benchmark_depthz: " + path + " is not a .dzbm file"); + } + const uint32_t version = read_pod(f); + if (version != 1) { + throw std::runtime_error("benchmark_depthz: unsupported .dzbm version " + + std::to_string(version)); + } + const uint32_t count = read_pod(f); + std::vector frames; + frames.reserve(count); + for (uint32_t i = 0; i < count; ++i) { + RawFrame frame; + const uint8_t enc_len = read_pod(f); + frame.encoding.resize(enc_len); + f.read(frame.encoding.data(), enc_len); + frame.width = read_pod(f); + frame.height = read_pod(f); + const uint32_t data_len = read_pod(f); + frame.data.resize(data_len); + f.read(reinterpret_cast(frame.data.data()), data_len); + if (!f) { + throw std::runtime_error("benchmark_depthz: truncated frame " + std::to_string(i)); + } + frames.push_back(std::move(frame)); + } + return frames; +} + +size_t bpp_of(const std::string & encoding) +{ + return encoding == "32FC1" ? 4 : 2; +} + +Image::SharedPtr make_image(const RawFrame & frame, uint32_t frame_idx) +{ + auto msg = std::make_shared(); + // frame_idx rides in header.stamp.nanosec so the decode callback can find + // the original frame to verify against (plugins copy the header verbatim). + msg->header.stamp.nanosec = frame_idx; + msg->header.frame_id = "bench"; + msg->encoding = frame.encoding; + msg->width = frame.width; + msg->height = frame.height; + msg->is_bigendian = 0; + msg->step = frame.width * static_cast(bpp_of(frame.encoding)); + msg->data = frame.data; + return msg; +} + +// ---- benchmark bookkeeping -------------------------------------------------- + +struct Stats +{ + std::atomic received{0}; + std::atomic compressed_bytes{0}; + std::atomic mismatches{0}; +}; + +bool spin_until( + rclcpp::executors::SingleThreadedExecutor & executor, + const std::atomic & counter, uint64_t target, + std::chrono::milliseconds timeout) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (counter.load() < target) { + executor.spin_some(std::chrono::milliseconds(10)); + if (std::chrono::steady_clock::now() > deadline) { + return false; + } + } + return true; +} + +int run(const Options & opt, const std::vector & frames, size_t raw_bytes_per_pass) +{ + auto node_options = rclcpp::NodeOptions().use_intra_process_comms(true); + // No slashes in the topic name: keeps the plugin's + // ".depthz.zstd_level" parameter name simple to predict + // (see depthz_publisher.cpp's advertiseImpl, which otherwise has to + // strip the node namespace out of base_topic). + const std::string base_topic = "depth_bench"; + auto node = std::make_shared("depthz_benchmark", node_options); + + rclcpp::QoS qos(opt.qos_depth); + qos.reliable(); + + image_transport::ImageTransport it(*node); + Stats stats; + + // Publisher side: always create it, even in "decode" mode, so the + // pre-encode capture pass below can use it. + image_transport::Publisher pub = it.advertise(base_topic, qos, false); + + if (opt.transport == "depthz") { + const std::string level_param = base_topic + ".depthz.zstd_level"; + const std::string quantization_param = base_topic + ".depthz.quantization"; + if (node->has_parameter(level_param) && node->has_parameter(quantization_param)) { + node->set_parameter(rclcpp::Parameter(level_param, opt.zstd_level)); + node->set_parameter(rclcpp::Parameter(quantization_param, opt.quantization_mm)); + } else { + std::cerr << "warning: depthz parameters were not declared -- is " + "depthz_image_transport built/sourced? Falling back to its defaults " + "(NOTE: the plugin's default is LOSSY 0.1 mm quantization).\n"; + } + } + + const std::string transport_topic = base_topic + "/" + opt.transport; + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + + // Raw subscription on the internal / topic: + // exists in every mode so image_transport sees a subscriber and + // actually runs the publisher plugin's encode path (transports are + // on-demand, see image_transport::Publisher's class doc) and so we can + // measure compressed size regardless of mode. + auto passthrough_cb = [&stats](const CompressedImage::ConstSharedPtr & msg) { + stats.compressed_bytes += msg->data.size(); + }; + auto raw_sub = node->create_subscription( + transport_topic, qos, passthrough_cb); + + image_transport::Subscriber sub; + rclcpp::Publisher::SharedPtr raw_pub; + std::vector captured_blobs; + + // Lossy depthz runs verify against the documented contract instead of + // bit-exactness: valid pixels within +/- step/2 (plus a few float ULPs), + // invalid pixels (NaN/inf/<= 0) decoded as NaN. + const bool lossy_depthz = opt.transport == "depthz" && opt.quantization_mm > 0.0; + const float qtol = static_cast(opt.quantization_mm * 1e-3 * 0.5); + auto decode_cb = + [&stats, &frames, &opt, lossy_depthz, qtol](const Image::ConstSharedPtr & msg) { + stats.received++; + if (!opt.verify) { + return; + } + const uint32_t frame_idx = msg->header.stamp.nanosec; + if (frame_idx >= frames.size()) { + stats.mismatches++; + return; + } + const RawFrame & original = frames[frame_idx]; + if (msg->data.size() != original.data.size()) { + stats.mismatches++; + return; + } + if (lossy_depthz && original.encoding == "32FC1") { + const float * in = reinterpret_cast(original.data.data()); + const float * out = reinterpret_cast(msg->data.data()); + const size_t n = original.data.size() / 4; + for (size_t i = 0; i < n; ++i) { + const bool valid_in = std::isfinite(in[i]) && in[i] > 0.0f; + const float tol = qtol + 4.0f * std::numeric_limits::epsilon() * in[i]; + if (valid_in ? !(std::fabs(out[i] - in[i]) <= tol) : !std::isnan(out[i])) { + stats.mismatches++; + return; + } + } + return; + } + if (std::memcmp(msg->data.data(), original.data.data(), original.data.size()) != 0) { + stats.mismatches++; + } + }; + + if (opt.mode == "roundtrip" || opt.mode == "decode") { + sub = image_transport::create_subscription( + *node, base_topic, decode_cb, opt.transport, qos); + } + if (opt.mode == "decode") { + raw_pub = node->create_publisher(transport_topic, qos); + } + + // Wait for the DDS/intra-process graph to actually match publishers + // with subscribers before timing anything. + const auto discovery_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (pub.getNumSubscribers() == 0 || + (sub && sub.getNumPublishers() == 0)) + { + executor.spin_some(std::chrono::milliseconds(10)); + if (std::chrono::steady_clock::now() > discovery_deadline) { + std::cerr << "error: publisher/subscriber never matched (pluginlib load failure? " + "did you source install/setup.bash after building with depthz_image_transport?)\n"; + return 1; + } + } + + if (opt.mode == "decode") { + // Untimed pre-pass: encode every frame once for real, capture the + // resulting blobs so the timed loop can republish them without + // paying encode cost. + captured_blobs.reserve(frames.size()); + std::atomic captured{0}; + auto capture_cb = [&captured_blobs, &captured](const CompressedImage::ConstSharedPtr & msg) { + captured_blobs.push_back(std::make_shared(*msg)); + captured++; + }; + auto capture_sub = node->create_subscription( + transport_topic, qos, capture_cb); + executor.spin_some(std::chrono::milliseconds(10)); // let it match too + for (uint32_t i = 0; i < frames.size(); ++i) { + pub.publish(*make_image(frames[i], i)); + if (!spin_until(executor, captured, i + 1, opt.drain_timeout)) { + std::cerr << "error: pre-encode capture timed out waiting for frame " << i << "\n"; + return 1; + } + } + if (captured_blobs.size() != frames.size()) { + std::cerr << "error: pre-encode capture got " << captured_blobs.size() << + "/" << frames.size() << " blobs\n"; + return 1; + } + stats.compressed_bytes = 0; // don't double-count the pre-pass + std::cout << "pre-encoded " << captured_blobs.size() << " frames for decode-only mode\n"; + } + + // Messages are built once and restamped per publish: a fresh Image + // would copy the multi-MB frame inside the timed loop and distort the + // measurement (publish takes the message by const ref). + std::vector images; + if (opt.mode != "decode") { + images.reserve(frames.size()); + for (uint32_t i = 0; i < frames.size(); ++i) { + images.push_back(make_image(frames[i], i)); + } + } + auto publish_one = [&](uint32_t iteration, uint32_t frame_idx) { + if (opt.mode == "decode") { + CompressedImage & blob = *captured_blobs[frame_idx]; + blob.header.stamp.sec = static_cast(iteration); + raw_pub->publish(blob); + } else { + Image & msg = *images[frame_idx]; + msg.header.stamp.sec = static_cast(iteration); + pub.publish(msg); + } + }; + + // Warmup: lets thread_local scratch buffers in the codec grow to their + // steady-state size and warms caches/branch predictors before we start + // the clock (see depth_codec.cpp's tl_encode_scratch()/tl_decode_scratch()). + for (int w = 0; w < opt.warmup; ++w) { + for (uint32_t f = 0; f < frames.size(); ++f) { + publish_one(1000000 + w, f); + executor.spin_some(std::chrono::milliseconds(20)); + } + } + stats.received = 0; + stats.compressed_bytes = 0; + stats.mismatches = 0; + + const uint64_t total_frames = static_cast(opt.iterations) * frames.size(); + const auto t0 = std::chrono::steady_clock::now(); + for (int it_idx = 0; it_idx < opt.iterations; ++it_idx) { + for (uint32_t f = 0; f < frames.size(); ++f) { + publish_one(static_cast(it_idx), f); + executor.spin_some(std::chrono::milliseconds(20)); + } + } + bool drained = true; + if (opt.mode == "encode") { + // Best effort: nothing decodes in this mode, so there is no reliable + // completion counter to wait on. + spin_until(executor, stats.compressed_bytes, 1, opt.drain_timeout); + } else { + drained = spin_until(executor, stats.received, total_frames, opt.drain_timeout); + } + const auto t1 = std::chrono::steady_clock::now(); + const double elapsed_s = std::chrono::duration(t1 - t0).count(); + + if (!drained) { + std::cerr << "warning: only " << stats.received.load() << "/" << total_frames << + " frames were decoded before the drain timeout -- results below are unreliable\n"; + } + + const double raw_mb = static_cast(raw_bytes_per_pass) * opt.iterations / 1e6; + const double compressed_mb = static_cast(stats.compressed_bytes.load()) / 1e6; + + std::cout << "\n--- " << opt.transport << " " << opt.mode << " benchmark ---\n" + << "frames published : " << total_frames << " (" << opt.iterations << + " x " << frames.size() << ")\n" + << "wall time : " << elapsed_s << " s\n" + << "throughput : " << (total_frames / elapsed_s) << " fps, " << + (raw_mb / elapsed_s) << " MB/s (raw)\n"; + if (stats.compressed_bytes.load() > 0) { + std::cout << "raw size : " << raw_mb << " MB\n" + << "compressed size : " << compressed_mb << " MB\n" + << "compression ratio: " << (raw_mb / compressed_mb) << "x\n"; + } + if (opt.verify && (opt.mode == "roundtrip" || opt.mode == "decode")) { + // For depthz a mismatch is a real bug: against bit-exactness when + // lossless, against the documented +/- step/2 error bound and NaN + // preservation when quantizing. Other transports (e.g. + // compressedDepth, which quantizes 32FC1 to 16 bits before PNG with + // no comparable contract) may legitimately be lossy, so mismatches + // are only reported, not treated as a failure. + const bool contract_expected = opt.transport == "depthz"; + const char * ok_label = lossy_depthz ? " (within error bound)" : " (bit-exact)"; + std::cout << "decoded frames : " << stats.received.load() << "\n" + << "verify mismatches: " << stats.mismatches.load() << + (stats.mismatches.load() == 0 ? ok_label : + contract_expected ? " <-- CORRUPTION" : " (expected: lossy transport)") << "\n"; + if (contract_expected && stats.mismatches.load() > 0) { + return 1; + } + } + return 0; +} + +} // namespace + +int main(int argc, char ** argv) +{ + const Options opt = parse_args(argc, argv); + + std::vector frames; + try { + frames = load_dzbm(opt.frames_path); + } catch (const std::exception & e) { + std::cerr << e.what() << "\n"; + return 1; + } + if (frames.empty()) { + std::cerr << "error: " << opt.frames_path << " contains no frames\n"; + return 1; + } + size_t raw_bytes_per_pass = 0; + for (const auto & f : frames) { + raw_bytes_per_pass += static_cast(f.width) * f.height * bpp_of(f.encoding); + } + std::cout << "loaded " << frames.size() << " frames (" << frames.front().encoding + << ", " << frames.front().width << "x" << frames.front().height << ") from " + << opt.frames_path << " -- " << raw_bytes_per_pass / 1e6 << " MB/pass\n"; + + rclcpp::init(0, nullptr); + const int result = run(opt, frames, raw_bytes_per_pass); + rclcpp::shutdown(); + return result; +} diff --git a/depthz_image_transport/benchmark/extract_frames.py b/depthz_image_transport/benchmark/extract_frames.py new file mode 100755 index 0000000..77b6420 --- /dev/null +++ b/depthz_image_transport/benchmark/extract_frames.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2026, Davide Faconti +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +""" +Extract raw sensor_msgs/Image frames from an MCAP topic into a .dzbm file. + +.dzbm is a minimal container used by benchmark_depthz.cpp so the benchmark +does not need to link an MCAP reader: it just mmaps/reads flat records. +Row padding (step != width * bpp) is stripped here, so every frame in the +file is stored contiguously. + +Format (little-endian): + magic : 4 bytes b'DZBM' + version : uint32 1 + count : uint32 + frames[count]: + encoding_len : uint8 + encoding : encoding_len bytes (ascii, e.g. '32FC1') + width : uint32 + height : uint32 + data_len : uint32 + data : data_len bytes, row-major, contiguous +""" +import argparse +import struct +import sys + +MAGIC = b'DZBM' +VERSION = 1 + +# bytes-per-pixel for the encodings depthz_image_transport supports. +BPP = {'32FC1': 4, '16UC1': 2} + + +def repack_contiguous(msg): + bpp = BPP.get(msg.encoding) + if bpp is None: + raise ValueError( + f"unsupported encoding '{msg.encoding}' (depthz only handles 32FC1/16UC1)") + row_bytes = msg.width * bpp + if msg.step == row_bytes: + return bytes(msg.data[:row_bytes * msg.height]) + # Rows are padded (step > width * bpp): copy row-by-row. + out = bytearray(row_bytes * msg.height) + src = bytes(msg.data) + for y in range(msg.height): + out[y * row_bytes:(y + 1) * row_bytes] = src[y * msg.step:y * msg.step + row_bytes] + return bytes(out) + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--mcap', required=True, help='input .mcap file') + parser.add_argument('--topic', required=True, help='sensor_msgs/Image topic to extract') + parser.add_argument('--out', required=True, help='output .dzbm file') + parser.add_argument( + '--max-frames', type=int, default=0, + help='stop after this many frames (0 = all)') + args = parser.parse_args() + + try: + from mcap.reader import make_reader + from mcap_ros2.decoder import DecoderFactory + except ImportError: + sys.exit( + "error: this script needs the 'mcap' and 'mcap_ros2' Python packages " + '(pip install mcap mcap-ros2-support)') + + frames = [] + with open(args.mcap, 'rb') as f: + reader = make_reader(f, decoder_factories=[DecoderFactory()]) + for _schema, _channel, _message, ros_msg in reader.iter_decoded_messages( + topics=[args.topic]): + frames.append(ros_msg) + if args.max_frames and len(frames) >= args.max_frames: + break + + if not frames: + sys.exit(f"error: no messages found on topic '{args.topic}' in {args.mcap}") + + with open(args.out, 'wb') as out: + out.write(MAGIC) + out.write(struct.pack(' {args.out} ' + f'({total_bytes / 1e6:.1f} MB raw)') + + +if __name__ == '__main__': + main() diff --git a/depthz_image_transport/benchmark/run_perf.sh b/depthz_image_transport/benchmark/run_perf.sh new file mode 100755 index 0000000..0ca84b9 --- /dev/null +++ b/depthz_image_transport/benchmark/run_perf.sh @@ -0,0 +1,97 @@ +#!/usr/bin/env bash +# Build depthz_image_transport with the benchmark tool enabled, extract real +# depth frames from an MCAP bag, and profile encode/decode/roundtrip with +# `perf`. See README.md for the full explanation of what this measures. +# +# Usage: +# ./run_perf.sh [outdir] +# +# Env overrides: +# MCAP_FILE path to the source .mcap (default: the ZED tomato bag used +# during development, see README.md) +# ROS_DISTRO_SETUP path to a ROS 2 setup.bash to source (default: guesses +# /opt/ros/*/setup.bash) +# ITERATIONS loop count per benchmark run (default: 300) +# QUANTIZATION depthz quantization step in mm (default: 0.1, the plugin's +# own default; set 0 to profile the lossless path) +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PKG_DIR="$(dirname "$SCRIPT_DIR")" +WS_DIR="$(cd "$PKG_DIR/../.." && pwd)" +OUT_DIR="${1:-$SCRIPT_DIR/results}" +mkdir -p "$OUT_DIR" "$SCRIPT_DIR/data" + +MCAP_FILE="${MCAP_FILE:-$HOME/ws_eternal/src/Harvesting/harvest_bringup/test/test_data/rosbags/van_noord_tomato_20250919_181407/20250919_181407_0.mcap}" +ITERATIONS="${ITERATIONS:-300}" +QUANTIZATION="${QUANTIZATION:-0.1}" + +if [ -z "${ROS_DISTRO_SETUP:-}" ]; then + ROS_DISTRO_SETUP="$(ls /opt/ros/*/setup.bash 2>/dev/null | head -1 || true)" +fi +if [ -z "$ROS_DISTRO_SETUP" ] || [ ! -f "$ROS_DISTRO_SETUP" ]; then + echo "error: could not find a ROS 2 setup.bash (set ROS_DISTRO_SETUP)" >&2 + exit 1 +fi +# shellcheck disable=SC1090 +source "$ROS_DISTRO_SETUP" + +echo "== building depthz_image_transport (benchmark enabled) ==" +COLCON_WS="$SCRIPT_DIR/colcon_ws" +mkdir -p "$COLCON_WS" +( + cd "$COLCON_WS" + colcon build --packages-select depthz_image_transport \ + --base-paths "$WS_DIR" \ + --cmake-args -DDEPTHZ_BUILD_BENCHMARK=ON -DCMAKE_BUILD_TYPE=RelWithDebInfo +) +# shellcheck disable=SC1091 +source "$COLCON_WS/install/setup.bash" + +BENCH_BIN="$COLCON_WS/install/depthz_image_transport/lib/depthz_image_transport/benchmark_depthz" +if [ ! -x "$BENCH_BIN" ]; then + echo "error: benchmark_depthz was not built at $BENCH_BIN" >&2 + exit 1 +fi + +if [ ! -f "$MCAP_FILE" ]; then + echo "error: MCAP_FILE not found: $MCAP_FILE" >&2 + echo " point MCAP_FILE at any bag with a raw 32FC1 or 16UC1 sensor_msgs/Image topic" >&2 + exit 1 +fi + +echo "== extracting real depth frames from $MCAP_FILE ==" +SMALL="$SCRIPT_DIR/data/base_960x600.dzbm" +LARGE="$SCRIPT_DIR/data/wrist_1920x1200.dzbm" +[ -f "$SMALL" ] || python3 "$SCRIPT_DIR/extract_frames.py" \ + --mcap "$MCAP_FILE" --topic /zed_base/zed_node/depth/depth_registered --out "$SMALL" +[ -f "$LARGE" ] || python3 "$SCRIPT_DIR/extract_frames.py" \ + --mcap "$MCAP_FILE" --topic /zed_wrist/zed_node/depth/depth_registered --out "$LARGE" + +run_one() { + local dataset="$1" mode="$2" tag="$3" + echo "-- perf stat: $tag / $mode --" + perf stat -d -o "$OUT_DIR/${tag}_${mode}.stat.txt" -- \ + "$BENCH_BIN" --frames "$dataset" --mode "$mode" --iterations "$ITERATIONS" \ + --quantization "$QUANTIZATION" --no-verify + echo "-- perf record: $tag / $mode --" + perf record -g --call-graph dwarf -o "$OUT_DIR/${tag}_${mode}.perf.data" -- \ + "$BENCH_BIN" --frames "$dataset" --mode "$mode" --iterations "$ITERATIONS" \ + --quantization "$QUANTIZATION" --no-verify +} + +for tag_dataset in "small:$SMALL" "large:$LARGE"; do + tag="${tag_dataset%%:*}" + dataset="${tag_dataset#*:}" + for mode in encode decode roundtrip; do + run_one "$dataset" "$mode" "$tag" + done +done + +echo +echo "== done ==" +echo "perf stat summaries : $OUT_DIR/*.stat.txt" +echo "perf record captures: $OUT_DIR/*.perf.data" +echo "inspect a capture with, e.g.:" +echo " perf report -i $OUT_DIR/large_encode.perf.data" +echo " perf script -i $OUT_DIR/large_encode.perf.data | stackcollapse-perf.pl | flamegraph.pl > large_encode.svg" diff --git a/depthz_image_transport/depthz_plugins.xml b/depthz_image_transport/depthz_plugins.xml new file mode 100644 index 0000000..912adbb --- /dev/null +++ b/depthz_image_transport/depthz_plugins.xml @@ -0,0 +1,13 @@ + + + + This plugin losslessly compresses 32FC1 and 16UC1 depth images with the dpred codec + (value dictionary + 2D MED prediction + zstd). + + + + + This plugin decodes depthz-compressed depth images back to 32FC1 or 16UC1. + + + diff --git a/depthz_image_transport/include/depthz_image_transport/depthz_publisher.hpp b/depthz_image_transport/include/depthz_image_transport/depthz_publisher.hpp new file mode 100644 index 0000000..98c03ac --- /dev/null +++ b/depthz_image_transport/include/depthz_image_transport/depthz_publisher.hpp @@ -0,0 +1,79 @@ +// Copyright (c) 2026, Davide Faconti +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef DEPTHZ_IMAGE_TRANSPORT__DEPTHZ_PUBLISHER_HPP_ +#define DEPTHZ_IMAGE_TRANSPORT__DEPTHZ_PUBLISHER_HPP_ + +#include + +#include +#include +#include +#include + +#include + +namespace depthz_image_transport +{ + +using CompressedImage = sensor_msgs::msg::CompressedImage; + +class DepthzPublisher : public image_transport::SimplePublisherPlugin +{ +public: + DepthzPublisher(); + ~DepthzPublisher() override = default; + + std::string getTransportName() const override + { + return "depthz"; + } + +protected: + void advertiseImpl( + image_transport::RequiredInterfaces node_interfaces, + const std::string & base_topic, + rclcpp::QoS custom_qos, + rclcpp::PublisherOptions options) final; + + void publish( + const sensor_msgs::msg::Image & message, + const PublisherT & publisher) const override; + + rclcpp::Logger logger_; + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_interface_; + +private: + std::string level_param_name_; + std::string quantization_param_name_; +}; + +} // namespace depthz_image_transport + +#endif // DEPTHZ_IMAGE_TRANSPORT__DEPTHZ_PUBLISHER_HPP_ diff --git a/depthz_image_transport/include/depthz_image_transport/depthz_subscriber.hpp b/depthz_image_transport/include/depthz_image_transport/depthz_subscriber.hpp new file mode 100644 index 0000000..ab429d7 --- /dev/null +++ b/depthz_image_transport/include/depthz_image_transport/depthz_subscriber.hpp @@ -0,0 +1,67 @@ +// Copyright (c) 2026, Davide Faconti +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef DEPTHZ_IMAGE_TRANSPORT__DEPTHZ_SUBSCRIBER_HPP_ +#define DEPTHZ_IMAGE_TRANSPORT__DEPTHZ_SUBSCRIBER_HPP_ + +#include + +#include +#include +#include + +#include + +namespace depthz_image_transport +{ + +using CompressedImage = sensor_msgs::msg::CompressedImage; + +class DepthzSubscriber : public image_transport::SimpleSubscriberPlugin +{ +public: + DepthzSubscriber(); + ~DepthzSubscriber() override = default; + + std::string getTransportName() const override + { + return "depthz"; + } + +protected: + void internalCallback( + const CompressedImage::ConstSharedPtr & message, + const Callback & user_cb) override; + + rclcpp::Logger logger_; +}; + +} // namespace depthz_image_transport + +#endif // DEPTHZ_IMAGE_TRANSPORT__DEPTHZ_SUBSCRIBER_HPP_ diff --git a/depthz_image_transport/package.xml b/depthz_image_transport/package.xml new file mode 100644 index 0000000..fd9a148 --- /dev/null +++ b/depthz_image_transport/package.xml @@ -0,0 +1,32 @@ + + + depthz_image_transport + 7.0.1 + + depthz_image_transport provides a plugin to image_transport for transparently sending + 32FC1 and 16UC1 depth images losslessly compressed with the dpred codec (per-image value + dictionary + 2D prediction + zstd). + + Davide Faconti + BSD + + http://www.ros.org/wiki/image_transport_plugins + Davide Faconti + + ament_cmake + + image_transport + libzstd-dev + pluginlib + rclcpp + sensor_msgs + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/depthz_image_transport/src/depth_codec.cpp b/depthz_image_transport/src/depth_codec.cpp new file mode 100644 index 0000000..e8797a4 --- /dev/null +++ b/depthz_image_transport/src/depth_codec.cpp @@ -0,0 +1,1285 @@ +// Copyright (c) 2026, Davide Faconti +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "depth_codec.hpp" + +#include + +// Runtime AVX2 dispatch for the dictionary probe: released binaries build +// with portable flags, so the SIMD path is selected per-CPU at load time +// (GCC/Clang on x86-64; other targets use the scalar probe). +#if defined(__x86_64__) && (defined(__GNUC__) || defined(__clang__)) +#define DEPTH_CODEC_AVX2_DISPATCH 1 +#include +#endif + +#include +#include // NOLINT(build/include_order) -- cpplint predates C++20 headers +#include +#include +#include +#include +#include +#include + +namespace depth_codec +{ +namespace +{ + +[[noreturn]] void fail(const char * what) +{ + throw std::runtime_error(what); +} + +// ---- zstd stage with per-thread context reuse ------------------------------ +struct ZstdCtx +{ + ZSTD_CCtx * c = ZSTD_createCCtx(); + ZSTD_DCtx * d = ZSTD_createDCtx(); + ~ZstdCtx() + { + ZSTD_freeCCtx(c); + ZSTD_freeDCtx(d); + } +}; + +ZstdCtx & tl_zstd() +{ + static thread_local ZstdCtx ctx; + return ctx; +} + +#if defined(DEPTH_CODEC_AVX2_DISPATCH) +// Single dispatch policy for every SIMD kernel in this file. +inline bool cpu_has_avx2() +{ + static const bool v = __builtin_cpu_supports("avx2"); + return v; +} +#endif + +// Compress src and append the zstd frame to `out`. Compresses into a +// reused thread-local buffer first: `out` is typically a fresh outgoing +// message, and sizing it to ZSTD_compressBound would cost a multi-MB +// alloc + zero-fill per frame and leave the published message holding +// bound-sized capacity for its whole lifetime. +void zstd_append(std::vector & out, const uint8_t * src, size_t n, int level) +{ + static thread_local std::vector comp; + const size_t bound = ZSTD_compressBound(n); + comp.resize(bound); + const size_t k = ZSTD_compressCCtx(tl_zstd().c, comp.data(), bound, src, n, level); + if (ZSTD_isError(k)) { + fail(ZSTD_getErrorName(k)); + } + out.insert(out.end(), comp.data(), comp.data() + k); +} + +// Decompress a zstd frame into `out` (capacity reused across calls -- see +// tl_decode_scratch()). `expected_size` bounds the frame's self-reported +// content size *before* anything is allocated: a hostile/corrupt frame can +// claim an arbitrary ZSTD_getFrameContentSize (e.g. a few compressed bytes +// "inflating" to many GiB), so the claim is validated against a size the +// caller already trusts (derived from the image dimensions) instead of +// being allocated blindly. +// exact_size = true: content size must equal `expected_size` exactly +// (the caller has no further variable-size header to +// parse, e.g. the fixed-layout 16UC1 payload). +// exact_size = false: `expected_size` is only an upper bound; the caller +// still validates the actual size afterwards once it +// has parsed enough of the payload to know the exact +// expected value (e.g. the dpred dictionary size, +// which is itself part of the compressed content). +void zstd_unpack( + const uint8_t * comp, size_t comp_size, size_t expected_size, bool exact_size, + std::vector & out) +{ + const unsigned long long sz = ZSTD_getFrameContentSize(comp, comp_size); // NOLINT + if (sz == ZSTD_CONTENTSIZE_ERROR || sz == ZSTD_CONTENTSIZE_UNKNOWN) { + fail("bad zstd frame"); + } + const bool size_ok = exact_size ? + (sz == expected_size) : (sz <= static_cast(expected_size)); // NOLINT + if (!size_ok) { + fail("zstd_unpack: frame content size out of the expected range"); + } + out.resize(static_cast(sz)); + const size_t k = ZSTD_decompressDCtx(tl_zstd().d, out.data(), out.size(), comp, comp_size); + if (ZSTD_isError(k)) { + fail(ZSTD_getErrorName(k)); + } + if (k != out.size()) { + fail("zstd_unpack: size mismatch"); + } +} + +// ---- decode-side thread-local scratch -------------------------------------- +// Steady-state decoding (stable image size) should not allocate: `plain` +// (the zstd_unpack destination) and `idx` (the dpred index plane) are +// resized, never freed, across calls. Neither buffer escapes decode_depth / +// decode_depth16 -- both write their final result into the caller-owned +// output pointer -- so reuse across frames/threads-of-one is safe. +struct DecodeScratch +{ + std::vector plain; + std::vector idx; + std::vector words32; // fpred ord plane / qpred code plane + std::vector residuals; // qpred zigzag residual plane +}; + +DecodeScratch & tl_decode_scratch() +{ + static thread_local DecodeScratch scratch; + return scratch; +} + +// ---- little-endian helpers -------------------------------------------------- +inline void put_u32(std::vector & v, uint32_t x) +{ + for (int i = 0; i < 4; ++i) { + v.push_back(static_cast(x >> (8 * i))); + } +} + +inline uint32_t get_u32(const uint8_t * p) +{ + uint32_t x = 0; + for (int i = 0; i < 4; ++i) { + x |= static_cast(p[i]) << (8 * i); + } + return x; +} + +// Total-order-preserving bijection: unsigned comparison of the mapped word +// equals IEEE-754 total order of the float (negatives fixed up, NaN at the +// top). +inline uint32_t float_to_ord(uint32_t w) +{ + return (w >> 31) ? ~w : (w | 0x80000000u); +} + +inline uint32_t ord_to_float(uint32_t m) +{ + return (m >> 31) ? (m ^ 0x80000000u) : ~m; +} + +// JPEG-LS / LOCO-I median-edge-detector predictor, branchless clamp form. +inline int32_t med_predict(int32_t a, int32_t b, int32_t c) +{ + const int32_t mn = std::min(a, b); + const int32_t mx = std::max(a, b); + return std::clamp(a + b - c, mn, mx); +} + +// uint32 variant for the fpred/qpred paths, identical to the standalone +// library's med_predict: min/max in the unsigned domain, and the gradient +// case wraps mod 2^32 (the decoder mirrors the wrap, so it is bijective). +inline uint32_t med_predict_u32(uint32_t a /*left*/, uint32_t b /*up*/, uint32_t c /*upleft*/) +{ + const uint32_t mx = std::max(a, b), mn = std::min(a, b); + if (c >= mx) {return mn;} + if (c <= mn) {return mx;} + return a + b - c; +} + +inline uint32_t zigzag32(uint32_t d) // d = wrapped (mod 2^32) difference +{ + const int32_t s = static_cast(d); + return (static_cast(s) << 1) ^ static_cast(s >> 31); +} + +inline uint32_t unzigzag32(uint32_t z) +{ + return (z >> 1) ^ (~(z & 1) + 1); +} + +// Raster-order MED predictor with the standalone library's edge rules +// (row 0 predicts from the left, column 0 from above). Shared by fpred +// encode and decode, which must agree bit-for-bit. +inline uint32_t raster_pred_u32(const uint32_t * v, size_t i, uint32_t x, uint32_t y, uint32_t w) +{ + if (y == 0) { + return x ? v[i - 1] : 0; + } + if (x == 0) { + return v[i - w]; + } + return med_predict_u32(v[i - 1], v[i - w], v[i - w - 1]); +} + +// ---- shared prediction core -------------------------------------------------- +// MED-predict each uint16 in raster order from its left/up/up-left +// neighbours, zigzag the mod-2^16 residual (bijective for any jump size) +// and split it into a low-byte and a high-byte plane. For smooth depth the +// planes are mostly zeros, which the zstd stage then collapses. +void predict_pack(const uint16_t * vals, uint32_t w, uint32_t h, uint8_t * lo, uint8_t * hi) +{ + const auto emit = [&](size_t i, int32_t pred) { + const int16_t s = static_cast(static_cast(vals[i] - pred)); + const uint16_t z = static_cast((s << 1) ^ (s >> 15)); + lo[i] = static_cast(z); + hi[i] = static_cast(z >> 8); + }; + if (w == 0 || h == 0) { + return; + } + emit(0, 0); + for (uint32_t x = 1; x < w; ++x) { + emit(x, vals[x - 1]); + } + for (uint32_t y = 1; y < h; ++y) { + const size_t row = static_cast(y) * w; + emit(row, vals[row - w]); + const uint16_t * up = vals + row - w; + const uint16_t * cur = vals + row; + for (uint32_t x = 1; x < w; ++x) { + emit(row + x, med_predict(cur[x - 1], up[x], up[x - 1])); + } + } +} + +// Inverse of predict_pack. Reconstruction is serial along a row (each +// prediction needs the value just decoded), but row y+1 at column c only +// needs row y up to column c: processing R rows along a skewed diagonal +// ("wavefront") therefore runs R independent dependency chains that the +// out-of-order core overlaps. Pure decoder-side optimization: the format +// and the results are identical to the serial scan. +void predict_unpack(const uint8_t * lo, const uint8_t * hi, uint32_t w, uint32_t h, uint16_t * vals) +{ + const auto unstep = [&](size_t i, int32_t pred) -> uint16_t { + const uint16_t z = static_cast(lo[i] | (hi[i] << 8)); + const uint16_t r = static_cast((z >> 1) ^ (~(z & 1) + 1)); + const uint16_t k = static_cast(pred + r); + vals[i] = k; + return k; + }; + if (w == 0 || h == 0) { + return; + } + // First row: pure left-prediction chain. + unstep(0, 0); + for (uint32_t x = 1; x < w; ++x) { + unstep(x, vals[x - 1]); + } + + constexpr uint32_t R = 4; // interleaved rows = parallel dependency chains + uint32_t y = 1; + if (w >= 2 * R) { + for (; y + R <= h; y += R) { + uint16_t left[R] = {}; + // Ramp-up: row y+r starts one diagonal step after row y+r-1, which + // keeps the in-strip dependency satisfied (row r reads row r-1 one + // step behind). + for (uint32_t t = 0; t < R; ++t) { + for (uint32_t r = 0; r <= t; ++r) { + const uint32_t c = t - r; + const size_t row = static_cast(y + r) * w; + const uint16_t * up = vals + row - w; + left[r] = (c == 0) ? + unstep(row, up[0]) : + unstep(row + c, med_predict(left[r], up[c], up[c - 1])); + } + } + // Steady state: all R chains active, no bounds checks. + for (uint32_t t = R; t < w; ++t) { + for (uint32_t r = 0; r < R; ++r) { + const uint32_t c = t - r; + const size_t row = static_cast(y + r) * w; + const uint16_t * up = vals + row - w; + left[r] = unstep(row + c, med_predict(left[r], up[c], up[c - 1])); + } + } + // Drain: finish the trailing columns of the lower rows. + for (uint32_t t = w; t < w + R - 1; ++t) { + for (uint32_t r = t - w + 1; r < R; ++r) { + const uint32_t c = t - r; + const size_t row = static_cast(y + r) * w; + const uint16_t * up = vals + row - w; + left[r] = unstep(row + c, med_predict(left[r], up[c], up[c - 1])); + } + } + } + } + // Remaining rows (strip remainder, or narrow images): serial scan. + for (; y < h; ++y) { + const size_t row = static_cast(y) * w; + int32_t left = unstep(row, vals[row - w]); + const uint16_t * up = vals + row - w; + for (uint32_t x = 1; x < w; ++x) { + left = unstep(row + x, med_predict(left, up[x], up[x - 1])); + } + } +} + +// ---- per-image value dictionary, bucketized + SIMD-probed ------------------- +// Maps each 32-bit pattern to a first-seen id. 8 keys per cache-line bucket, +// all compared at once; a per-bucket count masks stale slots. The found +// branch is ~99% predictable (only first occurrences miss), which keeps the +// pipeline from flushing. Returns false if > 65536 distinct patterns. +struct alignas(64) DictBucket +{ + uint32_t keys[8]; + uint16_t vals[8]; + uint16_t cnt; + uint16_t pad[7]; +}; +static_assert(sizeof(DictBucket) == 64, "bucket must be one cache line"); + +constexpr size_t kDictBuckets = 1u << 14; // 16384 buckets x 8 slots, > 2x max load + +// Reused across frames, but every bucket's `cnt` must return to 0: memset +// the reused block (cheaper than a fresh alloc + first-touch page faults). +void reset_dict_table(std::vector & table) +{ + table.resize(kDictBuckets); + std::memset(table.data(), 0, table.size() * sizeof(DictBucket)); +} + +bool build_value_dict_scalar( + const uint32_t * words, size_t n, std::vector & table, + std::vector & entries, std::vector & idx) +{ + constexpr size_t nb = kDictBuckets; + constexpr size_t max_dict = 1u << 16; + reset_dict_table(table); + entries.clear(); + entries.reserve(1u << 12); + idx.resize(n); + for (size_t i = 0; i < n; ++i) { + const uint32_t k32 = words[i]; + size_t b = (k32 * 2654435761u) >> 18; // top 14 bits + uint32_t id; + for (;; ) { + DictBucket & bucket = table[b]; + uint32_t m = 0; + for (unsigned k = 0; k < bucket.cnt; ++k) { + if (bucket.keys[k] == k32) { + m = 1u << k; + break; + } + } + if (m) { + id = bucket.vals[std::countr_zero(m)]; + break; + } + if (bucket.cnt < 8) { + if (entries.size() >= max_dict) { + return false; + } + id = static_cast(entries.size()); + bucket.keys[bucket.cnt] = k32; + bucket.vals[bucket.cnt] = static_cast(id); + ++bucket.cnt; + entries.push_back(k32); + break; + } + b = (b + 1) & (nb - 1); // bucket full: spill to the next one + } + idx[i] = static_cast(id); + } + return true; +} + +#if defined(DEPTH_CODEC_AVX2_DISPATCH) +// Verbatim copy of build_value_dict_scalar with the probe replaced by one +// 8-wide SIMD compare. Compiled with the avx2 target attribute so portable +// (non -mavx2) builds still contain it and can select it at runtime; a +// shared inline body cannot carry a per-caller target attribute. +__attribute__((target("avx2"))) bool build_value_dict_avx2( + const uint32_t * words, size_t n, std::vector & table, + std::vector & entries, std::vector & idx) +{ + constexpr size_t nb = kDictBuckets; + constexpr size_t max_dict = 1u << 16; + reset_dict_table(table); + entries.clear(); + entries.reserve(1u << 12); + idx.resize(n); + for (size_t i = 0; i < n; ++i) { + const uint32_t k32 = words[i]; + size_t b = (k32 * 2654435761u) >> 18; + uint32_t id; + for (;; ) { + DictBucket & bucket = table[b]; + const __m256i vk = _mm256_set1_epi32(static_cast(k32)); + const __m256i keys = + _mm256_loadu_si256(reinterpret_cast(bucket.keys)); + uint32_t m = static_cast( + _mm256_movemask_ps(_mm256_castsi256_ps(_mm256_cmpeq_epi32(keys, vk)))); + m &= (1u << bucket.cnt) - 1; + if (m) { + id = bucket.vals[std::countr_zero(m)]; + break; + } + if (bucket.cnt < 8) { + if (entries.size() >= max_dict) { + return false; + } + id = static_cast(entries.size()); + bucket.keys[bucket.cnt] = k32; + bucket.vals[bucket.cnt] = static_cast(id); + ++bucket.cnt; + entries.push_back(k32); + break; + } + b = (b + 1) & (nb - 1); + } + idx[i] = static_cast(id); + } + return true; +} +#endif // DEPTH_CODEC_AVX2_DISPATCH + +bool build_value_dict( + const uint32_t * words, size_t n, std::vector & table, + std::vector & entries, std::vector & idx) +{ +#if defined(DEPTH_CODEC_AVX2_DISPATCH) + if (cpu_has_avx2()) { + return build_value_dict_avx2(words, n, table, entries, idx); + } +#endif + return build_value_dict_scalar(words, n, table, entries, idx); +} + +// ---- encode-side thread-local scratch -------------------------------------- +// Same reuse/lifetime rationale as DecodeScratch; nothing here escapes the +// encode functions (the result is copied out via zstd_append). +struct EncodeScratch +{ + std::vector dict_table; + std::vector entries; + std::vector idx; + std::vector keys_a, keys_b; + std::vector radix_hist; + std::vector rank; + std::vector sorted_entries; + std::vector plain; + std::vector words32; // fpred ord plane / qpred code plane + std::vector residuals; // qpred zigzag residual plane +}; + +EncodeScratch & tl_encode_scratch() +{ + static thread_local EncodeScratch scratch; + return scratch; +} + +// ---- dpred payload (32FC1) --------------------------------------------------- +// A single zstd frame whose decompressed content is (little-endian): +// u8 mode 0 = legacy fallback (raw words; decode-only, the +// encoder now emits an "fpred" blob instead when the +// dictionary overflows), 1 = dictionary +// u8 idx_bytes always 2 +// u32 dict_size ds <= 65536 +// u32 x ds dictionary, sorted by float total order +// u8 x n low bytes of zigzag residuals +// u8 x n high bytes of zigzag residuals +// +// Precondition: build_value_dict succeeded and left the dictionary in +// scratch.entries and the first-seen index plane in scratch.idx (the caller +// runs it first, because the outcome decides the blob method name). +void dpred_encode_with_dict( + uint32_t w, uint32_t h, int level, EncodeScratch & scratch, std::vector & out) +{ + const size_t n = static_cast(w) * h; + std::vector & entries = scratch.entries; + std::vector & idx = scratch.idx; + std::vector & plain = scratch.plain; + + // Sort the dictionary by float total order and remap indices so that + // index distance ~ depth distance (what makes MED residuals small). + // LSD radix sort (two 16-bit digits) over key = ord<<16 | original_index. + const size_t ds = entries.size(); + std::vector & keys_a = scratch.keys_a; + std::vector & keys_b = scratch.keys_b; + keys_a.resize(ds); + keys_b.resize(ds); + for (size_t k = 0; k < ds; ++k) { + keys_a[k] = (static_cast(float_to_ord(entries[k])) << 16) | k; + } + { + std::vector & hist = scratch.radix_hist; + hist.resize(1u << 16); + uint64_t * src = keys_a.data(); + uint64_t * dst = keys_b.data(); + for (const int shift : {16, 32}) { + std::fill(hist.begin(), hist.end(), 0); + for (size_t k = 0; k < ds; ++k) { + ++hist[(src[k] >> shift) & 0xFFFF]; + } + uint32_t sum = 0; + for (uint32_t & slot : hist) { + const uint32_t c = slot; + slot = sum; + sum += c; + } + for (size_t k = 0; k < ds; ++k) { + dst[hist[(src[k] >> shift) & 0xFFFF]++] = src[k]; + } + std::swap(src, dst); + } + } + std::vector & rank = scratch.rank; + std::vector & sorted_entries = scratch.sorted_entries; + rank.resize(ds); + sorted_entries.resize(ds); + for (size_t k = 0; k < ds; ++k) { + const uint32_t orig = static_cast(keys_a[k] & 0xFFFF); + rank[orig] = static_cast(k); + sorted_entries[k] = entries[orig]; + } + for (size_t i = 0; i < n; ++i) { + idx[i] = rank[idx[i]]; + } + + plain.resize(6 + ds * 4 + n * 2); + plain[0] = 1; + plain[1] = 2; // residuals are always 2 bytes (split into two planes) + static_assert(std::endian::native == std::endian::little, "format is little-endian"); + const uint32_t ds32 = static_cast(ds); + std::memcpy(plain.data() + 2, &ds32, 4); + // ds == 0 (empty image): sorted_entries.data() may be null, and memcpy + // with a null src is UB even for 0 bytes. + if (ds > 0) { + std::memcpy(plain.data() + 6, sorted_entries.data(), ds * 4); + } + + uint8_t * lo = plain.data() + 6 + ds * 4; + predict_pack(idx.data(), w, h, lo, lo + n); + zstd_append(out, plain.data(), plain.size(), level); +} + +void dpred_decode(const uint8_t * comp, size_t comp_size, float * out, uint32_t w, uint32_t h) +{ + const size_t n = static_cast(w) * h; + DecodeScratch & scratch = tl_decode_scratch(); + std::vector & plain = scratch.plain; + + // The exact decompressed size depends on the payload's own mode byte and + // dict_size field, so pre-decompression only an n-derived upper bound can + // be enforced (mode 0: 1 + 4n; mode 1: <= 6 + 4*65536 + 2n); the exact + // size is validated below, after parsing. + const size_t max_plain = std::max(1 + n * 4, 6 + (size_t{1} << 16) * 4 + n * 2); + zstd_unpack(comp, comp_size, max_plain, /*exact_size=*/false, plain); + if (plain.empty()) { + fail("dpred_decode: empty payload"); + } + uint32_t * words = reinterpret_cast(out); + const uint8_t mode = plain[0]; + if (mode == 0) { + if (plain.size() != 1 + n * 4) { + fail("dpred_decode: size mismatch"); + } + // n == 0: `words` may be null, and memcpy with a null dst is UB even + // for 0 bytes. + if (n > 0) { + std::memcpy(words, plain.data() + 1, n * 4); + } + return; + } + if (mode != 1) { + fail("dpred_decode: invalid mode byte"); + } + if (plain.size() < 6) { + fail("dpred_decode: truncated header"); + } + if (plain[1] != 2) { + fail("dpred_decode: invalid idx_bytes (expected 2)"); + } + const size_t ds = get_u32(&plain[2]); + if (plain.size() != 6 + ds * 4 + n * 2) { + fail("dpred_decode: size mismatch"); + } + const uint8_t * dict = plain.data() + 6; // read in place (get_u32 handles alignment) + const uint8_t * lo = plain.data() + 6 + ds * 4; + + std::vector & idx = scratch.idx; + idx.resize(n); + predict_unpack(lo, lo + n, w, h, idx.data()); + for (size_t i = 0; i < n; ++i) { + if (idx[i] >= ds) { + fail("dpred_decode: bad index"); + } + words[i] = get_u32(dict + 4 * static_cast(idx[i])); + } +} + +// ---- fpred payload (32FC1) --------------------------------------------------- +// Dictionary-free fallback for images whose distinct-value count overflows +// the 16-bit dictionary (typical for full-precision float stereo depth, +// where most pixels carry a unique mantissa pattern). Byte-identical to the +// standalone library's "fpred" method: map every word to its total-order +// integer, MED-predict from the left/up/up-left neighbours, zigzag the +// mod-2^32 residual and split it into 4 byte planes for zstd. Smooth depth +// makes residuals tiny, so the high planes collapse to near-zero runs. +void fpred_encode( + const float * data, uint32_t w, uint32_t h, int level, + EncodeScratch & scratch, std::vector & out) +{ + const size_t n = static_cast(w) * h; + const uint32_t * words = reinterpret_cast(data); + std::vector & ord = scratch.words32; + ord.resize(n); + for (size_t i = 0; i < n; ++i) { + ord[i] = float_to_ord(words[i]); + } + + std::vector & planes = scratch.plain; + planes.resize(n * 4); + for (uint32_t y = 0; y < h; ++y) { + const size_t row = static_cast(y) * w; + for (uint32_t x = 0; x < w; ++x) { + const size_t i = row + x; + const uint32_t pred = raster_pred_u32(ord.data(), i, x, y, w); + const uint32_t z = zigzag32(ord[i] - pred); + planes[i] = static_cast(z); + planes[n + i] = static_cast(z >> 8); + planes[2 * n + i] = static_cast(z >> 16); + planes[3 * n + i] = static_cast(z >> 24); + } + } + zstd_append(out, planes.data(), planes.size(), level); +} + +void fpred_decode(const uint8_t * comp, size_t comp_size, float * out, uint32_t w, uint32_t h) +{ + const size_t n = static_cast(w) * h; + DecodeScratch & scratch = tl_decode_scratch(); + std::vector & planes = scratch.plain; + // The fpred payload has no variable-size header: exactly 4 byte planes. + zstd_unpack(comp, comp_size, n * 4, /*exact_size=*/true, planes); + uint32_t * words = reinterpret_cast(out); + std::vector & ord = scratch.words32; + ord.resize(n); + for (uint32_t y = 0; y < h; ++y) { + const size_t row = static_cast(y) * w; + for (uint32_t x = 0; x < w; ++x) { + const size_t i = row + x; + const uint32_t pred = raster_pred_u32(ord.data(), i, x, y, w); + const uint32_t z = planes[i] | (planes[n + i] << 8) | + (static_cast(planes[2 * n + i]) << 16) | + (static_cast(planes[3 * n + i]) << 24); + ord[i] = pred + unzigzag32(z); + words[i] = ord_to_float(ord[i]); + } + } +} + +// ---- qpred payload (32FC1, lossy) --------------------------------------------- +// Configurable uniform quantization. Payload layout (little-endian): +// f32 step quantization step in meters (> 0), UNCOMPRESSED so +// that read_header() can report it without touching the +// zstd frame -- together with the blob header this makes +// the message fully self-describing +// zstd frame of: +// LEB128 varint x n zigzag(code - MED(neighbours)), raster order +// with code 0 reserved for invalid pixels (NaN/inf/<= 0; decoded back as +// quiet NaN per REP 118) and code = round(depth/step) + 1 otherwise. The +// level count is unbounded (fine steps and long ranges exceed 16 bits, e.g. +// 10 m at 0.1 mm = 100k levels), which is why residuals are varint-coded: +// after MED prediction almost all of them fit one byte regardless of the +// code width, and depth discontinuities cost 2-3 bytes instead of forcing a +// fixed width on the whole plane. +constexpr size_t kMaxVarintBytes = 5; // 32-bit zigzag -> <= 35 payload bits + +// Quantize one depth value. Float ops in this exact sequence (mul, compare, +// add 0.5, truncate) so the scalar and AVX2 paths produce bit-identical +// code planes; round-half-up via +0.5/truncate instead of lrint/cvtps makes +// the result independent of the ambient FPU rounding mode as well. +// +// A depth too large to sit on the quantization grid (v/step beyond ~2^31, +// e.g. a garbage sample, or a far pixel under an absurdly fine step) is +// treated as INVALID (code 0 -> NaN), like NaN/inf/<= 0: every pixel the +// decoder reports as a number is genuinely within +/- step/2 of the input, +// and unrepresentable ones fail loudly as NaN instead of silently +// saturating with an unbounded error. With the default 0.1 mm step the +// cutoff is ~214 km, unreachable for any real sensor. +constexpr float kMaxQuantFloat = 2147483520.0f; // largest float < 2^31 + +inline uint32_t quantize_code(float v, float inv_step) +{ + const float q = v * inv_step; + // Single validity test, false for NaN/inf/<= 0 inputs, float overflow of + // the multiply, and codes past the grid. + if (!(v > 0.0f) || !(q <= kMaxQuantFloat)) { + return 0; + } + return static_cast(q + 0.5f) + 1; +} + +void qpred_quantize_scalar(const float * data, size_t n, float inv_step, uint32_t * code) +{ + for (size_t i = 0; i < n; ++i) { + code[i] = quantize_code(data[i], inv_step); + } +} + +// Zigzag residuals against the branch-form MED predictor. The encoder +// predicts from the ORIGINAL code plane (the decoder reconstructs the very +// same values), so unlike the decoder this has no serial dependency and the +// rows vectorize directly. The predictor must match med_predict_u32 +// bit-for-bit or the decoder diverges. +void qpred_residuals_scalar(const uint32_t * code, uint32_t w, uint32_t h, uint32_t * zres) +{ + zres[0] = zigzag32(code[0]); + for (uint32_t x = 1; x < w; ++x) { + zres[x] = zigzag32(code[x] - code[x - 1]); + } + for (uint32_t y = 1; y < h; ++y) { + const size_t row = static_cast(y) * w; + zres[row] = zigzag32(code[row] - code[row - w]); + for (uint32_t x = 1; x < w; ++x) { + const size_t i = row + x; + zres[i] = zigzag32(code[i] - med_predict_u32(code[i - 1], code[i - w], code[i - w - 1])); + } + } +} + +#if defined(DEPTH_CODEC_AVX2_DISPATCH) +__attribute__((target("avx2"))) void qpred_quantize_avx2( + const float * data, size_t n, float inv_step, uint32_t * code) +{ + const __m256 vinv = _mm256_set1_ps(inv_step); + const __m256 vzero = _mm256_setzero_ps(); + const __m256 vqmax = _mm256_set1_ps(kMaxQuantFloat); + const __m256 vhalf = _mm256_set1_ps(0.5f); + const __m256i vone = _mm256_set1_epi32(1); + size_t i = 0; + for (; i + 8 <= n; i += 8) { + const __m256 v = _mm256_loadu_ps(data + i); + const __m256 q = _mm256_mul_ps(v, vinv); + // valid = (v > 0) && (q <= max representable code): the ordered + // compares are false for NaN, the second one also rejects +inf, float + // overflow of the multiply, and off-grid codes (see quantize_code). + // Invalid lanes still go through the arithmetic below (defined, + // garbage) and are masked to 0 at the end. + const __m256 valid = _mm256_and_ps( + _mm256_cmp_ps(v, vzero, _CMP_GT_OQ), _mm256_cmp_ps(q, vqmax, _CMP_LE_OQ)); + // Round half up by adding 0.5 and truncating (cvtt), matching + // quantize_code bit-for-bit and independent of the FPU rounding mode. + const __m256i c = _mm256_add_epi32(_mm256_cvttps_epi32(_mm256_add_ps(q, vhalf)), vone); + _mm256_storeu_si256( + reinterpret_cast<__m256i *>(code + i), + _mm256_and_si256(c, _mm256_castps_si256(valid))); + } + for (; i < n; ++i) { + code[i] = quantize_code(data[i], inv_step); + } +} + +// Lane-exact vector form of med_predict_u32: unsigned min/max plus the two +// compares blended in the same priority order as the branches (c >= max +// first), with the gradient a+b-c wrapping mod 2^32 like the scalar code. +__attribute__((target("avx2"))) inline __m256i med_predict_u32_avx2( + __m256i a, __m256i b, __m256i c) +{ + const __m256i mx = _mm256_max_epu32(a, b); + const __m256i mn = _mm256_min_epu32(a, b); + const __m256i c_ge_mx = _mm256_cmpeq_epi32(_mm256_max_epu32(c, mx), c); + const __m256i c_le_mn = _mm256_cmpeq_epi32(_mm256_min_epu32(c, mn), c); + const __m256i grad = _mm256_sub_epi32(_mm256_add_epi32(a, b), c); + return _mm256_blendv_epi8(_mm256_blendv_epi8(grad, mx, c_le_mn), mn, c_ge_mx); +} + +__attribute__((target("avx2"))) void qpred_residuals_avx2( + const uint32_t * code, uint32_t w, uint32_t h, uint32_t * zres) +{ + zres[0] = zigzag32(code[0]); + for (uint32_t x = 1; x < w; ++x) { + zres[x] = zigzag32(code[x] - code[x - 1]); + } + for (uint32_t y = 1; y < h; ++y) { + const size_t row = static_cast(y) * w; + zres[row] = zigzag32(code[row] - code[row - w]); + uint32_t x = 1; + for (; x + 8 <= w; x += 8) { + const size_t i = row + x; + const __m256i left = _mm256_loadu_si256(reinterpret_cast(code + i - 1)); + const __m256i up = _mm256_loadu_si256(reinterpret_cast(code + i - w)); + const __m256i upl = _mm256_loadu_si256(reinterpret_cast(code + i - w - 1)); + const __m256i cur = _mm256_loadu_si256(reinterpret_cast(code + i)); + const __m256i s = _mm256_sub_epi32(cur, med_predict_u32_avx2(left, up, upl)); + const __m256i z = _mm256_xor_si256(_mm256_slli_epi32(s, 1), _mm256_srai_epi32(s, 31)); + _mm256_storeu_si256(reinterpret_cast<__m256i *>(zres + i), z); + } + for (; x < w; ++x) { + const size_t i = row + x; + zres[i] = zigzag32(code[i] - med_predict_u32(code[i - 1], code[i - w], code[i - w - 1])); + } + } +} +#endif // DEPTH_CODEC_AVX2_DISPATCH + +inline uint8_t * put_varint(uint8_t * p, uint32_t z) +{ + do { + uint8_t byte = z & 0x7F; + z >>= 7; + if (z) {byte |= 0x80;} + *p++ = byte; + } while (z); + return p; +} + +inline float code_to_float(uint32_t c, double step_d) +{ + return c == 0 ? + std::numeric_limits::quiet_NaN() : + static_cast(static_cast(c - 1) * step_d); +} + +#if defined(DEPTH_CODEC_AVX2_DISPATCH) +__attribute__((target("avx2"))) void qpred_codes_to_floats_avx2( + const uint32_t * code, size_t n, float step, float * out) +{ + const double step_d = static_cast(step); + const __m256d vstep_d = _mm256_set1_pd(step_d); + const __m256i vzero = _mm256_setzero_si256(); + const __m256i vone = _mm256_set1_epi32(1); + const __m256 vnan = _mm256_set1_ps(std::numeric_limits::quiet_NaN()); + size_t i = 0; + for (; i + 8 <= n; i += 8) { + const __m256i c = _mm256_loadu_si256(reinterpret_cast(code + i)); + // Codes >= 2^31 cannot come from this encoder (it rejects them as + // invalid) but a hostile/corrupt stream can produce them; the epi32 + // conversions would misread them as negative, so such blocks take the + // scalar path. + if (_mm256_movemask_ps(_mm256_castsi256_ps(c)) != 0) { + for (int k = 0; k < 8; ++k) { + out[i + k] = code_to_float(code[i + k], step_d); + } + continue; + } + // Multiply in DOUBLE, then narrow to float, exactly like + // code_to_float: int32->double is exact and the two roundings (double + // product, float narrowing) match the scalar path bit-for-bit, so a + // blob decodes identically with or without AVX2. c == 0 lanes run + // through the arithmetic (c-1 -> -1 -> -step, garbage) and are blended + // to NaN at the end. + const __m256i cm1 = _mm256_sub_epi32(c, vone); + const __m256d lo = + _mm256_mul_pd(_mm256_cvtepi32_pd(_mm256_castsi256_si128(cm1)), vstep_d); + const __m256d hi = + _mm256_mul_pd(_mm256_cvtepi32_pd(_mm256_extracti128_si256(cm1, 1)), vstep_d); + const __m256 f = _mm256_insertf128_ps( + _mm256_castps128_ps256(_mm256_cvtpd_ps(lo)), _mm256_cvtpd_ps(hi), 1); + const __m256i is_invalid = _mm256_cmpeq_epi32(c, vzero); + _mm256_storeu_ps(out + i, _mm256_blendv_ps(f, vnan, _mm256_castsi256_ps(is_invalid))); + } + for (; i < n; ++i) { + out[i] = code_to_float(code[i], step_d); + } +} +#endif // DEPTH_CODEC_AVX2_DISPATCH + +void qpred_codes_to_floats(const uint32_t * code, size_t n, float step, float * out) +{ +#if defined(DEPTH_CODEC_AVX2_DISPATCH) + if (cpu_has_avx2()) { + qpred_codes_to_floats_avx2(code, n, step, out); + return; + } +#endif + const double step_d = static_cast(step); + for (size_t i = 0; i < n; ++i) { + out[i] = code_to_float(code[i], step_d); + } +} + +void qpred_encode( + const float * data, uint32_t w, uint32_t h, float step, int level, + EncodeScratch & scratch, std::vector & out) +{ + const size_t n = static_cast(w) * h; + std::vector & code = scratch.words32; + std::vector & zres = scratch.residuals; + code.resize(n); + zres.resize(n); + const float inv_step = 1.0f / step; + + static_assert(sizeof(float) == 4, "IEEE-754 single precision expected"); + uint32_t step_bits; + std::memcpy(&step_bits, &step, 4); + put_u32(out, step_bits); + + if (n > 0) { +#if defined(DEPTH_CODEC_AVX2_DISPATCH) + if (cpu_has_avx2()) { + qpred_quantize_avx2(data, n, inv_step, code.data()); + qpred_residuals_avx2(code.data(), w, h, zres.data()); + } else // NOLINT(readability/braces) -- scalar block shared with non-x86 +#endif + { + qpred_quantize_scalar(data, n, inv_step, code.data()); + qpred_residuals_scalar(code.data(), w, h, zres.data()); + } + } + + // Varint emission through a raw pointer into a worst-case-sized reused + // buffer (no per-byte push_back). Groups of 8 whose residuals all fit one + // byte -- the overwhelmingly common case on smooth depth -- skip the + // per-value continuation logic entirely. + std::vector & plain = scratch.plain; + plain.resize(n * kMaxVarintBytes); + uint8_t * p = plain.data(); + size_t i = 0; + for (; i + 8 <= n; i += 8) { + const uint32_t any = zres[i] | zres[i + 1] | zres[i + 2] | zres[i + 3] | + zres[i + 4] | zres[i + 5] | zres[i + 6] | zres[i + 7]; + if (any < 0x80) { + for (int k = 0; k < 8; ++k) { + p[k] = static_cast(zres[i + k]); + } + p += 8; + } else { + for (int k = 0; k < 8; ++k) { + p = put_varint(p, zres[i + k]); + } + } + } + for (; i < n; ++i) { + p = put_varint(p, zres[i]); + } + zstd_append(out, plain.data(), static_cast(p - plain.data()), level); +} + +// `step` comes from the uncompressed payload prefix, already validated by +// parse_blob (positive, finite); `comp` points past it, at the zstd frame. +void qpred_decode( + const uint8_t * comp, size_t comp_size, float * out, uint32_t w, uint32_t h, float step) +{ + const size_t n = static_cast(w) * h; + DecodeScratch & scratch = tl_decode_scratch(); + std::vector & plain = scratch.plain; + // Upper bound: n varints of at most 5 bytes each. + zstd_unpack(comp, comp_size, n * kMaxVarintBytes, /*exact_size=*/false, plain); + + // Pass 1: parse the varint stream into a flat residual plane. Splitting + // this from the reconstruction lets pass 2 run a wavefront over rows + // (variable-length varints cannot be indexed randomly, a flat plane can) + // and keeps the single-byte fast path branch-predictable. + std::vector & zres = scratch.residuals; + zres.resize(n); + { + const uint8_t * p = plain.data(); + const uint8_t * const end = plain.data() + plain.size(); + for (size_t i = 0; i < n; ++i) { + if (p == end) { + fail("qpred_decode: truncated varint stream"); + } + uint8_t byte = *p++; + if (byte < 0x80) { // ~all residuals on smooth depth + zres[i] = byte; + continue; + } + uint64_t z = byte & 0x7F; + unsigned shift = 7; + while (true) { + if (p == end) { + fail("qpred_decode: truncated varint stream"); + } + byte = *p++; + z |= static_cast(byte & 0x7F) << shift; + if (!(byte & 0x80)) { + break; + } + shift += 7; + if (shift >= kMaxVarintBytes * 7) { + fail("qpred_decode: varint overflow"); + } + } + if (z > 0xFFFFFFFFull) { + fail("qpred_decode: varint overflow"); + } + zres[i] = static_cast(z); + } + if (p != end) { + fail("qpred_decode: trailing bytes after varint stream"); + } + } + if (n == 0) { + return; + } + + // Pass 2: reconstruct the code plane. Serial along a row (each prediction + // needs the code just reconstructed), so process R rows along a skewed + // diagonal -- the same wavefront as predict_unpack, with the u32 + // branch-form MED matching the encoder. + std::vector & code = scratch.words32; + code.resize(n); + const auto unstep = [&](size_t i, uint32_t pred) -> uint32_t { + const uint32_t c = pred + unzigzag32(zres[i]); + code[i] = c; + return c; + }; + unstep(0, 0); + for (uint32_t x = 1; x < w; ++x) { + unstep(x, code[x - 1]); + } + constexpr uint32_t R = 4; + uint32_t y = 1; + if (w >= 2 * R) { + for (; y + R <= h; y += R) { + uint32_t left[R] = {}; + for (uint32_t t = 0; t < R; ++t) { // ramp-up diagonals + for (uint32_t r = 0; r <= t; ++r) { + const uint32_t c = t - r; + const size_t row = static_cast(y + r) * w; + const uint32_t * up = code.data() + row - w; + left[r] = (c == 0) ? + unstep(row, up[0]) : + unstep(row + c, med_predict_u32(left[r], up[c], up[c - 1])); + } + } + for (uint32_t t = R; t < w; ++t) { // steady state + for (uint32_t r = 0; r < R; ++r) { + const uint32_t c = t - r; + const size_t row = static_cast(y + r) * w; + const uint32_t * up = code.data() + row - w; + left[r] = unstep(row + c, med_predict_u32(left[r], up[c], up[c - 1])); + } + } + for (uint32_t t = w; t < w + R - 1; ++t) { // drain + for (uint32_t r = t - w + 1; r < R; ++r) { + const uint32_t c = t - r; + const size_t row = static_cast(y + r) * w; + const uint32_t * up = code.data() + row - w; + left[r] = unstep(row + c, med_predict_u32(left[r], up[c], up[c - 1])); + } + } + } + } + for (; y < h; ++y) { // strip remainder, or narrow images + const size_t row = static_cast(y) * w; + uint32_t left = unstep(row, code[row - w]); + const uint32_t * up = code.data() + row - w; + for (uint32_t x = 1; x < w; ++x) { + left = unstep(row + x, med_predict_u32(left, up[x], up[x - 1])); + } + } + + // Pass 3: codes -> floats (code 0 -> NaN per REP 118). + qpred_codes_to_floats(code.data(), n, step, out); +} + +// ---- public self-describing blob ------------------------------------------- +// Layout: 'D' 'P' 'C' '1' | u8 name_len | name | i32 level | u32 w | u32 h +// | payload. Identical to the standalone depth_codec library. +constexpr char kMethod32[] = "dpred"; +constexpr char kMethod16[] = "dpred16"; +constexpr char kMethodFpred[] = "fpred"; +constexpr char kMethodQpred[] = "qpred"; + +// Reset `out` (keeping its capacity) and write the blob header; the payload +// is then appended in place by the caller. +void begin_blob( + std::vector & out, const char * name, int level, uint32_t width, uint32_t height) +{ + out.clear(); + const char magic[4] = {'D', 'P', 'C', '1'}; + out.insert(out.end(), magic, magic + 4); + const size_t name_len = std::strlen(name); + out.push_back(static_cast(name_len)); + out.insert(out.end(), name, name + name_len); + put_u32(out, static_cast(level)); + put_u32(out, width); + put_u32(out, height); +} + +enum class Method +{ + kDpred, // 32FC1, lossless, dictionary + MED on indices + kDpred16, // 16UC1, lossless, MED directly on values + kFpred, // 32FC1, lossless, dictionary-free MED on float total order + kQpred // 32FC1, lossy, configurable-step quantization + varint residuals +}; + +struct ParsedBlob +{ + BlobHeader header; + Method method; + const uint8_t * payload; + size_t payload_size; +}; + +ParsedBlob parse_blob(const uint8_t * blob, size_t size) +{ + if (size < 5 || std::memcmp(blob, "DPC1", 4) != 0) { + fail("depth_codec: bad magic"); + } + size_t pos = 4; + const uint8_t name_len = blob[pos++]; + if (pos + name_len + 12 > size) { + fail("depth_codec: truncated header"); + } + const std::string method(reinterpret_cast(blob + pos), name_len); + pos += name_len; + ParsedBlob parsed; + parsed.header.quantization_step = 0.0f; + if (method == kMethod32) { + parsed.method = Method::kDpred; + parsed.header.format = PixelFormat::FLOAT32; + } else if (method == kMethod16) { + parsed.method = Method::kDpred16; + parsed.header.format = PixelFormat::UINT16; + } else if (method == kMethodFpred) { + parsed.method = Method::kFpred; + parsed.header.format = PixelFormat::FLOAT32; + } else if (method == kMethodQpred) { + parsed.method = Method::kQpred; + parsed.header.format = PixelFormat::FLOAT32; + } else { + fail("depth_codec: unknown method (blob from a newer library?)"); + } + pos += 4; // level: not needed to decode + parsed.header.width = get_u32(blob + pos); + parsed.header.height = get_u32(blob + pos + 4); + pos += 8; + parsed.payload = blob + pos; + parsed.payload_size = size - pos; + if (parsed.method == Method::kQpred) { + // The quantization step is an uncompressed payload prefix, so the + // header alone (this function; no decompression) fully describes how + // to decode and interpret the blob. Validate it here: a header + // consumer must never see a nonsensical step. + if (parsed.payload_size < 4) { + fail("depth_codec: truncated qpred payload"); + } + float step; + const uint32_t step_bits = get_u32(parsed.payload); + std::memcpy(&step, &step_bits, 4); + if (!std::isfinite(step) || step <= 0.0f) { + fail("depth_codec: invalid qpred quantization step"); + } + parsed.header.quantization_step = step; + parsed.payload += 4; + parsed.payload_size -= 4; + } + return parsed; +} + +} // namespace + +void encode_depth( + const float * data, uint32_t width, uint32_t height, + std::vector & out, int zstd_level) +{ + const int level = std::clamp(zstd_level, 1, 3); + const size_t n = static_cast(width) * height; + const uint32_t * words = reinterpret_cast(data); + EncodeScratch & scratch = tl_encode_scratch(); + // The dictionary build decides the blob method, so it runs before the + // header is written: dpred when the distinct-value count fits 16 bits, + // fpred (dictionary-free, cardinality-unlimited) when it overflows -- + // which is the common case for full-precision float stereo depth. + if (build_value_dict(words, n, scratch.dict_table, scratch.entries, scratch.idx)) { + begin_blob(out, kMethod32, level, width, height); + dpred_encode_with_dict(width, height, level, scratch, out); + } else { + begin_blob(out, kMethodFpred, level, width, height); + fpred_encode(data, width, height, level, scratch, out); + } +} + +void encode_depth_quantized( + const float * data, uint32_t width, uint32_t height, + std::vector & out, float step, int zstd_level) +{ + if (!std::isfinite(step) || step <= 0.0f) { + fail("encode_depth_quantized: step must be a positive finite value"); + } + const int level = std::clamp(zstd_level, 1, 3); + EncodeScratch & scratch = tl_encode_scratch(); + begin_blob(out, kMethodQpred, level, width, height); + qpred_encode(data, width, height, step, level, scratch, out); +} + +// 16UC1 payload: a zstd frame of [low plane | high plane] (2 * w * h bytes). +// The pixel value is already a small monotone integer -- its own sorted +// dictionary index -- so no dictionary (and no overflow fallback) is needed. +void encode_depth16( + const uint16_t * data, uint32_t width, uint32_t height, + std::vector & out, int zstd_level) +{ + const int level = std::clamp(zstd_level, 1, 3); + const size_t n = static_cast(width) * height; + std::vector & plain = tl_encode_scratch().plain; + plain.resize(n * 2); + predict_pack(data, width, height, plain.data(), plain.data() + n); + begin_blob(out, kMethod16, level, width, height); + zstd_append(out, plain.data(), plain.size(), level); +} + +BlobHeader read_header(const uint8_t * blob, size_t size) +{ + return parse_blob(blob, size).header; +} + +void decode_depth(const uint8_t * blob, size_t size, float * out) +{ + const ParsedBlob parsed = parse_blob(blob, size); + if (parsed.header.format != PixelFormat::FLOAT32) { + fail("decode_depth: blob is not 32FC1"); + } + switch (parsed.method) { + case Method::kFpred: + fpred_decode( + parsed.payload, parsed.payload_size, out, parsed.header.width, parsed.header.height); + break; + case Method::kQpred: + qpred_decode( + parsed.payload, parsed.payload_size, out, parsed.header.width, parsed.header.height, + parsed.header.quantization_step); + break; + default: + dpred_decode( + parsed.payload, parsed.payload_size, out, parsed.header.width, parsed.header.height); + break; + } +} + +void decode_depth16(const uint8_t * blob, size_t size, uint16_t * out) +{ + const ParsedBlob parsed = parse_blob(blob, size); + if (parsed.header.format != PixelFormat::UINT16) { + fail("decode_depth16: blob is not 16UC1"); + } + const size_t n = static_cast(parsed.header.width) * parsed.header.height; + // The 16UC1 payload has no variable-size header: the decompressed size is + // known exactly upfront, so zstd_unpack enforces it exactly and no + // after-the-fact size check is needed here. + std::vector & plain = tl_decode_scratch().plain; + zstd_unpack(parsed.payload, parsed.payload_size, n * 2, /*exact_size=*/true, plain); + predict_unpack( + plain.data(), plain.data() + n, parsed.header.width, parsed.header.height, out); +} + +} // namespace depth_codec diff --git a/depthz_image_transport/src/depth_codec.hpp b/depthz_image_transport/src/depth_codec.hpp new file mode 100644 index 0000000..b6982d0 --- /dev/null +++ b/depthz_image_transport/src/depth_codec.hpp @@ -0,0 +1,136 @@ +// Copyright (c) 2026, Davide Faconti +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Vendored "contrib" copy of the depth codec from +// https://github.com/facontidavide/depth_image_compression +// The blob format is identical. Methods implemented here: +// - "dpred" (32FC1, lossless): per-image value dictionary sorted by float +// total order, 2D MED prediction on the index plane, zigzag residual +// byte planes, zstd entropy stage. Used when the image has <= 65536 +// distinct bit patterns. +// - "fpred" (32FC1, lossless): dictionary-free variant for images that +// overflow the 16-bit dictionary (typical full-precision float stereo +// depth): MED prediction directly on the float total-order integers, +// zigzag residuals in 4 byte planes, zstd. Automatic fallback of +// encode_depth(). +// - "dpred16" (16UC1, lossless): the pixel value is already a small +// monotone integer, i.e. its own sorted dictionary index, so the dpred +// prediction core applies directly with no dictionary at all. +// - "qpred" (32FC1, LOSSY): uniform quantization with a configurable step, +// MED prediction on the quantization codes, LEB128-varint zigzag +// residuals, zstd. Level count is unbounded (not limited to 16 bits). +// Invalid inputs (NaN/inf/<= 0) decode as quiet NaN (REP 118). +// Lossless "dpred"/"fpred"/"dpred16" blobs are interchangeable with the +// standalone library in both directions ("store"/"zstd"/"bss"/"alp"/ +// "alprd"/"dict" blobs from the standalone library are rejected here with +// "unknown method"). "qpred" originates in this plugin and is not decoded +// by standalone releases that predate it. +// See ALGORITHM.md in the source repository. + +#ifndef DEPTH_CODEC_HPP_ +#define DEPTH_CODEC_HPP_ + +#include +#include +#include + +namespace depth_codec +{ + +enum class PixelFormat +{ + FLOAT32, // 32FC1 + UINT16 // 16UC1 +}; + +struct BlobHeader +{ + uint32_t width; + uint32_t height; + PixelFormat format; + // 0.0 for the lossless methods; the uniform quantization step in meters + // for "qpred" blobs (stored uncompressed in the blob, so this is + // available without decompressing anything). Together with + // width/height/format this is everything needed to decode the blob and + // interpret the result. + float quantization_step; +}; + +constexpr size_t bytes_per_pixel(PixelFormat format) +{ + return format == PixelFormat::FLOAT32 ? 4 : 2; +} + +/// Losslessly compress width*height float32 (32FC1) depth pixels into `out` +/// (replacing its contents; existing capacity is reused, so a caller that +/// keeps the vector alive across frames pays no steady-state output +/// allocations). Bit-exact round trip, NaN payloads included. Emits a +/// "dpred" blob, or an "fpred" blob when the image has more than 65536 +/// distinct bit patterns. zstd_level is clamped to [1, 3]. The blob is +/// self-describing. +void encode_depth( + const float * data, uint32_t width, uint32_t height, + std::vector & out, int zstd_level = 1); + +/// LOSSY variant ("qpred" blob): quantizes valid depth to a uniform grid of +/// `step` meters before compressing, reconstructing within +/- step/2 (up +/// to float32 rounding of the reconstructed value, i.e. a few ULPs). +/// Invalid pixels (NaN, +/-inf, <= 0) are preserved as invalid and decode +/// as quiet NaN (REP 118); so does any depth too large for the grid +/// (v/step beyond ~2^31 -- with the default 0.1 mm step that is ~214 km), +/// rather than silently saturating with unbounded error. The quantized +/// level count is not limited to 16 bits, so arbitrarily fine steps and +/// long ranges are supported. Throws std::runtime_error unless step is +/// positive and finite. +void encode_depth_quantized( + const float * data, uint32_t width, uint32_t height, + std::vector & out, float step, int zstd_level = 1); + +/// Compress width*height uint16 (16UC1) depth pixels into `out`. +void encode_depth16( + const uint16_t * data, uint32_t width, uint32_t height, + std::vector & out, int zstd_level = 1); + +/// Parse the self-describing blob header (cheap, no decompression). +/// Use it to size the output buffer and select the decode function. +/// Throws std::runtime_error on malformed or unknown blobs. +BlobHeader read_header(const uint8_t * blob, size_t size); + +/// Decode a FLOAT32 blob into a caller-provided buffer of exactly +/// width*height floats (see read_header). Bit-exact round trip; no output +/// allocation or copy. Throws std::runtime_error on malformed input or a +/// pixel-format mismatch. +void decode_depth(const uint8_t * blob, size_t size, float * out); + +/// Decode a UINT16 blob into a caller-provided buffer of width*height values. +void decode_depth16(const uint8_t * blob, size_t size, uint16_t * out); + +} // namespace depth_codec + +#endif // DEPTH_CODEC_HPP_ diff --git a/depthz_image_transport/src/depthz_publisher.cpp b/depthz_image_transport/src/depthz_publisher.cpp new file mode 100644 index 0000000..b117833 --- /dev/null +++ b/depthz_image_transport/src/depthz_publisher.cpp @@ -0,0 +1,189 @@ +// Copyright (c) 2026, Davide Faconti +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "depthz_image_transport/depthz_publisher.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "depth_codec.hpp" + +namespace depthz_image_transport +{ + +DepthzPublisher::DepthzPublisher() +: logger_(rclcpp::get_logger("DepthzPublisher")) +{ +} + +void DepthzPublisher::advertiseImpl( + image_transport::RequiredInterfaces node_interfaces, + const std::string & base_topic, + rclcpp::QoS custom_qos, + rclcpp::PublisherOptions options) +{ + node_param_interface_ = node_interfaces.get_node_parameters_interface(); + typedef image_transport::SimplePublisherPlugin Base; + Base::advertiseImpl(node_interfaces, base_topic, custom_qos, options); + + // Transport-scoped parameter (e.g. image_raw.depthz.zstd_level). + const unsigned int ns_len = + std::string(node_interfaces.get_node_base_interface()->get_namespace()).length(); + std::string param_base_name = base_topic.substr(ns_len); + // A non-root namespace leaves a leading '/' after the substr (root eats + // it, ns_len == 1), which would yield dot-prefixed parameter names. + if (!param_base_name.empty() && param_base_name.front() == '/') { + param_base_name.erase(0, 1); + } + std::replace(param_base_name.begin(), param_base_name.end(), '/', '.'); + level_param_name_ = param_base_name + "." + getTransportName() + ".zstd_level"; + quantization_param_name_ = param_base_name + "." + getTransportName() + ".quantization"; + + const auto declare = [this]( + const std::string & name, const rclcpp::ParameterValue & default_value, + const rcl_interfaces::msg::ParameterDescriptor & descriptor) { + try { + node_param_interface_->declare_parameter(name, default_value, descriptor); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { + RCLCPP_DEBUG(logger_, "%s was previously declared", name.c_str()); + } + }; + + rcl_interfaces::msg::ParameterDescriptor level_descriptor; + level_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + level_descriptor.description = "zstd level of the depthz entropy stage (1-3)"; + level_descriptor.integer_range = {rcl_interfaces::msg::IntegerRange() + .set__from_value(1) + .set__to_value(3) + .set__step(1)}; + declare(level_param_name_, rclcpp::ParameterValue(1), level_descriptor); + + rcl_interfaces::msg::ParameterDescriptor quantization_descriptor; + quantization_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + quantization_descriptor.description = + "32FC1 quantization step in millimeters; the decoded depth is within " + "+/- half this step of the input. 0.0 = bit-exact lossless. Ignored for " + "16UC1 input (already integer, compressed losslessly)."; + quantization_descriptor.floating_point_range = {rcl_interfaces::msg::FloatingPointRange() + .set__from_value(0.0) + .set__to_value(100.0) + .set__step(0.0)}; + declare(quantization_param_name_, rclcpp::ParameterValue(0.1), quantization_descriptor); +} + +void DepthzPublisher::publish( + const sensor_msgs::msg::Image & message, + const PublisherT & publisher) const +{ + const bool is_32f = message.encoding == sensor_msgs::image_encodings::TYPE_32FC1; + const bool is_16u = message.encoding == sensor_msgs::image_encodings::TYPE_16UC1; + if (!is_32f && !is_16u) { + RCLCPP_ERROR_ONCE( + logger_, "depthz transport supports only 32FC1 and 16UC1 depth images, got '%s'. " + "Use compressed or zstd for other encodings.", message.encoding.c_str()); + return; + } + if (message.is_bigendian) { + RCLCPP_ERROR_ONCE(logger_, "depthz transport does not support big-endian images"); + return; + } + + const int level = static_cast( + node_param_interface_->get_parameter(level_param_name_).as_int()); + const double quantization_mm = + node_param_interface_->get_parameter(quantization_param_name_).as_double(); + + const uint32_t w = message.width; + const uint32_t h = message.height; + const size_t bpp = depth_codec::bytes_per_pixel( + is_32f ? depth_codec::PixelFormat::FLOAT32 : depth_codec::PixelFormat::UINT16); + // Validate step/data size for every message, not just the padded-row + // case below: even when step == w*bpp exactly, a truncated data buffer + // (e.g. a malformed or hand-built message) must not flow into the + // encoder, which trusts width/height/step to size its reads. + if (message.step < w * bpp || message.data.size() < static_cast(message.step) * h) { + RCLCPP_ERROR_ONCE(logger_, "inconsistent image step/size, dropping frame"); + return; + } + + const uint8_t * src = message.data.data(); + // Reused across frames (publish is const, so thread_local rather than a + // member), matching the codec's own scratch-buffer pattern. + static thread_local std::vector packed; + if (message.step != w * bpp) { // rows are padded: repack contiguously + packed.resize(static_cast(w) * h * bpp); + for (uint32_t y = 0; y < h; ++y) { + std::memcpy( + packed.data() + static_cast(y) * w * bpp, + src + static_cast(y) * message.step, + static_cast(w) * bpp); + } + src = packed.data(); + } + + try { + auto compressed = std::make_unique(); + compressed->header = message.header; + compressed->format = message.encoding + "; depthz"; + // The encoder writes the blob directly into the message field. + if (is_32f && quantization_mm > 0.0) { + // Default path: lossy, bounded error of +/- quantization/2. The + // format string advertises the lossiness so bag consumers can tell + // without decoding the blob (the blob itself is self-describing). + char suffix[32]; + std::snprintf(suffix, sizeof(suffix), "; lossy %.3fmm", quantization_mm); + compressed->format += suffix; + depth_codec::encode_depth_quantized( + reinterpret_cast(src), w, h, compressed->data, + static_cast(quantization_mm * 1e-3), level); + } else if (is_32f) { + depth_codec::encode_depth( + reinterpret_cast(src), w, h, compressed->data, level); + } else { + // 16UC1 is already integer depth: quantizing it below its native + // resolution would be a no-op, so it is always compressed losslessly. + depth_codec::encode_depth16( + reinterpret_cast(src), w, h, compressed->data, level); + } + publisher->publish(std::move(compressed)); + } catch (const std::exception & e) { + RCLCPP_ERROR(logger_, "depthz encoding failed: %s", e.what()); + } +} + +} // namespace depthz_image_transport diff --git a/depthz_image_transport/src/depthz_subscriber.cpp b/depthz_image_transport/src/depthz_subscriber.cpp new file mode 100644 index 0000000..bed1621 --- /dev/null +++ b/depthz_image_transport/src/depthz_subscriber.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2026, Davide Faconti +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "depthz_image_transport/depthz_subscriber.hpp" + +#include +#include + +#include +#include + +#include "depth_codec.hpp" + +namespace depthz_image_transport +{ + +DepthzSubscriber::DepthzSubscriber() +: logger_(rclcpp::get_logger("DepthzSubscriber")) +{ +} + +void DepthzSubscriber::internalCallback( + const CompressedImage::ConstSharedPtr & message, + const Callback & user_cb) +{ + try { + // The blob is self-describing: dimensions and pixel format come from + // its header, and the decoder writes directly into image->data (no + // intermediate buffer, no copy). + const depth_codec::BlobHeader header = + depth_codec::read_header(message->data.data(), message->data.size()); + const bool is_16u = header.format == depth_codec::PixelFormat::UINT16; + const size_t bpp = depth_codec::bytes_per_pixel(header.format); + + auto image = std::make_shared(); + image->header = message->header; + image->width = header.width; + image->height = header.height; + image->is_bigendian = false; + image->encoding = is_16u ? + sensor_msgs::image_encodings::TYPE_16UC1 : sensor_msgs::image_encodings::TYPE_32FC1; + image->step = header.width * bpp; + image->data.resize(static_cast(header.width) * header.height * bpp); + if (is_16u) { + depth_codec::decode_depth16( + message->data.data(), message->data.size(), + reinterpret_cast(image->data.data())); + } else { + depth_codec::decode_depth( + message->data.data(), message->data.size(), + reinterpret_cast(image->data.data())); + } + user_cb(image); + } catch (const std::exception & e) { + RCLCPP_ERROR(logger_, "depthz decoding failed: %s", e.what()); + } +} + +} // namespace depthz_image_transport diff --git a/depthz_image_transport/src/manifest.cpp b/depthz_image_transport/src/manifest.cpp new file mode 100644 index 0000000..37dffe5 --- /dev/null +++ b/depthz_image_transport/src/manifest.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2026, Davide Faconti +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include "depthz_image_transport/depthz_publisher.hpp" +#include "depthz_image_transport/depthz_subscriber.hpp" + +PLUGINLIB_EXPORT_CLASS( + depthz_image_transport::DepthzPublisher, + image_transport::PublisherPlugin) + +PLUGINLIB_EXPORT_CLASS( + depthz_image_transport::DepthzSubscriber, + image_transport::SubscriberPlugin) diff --git a/depthz_image_transport/test/test_depth_codec.cpp b/depthz_image_transport/test/test_depth_codec.cpp new file mode 100644 index 0000000..7573976 --- /dev/null +++ b/depthz_image_transport/test/test_depth_codec.cpp @@ -0,0 +1,558 @@ +// Copyright (c) 2026, Davide Faconti +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +#include +#include +#include +#include +#include + +#include "depth_codec.hpp" + +namespace +{ + +float from_bits(uint32_t u) +{ + float f; + std::memcpy(&f, &u, 4); + return f; +} + +// Overload shims so the round-trip harness below is pixel-format generic. +void codec_encode( + const float * data, uint32_t w, uint32_t h, std::vector & out, int level) +{ + depth_codec::encode_depth(data, w, h, out, level); +} +void codec_encode( + const uint16_t * data, uint32_t w, uint32_t h, std::vector & out, int level) +{ + depth_codec::encode_depth16(data, w, h, out, level); +} +void codec_decode(const uint8_t * blob, size_t size, float * out) +{ + depth_codec::decode_depth(blob, size, out); +} +void codec_decode(const uint8_t * blob, size_t size, uint16_t * out) +{ + depth_codec::decode_depth16(blob, size, out); +} + +// The documented qpred contract: +/- step/2 plus a few float ULPs of the +// value (reconstruction and the step itself are float-rounded). +float qpred_tol(float step, float v) +{ + return 0.5f * step + 4.0f * std::numeric_limits::epsilon() * v; +} + +template +constexpr depth_codec::PixelFormat expected_format() +{ + return std::is_same_v ? + depth_codec::PixelFormat::FLOAT32 : depth_codec::PixelFormat::UINT16; +} + +// Encode + read_header + decode into a preallocated buffer, and require a +// BIT-EXACT payload (memcmp over the raw bytes, so NaN payloads, +/-inf and +// -0.0 are checked too). +template +void expect_roundtrip(const std::vector & img, uint32_t w, uint32_t h) +{ + std::vector blob; // reused across levels: capacity-reuse path + for (int level : {1, 3}) { + codec_encode(img.data(), w, h, blob, level); + const depth_codec::BlobHeader header = + depth_codec::read_header(blob.data(), blob.size()); + ASSERT_EQ(header.width, w); + ASSERT_EQ(header.height, h); + ASSERT_EQ(header.format, expected_format()); + std::vector back(img.size()); + codec_decode(blob.data(), blob.size(), back.data()); + if (!img.empty()) { + ASSERT_EQ(0, std::memcmp(back.data(), img.data(), img.size() * sizeof(T))); + } + } +} + +std::vector make_depth_frame(uint32_t w, uint32_t h) +{ + std::vector img(static_cast(w) * h); + for (uint32_t y = 0; y < h; ++y) { + for (uint32_t x = 0; x < w; ++x) { + const int q = 1 + static_cast((x * 131 + y * 17) % 5000); + img[static_cast(y) * w + x] = 10100.0f / (static_cast(q) + 1009.0f); + } + } + // Canonical-NaN hole, like an invalid region from a depth camera. + for (uint32_t y = h / 4; y < h / 2; ++y) { + for (uint32_t x = w / 3; x < 2 * w / 3; ++x) { + img[static_cast(y) * w + x] = from_bits(0x7FC00000u); + } + } + return img; +} + +std::vector make_depth_frame16(uint32_t w, uint32_t h) +{ + std::vector img(static_cast(w) * h); + for (uint32_t y = 0; y < h; ++y) { + for (uint32_t x = 0; x < w; ++x) { + img[static_cast(y) * w + x] = + static_cast(300 + (x * 131 + y * 17) % 5000); + } + } + // Zero hole, the 16UC1 invalid-pixel convention. + for (uint32_t y = h / 4; y < h / 2; ++y) { + for (uint32_t x = w / 3; x < 2 * w / 3; ++x) { + img[static_cast(y) * w + x] = 0; + } + } + return img; +} + +// ---- helpers to hand-craft adversarial blobs -------------------------------- +// The public blob layout ('D''P''C''1' | u8 name_len | name | i32 level | +// u32 w | u32 h | payload) is documented in depth_codec.hpp; building it by +// hand here (rather than via encode_depth/encode_depth16) is how the +// decoder's robustness against malformed/hostile input is exercised. +void append_le(std::vector & v, uint64_t x, int nbytes) +{ + for (int i = 0; i < nbytes; ++i) { + v.push_back(static_cast(x >> (8 * i))); + } +} + +std::vector make_blob_header(const char * method, uint32_t level, uint32_t w, uint32_t h) +{ + std::vector blob; + const char magic[4] = {'D', 'P', 'C', '1'}; + blob.insert(blob.end(), magic, magic + 4); + const size_t name_len = std::strlen(method); + blob.push_back(static_cast(name_len)); + blob.insert(blob.end(), method, method + name_len); + append_le(blob, level, 4); + append_le(blob, w, 4); + append_le(blob, h, 4); + return blob; +} + +// A real zstd frame that compresses `plain` verbatim (so its declared +// content size is honest -- only the decompressed *content* is adversarial). +std::vector zstd_compress_raw(const std::vector & plain) +{ + const size_t bound = ZSTD_compressBound(plain.size()); + std::vector out(bound); + const size_t k = ZSTD_compress(out.data(), bound, plain.data(), plain.size(), 1); + out.resize(k); + return out; +} + +// A minimal, hand-built zstd frame header (magic + Single_Segment frame +// descriptor + an 8-byte Frame_Content_Size field) that declares an +// enormous decompressed size. It never needs to decompress successfully -- +// zstd_unpack must reject it from the header alone, before allocating +// anything sized off that claim. +std::vector fake_huge_zstd_frame() +{ + std::vector b; + append_le(b, 0xFD2FB528u, 4); // zstd magic number + // Frame_Header_Descriptor: Frame_Content_Size_flag=3 (8-byte field), + // Single_Segment_flag=1 (so Window_Descriptor is omitted). + b.push_back(static_cast((3u << 6) | (1u << 5))); + append_le(b, 1ULL << 40, 8); // ~1 TiB claimed content size + b.push_back(0); + b.push_back(0); + return b; +} + +} // namespace + +TEST(DepthCodec, depth_frame_roundtrip_32f) +{ + expect_roundtrip(make_depth_frame(97, 53), 97, 53); + expect_roundtrip(make_depth_frame(640, 480), 640, 480); +} + +TEST(DepthCodec, depth_frame_roundtrip_16u) +{ + expect_roundtrip(make_depth_frame16(97, 53), 97, 53); + expect_roundtrip(make_depth_frame16(640, 480), 640, 480); +} + +TEST(DepthCodec, special_values_32f) +{ + const uint32_t bits[] = { + 0x00000000u, // +0.0 + 0x80000000u, // -0.0 + 0x7F800000u, // +inf + 0xFF800000u, // -inf + 0x7FC00000u, // canonical quiet NaN + 0x7FC00001u, // NaN, non-canonical payload + 0x7F800001u, // signalling NaN + 0xFFC00000u, // negative NaN + 0xFFFFFFFFu, // NaN, all ones + 0x00000001u, // smallest positive denormal + 0x807FFFFFu, // negative denormal + 0x7F7FFFFFu, // FLT_MAX + 0xFF7FFFFFu, // -FLT_MAX + 0x00800000u, // FLT_MIN + 0xBF800000u, // -1.0 + 0x3F800000u, // 1.0 + }; + const uint32_t w = 8; + const uint32_t h = 8; + std::vector img(static_cast(w) * h); + for (size_t i = 0; i < img.size(); ++i) { + img[i] = from_bits(bits[i % std::size(bits)]); + } + expect_roundtrip(img, w, h); +} + +TEST(DepthCodec, special_values_16u) +{ + // Extremes and large jumps (exercises the mod-2^16 residual wrap). + const uint16_t vals[] = {0, 65535, 1, 65534, 32768, 32767, 0, 65535}; + const uint32_t w = 8; + const uint32_t h = 8; + std::vector img(static_cast(w) * h); + for (size_t i = 0; i < img.size(); ++i) { + img[i] = vals[i % std::size(vals)]; + } + expect_roundtrip(img, w, h); +} + +TEST(DepthCodec, dictionary_overflow_falls_back) +{ + // > 65536 distinct bit patterns forces the dictionary-free fpred fallback + // (32FC1 only: 16UC1 cannot overflow by construction). Still bit-exact. + std::mt19937 rng(12345); + const uint32_t w = 512; + const uint32_t h = 256; + std::vector img(static_cast(w) * h); + for (auto & f : img) { + f = from_bits(rng()); + } + expect_roundtrip(img, w, h); +} + +TEST(DepthCodec, lossless_method_selection) +{ + // The blob method name records which path encode_depth took: dictionary + // ("dpred") for low-cardinality images, "fpred" past 65536 distinct + // values. The name starts at byte 5 (after magic + name_len). + const auto method_of = [](const std::vector & blob) { + return std::string(reinterpret_cast(blob.data() + 5), blob[4]); + }; + + std::vector blob; + const auto few = make_depth_frame(200, 100); // ~5000 distinct values + depth_codec::encode_depth(few.data(), 200, 100, blob, 1); + EXPECT_EQ(method_of(blob), "dpred"); + + // Full-precision synthetic "stereo" depth: every pixel distinct. + std::vector many(400 * 300); + for (size_t i = 0; i < many.size(); ++i) { + many[i] = 0.15f + static_cast(i) * 1e-5f; + } + depth_codec::encode_depth(many.data(), 400, 300, blob, 1); + EXPECT_EQ(method_of(blob), "fpred"); + std::vector back(many.size()); + depth_codec::decode_depth(blob.data(), blob.size(), back.data()); + EXPECT_EQ(0, std::memcmp(back.data(), many.data(), many.size() * 4)); +} + +TEST(DepthCodec, degenerate_shapes) +{ + expect_roundtrip(std::vector(64 * 32, 1.25f), 64, 32); // constant + expect_roundtrip({0.5f}, 1, 1); + expect_roundtrip({1.f, 2.f, 3.f, 4.f, 5.f}, 5, 1); + expect_roundtrip({1.f, 2.f, 3.f, 4.f, 5.f, 6.f, 7.f}, 1, 7); + expect_roundtrip(std::vector(64 * 32, 1250), 64, 32); + expect_roundtrip({1234}, 1, 1); + expect_roundtrip({1, 2, 3, 4, 5}, 5, 1); + expect_roundtrip({1, 2, 3, 4, 5, 6, 7}, 1, 7); +} + +// Every w/h combination with n == 0 must encode and decode cleanly for +// both pixel formats (no OOB access, no null-pointer memcpy) -- ASan/UBSan +// must stay silent on all of these. +TEST(DepthCodec, degenerate_zero_dimension) +{ + expect_roundtrip({}, 0, 0); + expect_roundtrip({}, 0, 9); + expect_roundtrip({}, 9, 0); + expect_roundtrip({}, 0, 0); + expect_roundtrip({}, 0, 9); + expect_roundtrip({}, 9, 0); +} + +TEST(DepthCodec, malformed_input_throws) +{ + const uint8_t junk[8] = {'X', 'X', 'X', 'X', 0, 0, 0, 0}; + EXPECT_THROW(depth_codec::read_header(junk, sizeof(junk)), std::runtime_error); + + // Truncated valid blob must throw, not crash. + const float one = 1.0f; + std::vector blob; + depth_codec::encode_depth(&one, 1, 1, blob, 1); + float out = 0.0f; + EXPECT_THROW(depth_codec::decode_depth(blob.data(), blob.size() / 2, &out), + std::runtime_error); +} + +TEST(DepthCodec, pixel_format_mismatch_throws) +{ + const float onef = 1.0f; + const uint16_t oneu = 1; + std::vector blob32; + std::vector blob16; + depth_codec::encode_depth(&onef, 1, 1, blob32, 1); + depth_codec::encode_depth16(&oneu, 1, 1, blob16, 1); + + uint16_t out16 = 0; + float out32 = 0.0f; + EXPECT_THROW(depth_codec::decode_depth16(blob32.data(), blob32.size(), &out16), + std::runtime_error); + EXPECT_THROW(depth_codec::decode_depth(blob16.data(), blob16.size(), &out32), + std::runtime_error); +} + +// A zstd frame whose header claims an enormous decompressed size must be +// rejected before any allocation sized off that claim, for both +// pixel-format decode paths. +TEST(DepthCodec, decoder_rejects_huge_zstd_content_size) +{ + const auto payload = fake_huge_zstd_frame(); + + std::vector blob32 = make_blob_header("dpred", 1, 4, 4); + blob32.insert(blob32.end(), payload.begin(), payload.end()); + std::vector out32(16); + EXPECT_THROW(depth_codec::decode_depth(blob32.data(), blob32.size(), out32.data()), + std::runtime_error); + + std::vector blob16 = make_blob_header("dpred16", 1, 4, 4); + blob16.insert(blob16.end(), payload.begin(), payload.end()); + std::vector out16(16); + EXPECT_THROW(depth_codec::decode_depth16(blob16.data(), blob16.size(), out16.data()), + std::runtime_error); +} + +// A dpred payload whose mode byte is neither 0 (raw fallback) nor 1 +// (dictionary) must be rejected, not silently treated as a known mode. +TEST(DepthCodec, decoder_rejects_invalid_mode_byte) +{ + const std::vector plain = {5}; // mode 5: neither 0 nor 1 + const auto payload = zstd_compress_raw(plain); + std::vector blob = make_blob_header("dpred", 1, 1, 1); + blob.insert(blob.end(), payload.begin(), payload.end()); + float out = 0.0f; + EXPECT_THROW(depth_codec::decode_depth(blob.data(), blob.size(), &out), std::runtime_error); +} + +// Dictionary mode (byte 0 == 1) with an idx_bytes field other than 2 must +// be rejected explicitly. +TEST(DepthCodec, decoder_rejects_invalid_idx_bytes) +{ + std::vector plain = {1, 3, 0, 0, 0, 0}; // mode=1, idx_bytes=3 (invalid), ds=0 + const auto payload = zstd_compress_raw(plain); + std::vector blob = make_blob_header("dpred", 1, 0, 0); + blob.insert(blob.end(), payload.begin(), payload.end()); + float out = 0.0f; + EXPECT_THROW(depth_codec::decode_depth(blob.data(), blob.size(), &out), std::runtime_error); +} + +// ---- qpred (lossy, configurable quantization) -------------------------------- + +TEST(DepthCodec, qpred_error_bound_and_invalid_pixels) +{ + // Realistic stereo-like depth: every pixel distinct, range 0.15-10 m, a + // NaN hole plus assorted invalid values. At step = 0.1 mm the range + // needs ~100k quantization levels -- past 16 bits, exercising the varint + // path -- and every valid pixel must reconstruct within +/- step/2. + const uint32_t w = 200; + const uint32_t h = 150; + const float step = 0.0001f; + std::mt19937 rng(999); + std::uniform_real_distribution depth(0.15f, 10.0f); + std::vector img(static_cast(w) * h); + for (auto & f : img) { + f = depth(rng); + } + img[0] = from_bits(0x7FC00000u); // quiet NaN + img[1] = std::numeric_limits::infinity(); + img[2] = -std::numeric_limits::infinity(); + img[3] = -1.5f; // negative: invalid + img[4] = 0.0f; // zero: invalid + for (size_t i = w; i < 3 * w; ++i) { // a NaN hole spanning rows + img[i] = from_bits(0x7FC00000u); + } + + std::vector blob; + depth_codec::encode_depth_quantized(img.data(), w, h, blob, step, 1); + + const depth_codec::BlobHeader header = depth_codec::read_header(blob.data(), blob.size()); + ASSERT_EQ(header.width, w); + ASSERT_EQ(header.height, h); + ASSERT_EQ(header.format, depth_codec::PixelFormat::FLOAT32); + ASSERT_FLOAT_EQ(header.quantization_step, step); + + std::vector back(img.size()); + depth_codec::decode_depth(blob.data(), blob.size(), back.data()); + for (size_t i = 0; i < img.size(); ++i) { + const bool valid_in = std::isfinite(img[i]) && img[i] > 0.0f; + if (valid_in) { + ASSERT_TRUE(std::isfinite(back[i])) << "pixel " << i; + ASSERT_NEAR(back[i], img[i], qpred_tol(step, img[i])) << "pixel " << i; + } else { + ASSERT_TRUE(std::isnan(back[i])) << "pixel " << i; // REP 118 + } + } +} + +TEST(DepthCodec, qpred_lossless_header_reports_no_quantization) +{ + const auto img = make_depth_frame(64, 48); + std::vector blob; + depth_codec::encode_depth(img.data(), 64, 48, blob, 1); + const depth_codec::BlobHeader header = depth_codec::read_header(blob.data(), blob.size()); + ASSERT_FLOAT_EQ(header.quantization_step, 0.0f); +} + +TEST(DepthCodec, qpred_degenerate_shapes) +{ + const float step = 0.001f; + for (const auto [w, h] : {std::pair{0, 0}, {0, 9}, {9, 0}, {1, 1}}) { + std::vector img(static_cast(w) * h, 1.25f); + std::vector blob; + depth_codec::encode_depth_quantized(img.data(), w, h, blob, step, 1); + std::vector back(img.size()); + depth_codec::decode_depth(blob.data(), blob.size(), back.data()); + for (const float v : back) { + ASSERT_NEAR(v, 1.25f, qpred_tol(step, 1.25f)); + } + } +} + +TEST(DepthCodec, qpred_rejects_invalid_encode_step) +{ + const float one = 1.0f; + std::vector blob; + EXPECT_THROW( + depth_codec::encode_depth_quantized(&one, 1, 1, blob, 0.0f, 1), std::runtime_error); + EXPECT_THROW( + depth_codec::encode_depth_quantized(&one, 1, 1, blob, -0.001f, 1), std::runtime_error); + EXPECT_THROW( + depth_codec::encode_depth_quantized( + &one, 1, 1, blob, std::numeric_limits::quiet_NaN(), 1), std::runtime_error); +} + +// Hand-crafted qpred blobs: payload = f32 step (uncompressed) | zstd(varints). +namespace +{ +std::vector make_qpred_blob( + uint32_t w, uint32_t h, const std::vector & step_bytes, + const std::vector & varints) +{ + std::vector blob = make_blob_header("qpred", 1, w, h); + blob.insert(blob.end(), step_bytes.begin(), step_bytes.end()); + const auto payload = zstd_compress_raw(varints); + blob.insert(blob.end(), payload.begin(), payload.end()); + return blob; +} +} // namespace + +TEST(DepthCodec, qpred_rejects_malformed_blobs) +{ + const std::vector good_step = {0x6F, 0x12, 0x83, 0x3A}; // 0.001f LE + float out[4] = {}; + + // Step = 0.0 -> rejected by read_header already (header must be sane). + { + const auto blob = make_qpred_blob(1, 1, {0, 0, 0, 0}, {0x00}); + EXPECT_THROW(depth_codec::read_header(blob.data(), blob.size()), std::runtime_error); + EXPECT_THROW(depth_codec::decode_depth(blob.data(), blob.size(), out), std::runtime_error); + } + // Step = NaN -> rejected. + { + const auto blob = make_qpred_blob(1, 1, {0x00, 0x00, 0xC0, 0x7F}, {0x00}); + EXPECT_THROW(depth_codec::read_header(blob.data(), blob.size()), std::runtime_error); + } + // Payload shorter than the step field -> rejected. + { + std::vector blob = make_blob_header("qpred", 1, 1, 1); + blob.insert(blob.end(), {0x6F, 0x12}); + EXPECT_THROW(depth_codec::read_header(blob.data(), blob.size()), std::runtime_error); + } + // Truncated varint stream (2x2 image, only 1 varint) -> rejected. + { + const auto blob = make_qpred_blob(2, 2, good_step, {0x02}); + EXPECT_THROW(depth_codec::decode_depth(blob.data(), blob.size(), out), std::runtime_error); + } + // Trailing bytes after the varint stream -> rejected. + { + const auto blob = make_qpred_blob(1, 1, good_step, {0x02, 0x00}); + EXPECT_THROW(depth_codec::decode_depth(blob.data(), blob.size(), out), std::runtime_error); + } + // Varint continuation past the 32-bit zigzag range -> rejected. + { + const auto blob = make_qpred_blob( + 1, 1, good_step, {0x80, 0x80, 0x80, 0x80, 0x80, 0x01}); + EXPECT_THROW(depth_codec::decode_depth(blob.data(), blob.size(), out), std::runtime_error); + } + // Huge declared zstd content size -> rejected before allocation. + { + std::vector blob = make_blob_header("qpred", 1, 1, 1); + blob.insert(blob.end(), good_step.begin(), good_step.end()); + const auto payload = fake_huge_zstd_frame(); + blob.insert(blob.end(), payload.begin(), payload.end()); + EXPECT_THROW(depth_codec::decode_depth(blob.data(), blob.size(), out), std::runtime_error); + } +} + +// A valid depth whose v/step exceeds the representable code range must +// decode as NaN (invalid), never as a silently saturated value with +// unbounded error. Neighbouring on-grid pixels are unaffected. +TEST(DepthCodec, qpred_off_grid_depth_becomes_invalid) +{ + const float step = 1e-9f; // absurdly fine: 10 m needs 1e10 codes >> 2^31 + std::vector img = {10.0f, 1.0f, 2.0f, 0.5f}; // 10.0 is off-grid + std::vector blob; + depth_codec::encode_depth_quantized(img.data(), 2, 2, blob, step, 1); + std::vector back(4); + depth_codec::decode_depth(blob.data(), blob.size(), back.data()); + EXPECT_TRUE(std::isnan(back[0])); // off-grid -> invalid, NOT ~2.147 m + for (int i = 1; i < 4; ++i) { + ASSERT_NEAR(back[i], img[i], qpred_tol(step, img[i])) << i; + } +} diff --git a/image_transport_plugins/package.xml b/image_transport_plugins/package.xml index 3870af7..da66cad 100644 --- a/image_transport_plugins/package.xml +++ b/image_transport_plugins/package.xml @@ -25,6 +25,7 @@ compressed_depth_image_transport compressed_image_transport + depthz_image_transport theora_image_transport zstd_image_transport From 792f874400fef16e36e0a5b84435643bc09d5626 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 22 Jul 2026 13:23:39 +0200 Subject: [PATCH 2/2] Update depthz_image_transport/README.md MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- depthz_image_transport/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthz_image_transport/README.md b/depthz_image_transport/README.md index 4e83697..3ab9af3 100644 --- a/depthz_image_transport/README.md +++ b/depthz_image_transport/README.md @@ -44,7 +44,7 @@ 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 +parameter (in RViz 2: the *Transport Hint* dropdown of the Image/Camera display): ```bash