diff --git a/AGENTS.md b/AGENTS.md index 6a4f960..ff129ca 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -38,7 +38,7 @@ Camera behavior is configured entirely through environment variables (see README - **`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 + 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. `parse_depth_mode` runs in `Config::from_env()`, not in the constructor, so an unrecognised `ZED_DEPTH_MODE` exits before the node joins the dataflow rather than falling back to a default. -- **`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. +- **`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` and `depth`, plus `point_cloud` when `ZED_POINT_CLOUD` is on and `camera_info` on the first frame of a camera session (`camera_info_pending`, re-armed on reconnection). `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 65f6d9b..fa7bfbc 100644 --- a/README.md +++ b/README.md @@ -111,9 +111,19 @@ against, so depth quality on the robot matches what it expects. ### Point cloud +Off by default. With `ZED_POINT_CLOUD` unset the XYZRGBA measure is not retrieved at all +and no `point_cloud` output is published — at 1080p a full cloud is roughly 14.7 MB per +frame, and retrieving it costs capture time whether or not anything consumes it. +Consumers that need 3D back-project `depth` with the published intrinsics instead. + +| Variable | Default | Description | +|---|---|---| +| `ZED_POINT_CLOUD` | `false` | Retrieve and publish the point cloud | +| `ZED_PC_MODE` | `FULL` | Only applies when `ZED_POINT_CLOUD` is on | + | `ZED_PC_MODE` | Shape | Description | |---|---|---| -| `FULL` (default) | `[H, W, 4]` | Full resolution grid (~14.7 MB/frame at 720p) | +| `FULL` (default) | `[H, W, 4]` | Full resolution grid (~14.7 MB/frame at 1080p) | | `HALF` | `[H/2, W/2, 4]` | Stride-2 downsample (~4× smaller) | ## Inputs @@ -128,8 +138,8 @@ against, so depth quality on the robot matches what it expects. |---|---|---| | `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. | +| `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud, only when `ZED_POINT_CLOUD` is on. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics, timestamps. | +| `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients, timestamps. Published **once per camera session** — with the first frame after the camera opens, and again after a reconnection — not on every frame, since the intrinsics are fixed for a given camera and resolution. | 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. diff --git a/src/main.cc b/src/main.cc index 26d545c..00880da 100644 --- a/src/main.cc +++ b/src/main.cc @@ -104,6 +104,12 @@ int main() { std::string pc_mode_str = util::get_env_str("ZED_PC_MODE", "FULL"); PcMode pc_mode = (pc_mode_str == "HALF") ? PcMode::HALF : PcMode::FULL; + // fx, fy, cx, cy and the distortion coefficients are fixed for a given camera + // and resolution, so they are published once per camera session rather than on + // every frame. It goes out with the first frame of the session so that it + // carries that frame's timestamps like every other output. + bool camera_info_pending = true; + int64_t frame_id = 0; while (true) { auto event = dora_node.events->next(); @@ -122,12 +128,12 @@ int main() { camera.close(); camera.open(); calib = camera.calibration(); + camera_info_pending = true; continue; } 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(); int height = image_mat.getHeight(); @@ -181,66 +187,72 @@ int main() { 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(); + if (config.point_cloud) { + // --- point_cloud --- + sl::Mat& point_cloud_mat = camera.point_cloud(); + int pc_width = point_cloud_mat.getWidth(); + int pc_height = point_cloud_mat.getHeight(); - std::vector pc_buf; - const uint8_t* pc_data_ptr = nullptr; - size_t pc_data_size = 0; - auto pc_metadata = new_metadata(); - pc_metadata->set_string("dtype", "float32"); - pc_metadata->set_string("channels", "XYZRGBA"); - pc_metadata->set_string("units", "mm"); - set_intrinsics(*pc_metadata, calib); - set_frame_stamps(*pc_metadata, current_frame_id, device_ns, host_ns); + std::vector pc_buf; + const uint8_t* pc_data_ptr = nullptr; + size_t pc_data_size = 0; + auto pc_metadata = new_metadata(); + pc_metadata->set_string("dtype", "float32"); + pc_metadata->set_string("channels", "XYZRGBA"); + pc_metadata->set_string("units", "mm"); + set_intrinsics(*pc_metadata, calib); + set_frame_stamps(*pc_metadata, current_frame_id, device_ns, host_ns); - if (pc_mode == PcMode::HALF) { - int hw = pc_width / 2, hh = pc_height / 2; - pc_buf.resize(static_cast(hh * hw * 4)); - const sl::float4* src = point_cloud_mat.getPtr(); - for (int y = 0; y < hh; y++) { - for (int x = 0; x < hw; x++) { - const sl::float4& pt = src[(y * 2) * pc_width + (x * 2)]; - float* dst = pc_buf.data() + (y * hw + x) * 4; - dst[0] = pt.x; dst[1] = pt.y; dst[2] = pt.z; dst[3] = pt.w; + if (pc_mode == PcMode::HALF) { + int hw = pc_width / 2, hh = pc_height / 2; + pc_buf.resize(static_cast(hh * hw * 4)); + const sl::float4* src = point_cloud_mat.getPtr(); + for (int y = 0; y < hh; y++) { + for (int x = 0; x < hw; x++) { + const sl::float4& pt = src[(y * 2) * pc_width + (x * 2)]; + float* dst = pc_buf.data() + (y * hw + x) * 4; + dst[0] = pt.x; dst[1] = pt.y; dst[2] = pt.z; dst[3] = pt.w; + } } + pc_metadata->set_string("layout", "HWC"); + pc_metadata->set_list_int("shape", make_shape(hh, hw, 4)); + pc_data_ptr = reinterpret_cast(pc_buf.data()); + pc_data_size = pc_buf.size() * sizeof(float); + } else { + // FULL — send raw sl::Mat pointer directly (no copy) + pc_data_size = static_cast(pc_width * pc_height * sizeof(sl::float4)); + pc_data_ptr = reinterpret_cast(point_cloud_mat.getPtr()); + pc_metadata->set_string("layout", "HWC"); + pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4)); } - pc_metadata->set_string("layout", "HWC"); - pc_metadata->set_list_int("shape", make_shape(hh, hw, 4)); - pc_data_ptr = reinterpret_cast(pc_buf.data()); - pc_data_size = pc_buf.size() * sizeof(float); - } else { - // FULL — send raw sl::Mat pointer directly (no copy) - pc_data_size = static_cast(pc_width * pc_height * sizeof(sl::float4)); - pc_data_ptr = reinterpret_cast(point_cloud_mat.getPtr()); - pc_metadata->set_string("layout", "HWC"); - pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4)); - } - rust::Slice pc_slice{pc_data_ptr, pc_data_size}; - auto pc_result = send_output_with_metadata( - dora_node.send_output, "point_cloud", pc_slice, std::move(pc_metadata)); - if (!std::string(pc_result.error).empty()) { - std::cerr << "Error sending point_cloud: " << std::string(pc_result.error) << std::endl; + rust::Slice pc_slice{pc_data_ptr, pc_data_size}; + auto pc_result = send_output_with_metadata( + dora_node.send_output, "point_cloud", pc_slice, std::move(pc_metadata)); + if (!std::string(pc_result.error).empty()) { + std::cerr << "Error sending point_cloud: " << std::string(pc_result.error) << std::endl; + } } // --- camera_info --- - float camera_data[4] = {calib.fx, calib.fy, calib.cx, calib.cy}; - rust::Slice cam_slice{ - reinterpret_cast(camera_data), sizeof(camera_data)}; + if (camera_info_pending) { + float camera_data[4] = {calib.fx, calib.fy, calib.cx, calib.cy}; + rust::Slice cam_slice{ + reinterpret_cast(camera_data), sizeof(camera_data)}; - auto cam_metadata = new_metadata(); - cam_metadata->set_string("dtype", "float32"); - cam_metadata->set_string("layout", "C"); - cam_metadata->set_list_int("shape", make_vector_int({4})); - cam_metadata->set_list_float("distortion", make_distortion(calib.distortion)); - set_frame_stamps(*cam_metadata, current_frame_id, device_ns, host_ns); + auto cam_metadata = new_metadata(); + cam_metadata->set_string("dtype", "float32"); + cam_metadata->set_string("layout", "C"); + cam_metadata->set_list_int("shape", make_vector_int({4})); + cam_metadata->set_list_float("distortion", make_distortion(calib.distortion)); + set_frame_stamps(*cam_metadata, current_frame_id, device_ns, host_ns); - auto cam_result = send_output_with_metadata( - dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata)); - if (!std::string(cam_result.error).empty()) { - std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl; + auto cam_result = send_output_with_metadata( + dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata)); + if (!std::string(cam_result.error).empty()) { + std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl; + } + camera_info_pending = false; } } diff --git a/src/zed/camera.cc b/src/zed/camera.cc index 15bd821..ed52202 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -78,6 +78,7 @@ Config Config::from_env() { c.depth_min_mm = util::get_env_int("ZED_DEPTH_MIN_MM", c.depth_min_mm); c.depth_max_mm = util::get_env_int("ZED_DEPTH_MAX_MM", c.depth_max_mm); c.depth_fill = util::get_env_bool("ZED_DEPTH_FILL", c.depth_fill); + c.point_cloud = util::get_env_bool("ZED_POINT_CLOUD", c.point_cloud); c.confidence_threshold = util::get_env_int("ZED_CONFIDENCE_THRESHOLD", c.confidence_threshold); c.texture_confidence_threshold = util::get_env_int("ZED_TEXTURE_CONFIDENCE_THRESHOLD", c.texture_confidence_threshold); @@ -149,7 +150,11 @@ bool Camera::grab() { } zed_.retrieveImage(image_mat_, sl::VIEW::LEFT); zed_.retrieveMeasure(depth_mat_, sl::MEASURE::DEPTH); - zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA); + // At 1080p a full XYZRGBA cloud is ~14.7 MB per frame, and retrieving it + // costs capture time whether or not anything consumes it. + if (config_.point_cloud) { + zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA); + } return true; } diff --git a/src/zed/camera.h b/src/zed/camera.h index beec2dd..598dc48 100644 --- a/src/zed/camera.h +++ b/src/zed/camera.h @@ -14,6 +14,7 @@ struct Config { int depth_min_mm = 200; int depth_max_mm = 600; bool depth_fill = false; + bool point_cloud = false; int confidence_threshold = 60; int texture_confidence_threshold = 100; std::string flip = "ON";