diff --git a/.gitea/workflows/validate.yml b/.gitea/workflows/validate.yml new file mode 100644 index 0000000..9fc8ce3 --- /dev/null +++ b/.gitea/workflows/validate.yml @@ -0,0 +1,32 @@ +# Checks the dataflow against the typed contract in dora-node.yml. This runs no +# node, so it needs neither a camera nor the ZED SDK. +name: validate + +on: [push, pull_request] + +env: + DORA_VERSION: v1.0.1 + DORA_TARGET: x86_64-unknown-linux-gnu + +jobs: + validate: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Install the dora CLI + run: | + curl -fsSL -o dora-cli.tar.gz \ + "https://github.com/dora-rs/dora/releases/download/${DORA_VERSION}/dora-cli-${DORA_TARGET}.tar.gz" + tar xzf dora-cli.tar.gz + mkdir -p "$HOME/.local/bin" + install -m 755 "dora-cli-${DORA_TARGET}/dora" "$HOME/.local/bin/dora" + echo "$HOME/.local/bin" >> "$GITHUB_PATH" + rm -rf dora-cli.tar.gz "dora-cli-${DORA_TARGET}" + + - run: dora --version + + # strict_types: true in dataflow.yml turns a type warning into a non-zero + # exit, so a mismatched annotation fails here rather than at runtime. + - name: Validate the dataflow + run: dora validate dataflow.yml diff --git a/.gitignore b/.gitignore index d3aa2de..94824ed 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,7 @@ build/ bin/ tmp/ -test/assets/*.png -test/assets/*.jpg -test/assets/*_result.* + +# dora build working directories +git/ +out/ diff --git a/AGENTS.md b/AGENTS.md index 310a548..a6a0cea 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -4,17 +4,21 @@ This file provides guidance to AI coding assistants when working with code in th ## What this is -A dora-rs C++ node that interfaces with ZED stereo cameras. On each tick it grabs a frame, runs object detection, and publishes four outputs over the dora dataflow: a BGR image, an XYZRGBA point cloud, camera intrinsics, and detected objects with 3D positions. Camera management lives in `src/zed/`, detection engines in `src/engines/`, and the dora event loop in `src/main.cc`. +A dora-rs C++ node that interfaces with ZED stereo cameras. On each tick it grabs a frame and publishes it over the dora dataflow: a BGR image and a depth map every frame, camera intrinsics once per camera session, a camera state on every transition, and an XYZRGBA point cloud when `ZED_POINT_CLOUD` is on. Camera management lives in `src/zed/` and the dora event loop in `src/main.cc`. + +The node is a camera source only. It ran inference in-process through `v0.1.0`, under `src/engines/`; that was removed so the node has no inference dependency and builds on any platform the ZED SDK supports. The removed code remains reachable at tag `v0.1.0`. ## Build ```bash -mkdir -p build && cd build && cmake .. && make install +scripts/build.sh ``` -The binary is installed to `bin/dora_zed` relative to the repo root. +The binary is installed to `bin/dora_zed` relative to the repo root. The script is also +the dataflow's `build:` command: dora 1.0 executes build commands directly rather than +through a shell, so `cd`, `&&` and command substitution do not work there. -**Requirements:** CMake 3.21+, ZED SDK 5.x (at `/usr/local/zed`), CUDA Toolkit, ONNX Runtime 1.20+ (system install), Rust stable toolchain (for dora-rs C++ bindings). +**Requirements:** dora 1.0+ (verified with `dora-cli 1.0.1`), CMake 3.21+, ZED SDK 5.x (at `/usr/local/zed`), CUDA Toolkit, Rust stable toolchain (for dora-rs C++ bindings). The first build clones dora-rs from GitHub and compiles `dora-node-api-cxx` via Cargo. This takes a while. Subsequent builds are incremental. If `DORA_ROOT_DIR` is set to a local dora checkout, it builds against that instead. @@ -27,29 +31,26 @@ dora up dora start dataflow.yml ``` -Camera and detection behavior is configured entirely through environment variables (see README.md for the full list: `ZED_RESOLUTION`, `ZED_FPS`, `ZED_DEPTH_MODE`, `ZED_DETECTION_MODEL`, etc.). +Camera behavior is configured entirely through environment variables (see README.md for the full list: `ZED_RESOLUTION`, `ZED_FPS`, `ZED_DEPTH_MODE`, etc.). ## 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.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. -- **`src/engines/engine.h`** — `engines::DetectedObject` struct and `engines::Engine` abstract base class (`enable`, `detect`, `objects`, `label_names`). -- **`src/engines/zed_native.h/cc`** — `engines::ZedNative`: wraps ZED SDK's TensorRT-optimized object detection module. -- **`src/engines/yolo_helpers.h`** — Header-only shared helpers used by the ONNX engine: `RawDetection`, `nms()`, `letterbox()`, `fuse_bbox()`, `backproject()`. Lives in `engines::detail` namespace. -- **`src/engines/yolo_onnx.h/cc`** — `engines::YoloOnnx` / `YoloOnnxConfig`: closed-vocabulary detection for any single-input YOLO ONNX model (YOLOv8, YOLOv11, YOLOv5, etc.). Auto-detects output layout (`NC_FIRST` / `NC_LAST` / `NC_LAST_OBJ`) from tensor shape; override with `ZED_ONNX_OUTPUT_FORMAT`. Labels loaded from `ZED_ONNX_LABELS` or `ZED_ONNX_LABELS_FILE`. -- **`src/main.cc`** — Dora event loop. Selects engine via `ZED_ENGINE` env var (`yolo_onnx`/`onnx` or `native`), holds a `std::unique_ptr`. Parses `ZED_PC_MODE` (`FULL`/`HALF`/`ROI`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()` and `detector->detect()`, then sends `image_bgr`, `point_cloud`, `camera_info`, and `objects` outputs. Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension; `ROI` emits only valid points within detected bounding boxes as a flat `[N, 4]` array. Handles reconnection on grab failure. +- **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: `open()` (one attempt, returns success), warmup, grab (image + depth + point cloud), calibration, device timestamp, blank-frame check, `close()` and `reset()` (close plus `sl::Camera::reboot()` where the model allows it). Exposes `handle()` for engine access. +- **`src/zed/capture.h/cc`** — `zed::Capture` and `zed::Frame`. Owns the camera on a dedicated thread: open, warmup, grab, blank/stall detection, backoff and escalation all live here, along with the packing of every payload into owned buffers. The newest frame is handed to the reader by swapping under a mutex (`take_frame`), which also recycles the previous frame's buffers so steady-state capture does not reallocate. **Nothing outside this file may call the ZED SDK** — it blocks unboundedly on a disconnected camera, which is what froze the node when the loop drove the camera directly. +- **`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 and output publishing only. 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. The loop never touches the SDK: it drains `take_state_change` and `take_frame` from the capture thread and publishes. On exit it calls `capture.stop()` and waits 2 s for the thread; if the thread is still inside a blocking SDK call it gives up and `_Exit(0)`s rather than let dora time out and kill the node. - **`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. -- **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format. -- **`Dockerfile`** — Ubuntu 24.04 x86-64 build environment (CUDA 12.8, ZED SDK 5.2, ONNX Runtime, Rust). No ARM64/Jetson image; build natively there. No camera needed; run the compile with `--gpus all`. Sets `NVIDIA_DRIVER_CAPABILITIES=compute,utility,video` — the `video` capability is what makes the container runtime mount NVDEC/NVENC, which `libsl_zed.so` links against. ONNX Runtime's CMake package files are relocated to `/usr/local/lib/cmake` because Ubuntu's multiarch CMake does not search `lib64`. Rust lives at `/usr/local/{rustup,cargo}` rather than `/root` so the build can run as the invoking user. `docker build -t dora-zed-build . && docker run --rm --gpus all --user "$(id -u):$(id -g)" -v "$PWD:/src" -v dora-zed-cargo:/usr/local/cargo/registry dora-zed-build`. -- **`scripts/export_yolo.py`** — PEP 723 uv script. Exports a standard YOLOv11 model to ONNX for closed-vocabulary detection. Run: `uv run scripts/export_yolo.py --model n --output-dir models/`. -- **`src/test_yolo_onnx.cc`** — Standalone C++ test binary (`bin/test_yolo_onnx`). Exercises `YoloOnnx` with an image (default `test/assets/yolo.png`). No dora and no live camera required. Uses the same env vars as the main binary. Saves an annotated result image alongside the input. +- **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by `dataflow.yml`, the `Dockerfile` and the README. +- **`dora-node.yml`** — Node manifest (`apiVersion: 1`): typed input/output contract, entrypoint, build command. `dora validate` injects these types into any dataflow that reaches the node by `path:`; a `git:`-sourced node does **not** pick it up, which is why `dataflow.yml` builds from the checkout. Type URNs name the element type (`std/core/v1/UInt8`, `.../Float32`, `.../String`) because every payload is a raw buffer described by its metadata, not an Arrow struct. +- **`dataflow.yml`** — Example dora dataflow, built from this checkout with `path:` + `build:` and `strict_types: true`. A consumer wires the node from a tag instead (see README) — dora 1.0 pins a source with the mutually exclusive `tag:`, `rev:` or `branch:`. +- **`.gitea/workflows/validate.yml`** — CI gate running `dora validate dataflow.yml` on push and pull request. Needs no camera and no ZED SDK. +- **`Dockerfile`** — Ubuntu 24.04 x86-64 build environment (CUDA 12.8, ZED SDK 5.2, Rust). No ARM64/Jetson image; build natively there. No camera needed; run the compile with `--gpus all`. Sets `NVIDIA_DRIVER_CAPABILITIES=compute,utility,video` — the `video` capability is what makes the container runtime mount NVDEC/NVENC, which `libsl_zed.so` links against. Rust lives at `/usr/local/{rustup,cargo}` rather than `/root` so the build can run as the invoking user. `docker build -t dora-zed-build . && docker run --rm --gpus all --user "$(id -u):$(id -g)" -v "$PWD:/src" -v dora-zed-cargo:/usr/local/cargo/registry dora-zed-build`. ## Key APIs - **dora C++ API** (``): `init_dora_node()`, `dora_node.events->next()`, `event_type()`, `send_output_with_metadata()`, `new_metadata()`. -- **ZED SDK** (``): `sl::Camera`, `sl::InitParameters`, `sl::RuntimeParameters`, `sl::Mat`, `sl::ObjectDetectionParameters`, `sl::Objects`. -- **ONNX Runtime** (``): `Ort::Env`, `Ort::Session`, `Ort::SessionOptions`, `Ort::Value`. Used by the `yolo_onnx` engine. +- **ZED SDK** (``): `sl::Camera`, `sl::InitParameters`, `sl::RuntimeParameters`, `sl::Mat`. - Output data is sent as raw byte slices (`rust::Slice`) with typed metadata (shape, dtype, encoding). ## Tools diff --git a/CMakeLists.txt b/CMakeLists.txt index 71f3cd6..0fbd2c8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,9 +1,15 @@ cmake_minimum_required(VERSION 3.21) -project(dora_zed VERSION 0.1.0 LANGUAGES C CXX) +project(dora_zed VERSION 0.2.0 LANGUAGES C CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_FLAGS "-fPIC") +# DoraTargets.cmake picks the cargo profile from this, so an unset build type +# would link debug dora bindings into an otherwise unoptimized binary. +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) +endif() + include(DoraTargets.cmake) set(ZED_DIR "/usr/local/zed" CACHE PATH "Path to the ZED SDK") @@ -13,18 +19,13 @@ find_package(zed REQUIRED) find_package(OpenCV REQUIRED) -find_package(onnxruntime REQUIRED) -set(ONNXRUNTIME_INCLUDE_DIRS "") # propagated via imported target -set(ONNXRUNTIME_LIBRARIES onnxruntime::onnxruntime) - link_directories(${dora_link_dirs}) link_directories(${ZED_LIBRARY_DIR}) add_executable(dora_zed src/main.cc src/zed/camera.cc - src/engines/zed_native.cc - src/engines/yolo_onnx.cc + src/zed/capture.cc ${node_bridge} ) add_dependencies(dora_zed Dora_cxx) @@ -38,29 +39,8 @@ target_include_directories( ${ZED_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${CUDAToolkit_INCLUDE_DIRS} - ${ONNXRUNTIME_INCLUDE_DIRS} ) -target_link_libraries(dora_zed dora_node_api_cxx ${ZED_LIBRARIES} ${OpenCV_LIBS} ${ONNXRUNTIME_LIBRARIES}) +target_link_libraries(dora_zed dora_node_api_cxx ${ZED_LIBRARIES} ${OpenCV_LIBS}) install(TARGETS dora_zed DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/bin) - -# Standalone detector tests — no dora, no live camera required - -add_executable(test_yolo_onnx - src/test_yolo_onnx.cc - src/engines/yolo_onnx.cc -) -target_include_directories(test_yolo_onnx PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/src - ${ZED_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${CUDAToolkit_INCLUDE_DIRS} - ${ONNXRUNTIME_INCLUDE_DIRS} -) -target_link_libraries(test_yolo_onnx PRIVATE - ${ZED_LIBRARIES} - ${OpenCV_LIBS} - ${ONNXRUNTIME_LIBRARIES} -) -install(TARGETS test_yolo_onnx DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/bin) diff --git a/Dockerfile b/Dockerfile index 2e30c15..437d4bc 100644 --- a/Dockerfile +++ b/Dockerfile @@ -21,7 +21,6 @@ FROM nvidia/cuda:12.8.1-devel-ubuntu24.04 ARG ZED_VERSION=5.2 -ARG ORT_VERSION=1.24.4 ENV DEBIAN_FRONTEND=noninteractive @@ -51,25 +50,7 @@ RUN curl -fSL -o /tmp/ZED_SDK.run \ && chmod -R a+rX /usr/local/zed \ && rm -f /tmp/ZED_SDK.run -# The tarball puts its CMake package files under lib/, but onnxruntimeTargets.cmake -# resolves the library as /lib64. Ubuntu is multiarch, so CMake does not -# search lib64 for packages and ld.so does not search /usr/local/lib64 at all: -# keep the libraries in lib64, move only the package files to lib/cmake, and add -# lib64 to the linker path. -RUN curl -fSL -o /tmp/ort.tgz \ - "https://github.com/microsoft/onnxruntime/releases/download/v${ORT_VERSION}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz" \ - && tar -xzf /tmp/ort.tgz -C /tmp \ - && mkdir -p /usr/local/include/onnxruntime /usr/local/lib64 \ - && cp -r /tmp/onnxruntime-linux-x64-gpu-${ORT_VERSION}/include/* /usr/local/include/onnxruntime/ \ - && cp -r /tmp/onnxruntime-linux-x64-gpu-${ORT_VERSION}/lib/* /usr/local/lib64/ \ - && mkdir -p /usr/local/lib/cmake \ - && mv /usr/local/lib64/cmake/onnxruntime /usr/local/lib/cmake/ \ - && rmdir /usr/local/lib64/cmake \ - && echo /usr/local/lib64 > /etc/ld.so.conf.d/onnxruntime.conf \ - && ldconfig \ - && rm -rf /tmp/ort.tgz /tmp/onnxruntime-linux-x64-gpu-${ORT_VERSION} - ENV NVIDIA_DRIVER_CAPABILITIES=compute,utility,video WORKDIR /src -CMD ["bash", "-lc", "mkdir -p build && cd build && cmake .. && make -j$(nproc) install"] +CMD ["scripts/build.sh"] diff --git a/README.md b/README.md index 2bfe4cf..9b7650a 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # dora-zed -Dora node for ZED stereo cameras. Publishes BGR images, point clouds, camera intrinsics, and 3D object detections. +Dora node for ZED stereo cameras. Publishes a BGR image and a depth map on every frame, plus camera intrinsics, a camera state, and an optional point cloud. ## Usage @@ -10,29 +10,56 @@ Add to your dataflow: nodes: - id: zed-camera git: ssh://git@gitea.liria.cl:5001/liria/dora-zed.git - git_checkout: v0.1.0 - build: mkdir -p build && cd build && cmake .. && make install + tag: v0.2.0 + build: scripts/build.sh path: bin/dora_zed inputs: tick: dora/timer/millis/100 outputs: - image_bgr + - depth - point_cloud - camera_info - - objects + - camera_state ``` -See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera display node. +See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera display +node. That file builds the node from the checkout (`path:` + `build:`) rather than from a +tag, which is what lets `dora validate` pick up the manifest described below. + +### Node manifest + +[`dora-node.yml`](dora-node.yml) declares the node's inputs and outputs with type URNs. +No source package is shared between node repositories, so it is the only contract a +consumer has, and it is what makes a wiring mistake fail at build time instead of at +runtime: + +``` +$ dora validate dataflow.yml + node "zed-camera": injected 6 contract type(s) from .../dora-node.yml +All type annotations OK. +``` + +Types are `std/core/v1/UInt8` for `image_bgr`, `std/core/v1/Float32` for `depth`, +`camera_info` and `point_cloud`, and `std/core/v1/String` for `camera_state`; every +payload is a raw buffer whose shape and dtype are in the metadata, so the URN names the +element type rather than a struct. The `tick` input is `std/core/v1/UInt64`, matching how +dora types timer inputs. + +`strict_types: true` in `dataflow.yml` makes a type warning a non-zero exit. +[`.gitea/workflows/validate.yml`](.gitea/workflows/validate.yml) runs +`dora validate dataflow.yml` on every push and pull request; it needs no camera and no +ZED SDK, since validation never runs the node. ## Dependencies | Dependency | Version | Notes | |---|---|---| +| [dora-rs](https://github.com/dora-rs/dora) | 1.0+ | Verified against `dora-cli 1.0.1`; the C++ bindings are built from source at configure time | | [CMake](https://cmake.org/download/) | 3.21+ | | | [ZED SDK](https://www.stereolabs.com/developers/release) | 5.x | Bundles OpenCV; default path `/usr/local/zed` | | [CUDA Toolkit](https://developer.nvidia.com/cuda-downloads) | — | Required by ZED SDK | | [Rust toolchain](https://rustup.rs) | stable | Builds dora-rs C++ bindings | -| [ONNX Runtime](https://github.com/microsoft/onnxruntime/releases) | 1.20+ | Required at configure time; used at runtime only by `yolo_onnx` | ### Installation scripts @@ -42,23 +69,27 @@ See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera | NVIDIA Jetson (ARM64) | `sudo scripts/install_linux_arm.sh` | | Windows x64 (PowerShell admin) | `scripts/install_windows.ps1` | -> **Jetson:** No official ONNX Runtime GPU binary for ARM64. It is still needed to configure the build; only the `yolo_onnx` engine uses it at runtime. Requires [building from source](https://onnxruntime.ai/docs/build/eps.html#tensorrt). -> -> **Windows:** Install ZED SDK manually from [stereolabs.com](https://www.stereolabs.com/developers/release), then pass the ONNX Runtime path: `cmake .. -DCMAKE_PREFIX_PATH=C:\onnxruntime` +> **Windows:** Install the ZED SDK manually from [stereolabs.com](https://www.stereolabs.com/developers/release). ## Local build ```bash git clone ssh://git@gitea.liria.cl:5001/liria/dora-zed.git cd dora-zed -mkdir -p build && cd build && cmake .. && make install +scripts/build.sh ``` Binary installed to `bin/dora_zed`. +`scripts/build.sh` is also the `build:` command in the dataflow. dora 1.0 runs build +commands directly rather than through a shell, so `cd`, `&&` and command substitution are +not available there; keeping the sequence in one script is what makes the dataflow build +work. It configures a `Release` build, which is also what links the optimized dora +bindings. + ### Containerized build -[`Dockerfile`](Dockerfile) provides the full toolchain — CUDA, ZED SDK, ONNX Runtime and Rust — on Ubuntu 24.04, the newest release Stereolabs ships a ZED SDK for. No camera is needed. +[`Dockerfile`](Dockerfile) provides the full toolchain — CUDA, ZED SDK and Rust — on Ubuntu 24.04, the newest release Stereolabs ships a ZED SDK for. No camera is needed. ```bash docker build -t dora-zed-build . @@ -66,6 +97,8 @@ docker run --rm --gpus all --user "$(id -u):$(id -g)" \ -v "$PWD:/src" -v dora-zed-cargo:/usr/local/cargo/registry dora-zed-build ``` +The container runs the same `scripts/build.sh`. + The first run clones dora-rs and compiles it with Cargo; the named volume keeps that cache across runs. `--user` keeps `build/` and `bin/` owned by you rather than root. @@ -87,105 +120,111 @@ All behavior is controlled via environment variables. | Variable | Default | Description | |---|---|---| -| `ZED_RESOLUTION` | `720` | `720`, `1080`, or `2000` | +| `ZED_RESOLUTION` | `1080` | `720`, `1080`, or `2000` | | `ZED_FPS` | `15` | Frame rate | -| `ZED_DEPTH_MODE` | `NEURAL` | `PERFORMANCE`, `QUALITY`, `NEURAL`, or `NONE` | -| `ZED_DEPTH_MIN_MM` | `10` | Minimum depth (mm) | -| `ZED_DEPTH_MAX_MM` | `500` | Maximum depth (mm) | -| `ZED_DEPTH_FILL` | `false` | Enable depth fill mode | +| `ZED_DEPTH_MODE` | `NEURAL` | `NEURAL`, `NEURAL_PLUS`, `NEURAL_LIGHT`, `PERFORMANCE`, `QUALITY`, or `NONE`. An unrecognised value is a startup error, not a fallback. | +| `ZED_DEPTH_MIN_MM` | `200` | Minimum depth (mm) | +| `ZED_DEPTH_MAX_MM` | `600` | Maximum depth (mm) | +| `ZED_CONFIDENCE_THRESHOLD` | `60` | Reject depth less certain than this (1–100, lower is stricter) | +| `ZED_TEXTURE_CONFIDENCE_THRESHOLD` | `100` | Reject depth on surfaces less textured than this (1–100) | +| `ZED_DEPTH_FILL` | `false` | Interpolate depth over holes. Overrides both confidence thresholds. | | `ZED_FLIP` | `ON` | `ON` or `OFF` | | `ZED_WARMUP_FRAMES` | `30` | Frames to discard on startup | -| `ZED_RETRY_INTERVAL_SECONDS` | `5` | Reconnection interval (s) | -### Detection engine - -Set `ZED_ENGINE` to select the detection backend: - -| `ZED_ENGINE` | Custom model? | Classes | -|---|---|---| -| `native` (default) | No — ZED SDK built-in TensorRT | Fixed: 7 categories | -| `yolo_onnx` (alias: `onnx`) | Yes — any single-input YOLO ONNX (v5/v8/v11) | Fixed labels from file or env var | - -#### `native` engine - -Uses ZED SDK's built-in TensorRT models — no model files to download or configure. `ZED_DETECTION_MODEL` selects the speed/accuracy variant, not a file path. - -Detects 7 fixed categories: PERSON, VEHICLE, BAG, ANIMAL, ELECTRONICS, FRUIT_VEGETABLE, SPORT. - -| Variable | Default | Description | -|---|---|---| -| `ZED_DETECTION_MODEL` | `MULTI_CLASS_BOX_MEDIUM` | `MULTI_CLASS_BOX_FAST`, `MULTI_CLASS_BOX_MEDIUM`, or `MULTI_CLASS_BOX_ACCURATE` | -| `ZED_DETECTION_CONFIDENCE` | `40` | Confidence threshold (0–100) | -| `ZED_DETECTION_TRACKING` | `false` | Enable object tracking | - -#### `yolo_onnx` engine (closed-vocabulary) - -For any single-input YOLO ONNX model with fixed class names (YOLOv8, YOLOv11, YOLOv5, custom-trained). - -Export a YOLOv11 model once: - -```bash -uv run scripts/export_yolo.py --model n --output-dir models/ -``` - -Then run: - -```bash -ZED_ENGINE=yolo_onnx \ -ZED_ONNX_MODEL_PATH=models/yolo11n_i640.onnx \ -ZED_ONNX_LABELS_FILE=models/coco80.txt \ -dora start dataflow.yml -``` - -**Required:** - -| Variable | Description | -|---|---| -| `ZED_ONNX_MODEL_PATH` | Path to YOLO ONNX model | -| `ZED_ONNX_LABELS` or `ZED_ONNX_LABELS_FILE` | Class names (inline CSV or one-per-line file) | - -**Optional:** - -| Variable | Default | Description | -|---|---|---| -| `ZED_ONNX_CONFIDENCE` | `0.25` | Confidence threshold (0.0–1.0) | -| `ZED_ONNX_NMS_THRESHOLD` | `0.45` | NMS IoU threshold | -| `ZED_ONNX_INPUT_SIZE` | `640` | Model input size (px) | -| `ZED_ONNX_OUTPUT_FORMAT` | `auto` | Output layout: `auto`, `nc_first`/`yolov8`/`yolov11`, `nc_last`, `nc_last_obj`/`yolov5`. `auto` detects from tensor shape. | +The resolved depth mode, both confidence thresholds and the depth range are logged at +startup. The defaults match the reference camera driver the perception stack was tuned +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) | -| `ROI` | `[N, 4]` | Valid points within detected bounding boxes only | + +The cloud is copied out of the SDK buffer, since the capture thread and the event loop are +different threads. That copy is the main reason to leave `ZED_POINT_CLOUD` off unless a +consumer needs it. ## Inputs | Input | Description | |---|---| -| `tick` | Triggers a camera grab and detection. Connect to `dora/timer/millis/N`. | +| `tick` | Triggers a camera grab. Connect to `dora/timer/millis/N`. | ## Outputs | Output | Type | Description | |---|---|---| -| `image_bgr` | `uint8 [H, W, 3]` | BGR image. Metadata: `shape`, `encoding=bgr8`, intrinsics, `timestamp_ns`. | -| `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics. | -| `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients. | -| `objects` | `float32 [N, 37]` | Detected objects. See below. | +| `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, 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. | +| `camera_state` | `string` | UTF-8 state name, published on each change. Metadata: `dtype=string`, `encoding=utf8`, `host_timestamp_ns`. | -### `objects` layout +### Camera state and recovery -Each of the N rows contains 37 floats: +`camera_state` lets a downstream node tell a camera that has not delivered its first +frame yet from one that has been lost: -| Index | Field | Description | -|---|---|---| -| 0 | `label_id` | Class index | -| 1 | `confidence` | Score (0–100) | -| 2–4 | `x, y, z` | 3D center (mm) | -| 5–12 | `bb2d` | 2D bounding box: 4 corners × (x, y) px | -| 13–36 | `bb3d` | 3D bounding box: 8 corners × (x, y, z) mm | +| State | Meaning | +|---|---| +| `opening` | The camera has not delivered a usable frame yet in this run | +| `streaming` | Frames are being published | +| `recovering` | The camera was streaming and has stopped; the node is trying to bring it back | -Metadata includes `engine`, `label_names`, `image_width`, `image_height`, and `timestamp_ns`. An empty array `[0, 37]` is sent when no objects are detected. +The camera runs on its own thread. The ZED SDK can block for an unbounded time inside +`grab()` and `open()` when the camera is disconnected — measured blocking indefinitely on +a ZED-M even with `async_grab_camera_recovery` set — so driving it from the dora event +loop would freeze the node: it would stop reading events, leaving `Stop` unanswered and +the recovery logic itself unreachable. The event loop only reads the newest frame the +capture thread has already produced, and therefore never calls into the SDK. + +Reopening is paced by a backoff of 1, 2, 5 then 10 seconds, the last repeated. Frames that +are not collected before the next one arrives are dropped: a consumer always gets the +newest frame rather than a queue of stale ones. + +The node treats an all-black frame as a failed grab: a camera whose link has dropped does +not necessarily fail its grabs, and can keep reporting success while handing back a +uniform frame. A camera that delivers no usable frame for 5 seconds is recovered — 30 +seconds if the SDK reports it is reconnecting the link itself, since taking the handle +away mid-reconnect aborts a recovery that was already under way. + +The first recovery attempt closes the handle. Past that the device itself is wedged, and +the node escalates to `sl::Camera::reboot()`, which resets it over USB without unplugging +it. That call reaches the ZED 2 and newer only; on the first-generation ZED and the ZED +Mini the node closes the handle and lets the backoff give the endpoint time to clear. +Every attempt and every escalation is logged. + +### Timestamps + +Every frame output — `image_bgr`, `depth`, `point_cloud` and `camera_info` — carries both +clocks, as dora `timestamp` metadata parameters. `camera_state` carries +`host_timestamp_ns` only, since it is not tied to a frame. + +| Key | Epoch | Unit | Source | +|---|---|---|---| +| `device_timestamp_ns` | Unix epoch (UTC) | nanoseconds | `sl::Camera::getTimestamp(TIME_REFERENCE::IMAGE)` — when the camera captured the frame | +| `host_timestamp_ns` | Unix epoch (UTC) | nanoseconds | `std::chrono::system_clock` on the host — when this node published the frame | + +Both are needed. The device timestamp is the capture instant and is the value two +nodes reading the same frame agree on; the host timestamp is the one the rest of the +dataflow and the session recorder share. On a Jetson the wall clock can step when NTP +synchronises after boot, so the device timestamp is recorded alongside the host one +rather than in place of it. + +The node calls `sl::setTimestampClock(SYSTEM_CLOCK)` before opening the camera, which +is what makes the device timestamp a Unix-epoch value rather than an SDK-default clock. + +Every output for a frame also carries the same `frame_id`, an integer counter starting +at 0. diff --git a/dataflow.yml b/dataflow.yml index 80dcf14..28b6a9f 100644 --- a/dataflow.yml +++ b/dataflow.yml @@ -1,14 +1,20 @@ +# The repository's own example: builds and runs the node from this checkout, so +# `dora validate` picks up the typed contract in dora-node.yml next to it. +# A consumer wires the node from a tag instead — see README.md. +strict_types: true + nodes: - id: zed-camera - git: ssh://git@gitea.liria.cl:5001/liria/dora-zed.git - git_checkout: v0.1.0 - build: mkdir -p build && cd build && cmake .. && make install path: bin/dora_zed + build: scripts/build.sh inputs: tick: dora/timer/millis/100 outputs: - image_bgr - - objects + - depth + - point_cloud + - camera_info + - camera_state - id: plot git: https://github.com/dora-rs/dora-hub.git diff --git a/dora-node.yml b/dora-node.yml new file mode 100644 index 0000000..336e30a --- /dev/null +++ b/dora-node.yml @@ -0,0 +1,39 @@ +# Node manifest: the typed contract a consumer wires against. +# +# No source package is shared between node repositories, so this file and its +# type URNs are the only contract a consumer has. `dora validate` checks a +# dataflow against it without running the node, so CI needs no camera and no +# ZED SDK. +apiVersion: 1 +name: dora-zed +namespace: liria +description: ZED stereo camera source publishing a BGR image, a depth map, intrinsics and an optional point cloud +categories: [sensor] +keywords: [zed, stereolabs, camera, depth, stereo, jetson] + +runtime: cpp +entrypoint: bin/dora_zed +build: scripts/build.sh + +inputs: + tick: + type: std/core/v1/UInt64 + required: true + description: Each tick triggers one camera grab. Connect to dora/timer/millis/N. + +outputs: + image_bgr: + type: std/core/v1/UInt8 + description: BGR image bytes, [H, W, 3]. Shape and encoding are in the metadata. + depth: + type: std/core/v1/Float32 + description: Per-pixel depth in millimetres, [H, W]. Missing depth is 0, never inf or nan. + camera_info: + type: std/core/v1/Float32 + description: Intrinsics [fx, fy, cx, cy], distortion in the metadata. Published once per camera session. + point_cloud: + type: std/core/v1/Float32 + description: XYZRGBA point cloud. Published only when ZED_POINT_CLOUD is on. + camera_state: + type: std/core/v1/String + description: One of opening, streaming, recovering. Published on every change. diff --git a/scripts/build.sh b/scripts/build.sh new file mode 100755 index 0000000..8eb753a --- /dev/null +++ b/scripts/build.sh @@ -0,0 +1,14 @@ +#!/usr/bin/env bash +# Configure, compile and install the node into bin/. +# +# dora 1.0 runs a node's `build:` command directly rather than through a shell, +# so `cd`, `&&` and command substitution are not available there. The whole +# sequence lives here instead, and the dataflow only invokes this script. +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_DIR="$(dirname "$SCRIPT_DIR")" +BUILD_DIR="${REPO_DIR}/build" + +cmake -S "$REPO_DIR" -B "$BUILD_DIR" -DCMAKE_BUILD_TYPE=Release +cmake --build "$BUILD_DIR" --target install --parallel "$(nproc)" diff --git a/scripts/export_yolo.py b/scripts/export_yolo.py deleted file mode 100644 index 8e1c0b3..0000000 --- a/scripts/export_yolo.py +++ /dev/null @@ -1,111 +0,0 @@ -# /// script -# requires-python = ">=3.10" -# dependencies = ["ultralytics>=8.3"] -# /// -"""Export a standard YOLOv11 model to ONNX for closed-vocabulary detection. - -The exported model has one input: - images (1, 3, imgsz, imgsz) float32 - -Class names are fixed at training time. Provide them at runtime via -ZED_ONNX_LABELS or ZED_ONNX_LABELS_FILE. - -Usage: - uv run scripts/export_yolo.py - uv run scripts/export_yolo.py --model s --output-dir models/ - uv run scripts/export_yolo.py --model m --imgsz 1280 -""" - -import argparse -import shutil -import sys -from pathlib import Path - -MODELS = { - "n": "yolo11n", - "s": "yolo11s", - "m": "yolo11m", - "l": "yolo11l", - "x": "yolo11x", -} - - -def parse_args(): - p = argparse.ArgumentParser( - description="Export YOLOv11 to ONNX for closed-vocabulary detection" - ) - p.add_argument( - "--model", "-m", - choices=list(MODELS), - default="n", - help="Model size: n (~6 MB), s (~18 MB), m (~38 MB), l (~49 MB), x (~109 MB). Default: n", - ) - p.add_argument( - "--imgsz", - type=int, - default=640, - help="Input image size in pixels (square). Default: 640", - ) - p.add_argument( - "--output-dir", "-o", - default=".", - help="Output directory for the .onnx file. Default: current directory", - ) - return p.parse_args() - - -def verify_output(onnx_path: Path): - try: - import onnx - model = onnx.load(str(onnx_path)) - inputs = [(i.name, [d.dim_value for d in i.type.tensor_type.shape.dim]) - for i in model.graph.input] - outputs = [(o.name, [d.dim_value for d in o.type.tensor_type.shape.dim]) - for o in model.graph.output] - print(f" Inputs: {inputs}") - print(f" Outputs: {outputs}") - except ImportError: - print(" (install 'onnx' to verify model structure)") - - -def main(): - args = parse_args() - - try: - from ultralytics import YOLO - except ImportError: - print("Error: ultralytics not installed. Run: uv run scripts/export_yolo.py", file=sys.stderr) - sys.exit(1) - - model_name = MODELS[args.model] - out_dir = Path(args.output_dir) - out_dir.mkdir(parents=True, exist_ok=True) - out_name = f"{model_name}_i{args.imgsz}.onnx" - dst = out_dir / out_name - - print(f"Model: {model_name}.pt") - print(f"Input size: {args.imgsz}") - print(f"Output: {dst}") - print() - - model = YOLO(f"{model_name}.pt") - exported = Path(model.export(format="onnx", imgsz=args.imgsz)) - - if exported.resolve() != dst.resolve(): - shutil.move(str(exported), str(dst)) - - print(f"\nVerifying exported model...") - verify_output(dst) - - print(f"\nExported: {dst.resolve()}") - print(f"\nRun with (COCO labels example):") - print(f" ZED_ENGINE=onnx \\") - print(f" ZED_ONNX_MODEL_PATH={dst.resolve()} \\") - print(f" ZED_ONNX_LABELS_FILE=models/coco80.txt \\") - print(f" dora start dataflow.yml") - print(f"\nOr with inline labels:") - print(f" ZED_ONNX_LABELS=\"person,car,chair\" \\") - - -if __name__ == "__main__": - main() diff --git a/scripts/install_linux_arm.sh b/scripts/install_linux_arm.sh index f6d6afd..14a9016 100755 --- a/scripts/install_linux_arm.sh +++ b/scripts/install_linux_arm.sh @@ -9,11 +9,6 @@ # JetPack 6.2.2 (L4T 36.5) # JetPack 7.0 (L4T 38.2) — Jetson Thor # JetPack 7.1 (L4T 38.4) -# -# ONNX Runtime: Microsoft does not publish official GPU binaries for ARM64. -# ONNX Runtime is still needed to configure the build; only the yolo_onnx -# engine uses it at runtime. Build it from source: -# https://onnxruntime.ai/docs/build/eps.html#tensorrt set -euo pipefail ZED_VERSION="5.2" @@ -63,13 +58,5 @@ else sudo "${TMP_DIR}/ZED_SDK.run" -- silent skip_python skip_tools fi -# --- ONNX Runtime --- -echo "" -echo ">>> ONNX Runtime (ARM64 GPU): no official pre-built binary available." -echo " Needed to configure the build; only yolo_onnx uses it at runtime." -echo " Build from source:" -echo " https://onnxruntime.ai/docs/build/eps.html#tensorrt" -echo "" - -echo ">>> Done. You can now run: mkdir -p build && cd build && cmake .. && make install" +echo ">>> Done. You can now run: scripts/build.sh" echo ">>> Downloaded files are in ${TMP_DIR}" diff --git a/scripts/install_linux_x64.sh b/scripts/install_linux_x64.sh index ff05d0a..ed1e03d 100755 --- a/scripts/install_linux_x64.sh +++ b/scripts/install_linux_x64.sh @@ -6,7 +6,6 @@ set -euo pipefail ZED_VERSION="5.2" -ORT_VERSION="1.24.4" SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" TMP_DIR="${SCRIPT_DIR}/../tmp" @@ -34,32 +33,5 @@ else sudo "${TMP_DIR}/ZED_SDK.run" -- silent skip_python skip_tools fi -# --- ONNX Runtime (C++ GPU, installs to /usr/local) --- -if [ -f /usr/local/lib64/libonnxruntime.so ] && [ -d /usr/local/lib/cmake/onnxruntime ]; then - echo ">>> ONNX Runtime already installed — skipping." -else - echo ">>> Downloading ONNX Runtime ${ORT_VERSION} GPU..." - download \ - "https://github.com/microsoft/onnxruntime/releases/download/v${ORT_VERSION}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz" \ - "${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz" - echo ">>> Extracting..." - tar -xzf "${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz" -C "${TMP_DIR}" - echo ">>> Installing ONNX Runtime to /usr/local..." - sudo mkdir -p /usr/local/include/onnxruntime - sudo cp -r "${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}/include/"* /usr/local/include/onnxruntime/ - sudo mkdir -p /usr/local/lib64 - sudo cp -r "${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}/lib/"* /usr/local/lib64/ - # onnxruntimeTargets.cmake resolves the library as /lib64, but Ubuntu - # is multiarch: CMake does not search lib64 for packages and ld.so does not - # search /usr/local/lib64 at all. Move only the package files to lib/cmake - # and put lib64 on the linker path. - sudo mkdir -p /usr/local/lib/cmake - sudo rm -rf /usr/local/lib/cmake/onnxruntime - sudo mv /usr/local/lib64/cmake/onnxruntime /usr/local/lib/cmake/ - sudo rmdir /usr/local/lib64/cmake - echo /usr/local/lib64 | sudo tee /etc/ld.so.conf.d/onnxruntime.conf > /dev/null - sudo ldconfig -fi - -echo ">>> Done. You can now run: mkdir -p build && cd build && cmake .. && make install" +echo ">>> Done. You can now run: scripts/build.sh" echo ">>> Downloaded files are in ${TMP_DIR}" diff --git a/scripts/install_windows.ps1 b/scripts/install_windows.ps1 index 8f8979e..5a10cec 100755 --- a/scripts/install_windows.ps1 +++ b/scripts/install_windows.ps1 @@ -10,10 +10,6 @@ $ErrorActionPreference = "Stop" $ZED_INSTALL_DIR = "C:\Program Files (x86)\ZED SDK" -$ORT_VERSION = "1.24.4" -$ORT_NAME = "onnxruntime-win-x64-gpu_cuda13-$ORT_VERSION" -$ORT_URL = "https://github.com/microsoft/onnxruntime/releases/download/v$ORT_VERSION/$ORT_NAME.zip" -$ORT_DEST = "C:\onnxruntime" # --- ZED SDK --- if (Test-Path $ZED_INSTALL_DIR) { @@ -25,23 +21,5 @@ if (Test-Path $ZED_INSTALL_DIR) { Write-Host "" } -# --- ONNX Runtime --- -if (Test-Path "$ORT_DEST\lib\onnxruntime.dll") { - Write-Host ">>> ONNX Runtime already installed at '$ORT_DEST' — skipping." -} else { - Write-Host ">>> Downloading ONNX Runtime $ORT_VERSION GPU (CUDA 13)..." - $tmp = "$env:TEMP\$ORT_NAME.zip" - Invoke-WebRequest -Uri $ORT_URL -OutFile $tmp - - Write-Host ">>> Extracting to $ORT_DEST..." - Expand-Archive -Path $tmp -DestinationPath $env:TEMP -Force - if (Test-Path $ORT_DEST) { Remove-Item $ORT_DEST -Recurse -Force } - Move-Item "$env:TEMP\$ORT_NAME" $ORT_DEST - Remove-Item $tmp - - Write-Host ">>> ONNX Runtime installed to $ORT_DEST" - Write-Host " Pass it to CMake: cmake .. -DCMAKE_PREFIX_PATH=C:\onnxruntime" -} - Write-Host "" Write-Host ">>> Done." diff --git a/src/engines/engine.h b/src/engines/engine.h deleted file mode 100644 index eb3f07a..0000000 --- a/src/engines/engine.h +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace engines { - -struct DetectedObject { - int label_id; - float confidence; - float position[3]; // center x, y, z (mm) - float bounding_box_2d[8]; // 4 corners × 2 coords (pixels) - float bounding_box_3d[24]; // 8 corners × 3 coords (mm) -}; - -class Engine { -public: - virtual ~Engine() = default; - - virtual void enable(sl::Camera& camera) = 0; - virtual void detect(sl::Camera& camera) = 0; - virtual const std::vector& objects() const = 0; - virtual std::string label_names() const = 0; -}; - -} // namespace engines diff --git a/src/engines/yolo_helpers.h b/src/engines/yolo_helpers.h deleted file mode 100644 index 1f4645e..0000000 --- a/src/engines/yolo_helpers.h +++ /dev/null @@ -1,136 +0,0 @@ -#pragma once - -#include -#include - -#include -#include -#include - -namespace engines::detail { - -// --- NMS --- - -struct RawDetection { - float x1, y1, x2, y2; - float score; - int class_id; -}; - -inline float iou(const RawDetection& a, const RawDetection& b) { - float inter_x1 = std::max(a.x1, b.x1); - float inter_y1 = std::max(a.y1, b.y1); - float inter_x2 = std::min(a.x2, b.x2); - float inter_y2 = std::min(a.y2, b.y2); - float inter_w = std::max(0.0f, inter_x2 - inter_x1); - float inter_h = std::max(0.0f, inter_y2 - inter_y1); - float inter_area = inter_w * inter_h; - float area_a = (a.x2 - a.x1) * (a.y2 - a.y1); - float area_b = (b.x2 - b.x1) * (b.y2 - b.y1); - float union_area = area_a + area_b - inter_area; - return union_area > 0.0f ? inter_area / union_area : 0.0f; -} - -inline std::vector nms(std::vector dets, float threshold) { - std::sort(dets.begin(), dets.end(), - [](const RawDetection& a, const RawDetection& b) { - return a.score > b.score; - }); - std::vector suppressed(dets.size(), false); - std::vector result; - for (size_t i = 0; i < dets.size(); i++) { - if (suppressed[i]) continue; - result.push_back(dets[i]); - for (size_t j = i + 1; j < dets.size(); j++) { - if (!suppressed[j] && iou(dets[i], dets[j]) > threshold) - suppressed[j] = true; - } - } - return result; -} - -// --- Letterbox resize --- - -inline cv::Mat letterbox(const cv::Mat& src, int size, - float& scale, float& pad_x, float& pad_y) { - float scale_w = static_cast(size) / src.cols; - float scale_h = static_cast(size) / src.rows; - scale = std::min(scale_w, scale_h); - int new_w = static_cast(std::round(src.cols * scale)); - int new_h = static_cast(std::round(src.rows * scale)); - pad_x = (size - new_w) / 2.0f; - pad_y = (size - new_h) / 2.0f; - - cv::Mat resized; - cv::resize(src, resized, {new_w, new_h}); - - cv::Mat canvas(size, size, CV_8UC3, cv::Scalar(114, 114, 114)); - resized.copyTo(canvas(cv::Rect( - static_cast(pad_x), static_cast(pad_y), new_w, new_h))); - return canvas; -} - -// --- Point cloud fusion --- - -struct FusionResult { - float cx, cy, cz; - float depth_near, depth_far; - bool valid = false; -}; - -inline FusionResult fuse_bbox(const sl::Mat& pc, int x1, int y1, int x2, int y2) { - FusionResult result{}; - int w = static_cast(pc.getWidth()); - int h = static_cast(pc.getHeight()); - x1 = std::max(x1, 0); - y1 = std::max(y1, 0); - x2 = std::min(x2, w - 1); - y2 = std::min(y2, h - 1); - - std::vector xs, ys, zs; - xs.reserve((x2 - x1) * (y2 - y1)); - ys.reserve(xs.capacity()); - zs.reserve(xs.capacity()); - - for (int y = y1; y <= y2; y++) { - for (int x = x1; x <= x2; x++) { - sl::float4 pt; - pc.getValue(x, y, &pt); - if (!std::isnan(pt.x) && !std::isnan(pt.y) && !std::isnan(pt.z) && - !std::isinf(pt.x) && !std::isinf(pt.y) && !std::isinf(pt.z)) { - xs.push_back(pt.x); - ys.push_back(pt.y); - zs.push_back(pt.z); - } - } - } - - if (xs.empty()) return result; - - std::sort(xs.begin(), xs.end()); - std::sort(ys.begin(), ys.end()); - std::sort(zs.begin(), zs.end()); - - size_t n = xs.size(); - size_t mid = n / 2; - size_t p25 = n / 4; - size_t p75 = 3 * n / 4; - - result.cx = xs[mid]; - result.cy = ys[mid]; - result.cz = zs[mid]; - result.depth_near = zs[p25]; - result.depth_far = zs[p75]; - result.valid = true; - return result; -} - -inline void backproject(float u, float v, float z, - float fx, float fy, float cx, float cy, - float& X, float& Y, float& Z) { - Z = z; - X = (u - cx) * z / fx; - Y = (v - cy) * z / fy; -} - -} // namespace engines::detail diff --git a/src/engines/yolo_onnx.cc b/src/engines/yolo_onnx.cc deleted file mode 100644 index f170edd..0000000 --- a/src/engines/yolo_onnx.cc +++ /dev/null @@ -1,369 +0,0 @@ -#include "engines/yolo_onnx.h" -#include "engines/yolo_helpers.h" -#include "util/env.h" - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace engines { - -using namespace detail; - -// --- Impl --- - -struct YoloOnnx::Impl { - Ort::Env env{ORT_LOGGING_LEVEL_WARNING, "yolo_onnx"}; - Ort::SessionOptions session_options; - std::unique_ptr session; - Ort::AllocatorWithDefaultOptions allocator; - - std::string input_name; - std::string output_name; - int num_classes = 0; - YoloOutputLayout resolved_layout = YoloOutputLayout::NC_FIRST; - - std::vector run_inference(const YoloOnnxConfig& config, - const cv::Mat& image_bgr); -}; - -// --- Impl::run_inference --- - -std::vector YoloOnnx::Impl::run_inference( - const YoloOnnxConfig& config, const cv::Mat& image_bgr) -{ - if (!session) return {}; - - int orig_w = image_bgr.cols; - int orig_h = image_bgr.rows; - - cv::Mat rgb; - cv::cvtColor(image_bgr, rgb, cv::COLOR_BGR2RGB); - - float scale, pad_x, pad_y; - cv::Mat lb = letterbox(rgb, config.input_size, scale, pad_x, pad_y); - - cv::Mat lb_float; - lb.convertTo(lb_float, CV_32F, 1.0f / 255.0f); - - int sz = config.input_size; - std::vector input_data(3 * sz * sz); - for (int c = 0; c < 3; c++) { - for (int h = 0; h < sz; h++) { - for (int w = 0; w < sz; w++) { - input_data[c * sz * sz + h * sz + w] = - lb_float.at(h, w)[c]; - } - } - } - - auto mem_info = Ort::MemoryInfo::CreateCpu( - OrtAllocatorType::OrtArenaAllocator, OrtMemTypeDefault); - - std::vector image_shape{1, 3, sz, sz}; - Ort::Value image_tensor = Ort::Value::CreateTensor( - mem_info, input_data.data(), input_data.size(), - image_shape.data(), image_shape.size()); - - const char* input_name_cstr = input_name.c_str(); - const char* output_name_cstr = output_name.c_str(); - - std::vector outputs; - try { - outputs = session->Run( - Ort::RunOptions{nullptr}, - &input_name_cstr, &image_tensor, 1, - &output_name_cstr, 1); - } catch (const Ort::Exception& e) { - std::cerr << "YoloOnnx: inference failed: " << e.what() << std::endl; - return {}; - } - - auto out_shape = outputs[0].GetTensorTypeAndShapeInfo().GetShape(); - if (out_shape.size() < 3) return {}; - - const float* out = outputs[0].GetTensorData(); - int nc = num_classes; - if (nc <= 0) return {}; - - std::vector candidates; - - if (resolved_layout == YoloOutputLayout::NC_FIRST) { - // (1, 4+N, anchors) — YOLOv8, YOLOv11 - int num_anchors = static_cast(out_shape[2]); - for (int a = 0; a < num_anchors; a++) { - float cx_lb = out[0 * num_anchors + a]; - float cy_lb = out[1 * num_anchors + a]; - float w_lb = out[2 * num_anchors + a]; - float h_lb = out[3 * num_anchors + a]; - - float best_score = 0.0f; - int best_class = 0; - for (int c = 0; c < nc; c++) { - float s = out[(4 + c) * num_anchors + a]; - if (s > best_score) { best_score = s; best_class = c; } - } - if (best_score < config.confidence_threshold) continue; - - float x1_lb = cx_lb - w_lb / 2.0f; - float y1_lb = cy_lb - h_lb / 2.0f; - float x2_lb = cx_lb + w_lb / 2.0f; - float y2_lb = cy_lb + h_lb / 2.0f; - - float x1 = std::max(0.0f, std::min((x1_lb - pad_x) / scale, static_cast(orig_w - 1))); - float y1 = std::max(0.0f, std::min((y1_lb - pad_y) / scale, static_cast(orig_h - 1))); - float x2 = std::max(0.0f, std::min((x2_lb - pad_x) / scale, static_cast(orig_w - 1))); - float y2 = std::max(0.0f, std::min((y2_lb - pad_y) / scale, static_cast(orig_h - 1))); - candidates.push_back({x1, y1, x2, y2, best_score, best_class}); - } - } else { - // (1, anchors, 4+N) or (1, anchors, 5+N) - int num_anchors = static_cast(out_shape[1]); - int row_stride = static_cast(out_shape[2]); - int cls_offset = (resolved_layout == YoloOutputLayout::NC_LAST_OBJ) ? 5 : 4; - - for (int a = 0; a < num_anchors; a++) { - const float* row = out + a * row_stride; - float cx_lb = row[0], cy_lb = row[1], w_lb = row[2], h_lb = row[3]; - float obj_conf = (resolved_layout == YoloOutputLayout::NC_LAST_OBJ) ? row[4] : 1.0f; - - float best_score = 0.0f; - int best_class = 0; - for (int c = 0; c < nc; c++) { - float s = obj_conf * row[cls_offset + c]; - if (s > best_score) { best_score = s; best_class = c; } - } - if (best_score < config.confidence_threshold) continue; - - float x1_lb = cx_lb - w_lb / 2.0f; - float y1_lb = cy_lb - h_lb / 2.0f; - float x2_lb = cx_lb + w_lb / 2.0f; - float y2_lb = cy_lb + h_lb / 2.0f; - - float x1 = std::max(0.0f, std::min((x1_lb - pad_x) / scale, static_cast(orig_w - 1))); - float y1 = std::max(0.0f, std::min((y1_lb - pad_y) / scale, static_cast(orig_h - 1))); - float x2 = std::max(0.0f, std::min((x2_lb - pad_x) / scale, static_cast(orig_w - 1))); - float y2 = std::max(0.0f, std::min((y2_lb - pad_y) / scale, static_cast(orig_h - 1))); - candidates.push_back({x1, y1, x2, y2, best_score, best_class}); - } - } - - return nms(std::move(candidates), config.nms_threshold); -} - -// --- Config --- - -YoloOnnxConfig YoloOnnxConfig::from_env() { - YoloOnnxConfig c; - c.model_path = util::get_env_str("ZED_ONNX_MODEL_PATH", c.model_path); - c.confidence_threshold = util::get_env_float("ZED_ONNX_CONFIDENCE", c.confidence_threshold); - c.nms_threshold = util::get_env_float("ZED_ONNX_NMS_THRESHOLD", c.nms_threshold); - c.input_size = util::get_env_int("ZED_ONNX_INPUT_SIZE", c.input_size); - c.labels = util::get_env_str("ZED_ONNX_LABELS", c.labels); - c.labels_file = util::get_env_str("ZED_ONNX_LABELS_FILE", c.labels_file); - - std::string fmt = util::get_env_str("ZED_ONNX_OUTPUT_FORMAT", ""); - if (fmt == "nc_first" || fmt == "yolov8" || fmt == "yolov11") - c.output_layout = YoloOutputLayout::NC_FIRST; - else if (fmt == "nc_last") - c.output_layout = YoloOutputLayout::NC_LAST; - else if (fmt == "nc_last_obj" || fmt == "yolov5") - c.output_layout = YoloOutputLayout::NC_LAST_OBJ; - - return c; -} - -// --- YoloOnnx --- - -YoloOnnx::YoloOnnx(const YoloOnnxConfig& config) - : config_(config), impl_(std::make_unique()) {} - -YoloOnnx::~YoloOnnx() = default; - -void YoloOnnx::enable(sl::Camera& /* camera */) { - if (config_.model_path.empty()) { - std::cerr << "YoloOnnx: ZED_ONNX_MODEL_PATH is not set." << std::endl; - return; - } - - OrtCUDAProviderOptions cuda_opts{}; - cuda_opts.device_id = 0; - try { - impl_->session_options.AppendExecutionProvider_CUDA(cuda_opts); - } catch (const Ort::Exception& e) { - std::cerr << "YoloOnnx: CUDA unavailable (" << e.what() << "), using CPU." << std::endl; - } - impl_->session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL); - - std::cout << "YoloOnnx: loading model (" << config_.model_path << ")..." << std::endl; - try { - impl_->session = std::make_unique( - impl_->env, config_.model_path.c_str(), impl_->session_options); - } catch (const Ort::Exception& e) { - std::cerr << "YoloOnnx: failed to load model: " << e.what() << std::endl; - return; - } - - impl_->input_name = impl_->session->GetInputNameAllocated(0, impl_->allocator).get(); - impl_->output_name = impl_->session->GetOutputNameAllocated(0, impl_->allocator).get(); - - size_t num_inputs = impl_->session->GetInputCount(); - if (num_inputs >= 2) { - std::cerr << "YoloOnnx: model has " << num_inputs << " inputs — " - "this engine only supports single-input models." - << std::endl; - } - - auto out_info = impl_->session->GetOutputTypeInfo(0).GetTensorTypeAndShapeInfo(); - auto shape = out_info.GetShape(); - if (shape.size() == 3) { - int64_t dim1 = shape[1]; - int64_t dim2 = shape[2]; - - YoloOutputLayout resolved; - if (config_.output_layout == YoloOutputLayout::AUTO) { - // dim1 < dim2 → NC_FIRST (e.g. 84 vs 8400 for YOLOv8/11) - // dim1 > dim2 → NC_LAST_OBJ (e.g. 25200 vs 85 for YOLOv5) - resolved = (dim1 < dim2) ? YoloOutputLayout::NC_FIRST - : YoloOutputLayout::NC_LAST_OBJ; - } else { - resolved = config_.output_layout; - } - impl_->resolved_layout = resolved; - - if (resolved == YoloOutputLayout::NC_FIRST) { - impl_->num_classes = static_cast(dim1) - 4; - } else if (resolved == YoloOutputLayout::NC_LAST) { - impl_->num_classes = static_cast(dim2) - 4; - } else { // NC_LAST_OBJ - impl_->num_classes = static_cast(dim2) - 5; - } - } - - if (!config_.labels_file.empty() && config_.labels.empty()) { - std::ifstream lf(config_.labels_file); - if (!lf) { - std::cerr << "YoloOnnx: cannot open ZED_ONNX_LABELS_FILE: " - << config_.labels_file << std::endl; - } else { - std::string line, joined; - while (std::getline(lf, line)) { - size_t l = line.find_first_not_of(" \t\r"); - size_t r = line.find_last_not_of(" \t\r"); - if (l == std::string::npos) continue; - if (!joined.empty()) joined += ','; - joined += line.substr(l, r - l + 1); - } - if (!joined.empty()) { - config_.labels = joined; - std::cout << "YoloOnnx: loaded labels from file (" - << config_.labels_file << ")" << std::endl; - } - } - } - - std::cout << "YoloOnnx: model loaded." - << " Classes=" << impl_->num_classes - << " Layout=" << (impl_->resolved_layout == YoloOutputLayout::NC_FIRST ? "NC_FIRST" - : impl_->resolved_layout == YoloOutputLayout::NC_LAST ? "NC_LAST" - : "NC_LAST_OBJ") - << std::endl; -} - -void YoloOnnx::detect(sl::Camera& camera) { - detected_.clear(); - if (!impl_->session) return; - - sl::Mat img_mat; - camera.retrieveImage(img_mat, sl::VIEW::LEFT); - - sl::Mat pc_mat; - camera.retrieveMeasure(pc_mat, sl::MEASURE::XYZRGBA); - - sl::CameraInformation info = camera.getCameraInformation(); - auto calib = info.camera_configuration.calibration_parameters.left_cam; - float fx = calib.fx, fy = calib.fy, cx = calib.cx, cy = calib.cy; - - int orig_w = static_cast(img_mat.getWidth()); - int orig_h = static_cast(img_mat.getHeight()); - - cv::Mat rgba(orig_h, orig_w, CV_8UC4, img_mat.getPtr()); - cv::Mat bgr; - cv::cvtColor(rgba, bgr, cv::COLOR_RGBA2BGR); - - auto kept = impl_->run_inference(config_, bgr); - - for (const auto& det : kept) { - FusionResult fusion = fuse_bbox(pc_mat, - static_cast(det.x1), static_cast(det.y1), - static_cast(det.x2), static_cast(det.y2)); - - DetectedObject obj{}; - obj.label_id = det.class_id; - obj.confidence = det.score * 100.0f; - - if (fusion.valid) { - obj.position[0] = fusion.cx; - obj.position[1] = fusion.cy; - obj.position[2] = fusion.cz; - } - - obj.bounding_box_2d[0] = det.x1; obj.bounding_box_2d[1] = det.y1; - obj.bounding_box_2d[2] = det.x2; obj.bounding_box_2d[3] = det.y1; - obj.bounding_box_2d[4] = det.x2; obj.bounding_box_2d[5] = det.y2; - obj.bounding_box_2d[6] = det.x1; obj.bounding_box_2d[7] = det.y2; - - if (fusion.valid) { - float corners_u[4] = {det.x1, det.x2, det.x2, det.x1}; - float corners_v[4] = {det.y1, det.y1, det.y2, det.y2}; - float depths[2] = {fusion.depth_near, fusion.depth_far}; - for (int face = 0; face < 2; face++) { - for (int c = 0; c < 4; c++) { - float X, Y, Z; - backproject(corners_u[c], corners_v[c], depths[face], - fx, fy, cx, cy, X, Y, Z); - int idx = (face * 4 + c) * 3; - obj.bounding_box_3d[idx + 0] = X; - obj.bounding_box_3d[idx + 1] = Y; - obj.bounding_box_3d[idx + 2] = Z; - } - } - } - - detected_.push_back(obj); - } -} - -void YoloOnnx::detect(const cv::Mat& image_bgr) { - detected_.clear(); - if (!impl_->session) return; - - auto kept = impl_->run_inference(config_, image_bgr); - - for (const auto& det : kept) { - DetectedObject obj{}; - obj.label_id = det.class_id; - obj.confidence = det.score * 100.0f; - obj.bounding_box_2d[0] = det.x1; obj.bounding_box_2d[1] = det.y1; - obj.bounding_box_2d[2] = det.x2; obj.bounding_box_2d[3] = det.y1; - obj.bounding_box_2d[4] = det.x2; obj.bounding_box_2d[5] = det.y2; - obj.bounding_box_2d[6] = det.x1; obj.bounding_box_2d[7] = det.y2; - detected_.push_back(obj); - } -} - -const std::vector& YoloOnnx::objects() const { - return detected_; -} - -std::string YoloOnnx::label_names() const { - return config_.labels; -} - -} // namespace engines diff --git a/src/engines/yolo_onnx.h b/src/engines/yolo_onnx.h deleted file mode 100644 index 8dec6b4..0000000 --- a/src/engines/yolo_onnx.h +++ /dev/null @@ -1,48 +0,0 @@ -#pragma once - -#include "engines/engine.h" - -#include -#include -#include - -namespace engines { - -enum class YoloOutputLayout { - AUTO, // detect from output tensor shape at load time - NC_FIRST, // (1, 4+N, anchors) — YOLOv8, YOLOv11 - NC_LAST, // (1, anchors, 4+N) — rows-first, no objectness - NC_LAST_OBJ // (1, anchors, 5+N) — YOLOv5, rows-first with objectness -}; - -struct YoloOnnxConfig { - std::string model_path; // ZED_ONNX_MODEL_PATH (required) - float confidence_threshold = 0.25f; // ZED_ONNX_CONFIDENCE - float nms_threshold = 0.45f; // ZED_ONNX_NMS_THRESHOLD - int input_size = 640; // ZED_ONNX_INPUT_SIZE - std::string labels; // ZED_ONNX_LABELS (comma-separated) - std::string labels_file; // ZED_ONNX_LABELS_FILE (one class name per line) - YoloOutputLayout output_layout = YoloOutputLayout::AUTO; - - static YoloOnnxConfig from_env(); -}; - -class YoloOnnx : public Engine { -public: - explicit YoloOnnx(const YoloOnnxConfig& config); - ~YoloOnnx() override; - - void enable(sl::Camera& camera) override; - void detect(sl::Camera& camera) override; - void detect(const cv::Mat& image_bgr); // image-only; no 3D fusion - const std::vector& objects() const override; - std::string label_names() const override; - -private: - YoloOnnxConfig config_; - struct Impl; - std::unique_ptr impl_; - std::vector detected_; -}; - -} // namespace engines diff --git a/src/engines/zed_native.cc b/src/engines/zed_native.cc deleted file mode 100644 index efe6600..0000000 --- a/src/engines/zed_native.cc +++ /dev/null @@ -1,99 +0,0 @@ -#include "engines/zed_native.h" -#include "util/env.h" - -#include -#include - -namespace { - -sl::OBJECT_DETECTION_MODEL parse_detection_model(const std::string& model) { - if (model == "MULTI_CLASS_BOX_FAST") { - return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST; - } - if (model == "MULTI_CLASS_BOX_ACCURATE") { - return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE; - } - return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM; -} - -} // namespace - -namespace engines { - -DetectionConfig DetectionConfig::from_env() { - DetectionConfig c; - c.model = util::get_env_str("ZED_DETECTION_MODEL", c.model); - c.confidence_threshold = util::get_env_float("ZED_DETECTION_CONFIDENCE", c.confidence_threshold); - c.tracking = util::get_env_bool("ZED_DETECTION_TRACKING", c.tracking); - return c; -} - -ZedNative::ZedNative(const DetectionConfig& config) : config_(config) { - det_params_.detection_model = parse_detection_model(config_.model); - det_params_.enable_tracking = config_.tracking; - det_params_.enable_segmentation = false; - - det_rt_params_.detection_confidence_threshold = config_.confidence_threshold; -} - -void ZedNative::enable(sl::Camera& camera) { - if (config_.tracking) { - sl::PositionalTrackingParameters tracking_params; - sl::ERROR_CODE err = camera.enablePositionalTracking(tracking_params); - if (err != sl::ERROR_CODE::SUCCESS) { - std::cerr << "Failed to enable positional tracking: " - << sl::toString(err) << std::endl; - } - } - - std::cout << "Object Detection: loading model (" << config_.model << ")..." << std::endl; - sl::ERROR_CODE err = camera.enableObjectDetection(det_params_); - if (err != sl::ERROR_CODE::SUCCESS) { - std::cerr << "Failed to enable object detection: " - << sl::toString(err) << std::endl; - return; - } - std::cout << "Object Detection: model loaded." << std::endl; -} - -void ZedNative::detect(sl::Camera& camera) { - detected_.clear(); - - sl::ERROR_CODE err = camera.retrieveObjects(raw_objects_, det_rt_params_); - if (err != sl::ERROR_CODE::SUCCESS) { - return; - } - - for (const auto& obj : raw_objects_.object_list) { - DetectedObject det{}; - det.label_id = static_cast(obj.label); - det.confidence = obj.confidence; - - det.position[0] = obj.position.x; - det.position[1] = obj.position.y; - det.position[2] = obj.position.z; - - for (int i = 0; i < 4 && i < static_cast(obj.bounding_box_2d.size()); i++) { - det.bounding_box_2d[i * 2 + 0] = obj.bounding_box_2d[i].x; - det.bounding_box_2d[i * 2 + 1] = obj.bounding_box_2d[i].y; - } - - for (int i = 0; i < 8 && i < static_cast(obj.bounding_box.size()); i++) { - det.bounding_box_3d[i * 3 + 0] = obj.bounding_box[i].x; - det.bounding_box_3d[i * 3 + 1] = obj.bounding_box[i].y; - det.bounding_box_3d[i * 3 + 2] = obj.bounding_box[i].z; - } - - detected_.push_back(det); - } -} - -const std::vector& ZedNative::objects() const { - return detected_; -} - -std::string ZedNative::label_names() const { - return "PERSON,VEHICLE,BAG,ANIMAL,ELECTRONICS,FRUIT_VEGETABLE,SPORT"; -} - -} // namespace engines diff --git a/src/engines/zed_native.h b/src/engines/zed_native.h deleted file mode 100644 index 45df940..0000000 --- a/src/engines/zed_native.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include "engines/engine.h" - -#include - -namespace engines { - -struct DetectionConfig { - std::string model = "MULTI_CLASS_BOX_MEDIUM"; - float confidence_threshold = 40.0f; - bool tracking = false; - - static DetectionConfig from_env(); -}; - -class ZedNative : public Engine { -public: - explicit ZedNative(const DetectionConfig& config); - - void enable(sl::Camera& camera) override; - void detect(sl::Camera& camera) override; - const std::vector& objects() const override; - std::string label_names() const override; - -private: - DetectionConfig config_; - sl::ObjectDetectionParameters det_params_; - sl::ObjectDetectionRuntimeParameters det_rt_params_; - sl::Objects raw_objects_; - std::vector detected_; -}; - -} // namespace engines diff --git a/src/main.cc b/src/main.cc index c06c814..e9cd256 100644 --- a/src/main.cc +++ b/src/main.cc @@ -1,32 +1,22 @@ -#include "engines/engine.h" -#include "engines/yolo_onnx.h" -#include "engines/zed_native.h" -#include "util/env.h" -#include "zed/camera.h" +#include "zed/capture.h" #include -#include #include -#include +#include #include #include -#include #include #include namespace { -constexpr int FIELDS_PER_OBJECT = 37; - -enum class PcMode { FULL, HALF, ROI }; - -rust::Vec make_shape(int height, int width, int channels) { - rust::Vec shape; - shape.push_back(height); - shape.push_back(width); - shape.push_back(channels); - return shape; +rust::Vec make_vector_int(std::initializer_list values) { + rust::Vec out; + for (int64_t v : values) { + out.push_back(v); + } + return out; } rust::Vec make_distortion(const std::vector& distortion) { @@ -37,58 +27,55 @@ rust::Vec make_distortion(const std::vector& distortion) { return values; } -rust::Vec make_vector_int(std::initializer_list values) { - rust::Vec out; - for (int64_t v : values) { - out.push_back(v); - } - return out; +void set_intrinsics(Metadata& metadata, const zed::Calibration& calib) { + metadata.set_float("fx", calib.fx); + metadata.set_float("fy", calib.fy); + metadata.set_float("cx", calib.cx); + metadata.set_float("cy", calib.cy); + metadata.set_list_float("distortion", make_distortion(calib.distortion)); } -std::vector serialize_objects(const std::vector& objects) { - std::vector buffer(objects.size() * FIELDS_PER_OBJECT); - for (size_t i = 0; i < objects.size(); i++) { - float* row = buffer.data() + i * FIELDS_PER_OBJECT; - const auto& obj = objects[i]; - row[0] = static_cast(obj.label_id); - row[1] = obj.confidence; - row[2] = obj.position[0]; - row[3] = obj.position[1]; - row[4] = obj.position[2]; - std::memcpy(row + 5, obj.bounding_box_2d, sizeof(obj.bounding_box_2d)); - std::memcpy(row + 13, obj.bounding_box_3d, sizeof(obj.bounding_box_3d)); +// Both clocks are needed. The device timestamp is the instant the frame was +// captured and is the only value two nodes reading the same frame can agree on. +// The host timestamp is what the rest of the dataflow and the session recorder +// share, and on a Jetson it can step when NTP synchronises after boot - which is +// exactly why the device timestamp is recorded alongside it rather than instead +// of it. +void set_frame_stamps(Metadata& metadata, const zed::Frame& frame) { + metadata.set_int("frame_id", frame.frame_id); + metadata.set_timestamp("device_timestamp_ns", frame.device_ns); + metadata.set_timestamp("host_timestamp_ns", frame.host_ns); +} + +void report(const char* output, const DoraResult& result) { + if (!std::string(result.error).empty()) { + std::cerr << "Error sending " << output << ": " << std::string(result.error) << std::endl; } - return buffer; +} + +int64_t host_timestamp_ns() { + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); } } // namespace int main() { + // Read the environment before joining the dataflow, so a bad configuration + // fails with its own message rather than after the node has registered. + auto config = zed::Config::from_env(); + auto dora_node = init_dora_node(); - auto config = zed::Config::from_env(); - zed::Camera camera(config); - camera.open(); - camera.warmup(); - auto calib = camera.calibration(); + // The camera runs on its own thread. The SDK can block for an unbounded time + // inside grab() and open() when the camera is disconnected, so nothing below + // may call into it: this loop only reads what the capture thread has already + // produced, which is what keeps Stop answerable while the camera is down. + zed::Capture capture(config); + capture.start(); - std::string engine_name = util::get_env_str("ZED_ENGINE", "native"); - std::unique_ptr detector; - if (engine_name == "yolo_onnx" || engine_name == "onnx") { - auto cfg = engines::YoloOnnxConfig::from_env(); - detector = std::make_unique(cfg); - } else { - auto cfg = engines::DetectionConfig::from_env(); - detector = std::make_unique(cfg); - } - detector->enable(camera.handle()); - - std::string pc_mode_str = util::get_env_str("ZED_PC_MODE", "FULL"); - PcMode pc_mode = (pc_mode_str == "HALF") ? PcMode::HALF - : (pc_mode_str == "ROI") ? PcMode::ROI - : PcMode::FULL; - - int64_t frame_id = 0; + zed::Frame frame; while (true) { auto event = dora_node.events->next(); auto type = event_type(event); @@ -101,180 +88,104 @@ int main() { continue; } - if (!camera.grab()) { - std::cerr << "Grab failed; attempting to reconnect..." << std::endl; - camera.close(); - camera.open(); - calib = camera.calibration(); - detector->enable(camera.handle()); + std::string state; + if (capture.take_state_change(state)) { + rust::Slice state_slice{ + reinterpret_cast(state.data()), state.size()}; + + auto state_metadata = new_metadata(); + state_metadata->set_string("dtype", "string"); + state_metadata->set_string("encoding", "utf8"); + state_metadata->set_timestamp("host_timestamp_ns", host_timestamp_ns()); + + report("camera_state", + send_output_with_metadata(dora_node.send_output, "camera_state", state_slice, + std::move(state_metadata))); + } + + // No new frame since the last tick: the camera is slower than the timer, + // or it is down and the capture thread is recovering it. + if (!capture.take_frame(frame)) { continue; } - detector->detect(camera.handle()); - - sl::Mat& image_mat = camera.image(); - sl::Mat& point_cloud_mat = camera.point_cloud(); - - int width = image_mat.getWidth(); - int height = image_mat.getHeight(); - - int64_t current_frame_id = frame_id++; - int64_t timestamp_ns = static_cast( - std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch() - ).count() - ); - // --- image_bgr --- - cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr()); - cv::Mat image_bgr; - cv::cvtColor(image_rgba, image_bgr, cv::COLOR_BGRA2BGR); - - const auto image_size = static_cast(image_bgr.total() * image_bgr.elemSize()); - rust::Slice image_slice{ - reinterpret_cast(image_bgr.data), image_size}; + rust::Slice image_slice{frame.image_bgr.data(), frame.image_bgr.size()}; auto image_metadata = new_metadata(); image_metadata->set_string("dtype", "uint8"); image_metadata->set_string("encoding", "bgr8"); image_metadata->set_string("layout", "HWC"); - image_metadata->set_list_int("shape", make_shape(height, width, 3)); - image_metadata->set_float("fx", calib.fx); - image_metadata->set_float("fy", calib.fy); - image_metadata->set_float("cx", calib.cx); - image_metadata->set_float("cy", calib.cy); - image_metadata->set_list_float("distortion", make_distortion(calib.distortion)); - image_metadata->set_int("frame_id", current_frame_id); - image_metadata->set_int("timestamp_ns", timestamp_ns); + image_metadata->set_list_int("shape", make_vector_int({frame.height, frame.width, 3})); + set_intrinsics(*image_metadata, frame.calib); + set_frame_stamps(*image_metadata, frame); - auto image_result = send_output_with_metadata( - dora_node.send_output, "image_bgr", image_slice, std::move(image_metadata)); - if (!std::string(image_result.error).empty()) { - std::cerr << "Error sending image_bgr: " << std::string(image_result.error) << std::endl; - } + report("image_bgr", send_output_with_metadata(dora_node.send_output, "image_bgr", + image_slice, std::move(image_metadata))); + + // --- depth --- + rust::Slice depth_slice{ + reinterpret_cast(frame.depth.data()), + frame.depth.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({frame.height, frame.width})); + set_intrinsics(*depth_metadata, frame.calib); + set_frame_stamps(*depth_metadata, frame); + + report("depth", send_output_with_metadata(dora_node.send_output, "depth", depth_slice, + std::move(depth_metadata))); // --- point_cloud --- - int pc_width = point_cloud_mat.getWidth(); - int pc_height = point_cloud_mat.getHeight(); + if (frame.has_point_cloud) { + rust::Slice pc_slice{ + reinterpret_cast(frame.point_cloud.data()), + frame.point_cloud.size() * sizeof(float)}; - 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"); - pc_metadata->set_float("fx", calib.fx); - pc_metadata->set_float("fy", calib.fy); - pc_metadata->set_float("cx", calib.cx); - pc_metadata->set_float("cy", calib.cy); - pc_metadata->set_list_float("distortion", make_distortion(calib.distortion)); - - 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; - } - } + auto pc_metadata = new_metadata(); + pc_metadata->set_string("dtype", "float32"); + pc_metadata->set_string("channels", "XYZRGBA"); + pc_metadata->set_string("units", "mm"); 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 if (pc_mode == PcMode::ROI) { - const auto& objs = detector->objects(); - for (const auto& obj : objs) { - int x1 = std::max(0, static_cast(obj.bounding_box_2d[0])); - int y1 = std::max(0, static_cast(obj.bounding_box_2d[1])); - int x2 = std::min(pc_width - 1, static_cast(obj.bounding_box_2d[4])); - int y2 = std::min(pc_height - 1, static_cast(obj.bounding_box_2d[5])); - for (int y = y1; y <= y2; y++) { - for (int x = x1; x <= x2; x++) { - sl::float4 pt; - point_cloud_mat.getValue(x, y, &pt); - if (!std::isnan(pt.x) && !std::isinf(pt.x)) { - pc_buf.push_back(pt.x); pc_buf.push_back(pt.y); - pc_buf.push_back(pt.z); pc_buf.push_back(pt.w); - } - } - } - } - int N = static_cast(pc_buf.size() / 4); - pc_metadata->set_string("layout", "NC"); - pc_metadata->set_string("mode", "roi"); - pc_metadata->set_list_int("shape", make_vector_int({N, 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_list_int("shape", + make_vector_int({frame.pc_height, frame.pc_width, 4})); + set_intrinsics(*pc_metadata, frame.calib); + set_frame_stamps(*pc_metadata, frame); - 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; + report("point_cloud", send_output_with_metadata(dora_node.send_output, "point_cloud", + pc_slice, std::move(pc_metadata))); } // --- 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 (frame.camera_info_first) { + float camera_data[4] = {frame.calib.fx, frame.calib.fy, frame.calib.cx, + frame.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)); + 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(frame.calib.distortion)); + set_frame_stamps(*cam_metadata, frame); - 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; - } - - // --- objects --- - const auto& objects = detector->objects(); - auto obj_buffer = serialize_objects(objects); - int num_objects = static_cast(objects.size()); - - const size_t obj_size = obj_buffer.size() * sizeof(float); - rust::Slice obj_slice{ - reinterpret_cast(obj_buffer.data()), obj_size}; - - auto obj_metadata = new_metadata(); - obj_metadata->set_string("dtype", "float32"); - obj_metadata->set_string("layout", "RC"); - obj_metadata->set_string("units", "mm"); - obj_metadata->set_list_int("shape", make_vector_int({num_objects, FIELDS_PER_OBJECT})); - obj_metadata->set_string("fields", - "label_id,confidence,x,y,z," - "bb2d_x0,bb2d_y0,bb2d_x1,bb2d_y1,bb2d_x2,bb2d_y2,bb2d_x3,bb2d_y3," - "bb3d_x0,bb3d_y0,bb3d_z0,bb3d_x1,bb3d_y1,bb3d_z1," - "bb3d_x2,bb3d_y2,bb3d_z2,bb3d_x3,bb3d_y3,bb3d_z3," - "bb3d_x4,bb3d_y4,bb3d_z4,bb3d_x5,bb3d_y5,bb3d_z5," - "bb3d_x6,bb3d_y6,bb3d_z6,bb3d_x7,bb3d_y7,bb3d_z7"); - obj_metadata->set_string("label_names", detector->label_names()); - obj_metadata->set_string("engine", engine_name); - obj_metadata->set_int("frame_id", current_frame_id); - obj_metadata->set_int("timestamp_ns", timestamp_ns); - obj_metadata->set_int("image_width", width); - obj_metadata->set_int("image_height", height); - - auto obj_result = send_output_with_metadata( - dora_node.send_output, "objects", obj_slice, std::move(obj_metadata)); - if (!std::string(obj_result.error).empty()) { - std::cerr << "Error sending objects: " << std::string(obj_result.error) << std::endl; + report("camera_info", send_output_with_metadata(dora_node.send_output, "camera_info", + cam_slice, std::move(cam_metadata))); } } + capture.stop(); + if (!capture.wait_exit(std::chrono::seconds(2))) { + // The capture thread is inside a blocking SDK call, where nothing can + // reach it. Leave rather than let dora wait out its grace period and kill + // the node: the frames are gone either way, and Stop has been answered. + std::cerr << "capture thread still in the SDK; exiting anyway" << std::endl; + std::cout.flush(); + std::_Exit(0); + } return 0; } diff --git a/src/test_yolo_onnx.cc b/src/test_yolo_onnx.cc deleted file mode 100644 index e1439d1..0000000 --- a/src/test_yolo_onnx.cc +++ /dev/null @@ -1,113 +0,0 @@ -// Standalone test for the YoloOnnx (closed-vocabulary) detection engine. -// No dora framework, no live ZED camera — loads an image from disk. -// -// Usage: -// bin/test_yolo_onnx [image_path] -// image_path default: test/assets/yolo.png -// -// Required env vars: -// ZED_ONNX_MODEL_PATH path to YOLO ONNX model -// -// Optional env vars: -// ZED_ONNX_LABELS comma-separated class names -// ZED_ONNX_LABELS_FILE path to newline-separated class names file -// ZED_ONNX_CONFIDENCE detection threshold (default 0.25) -// ZED_ONNX_NMS_THRESHOLD NMS IoU threshold (default 0.45) -// ZED_ONNX_INPUT_SIZE model input size (default 640) -// ZED_ONNX_OUTPUT_FORMAT nc_first|nc_last|nc_last_obj|yolov5|yolov8|yolov11|auto - -#include "engines/yolo_onnx.h" -#include "util/env.h" - -#include -#include - -#include -#include -#include -#include -#include - -namespace fs = std::filesystem; - -namespace { - -std::vector parse_csv(const std::string& s) { - std::vector out; - std::istringstream ss(s); - std::string tok; - while (std::getline(ss, tok, ',')) { - size_t l = tok.find_first_not_of(" \t"); - size_t r = tok.find_last_not_of(" \t"); - if (l != std::string::npos) - out.push_back(tok.substr(l, r - l + 1)); - } - return out; -} - -void draw_detections(cv::Mat& img, - const std::vector& objects, - const std::vector& labels) { - for (const auto& obj : objects) { - int x1 = static_cast(obj.bounding_box_2d[0]); - int y1 = static_cast(obj.bounding_box_2d[1]); - int x2 = static_cast(obj.bounding_box_2d[4]); - int y2 = static_cast(obj.bounding_box_2d[5]); - - std::string name = (obj.label_id >= 0 && obj.label_id < static_cast(labels.size())) - ? labels[obj.label_id] : std::to_string(obj.label_id); - - char buf[64]; - std::snprintf(buf, sizeof(buf), " %.2f", obj.confidence / 100.0f); - - cv::rectangle(img, {x1, y1}, {x2, y2}, {0, 200, 0}, 2); - cv::putText(img, name + buf, {x1, std::max(y1 - 6, 14)}, - cv::FONT_HERSHEY_SIMPLEX, 0.55, {0, 200, 0}, 2, cv::LINE_AA); - } -} - -} // namespace - -int main(int argc, char* argv[]) { - std::string image_path = (argc > 1) ? argv[1] : "test/assets/yolo.png"; - - auto config = engines::YoloOnnxConfig::from_env(); - - sl::Camera dummy_cam; - engines::YoloOnnx engine(config); - engine.enable(dummy_cam); - - cv::Mat img = cv::imread(image_path); - if (img.empty()) { - std::cerr << "Error: cannot read image: " << image_path << std::endl; - return 1; - } - - engine.detect(img); - const auto& objects = engine.objects(); - auto labels = parse_csv(engine.label_names()); - - std::cout << "[" << image_path << "] " << objects.size() << " detection(s):" << std::endl; - for (const auto& obj : objects) { - std::string name = (obj.label_id >= 0 && obj.label_id < static_cast(labels.size())) - ? labels[obj.label_id] : std::to_string(obj.label_id); - std::cout << " " << name - << " conf=" << obj.confidence / 100.0f - << " bbox=[" - << obj.bounding_box_2d[0] << "," << obj.bounding_box_2d[1] - << " -> " - << obj.bounding_box_2d[4] << "," << obj.bounding_box_2d[5] - << "]" << std::endl; - } - - cv::Mat annotated = img.clone(); - draw_detections(annotated, objects, labels); - - fs::path stem = fs::path(image_path).stem(); - fs::path parent = fs::path(image_path).parent_path(); - std::string result_path = (parent / (stem.string() + "_result.png")).string(); - cv::imwrite(result_path, annotated); - std::cout << "Saved: " << result_path << std::endl; - - return 0; -} diff --git a/src/zed/camera.cc b/src/zed/camera.cc index ecaaa9b..e104c68 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -1,11 +1,12 @@ #include "zed/camera.h" #include "util/env.h" -#include +#include +#include #include #include #include -#include +#include namespace { @@ -21,17 +22,30 @@ sl::RESOLUTION parse_resolution(int resolution) { } } +const std::pair DEPTH_MODES[] = { + {"NEURAL", sl::DEPTH_MODE::NEURAL}, + {"NEURAL_PLUS", sl::DEPTH_MODE::NEURAL_PLUS}, + {"NEURAL_LIGHT", sl::DEPTH_MODE::NEURAL_LIGHT}, + {"PERFORMANCE", sl::DEPTH_MODE::PERFORMANCE}, + {"QUALITY", sl::DEPTH_MODE::QUALITY}, + {"NONE", sl::DEPTH_MODE::NONE}, +}; + +// A typo here silently changes depth quality if it falls back to a default, and +// the resulting depth is wrong in a way that only shows up downstream, so an +// unrecognised name stops the node instead. sl::DEPTH_MODE parse_depth_mode(const std::string& mode) { - if (mode == "PERFORMANCE") { - return sl::DEPTH_MODE::PERFORMANCE; + for (const auto& entry : DEPTH_MODES) { + if (mode == entry.first) { + return entry.second; + } } - if (mode == "QUALITY") { - return sl::DEPTH_MODE::QUALITY; + std::cerr << "Unknown ZED_DEPTH_MODE '" << mode << "'. Accepted values:"; + for (const auto& entry : DEPTH_MODES) { + std::cerr << " " << entry.first; } - if (mode == "NONE") { - return sl::DEPTH_MODE::NONE; - } - return sl::DEPTH_MODE::NEURAL; + std::cerr << std::endl; + std::exit(1); } sl::FLIP_MODE parse_flip(const std::string& mode) { @@ -41,6 +55,24 @@ sl::FLIP_MODE parse_flip(const std::string& mode) { return sl::FLIP_MODE::ON; } +// A blank frame is checked on a strided sample rather than the full image, which +// is just as conclusive and costs nothing at frame rate. +constexpr int BLANK_FRAME_STRIDE = 16; + +// sl::Camera::reboot() reaches the ZED 2 and newer. On the first-generation ZED +// and the ZED Mini it returns without touching the device, so calling it there +// only costs time and fills the log. +bool model_supports_reboot(sl::MODEL model) { + return model != sl::MODEL::ZED && model != sl::MODEL::ZED_M; +} + +// The SDK reconnects a camera whose link hiccuped by itself. Dropping the handle +// over one of these takes the recovery away from the SDK and hands it to us, +// which is slower and, on a device the kernel is about to suspend, often fatal. +bool is_transient_grab_error(sl::ERROR_CODE err) { + return err == sl::ERROR_CODE::CAMERA_REBOOTING || err == sl::ERROR_CODE::CORRUPTED_FRAME; +} + void list_available_cameras() { auto devices = sl::Camera::getDeviceList(); std::cout << "Detected ZED cameras: " << devices.size() << std::endl; @@ -59,50 +91,112 @@ Config Config::from_env() { Config c; c.resolution = util::get_env_int("ZED_RESOLUTION", c.resolution); c.fps = util::get_env_int("ZED_FPS", c.fps); - c.depth_mode = util::get_env_str("ZED_DEPTH_MODE", c.depth_mode); + c.depth_mode = parse_depth_mode(util::get_env_str("ZED_DEPTH_MODE", "NEURAL")); 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.pc_mode = util::get_env_str("ZED_PC_MODE", "FULL") == "HALF" ? PcMode::HALF : PcMode::FULL; + 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); c.flip = util::get_env_str("ZED_FLIP", c.flip); c.warmup_frames = util::get_env_int("ZED_WARMUP_FRAMES", c.warmup_frames); - int retry_seconds = util::get_env_int("ZED_RETRY_INTERVAL_SECONDS", 5); - c.retry_interval_ms = retry_seconds * 1000; return c; } Camera::Camera(const Config& config) : config_(config) { + // Process-wide, and must be set before any camera is opened. Without it the + // epoch of the timestamps published alongside every frame depends on the SDK + // default rather than on anything this node states. + sl::setTimestampClock(sl::TIMESTAMP_CLOCK::SYSTEM_CLOCK); + init_params_.camera_resolution = parse_resolution(config_.resolution); init_params_.camera_fps = config_.fps; - init_params_.depth_mode = parse_depth_mode(config_.depth_mode); + init_params_.depth_mode = config_.depth_mode; init_params_.coordinate_units = sl::UNIT::MILLIMETER; init_params_.camera_image_flip = parse_flip(config_.flip); init_params_.depth_minimum_distance = config_.depth_min_mm; init_params_.depth_maximum_distance = config_.depth_max_mm; + // Recover a hiccuping link inside the SDK rather than out here. Without this + // a grab blocks for the roughly 17 s the SDK spends reconnecting, freezing the + // dora event loop; with it the grab returns CAMERA_REBOOTING straight away and + // the SDK reconnects in the background, holding the device open meanwhile. + init_params_.async_grab_camera_recovery = true; + // Let the SDK flag the frames a stalled camera hands back looking valid. + init_params_.enable_image_validity_check = 1; runtime_params_.enable_depth = true; runtime_params_.enable_fill_mode = config_.depth_fill; + // The two thresholds discard the depth the SDK is least certain of, which is + // the depth at object edges: those pixels straddle the object and whatever is + // behind it, and belong to neither. + runtime_params_.confidence_threshold = config_.confidence_threshold; + runtime_params_.texture_confidence_threshold = config_.texture_confidence_threshold; + + std::cout << "ZED depth: mode=" << sl::toString(init_params_.depth_mode) + << ", range=" << config_.depth_min_mm << "-" << config_.depth_max_mm << " mm" + << ", confidence=" << config_.confidence_threshold + << ", texture_confidence=" << config_.texture_confidence_threshold + << ", fill=" << (config_.depth_fill ? "on" : "off") << std::endl; + + list_available_cameras(); } Camera::~Camera() { close(); } -void Camera::open() { - list_available_cameras(); - while (true) { - sl::ERROR_CODE err = zed_.open(init_params_); - if (err == sl::ERROR_CODE::SUCCESS) { - return; - } +bool Camera::open() { + sl::ERROR_CODE err = zed_.open(init_params_); + if (err != sl::ERROR_CODE::SUCCESS) { std::cerr << "Failed to open ZED camera: " << sl::toString(err) << std::endl; - std::this_thread::sleep_for(std::chrono::milliseconds(config_.retry_interval_ms)); + return false; } + recovering_ = false; + + sl::CameraInformation info = zed_.getCameraInformation(); + serial_number_ = info.serial_number; + model_ = info.camera_model; + // After the first open, reopen the same physical camera. Recovery runs while + // the device may be mid-re-enumeration, and picking whichever ZED answers + // first is not the same thing as picking ours. + init_params_.input.setFromSerialNumber(serial_number_); + + std::cout << "ZED camera opened (" << sl::toString(model_) << ", SN " << serial_number_ << ")" + << std::endl; + return true; } void Camera::close() { + recovering_ = false; zed_.close(); } +void Camera::reset() { + close(); + if (serial_number_ == 0 || !model_supports_reboot(model_)) { + return; + } + // The ZED can stop delivering frames while staying enumerated: the video + // endpoint halts, grabs fail, and reopening returns CAMERA FAILED TO SETUP + // because the endpoint is still halted. A device reset clears that, and + // reboot() is the SDK's way to ask for one without unplugging the camera. + // The device re-enumerates afterwards, so the caller's backoff before the + // next open is what lets it settle. + sl::ERROR_CODE err = sl::Camera::reboot(static_cast(serial_number_), true); + std::cout << "reset ZED camera (SN " << serial_number_ << "): " << sl::toString(err) + << std::endl; +} + +bool Camera::is_open() const { + return zed_.isOpened(); +} + +bool Camera::recovering() const { + return recovering_; +} + void Camera::warmup() { for (int i = 0; i < config_.warmup_frames; i++) { zed_.grab(runtime_params_); @@ -110,11 +204,34 @@ void Camera::warmup() { } bool Camera::grab() { - if (zed_.grab(runtime_params_) != sl::ERROR_CODE::SUCCESS) { + sl::ERROR_CODE err = zed_.grab(runtime_params_); + if (err != sl::ERROR_CODE::SUCCESS) { + if (is_transient_grab_error(err)) { + // Every grab repeats this for as long as the reconnect takes, so only + // the first one is worth a line in the log. + if (!recovering_) { + recovering_ = true; + std::cerr << "ZED camera link dropped (" << sl::toString(err) + << "); the SDK is reconnecting it" << std::endl; + } + } else { + recovering_ = false; + std::cerr << "Failed to grab frame: " << sl::toString(err) << std::endl; + } return false; } + if (recovering_) { + recovering_ = false; + std::cout << "ZED camera link restored" << std::endl; + } + zed_.retrieveImage(image_mat_, sl::VIEW::LEFT); - zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA); + zed_.retrieveMeasure(depth_mat_, sl::MEASURE::DEPTH); + // 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; } @@ -135,16 +252,48 @@ Calibration Camera::calibration() const { return result; } +bool Camera::image_is_blank() { + const int height = image_mat_.getHeight(); + const int width = image_mat_.getWidth(); + if (height == 0 || width == 0) { + return true; + } + + const size_t step = image_mat_.getStep(sl::MEM::CPU); + const sl::uchar4* src = image_mat_.getPtr(); + + unsigned char lowest[3] = {255, 255, 255}; + unsigned char highest[3] = {0, 0, 0}; + for (int y = 0; y < height; y += BLANK_FRAME_STRIDE) { + const sl::uchar4* row = src + static_cast(y) * step; + for (int x = 0; x < width; x += BLANK_FRAME_STRIDE) { + const unsigned char channels[3] = {row[x].x, row[x].y, row[x].z}; + for (int c = 0; c < 3; c++) { + lowest[c] = std::min(lowest[c], channels[c]); + highest[c] = std::max(highest[c], channels[c]); + } + } + } + // Sensor noise makes a single-colour frame impossible for a live camera, so + // this tells a dead device apart from any real scene. + return lowest[0] == highest[0] && lowest[1] == highest[1] && lowest[2] == highest[2]; +} + +int64_t Camera::device_timestamp_ns() { + return static_cast( + zed_.getTimestamp(sl::TIME_REFERENCE::IMAGE).getNanoseconds()); +} + sl::Mat& Camera::image() { return image_mat_; } +sl::Mat& Camera::depth() { + return depth_mat_; +} + sl::Mat& Camera::point_cloud() { return point_cloud_mat_; } -sl::Camera& Camera::handle() { - return zed_; -} - } // namespace zed diff --git a/src/zed/camera.h b/src/zed/camera.h index 2b2f2a4..6235694 100644 --- a/src/zed/camera.h +++ b/src/zed/camera.h @@ -1,27 +1,33 @@ #pragma once #include +#include #include #include namespace zed { +enum class PcMode { FULL, HALF }; + struct Config { - int resolution = 720; + int resolution = 1080; int fps = 15; - std::string depth_mode = "NEURAL"; - int depth_min_mm = 10; - int depth_max_mm = 500; + sl::DEPTH_MODE depth_mode = sl::DEPTH_MODE::NEURAL; + int depth_min_mm = 200; + int depth_max_mm = 600; bool depth_fill = false; + bool point_cloud = false; + PcMode pc_mode = PcMode::FULL; + int confidence_threshold = 60; + int texture_confidence_threshold = 100; std::string flip = "ON"; int warmup_frames = 30; - int retry_interval_ms = 5000; static Config from_env(); }; struct Calibration { - float fx, fy, cx, cy; + float fx = 0, fy = 0, cx = 0, cy = 0; std::vector distortion; }; @@ -30,14 +36,26 @@ public: explicit Camera(const Config& config); ~Camera(); - void open(); + // One attempt, returning whether it succeeded. The caller decides when to try + // again, so a camera that is down never blocks the dora event loop. + bool open(); void close(); + // Close the handle and reset the device over USB, where the model allows it. + void reset(); void warmup(); bool grab(); + bool is_open() const; + // True while the SDK is reconnecting the camera after a link hiccup. + bool recovering() const; + // True when the last grabbed frame carries no picture at all. A camera whose + // link has dropped does not necessarily fail its grabs: it can keep reporting + // success and hand back a uniform frame. + bool image_is_blank(); Calibration calibration() const; + int64_t device_timestamp_ns(); sl::Mat& image(); + sl::Mat& depth(); sl::Mat& point_cloud(); - sl::Camera& handle(); private: Config config_; @@ -45,7 +63,13 @@ private: sl::InitParameters init_params_; sl::RuntimeParameters runtime_params_; sl::Mat image_mat_; + sl::Mat depth_mat_; sl::Mat point_cloud_mat_; + // Read off the device once it is open and kept afterwards: resetting a wedged + // camera needs the serial and the model, and by then the handle is gone. + unsigned int serial_number_ = 0; + sl::MODEL model_ = sl::MODEL::LAST; + bool recovering_ = false; }; } // namespace zed diff --git a/src/zed/capture.cc b/src/zed/capture.cc new file mode 100644 index 0000000..f90c74d --- /dev/null +++ b/src/zed/capture.cc @@ -0,0 +1,302 @@ +#include "zed/capture.h" + +#include + +#include +#include +#include +#include + +namespace { + +using Clock = std::chrono::steady_clock; + +// Going this long without a usable frame means the camera is wedged rather than +// dropping the odd frame. Measured in elapsed time rather than in failed grabs, +// because a stalling camera can spend seconds inside a single grab. +constexpr double STALL_TIMEOUT_S = 5.0; + +// How long the SDK is left to reconnect the camera on its own before the loop +// stops waiting on it and rebuilds the camera anyway. A driver that says it is +// recovering is usually right, but it has no way to tell us it has given up. +constexpr double DRIVER_RECOVERY_TIMEOUT_S = 30.0; + +// How long to wait between grabs while the camera is not delivering. +constexpr std::chrono::milliseconds GRAB_RETRY{500}; + +// How long to wait before each reopen: one entry per attempt, the last repeated +// for every attempt beyond it. +constexpr int REOPEN_BACKOFF_S[] = {1, 2, 5, 10}; + +std::chrono::seconds reopen_backoff(int attempt) { + const size_t index = std::min(static_cast(attempt), std::size(REOPEN_BACKOFF_S)) - 1; + return std::chrono::seconds(REOPEN_BACKOFF_S[index]); +} + +double seconds_since(Clock::time_point since) { + return std::chrono::duration(Clock::now() - since).count(); +} + +int64_t host_timestamp_ns() { + return std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count(); +} + +// Copy the depth map 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. 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. +void pack_depth(sl::Mat& depth_mat, std::vector& out) { + 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(); + + out.resize(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; + } + } +} + +} // namespace + +namespace zed { + +Capture::Capture(const Config& config) : config_(config), camera_(config) {} + +Capture::~Capture() { + stop(); + if (thread_.joinable()) { + // The thread may still be inside the SDK. Detaching leaks it for the few + // moments the process has left, which is preferable to blocking here. + if (wait_exit(std::chrono::seconds(1))) { + thread_.join(); + } else { + thread_.detach(); + } + } +} + +void Capture::start() { + { + std::lock_guard lock(mutex_); + running_ = true; + } + thread_ = std::thread(&Capture::run, this); +} + +void Capture::stop() { + std::lock_guard lock(mutex_); + running_ = false; + wake_.notify_all(); +} + +bool Capture::wait_exit(std::chrono::milliseconds timeout) { + std::unique_lock lock(mutex_); + return exited_.wait_for(lock, timeout, [this] { return finished_; }); +} + +bool Capture::take_frame(Frame& out) { + std::lock_guard lock(mutex_); + if (!has_frame_) { + return false; + } + std::swap(out, latest_); + has_frame_ = false; + return true; +} + +bool Capture::take_state_change(std::string& out) { + std::lock_guard lock(mutex_); + if (!state_changed_) { + return false; + } + out = state_; + state_changed_ = false; + return true; +} + +void Capture::set_state(const char* state) { + std::lock_guard lock(mutex_); + if (state_ == state) { + return; + } + state_ = state; + state_changed_ = true; + std::cout << "camera state: " << state_ << std::endl; +} + +void Capture::sleep_interruptible(std::chrono::milliseconds duration) { + std::unique_lock lock(mutex_); + wake_.wait_for(lock, duration, [this] { return !running_; }); +} + +void Capture::begin_recovery(int attempt) { + // The first attempt only closes the handle, which is enough when the driver + // rather than the device lost its way; past that the device itself is wedged + // and nothing short of resetting it will bring the frames back. + if (attempt <= 1) { + camera_.close(); + } else { + camera_.reset(); + } + const auto wait = reopen_backoff(attempt); + std::cerr << "recovering camera (attempt " << attempt << "); next open in " << wait.count() + << "s" << std::endl; + sleep_interruptible(std::chrono::duration_cast(wait)); +} + +void Capture::pack_frame(Frame& frame, int64_t frame_id, bool camera_info_first) { + sl::Mat& image_mat = camera_.image(); + const int width = image_mat.getWidth(); + const int height = image_mat.getHeight(); + + frame.frame_id = frame_id; + frame.device_ns = camera_.device_timestamp_ns(); + frame.host_ns = host_timestamp_ns(); + frame.width = width; + frame.height = height; + frame.calib = camera_.calibration(); + frame.camera_info_first = camera_info_first; + + cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr(), + image_mat.getStep(sl::MEM::CPU) * sizeof(sl::uchar4)); + frame.image_bgr.resize(static_cast(height) * width * 3); + cv::Mat image_bgr(height, width, CV_8UC3, frame.image_bgr.data()); + cv::cvtColor(image_rgba, image_bgr, cv::COLOR_BGRA2BGR); + + pack_depth(camera_.depth(), frame.depth); + + frame.has_point_cloud = config_.point_cloud; + if (!frame.has_point_cloud) { + return; + } + + // Unlike the image and depth, the cloud used to be published straight from + // the SDK buffer. It has to be copied now that the reader is another thread. + sl::Mat& pc_mat = camera_.point_cloud(); + const int pc_width = pc_mat.getWidth(); + const int pc_height = pc_mat.getHeight(); + const size_t pc_step = pc_mat.getStep(sl::MEM::CPU); + const sl::float4* src = pc_mat.getPtr(); + + const int stride = config_.pc_mode == PcMode::HALF ? 2 : 1; + frame.pc_width = pc_width / stride; + frame.pc_height = pc_height / stride; + frame.point_cloud.resize(static_cast(frame.pc_height) * frame.pc_width * 4); + + for (int y = 0; y < frame.pc_height; y++) { + const sl::float4* row = src + static_cast(y * stride) * pc_step; + float* dst = frame.point_cloud.data() + static_cast(y) * frame.pc_width * 4; + for (int x = 0; x < frame.pc_width; x++) { + const sl::float4& point = row[x * stride]; + dst[x * 4 + 0] = point.x; + dst[x * 4 + 1] = point.y; + dst[x * 4 + 2] = point.z; + dst[x * 4 + 3] = point.w; + } + } +} + +void Capture::run() { + int recovery_attempts = 0; + bool streamed_once = false; + bool stall_reported = false; + bool camera_info_pending = false; + auto last_good_frame_at = Clock::now(); + int64_t frame_id = 0; + + set_state("opening"); + + while (true) { + { + std::lock_guard lock(mutex_); + if (!running_) { + break; + } + } + + const char* down_state = streamed_once ? "recovering" : "opening"; + + if (!camera_.is_open()) { + set_state(down_state); + if (!camera_.open()) { + recovery_attempts++; + begin_recovery(recovery_attempts); + continue; + } + // Discard the first frames so auto-exposure settles. This blocks, and + // on this thread that is fine. + camera_.warmup(); + camera_info_pending = true; + // Opening the ZED takes seconds. Start the stall clock from here so + // that cost is never mistaken for a stalled stream. + last_good_frame_at = Clock::now(); + // recovery_attempts is deliberately not cleared here. A wedged device + // can open and still deliver nothing; clearing on open would restart + // the escalation ladder every cycle and never reach the reset. + continue; + } + + // A blank frame counts as a failed grab: a camera whose link has dropped + // does not necessarily fail its grabs, it can keep reporting success and + // hand back a uniform frame with an empty depth map. + if (!camera_.grab() || camera_.image_is_blank()) { + set_state(down_state); + const double stalled_for = seconds_since(last_good_frame_at); + // The SDK holds the camera open while it reconnects a dropped link. + // Taking the handle away mid-reconnect aborts that and leaves us to + // redo it the slow way, so it gets longer than a plain stall. + const double patience = + camera_.recovering() ? DRIVER_RECOVERY_TIMEOUT_S : STALL_TIMEOUT_S; + if (stalled_for < patience) { + sleep_interruptible(GRAB_RETRY); + continue; + } + if (!stall_reported) { + stall_reported = true; + std::cerr << "camera delivered no usable frame for " << stalled_for << "s" + << std::endl; + } + recovery_attempts++; + begin_recovery(recovery_attempts); + continue; + } + + if (stall_reported) { + std::cout << "camera recovered after " << recovery_attempts << " attempt(s)" + << std::endl; + stall_reported = false; + } + last_good_frame_at = Clock::now(); + recovery_attempts = 0; + streamed_once = true; + + pack_frame(scratch_, frame_id++, camera_info_pending); + camera_info_pending = false; + { + std::lock_guard lock(mutex_); + // Swapping rather than assigning hands the previous frame's buffers + // back, so steady-state capture does not reallocate. + std::swap(latest_, scratch_); + has_frame_ = true; + } + set_state("streaming"); + } + + camera_.close(); + { + std::lock_guard lock(mutex_); + finished_ = true; + exited_.notify_all(); + } +} + +} // namespace zed diff --git a/src/zed/capture.h b/src/zed/capture.h new file mode 100644 index 0000000..a41305c --- /dev/null +++ b/src/zed/capture.h @@ -0,0 +1,94 @@ +#pragma once + +#include "zed/camera.h" + +#include +#include +#include +#include +#include +#include + +namespace zed { + +// One captured frame, owning its buffers. +// +// The SDK reuses its own buffers on the next grab, so nothing here can point +// into an sl::Mat: the capture thread copies every payload out before handing +// the frame over. +struct Frame { + int64_t frame_id = 0; + int64_t device_ns = 0; + int64_t host_ns = 0; + int width = 0; + int height = 0; + std::vector image_bgr; // height * width * 3 + std::vector depth; // height * width, millimetres + bool has_point_cloud = false; + int pc_width = 0; + int pc_height = 0; + std::vector point_cloud; // pc_height * pc_width * 4 + Calibration calib; + // Set on the first frame of a camera session, so camera_info is published + // once per session rather than per frame. + bool camera_info_first = false; +}; + +// Owns the camera on its own thread and hands the newest frame to the reader. +// +// The ZED SDK can block for an unbounded time inside grab() and open() — a +// disconnected camera has been measured blocking indefinitely, even with +// InitParameters::async_grab_camera_recovery set. Driving the camera from the +// dora event loop therefore froze the node: it stopped reading events, so Stop +// went unanswered and the recovery logic itself was never reached. Everything +// that can block lives on this thread instead, and the event loop only ever +// touches the buffer below. +class Capture { +public: + explicit Capture(const Config& config); + ~Capture(); + + void start(); + // Ask the thread to finish. Returns immediately: the thread may be inside a + // blocking SDK call and cannot be interrupted there. + void stop(); + // Wait for the thread to actually leave, up to timeout. False means it is + // still stuck in the SDK. + bool wait_exit(std::chrono::milliseconds timeout); + + // Move the newest frame out, if one arrived since the last call. Frames that + // are not collected are overwritten: the reader always wants the latest. + bool take_frame(Frame& out); + // Report the camera state, if it changed since the last call. + bool take_state_change(std::string& out); + +private: + void run(); + void set_state(const char* state); + // Release the camera and wait out the backoff for this attempt. The wait is + // interruptible, so stop() is honoured while the camera is down. + void begin_recovery(int attempt); + void sleep_interruptible(std::chrono::milliseconds duration); + void pack_frame(Frame& frame, int64_t frame_id, bool camera_info_first); + + Config config_; + Camera camera_; + std::thread thread_; + + std::mutex mutex_; + std::condition_variable wake_; + std::condition_variable exited_; + bool running_ = false; + bool finished_ = false; + + Frame latest_; + // Holds the previously published frame once the reader has taken the newest + // one, so its buffers are reused instead of reallocated every frame. + Frame scratch_; + bool has_frame_ = false; + + std::string state_; + bool state_changed_ = false; +}; + +} // namespace zed diff --git a/test/assets/.gitkeep b/test/assets/.gitkeep deleted file mode 100644 index e69de29..0000000