From 15134dc12ec5716857a0aa14c381aa7b13364b97 Mon Sep 17 00:00:00 2001 From: cesar Date: Tue, 8 Sep 2026 09:17:36 -0300 Subject: [PATCH] add a depth output MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Publish sl::MEASURE::DEPTH as float32 [H, W] millimetres on every frame that carries image_bgr, with the intrinsics, frame_id and timestamps the other outputs carry. The map is copied out rather than published in place: sl::Mat rows can be padded, so the buffer is not necessarily H * W floats. inf and nan are cleared to 0 in the same pass, matching the reference driver — a hole is meaningful and a consumer cannot back-project a non-finite value. Consumers that need 3D can now back-project depth with the published intrinsics instead of receiving a point cloud. Also drops the stale objects entry from the dataflow outputs, missed when detection was removed. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c --- AGENTS.md | 4 ++-- README.md | 5 +++++ dataflow.yml | 4 +++- src/main.cc | 47 +++++++++++++++++++++++++++++++++++++++++++++++ src/zed/camera.cc | 5 +++++ src/zed/camera.h | 2 ++ 6 files changed, 64 insertions(+), 3 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 457edd5..833131f 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -36,9 +36,9 @@ Camera behavior is configured entirely through environment variables (see README ## Architecture - **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`). -- **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + point cloud), calibration, and close. Exposes `handle()` for engine access. +- **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + depth + point cloud), calibration, device timestamp, and close. Exposes `handle()` for engine access. - **`src/zed/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. -- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr`, `point_cloud` and `camera_info`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. +- **`src/main.cc`** — Dora event loop. Parses `ZED_PC_MODE` (`FULL`/`HALF`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()`, then sends `image_bgr`, `depth`, `point_cloud` and `camera_info`. `pack_depth` copies the depth map into a tightly packed `[H, W]` float buffer (sl::Mat rows can be padded) and clears `inf`/`nan` to `0`. `set_intrinsics` and `set_frame_stamps` stamp every output of a frame with the same intrinsics, `frame_id` and the two timestamps (`device_timestamp_ns`, `host_timestamp_ns`; both Unix-epoch nanoseconds). Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension. Handles reconnection on grab failure. - **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue. - **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. - **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format, pinned with `tag:` — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. diff --git a/README.md b/README.md index 2b6cde3..1e585cb 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,7 @@ nodes: tick: dora/timer/millis/100 outputs: - image_bgr + - depth - point_cloud - camera_info ``` @@ -120,9 +121,13 @@ All behavior is controlled via environment variables. | Output | Type | Description | |---|---|---| | `image_bgr` | `uint8 [H, W, 3]` | BGR image. Metadata: `shape`, `encoding=bgr8`, intrinsics, timestamps. | +| `depth` | `float32 [H, W]` | Per-pixel depth in millimetres, published on every frame `image_bgr` is. Metadata: `dtype`, `layout=HW`, `shape`, `units=mm`, intrinsics, timestamps. `H` and `W` match `image_bgr` for the same `frame_id`. Missing depth is `0` — the payload contains no `inf` and no `nan`. Holes are not interpolated unless `ZED_DEPTH_FILL` is set. | | `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics, timestamps. | | `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients, timestamps. | +Consumers that need 3D back-project `depth` with the published intrinsics rather than +consuming `point_cloud`; the point cloud is not a prerequisite for 3D. + ### Timestamps Every output carries both clocks, as dora `timestamp` metadata parameters: diff --git a/dataflow.yml b/dataflow.yml index 5d7d4c4..df9c3e9 100644 --- a/dataflow.yml +++ b/dataflow.yml @@ -8,7 +8,9 @@ nodes: tick: dora/timer/millis/100 outputs: - image_bgr - - objects + - depth + - point_cloud + - camera_info - id: plot git: https://github.com/dora-rs/dora-hub.git diff --git a/src/main.cc b/src/main.cc index edb3ea6..9cfe22e 100644 --- a/src/main.cc +++ b/src/main.cc @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -58,6 +59,29 @@ void set_frame_stamps(Metadata& metadata, int64_t frame_id, int64_t device_ns, i metadata.set_timestamp("host_timestamp_ns", host_ns); } +// Copy the depth map out into a tightly packed [H, W] buffer of millimetres. +// +// The rows of an sl::Mat can be padded, so the buffer is not necessarily H * W +// floats and cannot be published in place. inf and nan mark depth the SDK could +// not measure; they are cleared to 0 here, as the reference driver does, because +// a hole is meaningful and a consumer cannot back-project a non-finite value. +std::vector pack_depth(sl::Mat& depth_mat) { + const int height = depth_mat.getHeight(); + const int width = depth_mat.getWidth(); + const size_t step = depth_mat.getStep(sl::MEM::CPU); + const float* src = depth_mat.getPtr(); + + std::vector out(static_cast(height) * static_cast(width)); + for (int y = 0; y < height; y++) { + const float* row = src + static_cast(y) * step; + float* dst = out.data() + static_cast(y) * static_cast(width); + for (int x = 0; x < width; x++) { + dst[x] = std::isfinite(row[x]) ? row[x] : 0.0f; + } + } + return out; +} + int64_t host_timestamp_ns() { return std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()) @@ -100,6 +124,7 @@ int main() { } sl::Mat& image_mat = camera.image(); + sl::Mat& depth_mat = camera.depth(); sl::Mat& point_cloud_mat = camera.point_cloud(); int width = image_mat.getWidth(); @@ -132,6 +157,28 @@ int main() { std::cerr << "Error sending image_bgr: " << std::string(image_result.error) << std::endl; } + // --- depth --- + int depth_width = depth_mat.getWidth(); + int depth_height = depth_mat.getHeight(); + + std::vector depth_buf = pack_depth(depth_mat); + rust::Slice depth_slice{ + reinterpret_cast(depth_buf.data()), depth_buf.size() * sizeof(float)}; + + auto depth_metadata = new_metadata(); + depth_metadata->set_string("dtype", "float32"); + depth_metadata->set_string("layout", "HW"); + depth_metadata->set_string("units", "mm"); + depth_metadata->set_list_int("shape", make_vector_int({depth_height, depth_width})); + set_intrinsics(*depth_metadata, calib); + set_frame_stamps(*depth_metadata, current_frame_id, device_ns, host_ns); + + auto depth_result = send_output_with_metadata( + dora_node.send_output, "depth", depth_slice, std::move(depth_metadata)); + if (!std::string(depth_result.error).empty()) { + std::cerr << "Error sending depth: " << std::string(depth_result.error) << std::endl; + } + // --- point_cloud --- int pc_width = point_cloud_mat.getWidth(); int pc_height = point_cloud_mat.getHeight(); diff --git a/src/zed/camera.cc b/src/zed/camera.cc index c89b502..45fbe78 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -119,6 +119,7 @@ bool Camera::grab() { return false; } zed_.retrieveImage(image_mat_, sl::VIEW::LEFT); + zed_.retrieveMeasure(depth_mat_, sl::MEASURE::DEPTH); zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA); return true; } @@ -149,6 +150,10 @@ sl::Mat& Camera::image() { return image_mat_; } +sl::Mat& Camera::depth() { + return depth_mat_; +} + sl::Mat& Camera::point_cloud() { return point_cloud_mat_; } diff --git a/src/zed/camera.h b/src/zed/camera.h index 9223562..240299d 100644 --- a/src/zed/camera.h +++ b/src/zed/camera.h @@ -38,6 +38,7 @@ public: Calibration calibration() const; int64_t device_timestamp_ns(); sl::Mat& image(); + sl::Mat& depth(); sl::Mat& point_cloud(); sl::Camera& handle(); @@ -47,6 +48,7 @@ private: sl::InitParameters init_params_; sl::RuntimeParameters runtime_params_; sl::Mat image_mat_; + sl::Mat depth_mat_; sl::Mat point_cloud_mat_; };