Compare commits
24
Commits
v0.1.0
..
a4cd493330
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
a4cd493330 | ||
|
|
c1a7d70ba6 | ||
|
|
d4c9f153c1 | ||
|
|
483da05a98 | ||
|
|
86b7a1a8f7 | ||
|
|
5822a870bb | ||
|
|
2f55ca3f74 | ||
|
|
ee48948de9 | ||
|
|
d8956f269d | ||
|
|
42c2b791fd | ||
|
|
617b8ceb50 | ||
|
|
15134dc12e | ||
|
|
dc7f9f5501 | ||
|
|
126dbf4f32 | ||
|
|
a734692b60 | ||
|
|
1dd1cefcc9 | ||
|
|
f40e697b2a | ||
|
|
b4144dfba5 | ||
|
|
d62f4fbb29 | ||
|
|
647be61b7b | ||
|
|
b16dfcd965 | ||
|
|
d6ae0f7fef | ||
|
|
f1934c9b4a | ||
|
|
44eec29b83 |
@@ -0,0 +1,5 @@
|
|||||||
|
.git/
|
||||||
|
build/
|
||||||
|
bin/
|
||||||
|
tmp/
|
||||||
|
models/
|
||||||
@@ -0,0 +1,50 @@
|
|||||||
|
# Two gates, neither of which needs a camera, the ZED SDK or CUDA: the dataflows
|
||||||
|
# are checked against the typed contract in dora-node.yml without running the
|
||||||
|
# node, and the payload packing is compiled and tested on its own.
|
||||||
|
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 each dataflow turns a type warning into a
|
||||||
|
# non-zero exit, so a mismatched annotation fails here rather than at
|
||||||
|
# runtime. The viewer file is checked too: it wires the same outputs, and
|
||||||
|
# a rename that breaks only the consumer should still fail the build.
|
||||||
|
- name: Validate the dataflows
|
||||||
|
run: |
|
||||||
|
dora validate dataflow.yml
|
||||||
|
dora validate dataflow_view.yml
|
||||||
|
|
||||||
|
test:
|
||||||
|
runs-on: ubuntu-latest
|
||||||
|
steps:
|
||||||
|
- uses: actions/checkout@v4
|
||||||
|
|
||||||
|
- name: Install uv
|
||||||
|
run: |
|
||||||
|
curl -LsSf https://astral.sh/uv/install.sh | sh
|
||||||
|
echo "$HOME/.local/bin" >> "$GITHUB_PATH"
|
||||||
|
|
||||||
|
- name: Test the payload packing
|
||||||
|
run: tests/run.sh
|
||||||
+4
-3
@@ -1,6 +1,7 @@
|
|||||||
build/
|
build/
|
||||||
bin/
|
bin/
|
||||||
tmp/
|
tmp/
|
||||||
test/assets/*.png
|
|
||||||
test/assets/*.jpg
|
# dora build working directories
|
||||||
test/assets/*_result.*
|
git/
|
||||||
|
out/
|
||||||
|
|||||||
@@ -4,51 +4,68 @@ This file provides guidance to AI coding assistants when working with code in th
|
|||||||
|
|
||||||
## What this is
|
## 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. Capture is free-running on its own thread at `ZED_FPS`; on each tick the event loop publishes the newest frame it has produced, or nothing if none arrived since the last tick. What goes out: 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. Every payload is a typed Arrow array — the two frames as `std/media/v1/Image`, the rest as `std/core/v1/Float32` and `.../String` — which is what lets the node feed [dora-image-view](https://gitea.liria.cl/liria/dora-image-view) and anything else built against those types. Camera management lives in `src/zed/`, payload packing in `src/arrow/`, 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`.
|
||||||
|
|
||||||
|
## Test
|
||||||
|
|
||||||
|
```bash
|
||||||
|
tests/run.sh
|
||||||
|
```
|
||||||
|
|
||||||
|
Needs `g++` and `uv`; no camera, no ZED SDK, no CUDA.
|
||||||
|
|
||||||
## Build
|
## Build
|
||||||
|
|
||||||
```bash
|
```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.
|
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.
|
||||||
|
|
||||||
|
For a build without installing the SDK stack on the host, see the `Dockerfile` (Ubuntu 24.04 — the newest release with a ZED SDK; there is no Stereolabs build for 26.04). Run the compile with `--gpus all`.
|
||||||
|
|
||||||
## Run
|
## Run
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
dora up
|
dora up
|
||||||
dora start dataflow.yml
|
dora start dataflow.yml # camera alone
|
||||||
|
dora start dataflow_view.yml --uv # camera plus dora-image-view
|
||||||
```
|
```
|
||||||
|
|
||||||
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.).
|
The two dataflows both declare the camera node, so run one at a time.
|
||||||
|
|
||||||
|
Camera behavior is configured entirely through environment variables (see README.md for the full list: `ZED_RESOLUTION`, `ZED_FPS`, `ZED_DEPTH_MODE`, etc.).
|
||||||
|
|
||||||
## Architecture
|
## Architecture
|
||||||
|
|
||||||
- **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`).
|
- **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`).
|
||||||
- **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + point cloud), calibration, and close. Exposes `handle()` for engine access.
|
- **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: `open()` (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/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace.
|
- **`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/engines/engine.h`** — `engines::DetectedObject` struct and `engines::Engine` abstract base class (`enable`, `detect`, `objects`, `label_names`).
|
- **`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/engines/zed_native.h/cc`** — `engines::ZedNative`: wraps ZED SDK's TensorRT-optimized object detection module.
|
- **`src/arrow/array.h/cc`** — Builds the Arrow arrays the node publishes, against the Arrow C data interface: the two ABI structs are defined in the header rather than pulled from Arrow C++, which would be a heavy dependency for a producer that exports one array per output and never reads one. `image()` packs an `std/media/v1/Image` row, `float32()` and `string()` the primitive payloads. The payload bytes are **borrowed**, not copied — they only have to outlive the send, since dora IPC-encodes the array into shared memory inside `send_arrow_output` and then calls the release callbacks. Each node's release frees its children before itself, so releasing the root frees the tree.
|
||||||
- **`src/engines/open_vocab.h/cc`** — `engines::OpenVocab`: YOLO-World open-vocabulary detection via ONNX Runtime. Supports two modes: single-input (fixed vocab, legacy) and two-input (open-vocab, `images` + `text_feats`). In two-input mode, reads `ZED_ONNX_LABELS` on every tick; when the labels change, re-tokenizes and re-encodes via the CLIP text encoder ONNX session, then passes the resulting `(1, N, 512)` L2-normalized embeddings to the visual model. Preprocesses images (letterbox, normalize, CHW), runs inference, applies NMS, then fuses 2D detections with the ZED point cloud to compute 3D positions and bounding boxes.
|
- **`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) it drains `take_state_change` and `take_frame` from the capture thread and publishes `image` 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). `send()` hands one `arrow_c::Exported` to `send_arrow_output`. `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); nothing about a frame's shape or encoding travels as metadata any more, since the Image struct carries it. The loop never touches the SDK. 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.
|
||||||
- **`src/util/clip_tokenizer.h`** — Header-only C++ CLIP BPE tokenizer (no external deps). Implements the GPT-2 `bytes_to_unicode()` byte encoder + iterative BPE merge algorithm. Loads `clip_vocab.txt` and `clip_merges.txt` generated by `scripts/export_clip_text_encoder.py`. Returns 77 `int64` token IDs (BOS=49406, EOS=49407, padded with 0).
|
|
||||||
- **`src/main.cc`** — Dora event loop. Selects engine via `ZED_ENGINE` env var, holds a `std::unique_ptr<engines::Engine>`. Parses `ZED_PC_MODE` (`FULL`/`HALF`/`ROI`) once before the loop. On `tick` input: 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. On `update_vocabulary` input: decodes payload as a UTF-8 label string and calls `detector->set_labels()` for immediate re-encoding. Handles reconnection on grab failure.
|
|
||||||
- **`DoraTargets.cmake`** — Handles building or fetching dora-rs and generating the CXX bridge files. Two paths: local (`DORA_ROOT_DIR`) or git clone. Produces `dora_node_api_cxx` library and the `node_bridge.cc` glue.
|
- **`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.
|
- **`scripts/build.sh`** — Configure/compile/install in one shell-free-callable step. Referenced by both dataflows, the `Dockerfile` and the README.
|
||||||
- **`scripts/export_yoloworld.py`** — PEP 723 uv script. Exports a YOLOv8-worldv2 model to ONNX with N class slots (labels arbitrary at runtime). Run: `uv run scripts/export_yoloworld.py --model s --num-classes 10`.
|
- **`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 both dataflows build from the checkout. `image` and `depth` are `std/media/v1/Image`; dora's compatibility graph has no edge from `std/core/v1/UInt8` to it, so a consumer declaring the struct rejects a producer declaring the buffer whatever it actually sends — the manifest has to move with the payload.
|
||||||
- **`scripts/export_clip_text_encoder.py`** — PEP 723 uv script. Exports CLIP ViT-B/32 text encoder to ONNX and saves `clip_vocab.txt` / `clip_merges.txt`. Run once; reused across all YOLO-World model sizes. Run: `uv run scripts/export_clip_text_encoder.py --output-dir models/`.
|
- **`dataflow.yml`** — Example dora dataflow: the camera alone, no viewer and no Python dependency. 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:`.
|
||||||
- **`src/test_open_vocab.cc`** — Standalone C++ test binary (`bin/test_open_vocab`). Exercises the `OpenVocab` engine with images from `test/assets/yolo.png` and `test/assets/yoloworld.png` (paths overridable via argv). No dora and no live camera required — `enable()` ignores the camera for this engine. Uses the same env vars as the main binary. Saves annotated result images alongside inputs.
|
- **`dataflow_view.yml`** — The same camera node plus [dora-image-view](https://gitea.liria.cl/liria/dora-image-view) on `image`, `depth` and `camera_info`, fetched from a tag. Run it with `--uv`, which gives the viewer its own managed environment. The viewer's manifest is not read at validate time (only a `path:` node's is), so the file repeats its contract in `input_types:` to get the two checked against each other. dora 1.0 has no cross-dataflow wiring (an input's `source` must resolve to a node in the same descriptor), so the camera node is duplicated here rather than referenced, and the two files are run one at a time.
|
||||||
|
- **`tests/run.sh`** — Builds and runs the payload tests: `tests/arrow_test.cc` under `-fsanitize=address,undefined` (the release callbacks have to free the whole tree), and `tests/test_arrow.py`, which imports the exported arrays through pyarrow's C data interface and checks the values. `tests/arrow_export.cc` is the C shim the Python side calls. Covers `src/arrow` only, so it needs no camera, no ZED SDK and no dora.
|
||||||
|
- **`.gitea/workflows/validate.yml`** — CI gates on push and pull request: `dora validate` on both dataflows, and `tests/run.sh`. Neither needs a camera, the ZED SDK or CUDA.
|
||||||
|
- **`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
|
## Key APIs
|
||||||
|
|
||||||
- **dora C++ API** (`<dora-node-api.h>`): `init_dora_node()`, `dora_node.events->next()`, `event_type()`, `send_output_with_metadata()`, `new_metadata()`.
|
- **dora C++ API** (`<dora-node-api.h>`): `init_dora_node()`, `dora_node.events->next()`, `event_type()`, `send_arrow_output()`, `new_metadata()`.
|
||||||
- **ZED SDK** (`<sl/Camera.hpp>`): `sl::Camera`, `sl::InitParameters`, `sl::RuntimeParameters`, `sl::Mat`, `sl::ObjectDetectionParameters`, `sl::Objects`.
|
- **ZED SDK** (`<sl/Camera.hpp>`): `sl::Camera`, `sl::InitParameters`, `sl::RuntimeParameters`, `sl::Mat`.
|
||||||
- **ONNX Runtime** (`<onnxruntime_cxx_api.h>`): `Ort::Env`, `Ort::Session`, `Ort::SessionOptions`, `Ort::Value`. Used by the `open_vocab` engine.
|
- Output data is sent as Arrow arrays: `send_arrow_output` takes an `ArrowArray`/`ArrowSchema` pair by pointer and moves both out, so a payload is built once with `src/arrow` and handed over.
|
||||||
- Output data is sent as raw byte slices (`rust::Slice<const uint8_t>`) with typed metadata (shape, dtype, encoding).
|
|
||||||
|
|
||||||
## Tools
|
## Tools
|
||||||
|
|
||||||
|
|||||||
+10
-28
@@ -1,9 +1,15 @@
|
|||||||
cmake_minimum_required(VERSION 3.21)
|
cmake_minimum_required(VERSION 3.21)
|
||||||
project(dora_zed VERSION 0.1.0 LANGUAGES C CXX)
|
project(dora_zed VERSION 0.3.0 LANGUAGES C CXX)
|
||||||
|
|
||||||
set(CMAKE_CXX_STANDARD 17)
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
set(CMAKE_CXX_FLAGS "-fPIC")
|
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)
|
include(DoraTargets.cmake)
|
||||||
|
|
||||||
set(ZED_DIR "/usr/local/zed" CACHE PATH "Path to the ZED SDK")
|
set(ZED_DIR "/usr/local/zed" CACHE PATH "Path to the ZED SDK")
|
||||||
@@ -13,18 +19,14 @@ find_package(zed REQUIRED)
|
|||||||
|
|
||||||
find_package(OpenCV 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(${dora_link_dirs})
|
||||||
link_directories(${ZED_LIBRARY_DIR})
|
link_directories(${ZED_LIBRARY_DIR})
|
||||||
|
|
||||||
add_executable(dora_zed
|
add_executable(dora_zed
|
||||||
src/main.cc
|
src/main.cc
|
||||||
|
src/arrow/array.cc
|
||||||
src/zed/camera.cc
|
src/zed/camera.cc
|
||||||
src/engines/zed_native.cc
|
src/zed/capture.cc
|
||||||
src/engines/open_vocab.cc
|
|
||||||
${node_bridge}
|
${node_bridge}
|
||||||
)
|
)
|
||||||
add_dependencies(dora_zed Dora_cxx)
|
add_dependencies(dora_zed Dora_cxx)
|
||||||
@@ -38,28 +40,8 @@ target_include_directories(
|
|||||||
${ZED_INCLUDE_DIRS}
|
${ZED_INCLUDE_DIRS}
|
||||||
${OpenCV_INCLUDE_DIRS}
|
${OpenCV_INCLUDE_DIRS}
|
||||||
${CUDAToolkit_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)
|
install(TARGETS dora_zed DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/bin)
|
||||||
|
|
||||||
# Standalone detector test — no dora, no live camera required
|
|
||||||
add_executable(test_open_vocab
|
|
||||||
src/test_open_vocab.cc
|
|
||||||
src/engines/open_vocab.cc
|
|
||||||
)
|
|
||||||
target_include_directories(test_open_vocab PRIVATE
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/src
|
|
||||||
${ZED_INCLUDE_DIRS}
|
|
||||||
${OpenCV_INCLUDE_DIRS}
|
|
||||||
${CUDAToolkit_INCLUDE_DIRS}
|
|
||||||
${ONNXRUNTIME_INCLUDE_DIRS}
|
|
||||||
)
|
|
||||||
target_link_libraries(test_open_vocab PRIVATE
|
|
||||||
${ZED_LIBRARIES}
|
|
||||||
${OpenCV_LIBS}
|
|
||||||
${ONNXRUNTIME_LIBRARIES}
|
|
||||||
)
|
|
||||||
install(TARGETS test_open_vocab DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/bin)
|
|
||||||
|
|||||||
+56
@@ -0,0 +1,56 @@
|
|||||||
|
# Reproducible build environment for dora-zed.
|
||||||
|
#
|
||||||
|
# Compiles only — no camera is needed. Requires an NVIDIA driver and the NVIDIA
|
||||||
|
# Container Toolkit on the host, as the ZED SDK does anyway.
|
||||||
|
#
|
||||||
|
# NVIDIA_DRIVER_CAPABILITIES below includes `video` because libsl_zed.so links
|
||||||
|
# against NVDEC (libnvcuvid) and NVENC (libnvidia-encode); the container runtime
|
||||||
|
# mounts those from the host only when `video` is in the list.
|
||||||
|
#
|
||||||
|
# Ubuntu 24.04 is the newest release Stereolabs ships a ZED SDK for.
|
||||||
|
#
|
||||||
|
# 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
|
||||||
|
#
|
||||||
|
# --user keeps build/ and bin/ owned by you instead of root.
|
||||||
|
#
|
||||||
|
# The first run clones dora-rs and compiles it with Cargo, which takes a while;
|
||||||
|
# the named volume keeps the registry cache across runs.
|
||||||
|
|
||||||
|
FROM nvidia/cuda:12.8.1-devel-ubuntu24.04
|
||||||
|
|
||||||
|
ARG ZED_VERSION=5.2
|
||||||
|
|
||||||
|
ENV DEBIAN_FRONTEND=noninteractive
|
||||||
|
|
||||||
|
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||||
|
build-essential cmake git curl ca-certificates pkg-config \
|
||||||
|
libopencv-dev zstd udev sudo \
|
||||||
|
&& rm -rf /var/lib/apt/lists/*
|
||||||
|
|
||||||
|
# The toolchain lives outside /root so the container can run as the invoking
|
||||||
|
# user (--user) and still write the cargo caches. registry/ is created here so
|
||||||
|
# that a named volume mounted over it inherits these permissions rather than
|
||||||
|
# being created root-owned.
|
||||||
|
ENV RUSTUP_HOME=/usr/local/rustup \
|
||||||
|
CARGO_HOME=/usr/local/cargo \
|
||||||
|
PATH=/usr/local/cargo/bin:${PATH}
|
||||||
|
RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs \
|
||||||
|
| sh -s -- -y --profile minimal --default-toolchain stable \
|
||||||
|
&& mkdir -p ${CARGO_HOME}/registry \
|
||||||
|
&& chmod -R a+w ${RUSTUP_HOME} ${CARGO_HOME}
|
||||||
|
|
||||||
|
# The installer leaves /usr/local/zed as drwxrwx--- root:zed, which an arbitrary
|
||||||
|
# --user uid cannot read, so relax it to make find_package(zed) work for one.
|
||||||
|
RUN curl -fSL -o /tmp/ZED_SDK.run \
|
||||||
|
"https://download.stereolabs.com/zedsdk/${ZED_VERSION}/cu12/ubuntu24" \
|
||||||
|
&& chmod +x /tmp/ZED_SDK.run \
|
||||||
|
&& /tmp/ZED_SDK.run -- silent skip_python skip_tools skip_drivers skip_cuda \
|
||||||
|
&& chmod -R a+rX /usr/local/zed \
|
||||||
|
&& rm -f /tmp/ZED_SDK.run
|
||||||
|
|
||||||
|
ENV NVIDIA_DRIVER_CAPABILITIES=compute,utility,video
|
||||||
|
|
||||||
|
WORKDIR /src
|
||||||
|
CMD ["scripts/build.sh"]
|
||||||
+33
-9
@@ -5,6 +5,16 @@ set(dora_c_include_dir "${CMAKE_CURRENT_BINARY_DIR}/include/c")
|
|||||||
set(dora_cxx_include_dir "${CMAKE_CURRENT_BINARY_DIR}/include/cxx")
|
set(dora_cxx_include_dir "${CMAKE_CURRENT_BINARY_DIR}/include/cxx")
|
||||||
set(node_bridge "${CMAKE_CURRENT_BINARY_DIR}/node_bridge.cc")
|
set(node_bridge "${CMAKE_CURRENT_BINARY_DIR}/node_bridge.cc")
|
||||||
|
|
||||||
|
# Match the cargo profile to the CMake build type, so a release build does not
|
||||||
|
# link unoptimized dora bindings.
|
||||||
|
if(CMAKE_BUILD_TYPE MATCHES "^(Release|RelWithDebInfo|MinSizeRel)$")
|
||||||
|
set(_cargo_profile_flag --release)
|
||||||
|
set(_cargo_target_subdir release)
|
||||||
|
else()
|
||||||
|
set(_cargo_profile_flag "")
|
||||||
|
set(_cargo_target_subdir debug)
|
||||||
|
endif()
|
||||||
|
|
||||||
if(DORA_ROOT_DIR AND EXISTS "${DORA_ROOT_DIR}/Cargo.toml")
|
if(DORA_ROOT_DIR AND EXISTS "${DORA_ROOT_DIR}/Cargo.toml")
|
||||||
include(ExternalProject)
|
include(ExternalProject)
|
||||||
ExternalProject_Add(Dora
|
ExternalProject_Add(Dora
|
||||||
@@ -12,10 +22,10 @@ if(DORA_ROOT_DIR AND EXISTS "${DORA_ROOT_DIR}/Cargo.toml")
|
|||||||
BUILD_IN_SOURCE True
|
BUILD_IN_SOURCE True
|
||||||
CONFIGURE_COMMAND ""
|
CONFIGURE_COMMAND ""
|
||||||
BUILD_COMMAND
|
BUILD_COMMAND
|
||||||
cargo build
|
cargo build ${_cargo_profile_flag}
|
||||||
--package dora-node-api-c
|
--package dora-node-api-c
|
||||||
&&
|
&&
|
||||||
cargo build
|
cargo build ${_cargo_profile_flag}
|
||||||
--package dora-node-api-cxx
|
--package dora-node-api-cxx
|
||||||
INSTALL_COMMAND ""
|
INSTALL_COMMAND ""
|
||||||
)
|
)
|
||||||
@@ -37,10 +47,18 @@ if(DORA_ROOT_DIR AND EXISTS "${DORA_ROOT_DIR}/Cargo.toml")
|
|||||||
|
|
||||||
add_custom_target(Dora_c DEPENDS ${dora_c_include_dir})
|
add_custom_target(Dora_c DEPENDS ${dora_c_include_dir})
|
||||||
add_custom_target(Dora_cxx DEPENDS ${node_bridge} ${dora_cxx_include_dir})
|
add_custom_target(Dora_cxx DEPENDS ${node_bridge} ${dora_cxx_include_dir})
|
||||||
set(dora_link_dirs ${DORA_ROOT_DIR}/target/debug)
|
set(dora_link_dirs ${DORA_ROOT_DIR}/target/${_cargo_target_subdir})
|
||||||
else()
|
else()
|
||||||
include(ExternalProject)
|
include(ExternalProject)
|
||||||
set(DORA_GIT_TAG "main")
|
|
||||||
|
# This node targets dora 1.0+. Set DORA_GIT_TAG to pin a tag explicitly;
|
||||||
|
# left empty it follows the installed dora CLI so the bindings match the
|
||||||
|
# runtime, and falls back to the default below when no CLI is present.
|
||||||
|
set(DORA_GIT_TAG "" CACHE STRING "dora-rs git tag to build against (empty: follow the dora CLI)")
|
||||||
|
set(_dora_tag "${DORA_GIT_TAG}")
|
||||||
|
|
||||||
|
if(NOT _dora_tag)
|
||||||
|
set(_dora_tag "v1.0.1")
|
||||||
find_program(DORA_CLI dora)
|
find_program(DORA_CLI dora)
|
||||||
if(DORA_CLI)
|
if(DORA_CLI)
|
||||||
execute_process(
|
execute_process(
|
||||||
@@ -53,23 +71,29 @@ else()
|
|||||||
if(_dora_version_result EQUAL 0)
|
if(_dora_version_result EQUAL 0)
|
||||||
string(REGEX MATCH "([0-9]+\\.[0-9]+\\.[0-9]+)" _dora_version_match "${_dora_version_out}")
|
string(REGEX MATCH "([0-9]+\\.[0-9]+\\.[0-9]+)" _dora_version_match "${_dora_version_out}")
|
||||||
if(_dora_version_match)
|
if(_dora_version_match)
|
||||||
set(DORA_GIT_TAG "v${CMAKE_MATCH_1}")
|
set(_dora_tag "v${CMAKE_MATCH_1}")
|
||||||
|
if(CMAKE_MATCH_1 VERSION_LESS 1.0)
|
||||||
|
message(WARNING "dora CLI ${CMAKE_MATCH_1} is older than the 1.0 this node targets")
|
||||||
|
endif()
|
||||||
|
endif()
|
||||||
endif()
|
endif()
|
||||||
endif()
|
endif()
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
|
message(STATUS "dora-rs: building C++ bindings from ${_dora_tag}")
|
||||||
|
|
||||||
ExternalProject_Add(Dora
|
ExternalProject_Add(Dora
|
||||||
PREFIX ${CMAKE_CURRENT_BINARY_DIR}/dora
|
PREFIX ${CMAKE_CURRENT_BINARY_DIR}/dora
|
||||||
GIT_REPOSITORY https://github.com/dora-rs/dora.git
|
GIT_REPOSITORY https://github.com/dora-rs/dora.git
|
||||||
GIT_TAG ${DORA_GIT_TAG}
|
GIT_TAG ${_dora_tag}
|
||||||
BUILD_IN_SOURCE True
|
BUILD_IN_SOURCE True
|
||||||
CONFIGURE_COMMAND ""
|
CONFIGURE_COMMAND ""
|
||||||
BUILD_COMMAND
|
BUILD_COMMAND
|
||||||
cargo build
|
cargo build ${_cargo_profile_flag}
|
||||||
--package dora-node-api-c
|
--package dora-node-api-c
|
||||||
--target-dir ${CMAKE_CURRENT_BINARY_DIR}/dora/src/Dora/target
|
--target-dir ${CMAKE_CURRENT_BINARY_DIR}/dora/src/Dora/target
|
||||||
&&
|
&&
|
||||||
cargo build
|
cargo build ${_cargo_profile_flag}
|
||||||
--package dora-node-api-cxx
|
--package dora-node-api-cxx
|
||||||
--target-dir ${CMAKE_CURRENT_BINARY_DIR}/dora/src/Dora/target
|
--target-dir ${CMAKE_CURRENT_BINARY_DIR}/dora/src/Dora/target
|
||||||
INSTALL_COMMAND ""
|
INSTALL_COMMAND ""
|
||||||
@@ -90,7 +114,7 @@ else()
|
|||||||
cp ../apis/c/node ${CMAKE_CURRENT_BINARY_DIR}/include/c -r
|
cp ../apis/c/node ${CMAKE_CURRENT_BINARY_DIR}/include/c -r
|
||||||
)
|
)
|
||||||
|
|
||||||
set(dora_link_dirs ${CMAKE_CURRENT_BINARY_DIR}/dora/src/Dora/target/debug)
|
set(dora_link_dirs ${CMAKE_CURRENT_BINARY_DIR}/dora/src/Dora/target/${_cargo_target_subdir})
|
||||||
|
|
||||||
add_custom_target(Dora_c DEPENDS ${dora_c_include_dir})
|
add_custom_target(Dora_c DEPENDS ${dora_c_include_dir})
|
||||||
add_custom_target(Dora_cxx DEPENDS ${node_bridge} ${dora_cxx_include_dir})
|
add_custom_target(Dora_cxx DEPENDS ${node_bridge} ${dora_cxx_include_dir})
|
||||||
|
|||||||
@@ -1,198 +1,198 @@
|
|||||||
# dora-zed
|
# dora-zed
|
||||||
|
|
||||||
Dora C++ node for ZED stereo cameras. Captures frames and publishes BGR images, XYZRGBA point clouds, camera intrinsics, and 3D object detections.
|
Dora node for ZED stereo cameras. On every tick it publishes a colour image and a depth
|
||||||
|
map as `std/media/v1/Image`, plus camera intrinsics, a camera state, and an optional
|
||||||
|
point cloud. A source for a vision pipeline: it takes a tick and produces frames.
|
||||||
|
|
||||||
## Usage via `git:` in a dataflow
|
## Usage
|
||||||
|
|
||||||
|
Add to your dataflow:
|
||||||
|
|
||||||
```yaml
|
```yaml
|
||||||
nodes:
|
nodes:
|
||||||
- id: zed-camera
|
- id: zed-camera
|
||||||
git: ssh://git@gitea.liria.cl:5001/liria/dora-zed.git
|
git: https://gitea.liria.cl/liria/dora-zed.git
|
||||||
build: mkdir -p build && cd build && cmake .. && make install
|
tag: v0.3.0
|
||||||
|
build: scripts/build.sh
|
||||||
path: bin/dora_zed
|
path: bin/dora_zed
|
||||||
inputs:
|
inputs:
|
||||||
tick: dora/timer/millis/100
|
tick: dora/timer/millis/100
|
||||||
outputs:
|
outputs:
|
||||||
- image_bgr
|
- image
|
||||||
|
- depth
|
||||||
- point_cloud
|
- point_cloud
|
||||||
- camera_info
|
- camera_info
|
||||||
- objects
|
- camera_state
|
||||||
```
|
```
|
||||||
|
|
||||||
Then run:
|
The node is C++: building it needs the ZED SDK and CUDA on the machine that runs the
|
||||||
|
dataflow — see [Build](#build). An output left out of `outputs:` is simply not published,
|
||||||
|
so a dataflow that never turns the point cloud on can drop that line.
|
||||||
|
|
||||||
```bash
|
### Runnable dataflows
|
||||||
dora build dataflow.yml
|
|
||||||
dora up
|
|
||||||
dora start dataflow.yml
|
|
||||||
```
|
|
||||||
|
|
||||||
## Local build
|
| File | Contents | Run |
|
||||||
|
|
||||||
```bash
|
|
||||||
git clone ssh://git@gitea.liria.cl:5001/liria/dora-zed.git
|
|
||||||
cd dora-zed
|
|
||||||
mkdir -p build && cd build
|
|
||||||
cmake ..
|
|
||||||
make install
|
|
||||||
```
|
|
||||||
|
|
||||||
The binary is installed to `bin/dora_zed` relative to the repository root.
|
|
||||||
|
|
||||||
## Dependencies
|
|
||||||
|
|
||||||
| Dependency | Minimum version | Notes |
|
|
||||||
|---|---|---|
|
|---|---|---|
|
||||||
| [CMake](https://cmake.org/download/) | 3.21 | |
|
| [`dataflow.yml`](dataflow.yml) | The camera alone, no viewer and no Python dependency | `dora run dataflow.yml` |
|
||||||
| [ZED SDK](https://www.stereolabs.com/developers/release) | 5.x | Default path: `/usr/local/zed`; bundles OpenCV |
|
| [`dataflow_view.yml`](dataflow_view.yml) | The camera plus [dora-image-view][view] | `dora run dataflow_view.yml --uv` |
|
||||||
| [CUDA Toolkit](https://developer.nvidia.com/cuda-downloads) | Required by ZED SDK | |
|
|
||||||
| [Rust toolchain](https://rustup.rs) | stable | Used to build dora-rs C++ bindings |
|
|
||||||
| [ONNX Runtime](https://github.com/microsoft/onnxruntime/releases) | 1.20+ | Must be installed before building; required only for `open_vocab` engine |
|
|
||||||
|
|
||||||
## Installation scripts
|
[view]: https://gitea.liria.cl/liria/dora-image-view
|
||||||
|
|
||||||
Helper scripts are in [`scripts/`](scripts/):
|
`dataflow_view.yml` is the worked example of consuming this node: a window opens with
|
||||||
|
the colour image and the depth map side by side, the camera model captioned across them.
|
||||||
|
Press `q` to quit. It needs a display, and `--uv` so the viewer gets its own managed
|
||||||
|
environment.
|
||||||
|
|
||||||
| Platform | Script | Notes |
|
dora has no cross-dataflow wiring, so both files declare the camera node themselves: run
|
||||||
|---|---|---|
|
one at a time. Both build it from the checkout, which is what lets `dora validate` check
|
||||||
| Ubuntu 24.04 x86_64 | [`scripts/install_linux_x64.sh`](scripts/install_linux_x64.sh) | Installs ZED SDK + ONNX Runtime |
|
them against [`dora-node.yml`](dora-node.yml) — the typed contract a consumer wires
|
||||||
| NVIDIA Jetson (ARM64) | [`scripts/install_linux_arm.sh`](scripts/install_linux_arm.sh) | Installs ZED SDK; auto-detects L4T version |
|
against, and what [CI](.gitea/workflows/validate.yml) validates on every push.
|
||||||
| Windows x64 | [`scripts/install_windows.ps1`](scripts/install_windows.ps1) | Installs ONNX Runtime; ZED SDK downloaded manually |
|
|
||||||
|
|
||||||
**Linux / Jetson:**
|
|
||||||
|
|
||||||
```bash
|
|
||||||
chmod +x scripts/install_linux_x64.sh # or install_linux_arm.sh
|
|
||||||
sudo scripts/install_linux_x64.sh
|
|
||||||
```
|
|
||||||
|
|
||||||
**Windows** (elevated PowerShell):
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
Set-ExecutionPolicy -Scope Process -ExecutionPolicy Bypass
|
|
||||||
.\scripts\install_windows.ps1
|
|
||||||
```
|
|
||||||
|
|
||||||
> **Jetson note:** No official ONNX Runtime GPU binary exists for ARM64. The `native` engine (ZED SDK built-in TensorRT) works without it. The `open_vocab` engine requires [building ONNX Runtime from source](https://onnxruntime.ai/docs/build/eps.html#tensorrt).
|
|
||||||
|
|
||||||
> **Windows note:** ZED SDK must be downloaded and installed manually from [stereolabs.com/developers/release](https://www.stereolabs.com/developers/release). Pass the ONNX Runtime path to CMake: `cmake .. -DCMAKE_PREFIX_PATH=C:\onnxruntime`
|
|
||||||
|
|
||||||
## Environment variables
|
|
||||||
|
|
||||||
| Variable | Default | Description |
|
|
||||||
|---|---|---|
|
|
||||||
| `ZED_RESOLUTION` | `720` | Camera resolution: `720`, `1080`, or `2000` |
|
|
||||||
| `ZED_FPS` | `15` | Camera frame rate |
|
|
||||||
| `ZED_DEPTH_MODE` | `NEURAL` | Depth mode: `PERFORMANCE`, `QUALITY`, `NEURAL`, or `NONE` |
|
|
||||||
| `ZED_DEPTH_MIN_MM` | `10` | Minimum depth distance in millimeters |
|
|
||||||
| `ZED_DEPTH_MAX_MM` | `500` | Maximum depth distance in millimeters |
|
|
||||||
| `ZED_DEPTH_FILL` | `false` | Enable depth fill mode |
|
|
||||||
| `ZED_FLIP` | `ON` | Image flip: `ON` or `OFF` |
|
|
||||||
| `ZED_WARMUP_FRAMES` | `30` | Number of warmup frames to discard |
|
|
||||||
| `ZED_RETRY_INTERVAL_SECONDS` | `5` | Seconds between reconnection attempts |
|
|
||||||
| `ZED_ENGINE` | `native` | Detection engine: `native` (ZED SDK) or `open_vocab` (YOLO-World via ONNX Runtime) |
|
|
||||||
| `ZED_DETECTION_MODEL` | `MULTI_CLASS_BOX_MEDIUM` | (`native`) Detection model: `MULTI_CLASS_BOX_FAST`, `MULTI_CLASS_BOX_MEDIUM`, or `MULTI_CLASS_BOX_ACCURATE` |
|
|
||||||
| `ZED_DETECTION_CONFIDENCE` | `40` | (`native`) Detection confidence threshold (0-100) |
|
|
||||||
| `ZED_DETECTION_TRACKING` | `false` | (`native`) Enable object tracking across frames |
|
|
||||||
| `ZED_ONNX_MODEL_PATH` | _(required)_ | (`open_vocab`) Path to YOLO-World ONNX model file |
|
|
||||||
| `ZED_ONNX_LABELS` | _(required)_ | (`open_vocab`) Comma-separated label names (e.g. `person,cup,chair`). Can be changed between ticks — the node re-encodes embeddings automatically |
|
|
||||||
| `ZED_ONNX_CONFIDENCE` | `0.25` | (`open_vocab`) Detection confidence threshold (0.0-1.0) |
|
|
||||||
| `ZED_ONNX_NMS_THRESHOLD` | `0.45` | (`open_vocab`) NMS IoU threshold |
|
|
||||||
| `ZED_ONNX_INPUT_SIZE` | `640` | (`open_vocab`) Model input image size in pixels |
|
|
||||||
| `ZED_ONNX_TEXT_ENCODER_PATH` | _(required for open-vocab)_ | (`open_vocab`) Path to `clip_text_encoder.onnx`. Required when the visual model has two inputs (`images` + `text_feats`) |
|
|
||||||
| `ZED_ONNX_CLIP_VOCAB` | _(required for open-vocab)_ | (`open_vocab`) Path to `clip_vocab.txt` BPE vocabulary file |
|
|
||||||
| `ZED_ONNX_CLIP_MERGES` | _(required for open-vocab)_ | (`open_vocab`) Path to `clip_merges.txt` BPE merge rules file |
|
|
||||||
| `ZED_PC_MODE` | `FULL` | Point cloud output mode: `FULL` (full HxW grid), `HALF` (stride-2 downsample, ~4× smaller), or `ROI` (valid points within detected bounding boxes only, flat list) |
|
|
||||||
|
|
||||||
## Inputs
|
## Inputs
|
||||||
|
|
||||||
| Input | Description |
|
| Input | Description |
|
||||||
|---|---|
|
|---|---|
|
||||||
| `tick` | Required. Triggers a camera grab and detection on each message. Connect to `dora/timer/millis/N`. |
|
| `tick` | Publishes the newest frame captured since the last tick, or nothing. Wire to `dora/timer/millis/N`. |
|
||||||
| `update_vocabulary` | Optional (`open_vocab` engine only). Payload: raw UTF-8 bytes of a comma-separated label string (e.g. `"person,cup,chair"`). Immediately re-encodes CLIP embeddings; the next `tick` uses the new labels. |
|
|
||||||
|
The tick does not drive the camera. Capture is free-running on its own thread, paced by
|
||||||
|
the SDK to `ZED_FPS`, and each frame overwrites whatever the previous one left
|
||||||
|
uncollected; a tick forwards the newest there is, and publishes nothing when none arrived
|
||||||
|
since the last one. So the tick sets the publish rate, never the capture rate: below
|
||||||
|
`ZED_FPS` it drops frames — the default 100 ms tick against the default 15 fps keeps two
|
||||||
|
out of every three — and above it, the ticks that find nothing publish nothing.
|
||||||
|
`dora/timer/millis/66` at 15 fps forwards every frame, each at most one capture period
|
||||||
|
old and none of them twice.
|
||||||
|
|
||||||
## Outputs
|
## Outputs
|
||||||
|
|
||||||
### `image_bgr`
|
| Output | Payload | Metadata |
|
||||||
|
|
||||||
BGR image as raw bytes. Metadata includes `shape` (HxWx3), `encoding` (`bgr8`), camera intrinsics (`fx`, `fy`, `cx`, `cy`), `distortion`, `frame_id`, and `timestamp_ns`.
|
|
||||||
|
|
||||||
### `point_cloud`
|
|
||||||
|
|
||||||
XYZRGBA point cloud as float32 bytes. Controlled by `ZED_PC_MODE`:
|
|
||||||
|
|
||||||
| Mode | Shape | Layout | Description |
|
|
||||||
|---|---|---|---|
|
|
||||||
| `FULL` | `[H, W, 4]` | `HWC` | Full resolution grid (default). ~14.7 MB/frame at 720p |
|
|
||||||
| `HALF` | `[H/2, W/2, 4]` | `HWC` | Stride-2 downsample (~4× smaller) |
|
|
||||||
| `ROI` | `[N, 4]` | `NC` | Valid points within detected object bounding boxes only. `N` varies by scene. Metadata includes `mode=roi` |
|
|
||||||
|
|
||||||
Metadata includes `units` (`mm`) and camera intrinsics.
|
|
||||||
|
|
||||||
### `camera_info`
|
|
||||||
|
|
||||||
Four float32 values: `[fx, fy, cx, cy]`. Metadata includes `distortion` coefficients.
|
|
||||||
|
|
||||||
### `objects`
|
|
||||||
|
|
||||||
Detected objects as a flat float32 array with shape `[num_objects, 37]`. Each row contains 37 floats:
|
|
||||||
|
|
||||||
| Index | Field | Description |
|
|
||||||
|---|---|---|
|
|---|---|---|
|
||||||
| 0 | `label_id` | Object class (0=PERSON, 1=VEHICLE, 2=BAG, 3=ANIMAL, 4=ELECTRONICS, 5=FRUIT_VEGETABLE, 6=SPORT) |
|
| `image` | `std/media/v1/Image` struct, `bgr8` | intrinsics, `frame_id`, timestamps |
|
||||||
| 1 | `confidence` | Detection confidence (0-100) |
|
| `depth` | `std/media/v1/Image` struct, `32FC1` | `units=mm`, intrinsics, `frame_id`, timestamps |
|
||||||
| 2-4 | `x, y, z` | 3D center position in mm |
|
| `camera_info` | `float32` `[fx, fy, cx, cy]` | `distortion`, `frame_id`, timestamps |
|
||||||
| 5-12 | `bb2d_*` | 2D bounding box: 4 corners × 2 pixel coords |
|
| `point_cloud` | `float32` XYZRGBA, off by default | `shape`, `channels`, `units=mm`, intrinsics, timestamps |
|
||||||
| 13-36 | `bb3d_*` | 3D bounding box: 8 corners × 3 world coords (mm) |
|
| `camera_state` | `string` | `host_timestamp_ns` |
|
||||||
|
|
||||||
Metadata includes `frame_id` and `timestamp_ns` matching the corresponding `image_bgr`, `image_width` and `image_height` for interpreting 2D coordinates, `engine` (`native` or `open_vocab`), and `label_names` (comma-separated class names for the active engine). When no objects are detected, an empty array with shape `[0, 37]` is sent.
|
`image` and `depth` are both `std/media/v1/Image`: an Arrow struct of `width: UInt32`,
|
||||||
|
`height: UInt32`, `encoding: Utf8` and `data: LargeBinary`. The payload describes itself,
|
||||||
|
so nothing about its shape travels as message metadata. A depth map is an image whose
|
||||||
|
pixels are distances, as ROS also treats it: `32FC1` is one float32 per pixel, `units`
|
||||||
|
says what those floats measure, and `0` means missing depth — the payload contains no
|
||||||
|
`inf` and no `nan`. `depth` is published on every frame `image` is, at the same size and
|
||||||
|
`frame_id`.
|
||||||
|
|
||||||
**`open_vocab` engine:** Uses a YOLO-World model exported with open-vocabulary support (two ONNX inputs: `images` and `text_feats`). Labels are set at runtime via `ZED_ONNX_LABELS` and re-encoded automatically on each tick when they change — no node restart required. Export the model and CLIP text encoder with the helper scripts:
|
`camera_info` is published **once per camera session** — with the first frame after the
|
||||||
|
camera opens, and again after a reconnection — since the intrinsics are fixed for a given
|
||||||
|
camera and resolution. Every frame also carries `fx`, `fy`, `cx` and `cy` in its
|
||||||
|
metadata, so a consumer that started after the camera did still ends up with them.
|
||||||
|
|
||||||
```bash
|
Every output of a frame carries the same `frame_id` and both clocks as dora `timestamp`
|
||||||
# Export the visual model (N=10 class slots, can be changed)
|
metadata: `device_timestamp_ns`, when the camera captured the frame, and
|
||||||
uv run scripts/export_yoloworld.py --model s --num-classes 10 --output-dir models/
|
`host_timestamp_ns`, when this node published it — both Unix-epoch nanoseconds. Two nodes
|
||||||
|
reading the same frame agree on the device clock; the rest of the dataflow shares the
|
||||||
|
host one.
|
||||||
|
|
||||||
# Export the CLIP text encoder + vocab files (once, reused for all model sizes)
|
### Camera state
|
||||||
uv run scripts/export_clip_text_encoder.py --output-dir models/
|
|
||||||
|
|
||||||
# Run
|
`camera_state` lets a consumer tell a camera that has not delivered its first frame yet
|
||||||
ZED_ENGINE=open_vocab \
|
from one that has been lost:
|
||||||
ZED_ONNX_MODEL_PATH=models/yolov8s-worldv2_n10_i640.onnx \
|
|
||||||
ZED_ONNX_TEXT_ENCODER_PATH=models/clip_text_encoder.onnx \
|
| State | Meaning |
|
||||||
ZED_ONNX_CLIP_VOCAB=models/clip_vocab.txt \
|
|---|---|
|
||||||
ZED_ONNX_CLIP_MERGES=models/clip_merges.txt \
|
| `opening` | The camera has not delivered a usable frame yet in this run |
|
||||||
ZED_ONNX_LABELS="person,car,bag,chair,laptop" \
|
| `streaming` | Frames are being published |
|
||||||
dora start dataflow.yml
|
| `recovering` | The camera was streaming and has stopped; the node is bringing it back |
|
||||||
|
|
||||||
|
The camera runs on its own thread, since the SDK can block indefinitely inside `grab()`
|
||||||
|
and `open()` on a disconnected camera and would otherwise freeze the event loop. A camera
|
||||||
|
that delivers no usable frame for 5 seconds is recovered, reopening on a 1, 2, 5 then 10
|
||||||
|
second backoff and escalating to `sl::Camera::reboot()` past the first attempt; an
|
||||||
|
all-black frame counts as a failed grab, since a camera whose link has dropped can keep
|
||||||
|
reporting success while handing back a uniform one. Uncollected frames are dropped, so a
|
||||||
|
consumer always gets the newest one.
|
||||||
|
|
||||||
|
## Configuration
|
||||||
|
|
||||||
|
Settings are environment variables, which a dataflow can carry per node, keeping them
|
||||||
|
with the wiring rather than in whatever shell starts it:
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
env:
|
||||||
|
ZED_RESOLUTION: 720
|
||||||
|
ZED_FPS: 30
|
||||||
```
|
```
|
||||||
|
|
||||||
To verify the model works before connecting the camera, build and run the standalone C++ test binary (`bin/test_open_vocab`). It requires no live camera — images are loaded from disk:
|
A node's `env:` wins over the environment the dataflow was started in; variables it does
|
||||||
|
not set still pass through.
|
||||||
|
|
||||||
|
| Variable | Default | Meaning |
|
||||||
|
|---|---|---|
|
||||||
|
| `ZED_RESOLUTION` | `1080` | `720`, `1080` or `2000` |
|
||||||
|
| `ZED_FPS` | `15` | Frame rate |
|
||||||
|
| `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` | Near end of the depth range, in millimetres |
|
||||||
|
| `ZED_DEPTH_MAX_MM` | `600` | Far end of the depth range, in millimetres |
|
||||||
|
| `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_POINT_CLOUD` | `false` | Retrieve and publish the point cloud |
|
||||||
|
| `ZED_PC_MODE` | `FULL` | `FULL` is a `[H, W, 4]` grid; `HALF` is a stride-2 downsample. Only applies with `ZED_POINT_CLOUD` on. |
|
||||||
|
|
||||||
|
The defaults match the reference camera driver the perception stack was tuned against, and
|
||||||
|
the resolved depth settings are logged at startup.
|
||||||
|
|
||||||
|
The point cloud is off because it is expensive: roughly 14.7 MB per frame at 1080p, paid
|
||||||
|
in retrieval and in a copy whether or not anything consumes it. Consumers that need 3D
|
||||||
|
back-project `depth` with the published intrinsics instead.
|
||||||
|
|
||||||
|
## Build
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
cd build && cmake .. && make test_open_vocab
|
scripts/build.sh
|
||||||
|
|
||||||
# Place test images:
|
|
||||||
cp street.jpg test/assets/yolo.png
|
|
||||||
cp custom.jpg test/assets/yoloworld.png
|
|
||||||
|
|
||||||
# Fixed-vocab YOLO (both images use the same model):
|
|
||||||
ZED_ONNX_MODEL_PATH=models/yolov8s_n80_i640.onnx \
|
|
||||||
ZED_ONNX_LABELS="person,car,bus,bicycle" \
|
|
||||||
bin/test_open_vocab
|
|
||||||
|
|
||||||
# Open-vocab YOLO-World (separate model for second image via ZED_ONNX_WORLD_MODEL_PATH):
|
|
||||||
ZED_ONNX_MODEL_PATH=models/yolov8s_n80_i640.onnx \
|
|
||||||
ZED_ONNX_WORLD_MODEL_PATH=models/yolov8s-worldv2_n10_i640.onnx \
|
|
||||||
ZED_ONNX_TEXT_ENCODER_PATH=models/clip_text_encoder.onnx \
|
|
||||||
ZED_ONNX_CLIP_VOCAB=models/clip_vocab.txt \
|
|
||||||
ZED_ONNX_CLIP_MERGES=models/clip_merges.txt \
|
|
||||||
ZED_ONNX_LABELS="person,car,bus,bicycle" \
|
|
||||||
bin/test_open_vocab
|
|
||||||
|
|
||||||
# Override image paths:
|
|
||||||
bin/test_open_vocab /path/to/img1.jpg /path/to/img2.jpg
|
|
||||||
```
|
```
|
||||||
|
|
||||||
Annotated results are saved alongside the input images as `yolo_result.png` and `yoloworld_result.png`.
|
The binary is installed to `bin/dora_zed`. The script is also the dataflow's `build:`
|
||||||
|
command, since dora 1.0 runs build commands directly rather than through a shell — `cd`
|
||||||
|
and `&&` are not available there. The first build clones dora-rs and compiles its C++
|
||||||
|
bindings with Cargo, which takes a while; later builds are incremental, and
|
||||||
|
`DORA_ROOT_DIR` builds against a local dora checkout instead.
|
||||||
|
|
||||||
3D positions are computed by fusing the 2D detections with the ZED point cloud (median of valid depth samples within each bounding box). 3D bounding box corners are back-projected at the p25/p75 depth percentiles.
|
**Requirements:** dora 1.0+ (verified with `dora-cli 1.0.1`), CMake 3.21+, ZED SDK 5.x at
|
||||||
|
`/usr/local/zed` (it bundles OpenCV), the CUDA Toolkit it needs, and a stable Rust
|
||||||
|
toolchain for the dora bindings.
|
||||||
|
|
||||||
|
| Platform | Install the toolchain with |
|
||||||
|
|---|---|
|
||||||
|
| Ubuntu 24.04 x86_64 | `sudo scripts/install_linux_x64.sh` |
|
||||||
|
| NVIDIA Jetson (ARM64) | `sudo scripts/install_linux_arm.sh` |
|
||||||
|
| Windows x64 (PowerShell admin) | `scripts/install_windows.ps1`, plus the [ZED SDK][sdk] by hand |
|
||||||
|
|
||||||
|
[sdk]: https://www.stereolabs.com/developers/release
|
||||||
|
|
||||||
|
[`Dockerfile`](Dockerfile) carries the same toolchain on Ubuntu 24.04 — the newest release
|
||||||
|
Stereolabs ships a ZED SDK for — so the host needs none of it, only an NVIDIA driver and
|
||||||
|
the [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html):
|
||||||
|
|
||||||
|
```bash
|
||||||
|
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
|
||||||
|
```
|
||||||
|
|
||||||
|
It runs the same `scripts/build.sh`. The named volume keeps the Cargo cache across runs
|
||||||
|
and `--user` keeps `build/` and `bin/` owned by you. x86-64 only: on Jetson, run
|
||||||
|
[`scripts/install_linux_arm.sh`](scripts/install_linux_arm.sh) and build natively.
|
||||||
|
|
||||||
|
## Test
|
||||||
|
|
||||||
|
```bash
|
||||||
|
tests/run.sh
|
||||||
|
```
|
||||||
|
|
||||||
|
Unit tests over `src/arrow`, which packs every payload this node sends: each exported
|
||||||
|
Arrow array is round-tripped through pyarrow to check what a consumer reads back, and
|
||||||
|
built again under the sanitizers to check that releasing one frees it. They need neither a
|
||||||
|
camera, the ZED SDK nor dora, which is what lets CI run them; everything past the packing
|
||||||
|
is covered by running a dataflow against a real camera.
|
||||||
|
|||||||
+12
-5
@@ -1,13 +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.
|
||||||
|
#
|
||||||
|
# The camera alone, with no viewer and no Python dependency. For a run that
|
||||||
|
# also displays the stream, use dataflow_view.yml.
|
||||||
|
strict_types: true
|
||||||
|
|
||||||
nodes:
|
nodes:
|
||||||
- id: zed-camera
|
- id: zed-camera
|
||||||
git: ssh://git@gitea.liria.cl:5001/liria/dora-zed.git
|
|
||||||
build: mkdir -p build && cd build && cmake .. && make install
|
|
||||||
path: bin/dora_zed
|
path: bin/dora_zed
|
||||||
|
build: scripts/build.sh
|
||||||
inputs:
|
inputs:
|
||||||
tick: dora/timer/millis/100
|
tick: dora/timer/millis/100
|
||||||
# update_vocabulary: vocabulary-node/labels # optional: send new comma-separated labels at runtime
|
|
||||||
outputs:
|
outputs:
|
||||||
- image_bgr
|
- image
|
||||||
|
- depth
|
||||||
- point_cloud
|
- point_cloud
|
||||||
- camera_info
|
- camera_info
|
||||||
- objects
|
- camera_state
|
||||||
|
|||||||
@@ -0,0 +1,46 @@
|
|||||||
|
# The camera into dora-image-view, and the worked example of wiring this node to
|
||||||
|
# a consumer.
|
||||||
|
#
|
||||||
|
# dora has no cross-dataflow wiring, so the camera node is declared here too
|
||||||
|
# rather than referenced — run this file or dataflow.yml, not both.
|
||||||
|
#
|
||||||
|
# Run it with `--uv`, which gives the viewer its own managed environment:
|
||||||
|
#
|
||||||
|
# dora run dataflow_view.yml --uv
|
||||||
|
#
|
||||||
|
# Fetching the viewer needs anonymous read access to its repository. dora-cli
|
||||||
|
# 1.0.1 fetches through libgit2 with no credential callback, so ssh:// cannot
|
||||||
|
# authenticate at all; on Gitea, marking the repository public is not enough
|
||||||
|
# either, since a repository inside a private organization answers an anonymous
|
||||||
|
# clone with 401.
|
||||||
|
strict_types: true
|
||||||
|
|
||||||
|
nodes:
|
||||||
|
- id: zed-camera
|
||||||
|
path: bin/dora_zed
|
||||||
|
build: scripts/build.sh
|
||||||
|
inputs:
|
||||||
|
tick: dora/timer/millis/100
|
||||||
|
outputs:
|
||||||
|
- image
|
||||||
|
- depth
|
||||||
|
- point_cloud
|
||||||
|
- camera_info
|
||||||
|
- camera_state
|
||||||
|
|
||||||
|
- id: view
|
||||||
|
git: https://gitea.liria.cl/liria/dora-image-view.git
|
||||||
|
tag: v0.1.0
|
||||||
|
build: uv pip install .
|
||||||
|
path: dora-image-view
|
||||||
|
inputs:
|
||||||
|
image: zed-camera/image
|
||||||
|
depth: zed-camera/depth
|
||||||
|
camera_info: zed-camera/camera_info
|
||||||
|
# The viewer's own manifest says this, but a `git:` node's manifest is not
|
||||||
|
# read at validate time — only at build. Repeating it here is what makes the
|
||||||
|
# two contracts checked against each other now rather than at runtime.
|
||||||
|
input_types:
|
||||||
|
image: std/media/v1/Image
|
||||||
|
depth: std/media/v1/Image
|
||||||
|
camera_info: std/core/v1/Float32
|
||||||
@@ -0,0 +1,66 @@
|
|||||||
|
# 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 an 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
|
||||||
|
dora: ">=1.0"
|
||||||
|
|
||||||
|
inputs:
|
||||||
|
tick:
|
||||||
|
type: std/core/v1/UInt64
|
||||||
|
required: true
|
||||||
|
description: >-
|
||||||
|
Publishes the newest frame the capture thread has grabbed, or nothing if
|
||||||
|
none arrived since the last tick. It does not drive the camera: capture is
|
||||||
|
free-running at ZED_FPS, so this sets the publish rate and anything
|
||||||
|
captured between two ticks is dropped. Connect to dora/timer/millis/N.
|
||||||
|
|
||||||
|
outputs:
|
||||||
|
image:
|
||||||
|
type: std/media/v1/Image
|
||||||
|
description: >-
|
||||||
|
Captured frame as an Arrow struct {width, height, encoding, data}. The
|
||||||
|
encoding is always bgr8, three bytes per pixel.
|
||||||
|
depth:
|
||||||
|
type: std/media/v1/Image
|
||||||
|
description: >-
|
||||||
|
Depth map as an Arrow struct {width, height, encoding, data}, one channel
|
||||||
|
per pixel -- dora has no depth type, and a depth map is an image whose
|
||||||
|
pixels are distances, which is how ROS spells it too. The encoding is
|
||||||
|
32FC1 and the `units` metadata is mm. 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, shape in the metadata. 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.
|
||||||
|
|
||||||
|
example: |
|
||||||
|
- id: zed-camera
|
||||||
|
git: https://gitea.liria.cl/liria/dora-zed.git
|
||||||
|
tag: v0.3.0
|
||||||
|
build: scripts/build.sh
|
||||||
|
path: bin/dora_zed
|
||||||
|
inputs:
|
||||||
|
tick: dora/timer/millis/100
|
||||||
|
outputs:
|
||||||
|
- image
|
||||||
|
- depth
|
||||||
|
- point_cloud
|
||||||
|
- camera_info
|
||||||
|
- camera_state
|
||||||
Executable
+14
@@ -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)"
|
||||||
@@ -1,105 +0,0 @@
|
|||||||
# /// script
|
|
||||||
# requires-python = ">=3.10"
|
|
||||||
# dependencies = ["transformers>=4.30", "torch", "onnx"]
|
|
||||||
# ///
|
|
||||||
"""Export the CLIP ViT-B/32 text encoder to ONNX and save vocab files.
|
|
||||||
|
|
||||||
Compatible with all YOLOv8-worldv2 sizes (s/m/l/x) — run once and reuse.
|
|
||||||
|
|
||||||
The C++ node uses these three files to compute text embeddings at runtime:
|
|
||||||
clip_text_encoder.onnx input: input_ids (1, 77) int64
|
|
||||||
output: text_embeds (1, 512) float32 (L2-normalize in C++)
|
|
||||||
clip_vocab.txt token → id mapping (tab-separated, one per line)
|
|
||||||
clip_merges.txt BPE merge rules (space-separated pairs, one per line)
|
|
||||||
|
|
||||||
Usage:
|
|
||||||
uv run scripts/export_clip_text_encoder.py
|
|
||||||
uv run scripts/export_clip_text_encoder.py --output-dir models/
|
|
||||||
"""
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import sys
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
CLIP_MODEL = "openai/clip-vit-base-patch32"
|
|
||||||
|
|
||||||
|
|
||||||
def parse_args():
|
|
||||||
p = argparse.ArgumentParser(description="Export CLIP ViT-B/32 text encoder to ONNX")
|
|
||||||
p.add_argument(
|
|
||||||
"--output-dir", "-o",
|
|
||||||
default="models",
|
|
||||||
help="Directory for output files. Default: models/",
|
|
||||||
)
|
|
||||||
return p.parse_args()
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
args = parse_args()
|
|
||||||
|
|
||||||
try:
|
|
||||||
import torch
|
|
||||||
from transformers import CLIPTextModelWithProjection, CLIPTokenizer
|
|
||||||
except ImportError as e:
|
|
||||||
print(f"Error: {e}\nRun: uv run scripts/export_clip_text_encoder.py", file=sys.stderr)
|
|
||||||
sys.exit(1)
|
|
||||||
|
|
||||||
out = Path(args.output_dir)
|
|
||||||
out.mkdir(parents=True, exist_ok=True)
|
|
||||||
|
|
||||||
print(f"Loading {CLIP_MODEL} ...")
|
|
||||||
model = CLIPTextModelWithProjection.from_pretrained(CLIP_MODEL)
|
|
||||||
tokenizer = CLIPTokenizer.from_pretrained(CLIP_MODEL)
|
|
||||||
model.eval()
|
|
||||||
|
|
||||||
# --- Export ONNX text encoder ---
|
|
||||||
# Input: input_ids (1, 77) int64 — BPE token IDs, padded/truncated to 77
|
|
||||||
# Output: text_embeds (1, 512) float32 — projected features (NOT normalized; C++ normalizes)
|
|
||||||
onnx_path = out / "clip_text_encoder.onnx"
|
|
||||||
print(f"Exporting ONNX → {onnx_path} ...")
|
|
||||||
dummy = torch.zeros(1, 77, dtype=torch.long)
|
|
||||||
torch.onnx.export(
|
|
||||||
model,
|
|
||||||
dummy,
|
|
||||||
str(onnx_path),
|
|
||||||
input_names=["input_ids"],
|
|
||||||
output_names=["text_embeds"],
|
|
||||||
dynamic_axes={
|
|
||||||
"input_ids": {0: "batch_size"},
|
|
||||||
"text_embeds": {0: "batch_size"},
|
|
||||||
},
|
|
||||||
opset_version=14,
|
|
||||||
)
|
|
||||||
|
|
||||||
# --- Save vocab as tab-separated text (token\tid) ---
|
|
||||||
# Avoids JSON parsing in C++. Tokens with special chars are escaped.
|
|
||||||
vocab_path = out / "clip_vocab.txt"
|
|
||||||
print(f"Saving vocab → {vocab_path} ...")
|
|
||||||
vocab = tokenizer.get_vocab()
|
|
||||||
with open(vocab_path, "w", encoding="utf-8") as f:
|
|
||||||
for token, idx in sorted(vocab.items(), key=lambda x: x[1]):
|
|
||||||
token_esc = (token
|
|
||||||
.replace("\\", "\\\\")
|
|
||||||
.replace("\t", "\\t")
|
|
||||||
.replace("\n", "\\n"))
|
|
||||||
f.write(f"{token_esc}\t{idx}\n")
|
|
||||||
|
|
||||||
# --- Save BPE merge rules ---
|
|
||||||
merges_path = out / "clip_merges.txt"
|
|
||||||
print(f"Saving merges → {merges_path} ...")
|
|
||||||
with open(merges_path, "w", encoding="utf-8") as f:
|
|
||||||
for (a, b) in tokenizer.bpe_ranks.keys():
|
|
||||||
f.write(f"{a} {b}\n")
|
|
||||||
|
|
||||||
print(f"\nDone. Files written to {out.resolve()}/")
|
|
||||||
print(f" clip_text_encoder.onnx — ONNX model")
|
|
||||||
print(f" clip_vocab.txt — {len(vocab)} tokens")
|
|
||||||
print(f" clip_merges.txt — {len(tokenizer.bpe_ranks)} merge rules")
|
|
||||||
print(f"\nSet env vars:")
|
|
||||||
print(f" ZED_ONNX_TEXT_ENCODER_PATH={out.resolve()}/clip_text_encoder.onnx")
|
|
||||||
print(f" ZED_ONNX_CLIP_VOCAB={out.resolve()}/clip_vocab.txt")
|
|
||||||
print(f" ZED_ONNX_CLIP_MERGES={out.resolve()}/clip_merges.txt")
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -1,124 +0,0 @@
|
|||||||
# /// script
|
|
||||||
# requires-python = ">=3.10"
|
|
||||||
# dependencies = ["ultralytics>=8.0"]
|
|
||||||
# ///
|
|
||||||
"""Export a YOLOv8-worldv2 model to ONNX with N open-vocabulary class slots.
|
|
||||||
|
|
||||||
The exported model has two inputs:
|
|
||||||
images (1, 3, imgsz, imgsz) float32
|
|
||||||
text_feats (1, N, 512) float32 ← CLIP embeddings, set at runtime
|
|
||||||
|
|
||||||
N is fixed per export. Use export_clip_text_encoder.py to generate the text
|
|
||||||
encoder and vocab files used to compute embeddings at runtime in the C++ node.
|
|
||||||
|
|
||||||
Usage:
|
|
||||||
uv run scripts/export_yoloworld.py
|
|
||||||
uv run scripts/export_yoloworld.py --model m --num-classes 20
|
|
||||||
uv run scripts/export_yoloworld.py --model l --num-classes 5 --output-dir models/
|
|
||||||
"""
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import shutil
|
|
||||||
import sys
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
MODELS = {
|
|
||||||
"s": "yolov8s-worldv2",
|
|
||||||
"m": "yolov8m-worldv2",
|
|
||||||
"l": "yolov8l-worldv2",
|
|
||||||
"x": "yolov8x-worldv2",
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def parse_args():
|
|
||||||
p = argparse.ArgumentParser(
|
|
||||||
description="Export YOLOv8-worldv2 to ONNX with open-vocabulary text inputs"
|
|
||||||
)
|
|
||||||
p.add_argument(
|
|
||||||
"--model", "-m",
|
|
||||||
choices=list(MODELS),
|
|
||||||
default="s",
|
|
||||||
help="Model size: s (~14 MB), m (~30 MB), l (~50 MB), x (~100 MB). Default: s",
|
|
||||||
)
|
|
||||||
p.add_argument(
|
|
||||||
"--num-classes", "-n",
|
|
||||||
type=int,
|
|
||||||
default=10,
|
|
||||||
help="Number of concurrent class slots (must match number of labels at runtime). Default: 10",
|
|
||||||
)
|
|
||||||
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_inputs(onnx_path: Path, expected_inputs: int):
|
|
||||||
try:
|
|
||||||
import onnx
|
|
||||||
model = onnx.load(str(onnx_path))
|
|
||||||
actual = len(model.graph.input)
|
|
||||||
if actual != expected_inputs:
|
|
||||||
print(f"\nWarning: expected {expected_inputs} inputs, got {actual}.")
|
|
||||||
print("The visual model may have baked text embeddings (ultralytics version dependent).")
|
|
||||||
print("In that case, use a fixed vocabulary instead of the text encoder.")
|
|
||||||
else:
|
|
||||||
inputs = [(i.name, [d.dim_value for d in i.type.tensor_type.shape.dim])
|
|
||||||
for i in model.graph.input]
|
|
||||||
print(f" Inputs: {inputs}")
|
|
||||||
except ImportError:
|
|
||||||
print(" (install 'onnx' to verify model inputs)")
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
args = parse_args()
|
|
||||||
|
|
||||||
try:
|
|
||||||
from ultralytics import YOLO
|
|
||||||
except ImportError:
|
|
||||||
print("Error: ultralytics not installed. Run: uv run scripts/export_yoloworld.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}_n{args.num_classes}_i{args.imgsz}.onnx"
|
|
||||||
dst = out_dir / out_name
|
|
||||||
|
|
||||||
print(f"Model: {model_name}.pt")
|
|
||||||
print(f"Class slots: {args.num_classes}")
|
|
||||||
print(f"Input size: {args.imgsz}")
|
|
||||||
print(f"Output: {dst}")
|
|
||||||
print()
|
|
||||||
|
|
||||||
model = YOLO(f"{model_name}.pt")
|
|
||||||
# Set N dummy class slots — actual label meanings are provided at runtime via text_feats
|
|
||||||
model.set_classes(["object"] * args.num_classes)
|
|
||||||
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_inputs(dst, expected_inputs=2)
|
|
||||||
|
|
||||||
print(f"\nExported: {dst.resolve()}")
|
|
||||||
print(f"\nNext step — export the CLIP text encoder (once, reused for all model sizes):")
|
|
||||||
print(f" uv run scripts/export_clip_text_encoder.py --output-dir models/")
|
|
||||||
print(f"\nRun with:")
|
|
||||||
print(f" ZED_ENGINE=open_vocab \\")
|
|
||||||
print(f" ZED_ONNX_MODEL_PATH={dst.resolve()} \\")
|
|
||||||
print(f" ZED_ONNX_TEXT_ENCODER_PATH=models/clip_text_encoder.onnx \\")
|
|
||||||
print(f' ZED_ONNX_LABELS="person,car,bag,... (exactly {args.num_classes} labels)" \\')
|
|
||||||
print(f" dora start dataflow.yml")
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -9,11 +9,6 @@
|
|||||||
# JetPack 6.2.2 (L4T 36.5)
|
# JetPack 6.2.2 (L4T 36.5)
|
||||||
# JetPack 7.0 (L4T 38.2) — Jetson Thor
|
# JetPack 7.0 (L4T 38.2) — Jetson Thor
|
||||||
# JetPack 7.1 (L4T 38.4)
|
# JetPack 7.1 (L4T 38.4)
|
||||||
#
|
|
||||||
# ONNX Runtime: Microsoft does not publish official GPU binaries for ARM64.
|
|
||||||
# The native engine (ZED SDK built-in TensorRT) works without ONNX Runtime.
|
|
||||||
# To use the open_vocab engine, build ONNX Runtime from source:
|
|
||||||
# https://onnxruntime.ai/docs/build/eps.html#tensorrt
|
|
||||||
set -euo pipefail
|
set -euo pipefail
|
||||||
|
|
||||||
ZED_VERSION="5.2"
|
ZED_VERSION="5.2"
|
||||||
@@ -63,13 +58,5 @@ else
|
|||||||
sudo "${TMP_DIR}/ZED_SDK.run" -- silent skip_python skip_tools
|
sudo "${TMP_DIR}/ZED_SDK.run" -- silent skip_python skip_tools
|
||||||
fi
|
fi
|
||||||
|
|
||||||
# --- ONNX Runtime ---
|
echo ">>> Done. You can now run: scripts/build.sh"
|
||||||
echo ""
|
|
||||||
echo ">>> ONNX Runtime (ARM64 GPU): no official pre-built binary available."
|
|
||||||
echo " The native ZED engine works without ONNX Runtime."
|
|
||||||
echo " To use the open_vocab engine, 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 ">>> Downloaded files are in ${TMP_DIR}"
|
echo ">>> Downloaded files are in ${TMP_DIR}"
|
||||||
|
|||||||
@@ -6,7 +6,6 @@
|
|||||||
set -euo pipefail
|
set -euo pipefail
|
||||||
|
|
||||||
ZED_VERSION="5.2"
|
ZED_VERSION="5.2"
|
||||||
ORT_VERSION="1.24.4"
|
|
||||||
|
|
||||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||||
TMP_DIR="${SCRIPT_DIR}/../tmp"
|
TMP_DIR="${SCRIPT_DIR}/../tmp"
|
||||||
@@ -34,23 +33,5 @@ else
|
|||||||
sudo "${TMP_DIR}/ZED_SDK.run" -- silent skip_python skip_tools
|
sudo "${TMP_DIR}/ZED_SDK.run" -- silent skip_python skip_tools
|
||||||
fi
|
fi
|
||||||
|
|
||||||
# --- ONNX Runtime (C++ GPU, installs to /usr/local) ---
|
echo ">>> Done. You can now run: scripts/build.sh"
|
||||||
if [ -f /usr/local/lib64/libonnxruntime.so ] && [ -d /usr/local/include/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/
|
|
||||||
sudo ldconfig
|
|
||||||
fi
|
|
||||||
|
|
||||||
echo ">>> Done. You can now run: mkdir -p build && cd build && cmake .. && make install"
|
|
||||||
echo ">>> Downloaded files are in ${TMP_DIR}"
|
echo ">>> Downloaded files are in ${TMP_DIR}"
|
||||||
|
|||||||
@@ -10,10 +10,6 @@
|
|||||||
$ErrorActionPreference = "Stop"
|
$ErrorActionPreference = "Stop"
|
||||||
|
|
||||||
$ZED_INSTALL_DIR = "C:\Program Files (x86)\ZED SDK"
|
$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 ---
|
# --- ZED SDK ---
|
||||||
if (Test-Path $ZED_INSTALL_DIR) {
|
if (Test-Path $ZED_INSTALL_DIR) {
|
||||||
@@ -25,23 +21,5 @@ if (Test-Path $ZED_INSTALL_DIR) {
|
|||||||
Write-Host ""
|
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 ""
|
||||||
Write-Host ">>> Done."
|
Write-Host ">>> Done."
|
||||||
|
|||||||
@@ -0,0 +1,179 @@
|
|||||||
|
#include "arrow/array.h"
|
||||||
|
|
||||||
|
#include <cstring>
|
||||||
|
#include <string>
|
||||||
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace arrow_c {
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
// Producer-owned storage for one schema node. The struct's `format`, `name` and
|
||||||
|
// `children` pointers point in here, which is why it outlives the call that
|
||||||
|
// built it and is freed by the release callback rather than by the builder.
|
||||||
|
struct SchemaData {
|
||||||
|
std::string format;
|
||||||
|
std::string name;
|
||||||
|
std::vector<ArrowSchema*> children;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Producer-owned storage for one array node. `owned` holds the bytes this
|
||||||
|
// node's own buffers point into - offset arrays, and scalars that have no
|
||||||
|
// storage elsewhere. The payload itself is borrowed from the caller.
|
||||||
|
struct ArrayData {
|
||||||
|
std::vector<uint8_t> owned;
|
||||||
|
std::vector<const void*> buffers;
|
||||||
|
std::vector<ArrowArray*> children;
|
||||||
|
};
|
||||||
|
|
||||||
|
// A node's release callback releases its children before freeing itself, so a
|
||||||
|
// consumer that only releases the root - which is all the interface requires -
|
||||||
|
// still frees the whole tree.
|
||||||
|
void release_schema(ArrowSchema* schema) {
|
||||||
|
auto* data = static_cast<SchemaData*>(schema->private_data);
|
||||||
|
|
||||||
|
for (ArrowSchema* child : data->children) {
|
||||||
|
if (child->release != nullptr) {
|
||||||
|
child->release(child);
|
||||||
|
}
|
||||||
|
delete child;
|
||||||
|
}
|
||||||
|
|
||||||
|
delete data;
|
||||||
|
schema->release = nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
void release_array(ArrowArray* array) {
|
||||||
|
auto* data = static_cast<ArrayData*>(array->private_data);
|
||||||
|
|
||||||
|
for (ArrowArray* child : data->children) {
|
||||||
|
if (child->release != nullptr) {
|
||||||
|
child->release(child);
|
||||||
|
}
|
||||||
|
delete child;
|
||||||
|
}
|
||||||
|
|
||||||
|
delete data;
|
||||||
|
array->release = nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_schema(ArrowSchema& schema, const char* format, const char* name,
|
||||||
|
std::vector<ArrowSchema*> children) {
|
||||||
|
auto* data = new SchemaData{format, name, std::move(children)};
|
||||||
|
|
||||||
|
schema.format = data->format.c_str();
|
||||||
|
schema.name = data->name.c_str();
|
||||||
|
schema.metadata = nullptr;
|
||||||
|
// Nullable everywhere, which is what pyarrow's defaults produce: the same
|
||||||
|
// struct built by a Python node has to compare equal to this one.
|
||||||
|
schema.flags = ARROW_FLAG_NULLABLE;
|
||||||
|
schema.n_children = static_cast<int64_t>(data->children.size());
|
||||||
|
schema.children = data->children.empty() ? nullptr : data->children.data();
|
||||||
|
schema.dictionary = nullptr;
|
||||||
|
schema.release = release_schema;
|
||||||
|
schema.private_data = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
// `buffers` may point into `owned`: moving a vector keeps its heap block, so
|
||||||
|
// pointers taken before the move stay valid after it.
|
||||||
|
void set_array(ArrowArray& array, int64_t length, std::vector<uint8_t> owned,
|
||||||
|
std::vector<const void*> buffers, std::vector<ArrowArray*> children) {
|
||||||
|
auto* data = new ArrayData{std::move(owned), std::move(buffers), std::move(children)};
|
||||||
|
|
||||||
|
array.length = length;
|
||||||
|
// Nothing this node sends is null, so the validity bitmap - the first
|
||||||
|
// buffer of every node below - is left out entirely.
|
||||||
|
array.null_count = 0;
|
||||||
|
array.offset = 0;
|
||||||
|
array.n_buffers = static_cast<int64_t>(data->buffers.size());
|
||||||
|
array.n_children = static_cast<int64_t>(data->children.size());
|
||||||
|
array.buffers = data->buffers.data();
|
||||||
|
array.children = data->children.empty() ? nullptr : data->children.data();
|
||||||
|
array.dictionary = nullptr;
|
||||||
|
array.release = release_array;
|
||||||
|
array.private_data = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
// One field of a struct, owned by its parent.
|
||||||
|
struct Child {
|
||||||
|
ArrowSchema* schema;
|
||||||
|
ArrowArray* array;
|
||||||
|
};
|
||||||
|
|
||||||
|
Child new_child() {
|
||||||
|
return {new ArrowSchema{}, new ArrowArray{}};
|
||||||
|
}
|
||||||
|
|
||||||
|
void fill_uint32(ArrowSchema& schema, ArrowArray& array, const char* name, uint32_t value) {
|
||||||
|
std::vector<uint8_t> owned(sizeof(value));
|
||||||
|
std::memcpy(owned.data(), &value, sizeof(value));
|
||||||
|
const void* values = owned.data();
|
||||||
|
|
||||||
|
set_schema(schema, "I", name, {});
|
||||||
|
set_array(array, 1, std::move(owned), {nullptr, values}, {});
|
||||||
|
}
|
||||||
|
|
||||||
|
// One string, offsets and characters in a single allocation.
|
||||||
|
void fill_utf8(ArrowSchema& schema, ArrowArray& array, const char* name, const char* text,
|
||||||
|
size_t size) {
|
||||||
|
const int32_t offsets[2] = {0, static_cast<int32_t>(size)};
|
||||||
|
|
||||||
|
std::vector<uint8_t> owned(sizeof(offsets) + size);
|
||||||
|
std::memcpy(owned.data(), offsets, sizeof(offsets));
|
||||||
|
std::memcpy(owned.data() + sizeof(offsets), text, size);
|
||||||
|
const void* offsets_buffer = owned.data();
|
||||||
|
const void* values_buffer = owned.data() + sizeof(offsets);
|
||||||
|
|
||||||
|
set_schema(schema, "u", name, {});
|
||||||
|
set_array(array, 1, std::move(owned), {nullptr, offsets_buffer, values_buffer}, {});
|
||||||
|
}
|
||||||
|
|
||||||
|
// One byte string. Only the offsets are owned; the bytes are the caller's.
|
||||||
|
void fill_large_binary(ArrowSchema& schema, ArrowArray& array, const char* name,
|
||||||
|
const uint8_t* data, size_t size) {
|
||||||
|
const int64_t offsets[2] = {0, static_cast<int64_t>(size)};
|
||||||
|
|
||||||
|
std::vector<uint8_t> owned(sizeof(offsets));
|
||||||
|
std::memcpy(owned.data(), offsets, sizeof(offsets));
|
||||||
|
const void* offsets_buffer = owned.data();
|
||||||
|
|
||||||
|
set_schema(schema, "Z", name, {});
|
||||||
|
set_array(array, 1, std::move(owned), {nullptr, offsets_buffer, data}, {});
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
Exported image(uint32_t width, uint32_t height, const char* encoding, const uint8_t* data,
|
||||||
|
size_t size) {
|
||||||
|
Child w = new_child();
|
||||||
|
fill_uint32(*w.schema, *w.array, "width", width);
|
||||||
|
|
||||||
|
Child h = new_child();
|
||||||
|
fill_uint32(*h.schema, *h.array, "height", height);
|
||||||
|
|
||||||
|
Child e = new_child();
|
||||||
|
fill_utf8(*e.schema, *e.array, "encoding", encoding, std::strlen(encoding));
|
||||||
|
|
||||||
|
Child d = new_child();
|
||||||
|
fill_large_binary(*d.schema, *d.array, "data", data, size);
|
||||||
|
|
||||||
|
Exported out;
|
||||||
|
set_schema(out.schema, "+s", "", {w.schema, h.schema, e.schema, d.schema});
|
||||||
|
set_array(out.array, 1, {}, {nullptr}, {w.array, h.array, e.array, d.array});
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
Exported float32(const float* values, size_t count) {
|
||||||
|
Exported out;
|
||||||
|
set_schema(out.schema, "f", "", {});
|
||||||
|
set_array(out.array, static_cast<int64_t>(count), {}, {nullptr, values}, {});
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
Exported string(const char* text, size_t size) {
|
||||||
|
Exported out;
|
||||||
|
fill_utf8(out.schema, out.array, "", text, size);
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace arrow_c
|
||||||
@@ -0,0 +1,75 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <cstddef>
|
||||||
|
#include <cstdint>
|
||||||
|
|
||||||
|
// The Arrow C data interface, verbatim from the Arrow specification.
|
||||||
|
//
|
||||||
|
// These two structs are the whole ABI dora's send_arrow_output takes, so
|
||||||
|
// defining them here is what lets this node publish typed Arrow arrays without
|
||||||
|
// linking Arrow C++ - a heavy dependency for a producer that exports one array
|
||||||
|
// per output and never reads one.
|
||||||
|
#ifndef ARROW_C_DATA_INTERFACE
|
||||||
|
#define ARROW_C_DATA_INTERFACE
|
||||||
|
|
||||||
|
#define ARROW_FLAG_DICTIONARY_ORDERED 1
|
||||||
|
#define ARROW_FLAG_NULLABLE 2
|
||||||
|
#define ARROW_FLAG_MAP_KEYS_SORTED 4
|
||||||
|
|
||||||
|
struct ArrowSchema {
|
||||||
|
const char* format;
|
||||||
|
const char* name;
|
||||||
|
const char* metadata;
|
||||||
|
int64_t flags;
|
||||||
|
int64_t n_children;
|
||||||
|
struct ArrowSchema** children;
|
||||||
|
struct ArrowSchema* dictionary;
|
||||||
|
|
||||||
|
void (*release)(struct ArrowSchema*);
|
||||||
|
void* private_data;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct ArrowArray {
|
||||||
|
int64_t length;
|
||||||
|
int64_t null_count;
|
||||||
|
int64_t offset;
|
||||||
|
int64_t n_buffers;
|
||||||
|
int64_t n_children;
|
||||||
|
const void** buffers;
|
||||||
|
struct ArrowArray** children;
|
||||||
|
struct ArrowArray* dictionary;
|
||||||
|
|
||||||
|
void (*release)(struct ArrowArray*);
|
||||||
|
void* private_data;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // ARROW_C_DATA_INTERFACE
|
||||||
|
|
||||||
|
namespace arrow_c {
|
||||||
|
|
||||||
|
// One array with the schema describing it.
|
||||||
|
//
|
||||||
|
// dora moves both structs out of here and calls their release callbacks once it
|
||||||
|
// has copied the payload into shared memory, so an Exported is built, sent
|
||||||
|
// once, and never touched again.
|
||||||
|
struct Exported {
|
||||||
|
ArrowArray array{};
|
||||||
|
ArrowSchema schema{};
|
||||||
|
};
|
||||||
|
|
||||||
|
// std/media/v1/Image: one row of {width, height, encoding, data}.
|
||||||
|
//
|
||||||
|
// The payload is borrowed rather than copied, so it has to stay valid until the
|
||||||
|
// send returns. A capture-thread frame buffer does: the event loop holds the
|
||||||
|
// frame across the call.
|
||||||
|
Exported image(uint32_t width, uint32_t height, const char* encoding, const uint8_t* data,
|
||||||
|
size_t size);
|
||||||
|
|
||||||
|
// std/core/v1/Float32: `count` values, borrowed on the same terms as image().
|
||||||
|
Exported float32(const float* values, size_t count);
|
||||||
|
|
||||||
|
// std/core/v1/String: one UTF-8 string. Copied, since every string this node
|
||||||
|
// sends is a short state name.
|
||||||
|
Exported string(const char* text, size_t size);
|
||||||
|
|
||||||
|
} // namespace arrow_c
|
||||||
@@ -1,28 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include <sl/Camera.hpp>
|
|
||||||
#include <string>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
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 void set_labels(const std::string& /*labels*/) {}
|
|
||||||
virtual const std::vector<DetectedObject>& objects() const = 0;
|
|
||||||
virtual std::string label_names() const = 0;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace engines
|
|
||||||
@@ -1,592 +0,0 @@
|
|||||||
#include "engines/open_vocab.h"
|
|
||||||
#include "util/clip_tokenizer.h"
|
|
||||||
#include "util/env.h"
|
|
||||||
|
|
||||||
#include <onnxruntime_cxx_api.h>
|
|
||||||
#include <opencv2/opencv.hpp>
|
|
||||||
#include <sl/Camera.hpp>
|
|
||||||
|
|
||||||
#include <algorithm>
|
|
||||||
#include <cmath>
|
|
||||||
#include <iostream>
|
|
||||||
#include <numeric>
|
|
||||||
#include <sstream>
|
|
||||||
#include <string>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
namespace {
|
|
||||||
|
|
||||||
// --- NMS helpers ---
|
|
||||||
|
|
||||||
struct RawDetection {
|
|
||||||
float x1, y1, x2, y2;
|
|
||||||
float score;
|
|
||||||
int class_id;
|
|
||||||
};
|
|
||||||
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<RawDetection> nms(std::vector<RawDetection> dets, float threshold) {
|
|
||||||
std::sort(dets.begin(), dets.end(),
|
|
||||||
[](const RawDetection& a, const RawDetection& b) {
|
|
||||||
return a.score > b.score;
|
|
||||||
});
|
|
||||||
std::vector<bool> suppressed(dets.size(), false);
|
|
||||||
std::vector<RawDetection> 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 ---
|
|
||||||
cv::Mat letterbox(const cv::Mat& src, int size, float& scale, float& pad_x, float& pad_y) {
|
|
||||||
float scale_w = static_cast<float>(size) / src.cols;
|
|
||||||
float scale_h = static_cast<float>(size) / src.rows;
|
|
||||||
scale = std::min(scale_w, scale_h);
|
|
||||||
int new_w = static_cast<int>(std::round(src.cols * scale));
|
|
||||||
int new_h = static_cast<int>(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<int>(pad_x), static_cast<int>(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;
|
|
||||||
};
|
|
||||||
|
|
||||||
FusionResult fuse_bbox(const sl::Mat& pc, int x1, int y1, int x2, int y2) {
|
|
||||||
FusionResult result{};
|
|
||||||
int w = static_cast<int>(pc.getWidth());
|
|
||||||
int h = static_cast<int>(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<float> 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Split a comma-separated string into a vector of trimmed tokens
|
|
||||||
std::vector<std::string> split_labels(const std::string& s) {
|
|
||||||
std::vector<std::string> out;
|
|
||||||
std::stringstream 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
namespace engines {
|
|
||||||
|
|
||||||
// --- Impl ---
|
|
||||||
|
|
||||||
struct OpenVocab::Impl {
|
|
||||||
// Visual model
|
|
||||||
Ort::Env env{ORT_LOGGING_LEVEL_WARNING, "open_vocab"};
|
|
||||||
Ort::SessionOptions session_options;
|
|
||||||
std::unique_ptr<Ort::Session> session;
|
|
||||||
Ort::AllocatorWithDefaultOptions allocator;
|
|
||||||
|
|
||||||
std::string input_name; // visual model: image input
|
|
||||||
std::string output_name; // visual model: detection output
|
|
||||||
std::string text_input_name; // visual model: text_feats input (empty if single-input model)
|
|
||||||
int num_classes = 0;
|
|
||||||
bool has_text_input = false;
|
|
||||||
|
|
||||||
// Text encoder (optional; used when has_text_input == true)
|
|
||||||
Ort::SessionOptions text_session_options;
|
|
||||||
std::unique_ptr<Ort::Session> text_session;
|
|
||||||
std::string text_enc_input_name; // "input_ids"
|
|
||||||
std::string text_enc_output_name; // "text_embeds"
|
|
||||||
|
|
||||||
// Tokenizer + embedding cache
|
|
||||||
util::ClipTokenizer tokenizer;
|
|
||||||
std::vector<float> cached_text_feats; // (num_classes * 512) flat, row-major
|
|
||||||
std::string cached_labels;
|
|
||||||
|
|
||||||
// Preprocessing + ONNX inference + NMS.
|
|
||||||
// Takes a BGR image (cv::imread format); returns 2D detections in original image coords.
|
|
||||||
std::vector<RawDetection> run_inference(const OpenVocabConfig& config,
|
|
||||||
const cv::Mat& image_bgr);
|
|
||||||
};
|
|
||||||
|
|
||||||
// --- Impl::run_inference ---
|
|
||||||
|
|
||||||
std::vector<RawDetection> OpenVocab::Impl::run_inference(
|
|
||||||
const OpenVocabConfig& config, const cv::Mat& image_bgr)
|
|
||||||
{
|
|
||||||
if (!session) return {};
|
|
||||||
if (has_text_input && cached_text_feats.empty()) return {};
|
|
||||||
|
|
||||||
int orig_w = image_bgr.cols;
|
|
||||||
int orig_h = image_bgr.rows;
|
|
||||||
|
|
||||||
// BGR → RGB (YOLO expects RGB)
|
|
||||||
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<float> 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<cv::Vec3f>(h, w)[c];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
auto mem_info = Ort::MemoryInfo::CreateCpu(
|
|
||||||
OrtAllocatorType::OrtArenaAllocator, OrtMemTypeDefault);
|
|
||||||
|
|
||||||
std::vector<int64_t> image_shape{1, 3, sz, sz};
|
|
||||||
Ort::Value image_tensor = Ort::Value::CreateTensor<float>(
|
|
||||||
mem_info, input_data.data(), input_data.size(),
|
|
||||||
image_shape.data(), image_shape.size());
|
|
||||||
|
|
||||||
const char* output_name_cstr = output_name.c_str();
|
|
||||||
std::vector<Ort::Value> outputs;
|
|
||||||
|
|
||||||
try {
|
|
||||||
if (has_text_input) {
|
|
||||||
std::vector<int64_t> text_shape{1, num_classes, 512};
|
|
||||||
Ort::Value text_tensor = Ort::Value::CreateTensor<float>(
|
|
||||||
mem_info,
|
|
||||||
cached_text_feats.data(),
|
|
||||||
cached_text_feats.size(),
|
|
||||||
text_shape.data(), text_shape.size());
|
|
||||||
|
|
||||||
const char* input_names[2] = {
|
|
||||||
input_name.c_str(),
|
|
||||||
text_input_name.c_str()
|
|
||||||
};
|
|
||||||
Ort::Value input_tensors[2] = {
|
|
||||||
std::move(image_tensor),
|
|
||||||
std::move(text_tensor)
|
|
||||||
};
|
|
||||||
outputs = session->Run(
|
|
||||||
Ort::RunOptions{nullptr},
|
|
||||||
input_names, input_tensors, 2,
|
|
||||||
&output_name_cstr, 1);
|
|
||||||
} else {
|
|
||||||
const char* input_name_cstr = input_name.c_str();
|
|
||||||
outputs = session->Run(
|
|
||||||
Ort::RunOptions{nullptr},
|
|
||||||
&input_name_cstr, &image_tensor, 1,
|
|
||||||
&output_name_cstr, 1);
|
|
||||||
}
|
|
||||||
} catch (const Ort::Exception& e) {
|
|
||||||
std::cerr << "OpenVocab: inference failed: " << e.what() << std::endl;
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
|
|
||||||
// Post-process: output shape (1, 4+num_classes, num_anchors)
|
|
||||||
auto out_shape = outputs[0].GetTensorTypeAndShapeInfo().GetShape();
|
|
||||||
if (out_shape.size() < 3) return {};
|
|
||||||
|
|
||||||
int num_anchors = static_cast<int>(out_shape[2]);
|
|
||||||
int nc = num_classes;
|
|
||||||
if (nc <= 0) return {};
|
|
||||||
|
|
||||||
const float* out = outputs[0].GetTensorData<float>();
|
|
||||||
|
|
||||||
std::vector<RawDetection> candidates;
|
|
||||||
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<float>(orig_w - 1)));
|
|
||||||
float y1 = std::max(0.0f, std::min((y1_lb - pad_y) / scale, static_cast<float>(orig_h - 1)));
|
|
||||||
float x2 = std::max(0.0f, std::min((x2_lb - pad_x) / scale, static_cast<float>(orig_w - 1)));
|
|
||||||
float y2 = std::max(0.0f, std::min((y2_lb - pad_y) / scale, static_cast<float>(orig_h - 1)));
|
|
||||||
|
|
||||||
candidates.push_back({x1, y1, x2, y2, best_score, best_class});
|
|
||||||
}
|
|
||||||
|
|
||||||
return nms(std::move(candidates), config.nms_threshold);
|
|
||||||
}
|
|
||||||
|
|
||||||
// --- Config ---
|
|
||||||
|
|
||||||
OpenVocabConfig OpenVocabConfig::from_env() {
|
|
||||||
OpenVocabConfig 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.text_encoder_path = util::get_env_str("ZED_ONNX_TEXT_ENCODER_PATH", c.text_encoder_path);
|
|
||||||
c.clip_vocab_path = util::get_env_str("ZED_ONNX_CLIP_VOCAB", c.clip_vocab_path);
|
|
||||||
c.clip_merges_path = util::get_env_str("ZED_ONNX_CLIP_MERGES", c.clip_merges_path);
|
|
||||||
return c;
|
|
||||||
}
|
|
||||||
|
|
||||||
// --- OpenVocab ---
|
|
||||||
|
|
||||||
OpenVocab::OpenVocab(const OpenVocabConfig& config)
|
|
||||||
: config_(config), impl_(std::make_unique<Impl>()) {}
|
|
||||||
|
|
||||||
OpenVocab::~OpenVocab() = default;
|
|
||||||
|
|
||||||
void OpenVocab::enable(sl::Camera& /* camera */) {
|
|
||||||
if (config_.model_path.empty()) {
|
|
||||||
std::cerr << "OpenVocab: ZED_ONNX_MODEL_PATH is not set." << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// --- Load visual model ---
|
|
||||||
OrtCUDAProviderOptions cuda_opts{};
|
|
||||||
cuda_opts.device_id = 0;
|
|
||||||
try {
|
|
||||||
impl_->session_options.AppendExecutionProvider_CUDA(cuda_opts);
|
|
||||||
} catch (const Ort::Exception& e) {
|
|
||||||
std::cerr << "OpenVocab: CUDA unavailable (" << e.what() << "), using CPU." << std::endl;
|
|
||||||
}
|
|
||||||
impl_->session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL);
|
|
||||||
|
|
||||||
std::cout << "OpenVocab: loading visual model (" << config_.model_path << ")..." << std::endl;
|
|
||||||
try {
|
|
||||||
impl_->session = std::make_unique<Ort::Session>(
|
|
||||||
impl_->env, config_.model_path.c_str(), impl_->session_options);
|
|
||||||
} catch (const Ort::Exception& e) {
|
|
||||||
std::cerr << "OpenVocab: failed to load visual 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();
|
|
||||||
impl_->has_text_input = (num_inputs >= 2);
|
|
||||||
if (impl_->has_text_input) {
|
|
||||||
impl_->text_input_name = impl_->session->GetInputNameAllocated(1, impl_->allocator).get();
|
|
||||||
}
|
|
||||||
|
|
||||||
auto out_info = impl_->session->GetOutputTypeInfo(0).GetTensorTypeAndShapeInfo();
|
|
||||||
auto shape = out_info.GetShape();
|
|
||||||
if (shape.size() == 3 && shape[1] > 4) {
|
|
||||||
impl_->num_classes = static_cast<int>(shape[1]) - 4;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::cout << "OpenVocab: visual model loaded."
|
|
||||||
<< " Inputs=" << num_inputs
|
|
||||||
<< " Classes=" << impl_->num_classes
|
|
||||||
<< (impl_->has_text_input ? " (open-vocab)" : " (fixed-vocab)")
|
|
||||||
<< std::endl;
|
|
||||||
|
|
||||||
// --- Load text encoder + tokenizer (only needed for open-vocab model) ---
|
|
||||||
if (impl_->has_text_input) {
|
|
||||||
if (config_.text_encoder_path.empty()) {
|
|
||||||
std::cerr << "OpenVocab: ZED_ONNX_TEXT_ENCODER_PATH is not set "
|
|
||||||
"(required for open-vocab model)." << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
impl_->text_session_options.SetGraphOptimizationLevel(
|
|
||||||
GraphOptimizationLevel::ORT_ENABLE_ALL);
|
|
||||||
|
|
||||||
std::cout << "OpenVocab: loading text encoder (" << config_.text_encoder_path
|
|
||||||
<< ")..." << std::endl;
|
|
||||||
try {
|
|
||||||
impl_->text_session = std::make_unique<Ort::Session>(
|
|
||||||
impl_->env, config_.text_encoder_path.c_str(), impl_->text_session_options);
|
|
||||||
} catch (const Ort::Exception& e) {
|
|
||||||
std::cerr << "OpenVocab: failed to load text encoder: " << e.what() << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
impl_->text_enc_input_name =
|
|
||||||
impl_->text_session->GetInputNameAllocated(0, impl_->allocator).get();
|
|
||||||
impl_->text_enc_output_name =
|
|
||||||
impl_->text_session->GetOutputNameAllocated(0, impl_->allocator).get();
|
|
||||||
|
|
||||||
if (!config_.clip_vocab_path.empty() && !config_.clip_merges_path.empty()) {
|
|
||||||
try {
|
|
||||||
impl_->tokenizer.load(config_.clip_vocab_path, config_.clip_merges_path);
|
|
||||||
std::cout << "OpenVocab: tokenizer loaded." << std::endl;
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
std::cerr << "OpenVocab: failed to load tokenizer: " << e.what() << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
std::cerr << "OpenVocab: ZED_ONNX_CLIP_VOCAB / ZED_ONNX_CLIP_MERGES not set." << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!config_.labels.empty()) {
|
|
||||||
encode_labels(config_.labels);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Encode a comma-separated label string into cached_text_feats.
|
|
||||||
// Always fills exactly num_classes * 512 floats (pads with zeros if fewer labels).
|
|
||||||
void OpenVocab::encode_labels(const std::string& labels_str) {
|
|
||||||
if (!impl_->text_session || !impl_->tokenizer.is_loaded()) return;
|
|
||||||
|
|
||||||
auto label_vec = split_labels(labels_str);
|
|
||||||
int N = impl_->num_classes;
|
|
||||||
impl_->cached_text_feats.assign(static_cast<size_t>(N) * 512, 0.0f);
|
|
||||||
|
|
||||||
auto mem_info = Ort::MemoryInfo::CreateCpu(
|
|
||||||
OrtAllocatorType::OrtArenaAllocator, OrtMemTypeDefault);
|
|
||||||
|
|
||||||
int encode_n = std::min(static_cast<int>(label_vec.size()), N);
|
|
||||||
for (int i = 0; i < encode_n; i++) {
|
|
||||||
auto tokens = impl_->tokenizer.tokenize(label_vec[i]);
|
|
||||||
|
|
||||||
std::vector<int64_t> input_shape{1, util::ClipTokenizer::SEQ_LEN};
|
|
||||||
Ort::Value token_tensor = Ort::Value::CreateTensor<int64_t>(
|
|
||||||
mem_info, tokens.data(), tokens.size(),
|
|
||||||
input_shape.data(), input_shape.size());
|
|
||||||
|
|
||||||
const char* in = impl_->text_enc_input_name.c_str();
|
|
||||||
const char* out = impl_->text_enc_output_name.c_str();
|
|
||||||
std::vector<Ort::Value> outputs;
|
|
||||||
try {
|
|
||||||
outputs = impl_->text_session->Run(
|
|
||||||
Ort::RunOptions{nullptr}, &in, &token_tensor, 1, &out, 1);
|
|
||||||
} catch (const Ort::Exception& e) {
|
|
||||||
std::cerr << "OpenVocab: text encoder failed for \"" << label_vec[i]
|
|
||||||
<< "\": " << e.what() << std::endl;
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
const float* emb = outputs[0].GetTensorData<float>();
|
|
||||||
|
|
||||||
// L2 normalize
|
|
||||||
float norm = 0.0f;
|
|
||||||
for (int j = 0; j < 512; j++) norm += emb[j] * emb[j];
|
|
||||||
norm = std::sqrt(norm);
|
|
||||||
float inv_norm = (norm > 1e-8f) ? 1.0f / norm : 0.0f;
|
|
||||||
for (int j = 0; j < 512; j++)
|
|
||||||
impl_->cached_text_feats[i * 512 + j] = emb[j] * inv_norm;
|
|
||||||
}
|
|
||||||
|
|
||||||
impl_->cached_labels = labels_str;
|
|
||||||
std::cout << "OpenVocab: encoded " << encode_n << " label(s): " << labels_str << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
// --- detect(sl::Camera&) — production path with 3D fusion ---
|
|
||||||
|
|
||||||
void OpenVocab::detect(sl::Camera& camera) {
|
|
||||||
detected_.clear();
|
|
||||||
if (!impl_->session) return;
|
|
||||||
|
|
||||||
// Re-encode if labels changed (on-the-fly vocabulary switching)
|
|
||||||
if (impl_->has_text_input) {
|
|
||||||
const char* env_labels = std::getenv("ZED_ONNX_LABELS");
|
|
||||||
std::string current_labels = env_labels ? env_labels : config_.labels;
|
|
||||||
if (current_labels != impl_->cached_labels) {
|
|
||||||
encode_labels(current_labels);
|
|
||||||
}
|
|
||||||
if (impl_->cached_text_feats.empty()) return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Retrieve image and point cloud from camera
|
|
||||||
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<int>(img_mat.getWidth());
|
|
||||||
int orig_h = static_cast<int>(img_mat.getHeight());
|
|
||||||
|
|
||||||
// RGBA → BGR for run_inference
|
|
||||||
cv::Mat rgba(orig_h, orig_w, CV_8UC4, img_mat.getPtr<sl::uchar1>());
|
|
||||||
cv::Mat bgr;
|
|
||||||
cv::cvtColor(rgba, bgr, cv::COLOR_RGBA2BGR);
|
|
||||||
|
|
||||||
auto kept = impl_->run_inference(config_, bgr);
|
|
||||||
|
|
||||||
// 3D fusion
|
|
||||||
for (const auto& det : kept) {
|
|
||||||
FusionResult fusion = fuse_bbox(pc_mat,
|
|
||||||
static_cast<int>(det.x1), static_cast<int>(det.y1),
|
|
||||||
static_cast<int>(det.x2), static_cast<int>(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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// --- detect(const cv::Mat&) — image-only path, no 3D fusion ---
|
|
||||||
|
|
||||||
void OpenVocab::detect(const cv::Mat& image_bgr) {
|
|
||||||
detected_.clear();
|
|
||||||
if (!impl_->session) return;
|
|
||||||
|
|
||||||
// Re-encode if labels changed
|
|
||||||
if (impl_->has_text_input) {
|
|
||||||
const char* env_labels = std::getenv("ZED_ONNX_LABELS");
|
|
||||||
std::string current_labels = env_labels ? env_labels : config_.labels;
|
|
||||||
if (current_labels != impl_->cached_labels) {
|
|
||||||
encode_labels(current_labels);
|
|
||||||
}
|
|
||||||
if (impl_->cached_text_feats.empty()) 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;
|
|
||||||
// position stays zero (no point cloud available)
|
|
||||||
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;
|
|
||||||
// bounding_box_3d stays zero
|
|
||||||
detected_.push_back(obj);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void OpenVocab::set_labels(const std::string& labels) {
|
|
||||||
config_.labels = labels;
|
|
||||||
encode_labels(labels);
|
|
||||||
}
|
|
||||||
|
|
||||||
const std::vector<DetectedObject>& OpenVocab::objects() const {
|
|
||||||
return detected_;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::string OpenVocab::label_names() const {
|
|
||||||
return impl_->cached_labels.empty() ? config_.labels : impl_->cached_labels;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace engines
|
|
||||||
@@ -1,47 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "engines/engine.h"
|
|
||||||
|
|
||||||
#include <opencv2/core.hpp>
|
|
||||||
#include <memory>
|
|
||||||
#include <string>
|
|
||||||
|
|
||||||
namespace engines {
|
|
||||||
|
|
||||||
struct OpenVocabConfig {
|
|
||||||
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)
|
|
||||||
|
|
||||||
// Open-vocabulary text encoder (required when model has two inputs)
|
|
||||||
std::string text_encoder_path; // ZED_ONNX_TEXT_ENCODER_PATH
|
|
||||||
std::string clip_vocab_path; // ZED_ONNX_CLIP_VOCAB
|
|
||||||
std::string clip_merges_path; // ZED_ONNX_CLIP_MERGES
|
|
||||||
|
|
||||||
static OpenVocabConfig from_env();
|
|
||||||
};
|
|
||||||
|
|
||||||
class OpenVocab : public Engine {
|
|
||||||
public:
|
|
||||||
explicit OpenVocab(const OpenVocabConfig& config);
|
|
||||||
~OpenVocab() 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
|
|
||||||
void set_labels(const std::string& labels) override;
|
|
||||||
const std::vector<DetectedObject>& objects() const override;
|
|
||||||
std::string label_names() const override;
|
|
||||||
|
|
||||||
private:
|
|
||||||
OpenVocabConfig config_;
|
|
||||||
struct Impl;
|
|
||||||
std::unique_ptr<Impl> impl_;
|
|
||||||
std::vector<DetectedObject> detected_;
|
|
||||||
|
|
||||||
void encode_labels(const std::string& labels_str);
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace engines
|
|
||||||
@@ -1,99 +0,0 @@
|
|||||||
#include "engines/zed_native.h"
|
|
||||||
#include "util/env.h"
|
|
||||||
|
|
||||||
#include <iostream>
|
|
||||||
#include <string>
|
|
||||||
|
|
||||||
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<int>(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<int>(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<int>(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<DetectedObject>& ZedNative::objects() const {
|
|
||||||
return detected_;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::string ZedNative::label_names() const {
|
|
||||||
return "PERSON,VEHICLE,BAG,ANIMAL,ELECTRONICS,FRUIT_VEGETABLE,SPORT";
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace engines
|
|
||||||
@@ -1,34 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "engines/engine.h"
|
|
||||||
|
|
||||||
#include <string>
|
|
||||||
|
|
||||||
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<DetectedObject>& 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<DetectedObject> detected_;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace engines
|
|
||||||
+120
-224
@@ -1,32 +1,25 @@
|
|||||||
#include "engines/engine.h"
|
#include "arrow/array.h"
|
||||||
#include "engines/open_vocab.h"
|
#include "zed/capture.h"
|
||||||
#include "engines/zed_native.h"
|
|
||||||
#include "util/env.h"
|
|
||||||
#include "zed/camera.h"
|
|
||||||
|
|
||||||
#include <dora-node-api.h>
|
#include <dora-node-api.h>
|
||||||
#include <opencv2/opencv.hpp>
|
|
||||||
|
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <cstring>
|
#include <cstdint>
|
||||||
|
#include <cstdlib>
|
||||||
#include <initializer_list>
|
#include <initializer_list>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <memory>
|
|
||||||
#include <string>
|
#include <string>
|
||||||
|
#include <utility>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
constexpr int FIELDS_PER_OBJECT = 37;
|
rust::Vec<int64_t> make_vector_int(std::initializer_list<int64_t> values) {
|
||||||
|
rust::Vec<int64_t> out;
|
||||||
enum class PcMode { FULL, HALF, ROI };
|
for (int64_t v : values) {
|
||||||
|
out.push_back(v);
|
||||||
rust::Vec<int64_t> make_shape(int height, int width, int channels) {
|
}
|
||||||
rust::Vec<int64_t> shape;
|
return out;
|
||||||
shape.push_back(height);
|
|
||||||
shape.push_back(width);
|
|
||||||
shape.push_back(channels);
|
|
||||||
return shape;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
rust::Vec<double> make_distortion(const std::vector<double>& distortion) {
|
rust::Vec<double> make_distortion(const std::vector<double>& distortion) {
|
||||||
@@ -37,58 +30,64 @@ rust::Vec<double> make_distortion(const std::vector<double>& distortion) {
|
|||||||
return values;
|
return values;
|
||||||
}
|
}
|
||||||
|
|
||||||
rust::Vec<int64_t> make_vector_int(std::initializer_list<int64_t> values) {
|
void set_intrinsics(Metadata& metadata, const zed::Calibration& calib) {
|
||||||
rust::Vec<int64_t> out;
|
metadata.set_float("fx", calib.fx);
|
||||||
for (int64_t v : values) {
|
metadata.set_float("fy", calib.fy);
|
||||||
out.push_back(v);
|
metadata.set_float("cx", calib.cx);
|
||||||
}
|
metadata.set_float("cy", calib.cy);
|
||||||
return out;
|
metadata.set_list_float("distortion", make_distortion(calib.distortion));
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<float> serialize_objects(const std::vector<engines::DetectedObject>& objects) {
|
// Both clocks are needed. The device timestamp is the instant the frame was
|
||||||
std::vector<float> buffer(objects.size() * FIELDS_PER_OBJECT);
|
// captured and is the only value two nodes reading the same frame can agree on.
|
||||||
for (size_t i = 0; i < objects.size(); i++) {
|
// The host timestamp is what the rest of the dataflow and the session recorder
|
||||||
float* row = buffer.data() + i * FIELDS_PER_OBJECT;
|
// share, and on a Jetson it can step when NTP synchronises after boot - which is
|
||||||
const auto& obj = objects[i];
|
// exactly why the device timestamp is recorded alongside it rather than instead
|
||||||
row[0] = static_cast<float>(obj.label_id);
|
// of it.
|
||||||
row[1] = obj.confidence;
|
void set_frame_stamps(Metadata& metadata, const zed::Frame& frame) {
|
||||||
row[2] = obj.position[0];
|
metadata.set_int("frame_id", frame.frame_id);
|
||||||
row[3] = obj.position[1];
|
metadata.set_timestamp("device_timestamp_ns", frame.device_ns);
|
||||||
row[4] = obj.position[2];
|
metadata.set_timestamp("host_timestamp_ns", frame.host_ns);
|
||||||
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));
|
|
||||||
}
|
}
|
||||||
return buffer;
|
|
||||||
|
// Publish one Arrow array. dora moves both structs out and releases them once
|
||||||
|
// the payload is in shared memory, so the buffers they borrow only have to
|
||||||
|
// outlive this call - which the frame the caller is holding does.
|
||||||
|
void send(DoraNode& dora_node, const char* id, arrow_c::Exported payload,
|
||||||
|
rust::Box<Metadata> metadata) {
|
||||||
|
auto result = send_arrow_output(dora_node.send_output, id,
|
||||||
|
reinterpret_cast<uint8_t*>(&payload.array),
|
||||||
|
reinterpret_cast<uint8_t*>(&payload.schema),
|
||||||
|
std::move(metadata));
|
||||||
|
|
||||||
|
if (!std::string(result.error).empty()) {
|
||||||
|
std::cerr << "Error sending " << id << ": " << std::string(result.error) << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int64_t host_timestamp_ns() {
|
||||||
|
return std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||||
|
std::chrono::system_clock::now().time_since_epoch())
|
||||||
|
.count();
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
int main() {
|
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 dora_node = init_dora_node();
|
||||||
|
|
||||||
auto config = zed::Config::from_env();
|
// The camera runs on its own thread. The SDK can block for an unbounded time
|
||||||
zed::Camera camera(config);
|
// inside grab() and open() when the camera is disconnected, so nothing below
|
||||||
camera.open();
|
// may call into it: this loop only reads what the capture thread has already
|
||||||
camera.warmup();
|
// produced, which is what keeps Stop answerable while the camera is down.
|
||||||
auto calib = camera.calibration();
|
zed::Capture capture(config);
|
||||||
|
capture.start();
|
||||||
|
|
||||||
std::string engine_name = util::get_env_str("ZED_ENGINE", "native");
|
zed::Frame frame;
|
||||||
std::unique_ptr<engines::Engine> detector;
|
|
||||||
if (engine_name == "open_vocab") {
|
|
||||||
auto cfg = engines::OpenVocabConfig::from_env();
|
|
||||||
detector = std::make_unique<engines::OpenVocab>(cfg);
|
|
||||||
} else {
|
|
||||||
auto cfg = engines::DetectionConfig::from_env();
|
|
||||||
detector = std::make_unique<engines::ZedNative>(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;
|
|
||||||
while (true) {
|
while (true) {
|
||||||
auto event = dora_node.events->next();
|
auto event = dora_node.events->next();
|
||||||
auto type = event_type(event);
|
auto type = event_type(event);
|
||||||
@@ -101,194 +100,91 @@ int main() {
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
auto input = event_as_input(std::move(event));
|
std::string state;
|
||||||
std::string input_id = std::string(input.id);
|
if (capture.take_state_change(state)) {
|
||||||
|
auto state_metadata = new_metadata();
|
||||||
|
state_metadata->set_timestamp("host_timestamp_ns", host_timestamp_ns());
|
||||||
|
|
||||||
if (input_id == "update_vocabulary") {
|
send(dora_node, "camera_state", arrow_c::string(state.data(), state.size()),
|
||||||
if (!input.data.empty()) {
|
std::move(state_metadata));
|
||||||
std::string labels(
|
|
||||||
reinterpret_cast<const char*>(input.data.data()),
|
|
||||||
input.data.size());
|
|
||||||
detector->set_labels(labels);
|
|
||||||
std::cout << "Vocabulary updated: " << labels << std::endl;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 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;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!camera.grab()) {
|
auto width = static_cast<uint32_t>(frame.width);
|
||||||
std::cerr << "Grab failed; attempting to reconnect..." << std::endl;
|
auto height = static_cast<uint32_t>(frame.height);
|
||||||
camera.close();
|
|
||||||
camera.open();
|
|
||||||
calib = camera.calibration();
|
|
||||||
detector->enable(camera.handle());
|
|
||||||
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<int64_t>(
|
|
||||||
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
|
||||||
std::chrono::steady_clock::now().time_since_epoch()
|
|
||||||
).count()
|
|
||||||
);
|
|
||||||
|
|
||||||
// --- image_bgr ---
|
|
||||||
cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr<sl::uchar1>());
|
|
||||||
cv::Mat image_bgr;
|
|
||||||
cv::cvtColor(image_rgba, image_bgr, cv::COLOR_BGRA2BGR);
|
|
||||||
|
|
||||||
const auto image_size = static_cast<size_t>(image_bgr.total() * image_bgr.elemSize());
|
|
||||||
rust::Slice<const uint8_t> image_slice{
|
|
||||||
reinterpret_cast<const uint8_t*>(image_bgr.data), image_size};
|
|
||||||
|
|
||||||
|
// --- image ---
|
||||||
|
// The payload carries its own width, height and encoding, so none of
|
||||||
|
// that travels as metadata: what is left is what describes the frame
|
||||||
|
// rather than the picture.
|
||||||
auto image_metadata = new_metadata();
|
auto image_metadata = new_metadata();
|
||||||
image_metadata->set_string("dtype", "uint8");
|
set_intrinsics(*image_metadata, frame.calib);
|
||||||
image_metadata->set_string("encoding", "bgr8");
|
set_frame_stamps(*image_metadata, frame);
|
||||||
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);
|
|
||||||
|
|
||||||
auto image_result = send_output_with_metadata(
|
send(dora_node, "image",
|
||||||
dora_node.send_output, "image_bgr", image_slice, std::move(image_metadata));
|
arrow_c::image(width, height, "bgr8", frame.image_bgr.data(), frame.image_bgr.size()),
|
||||||
if (!std::string(image_result.error).empty()) {
|
std::move(image_metadata));
|
||||||
std::cerr << "Error sending image_bgr: " << std::string(image_result.error) << std::endl;
|
|
||||||
}
|
// --- depth ---
|
||||||
|
// A depth map is an image whose pixels are distances, which is how ROS
|
||||||
|
// spells it too: 32FC1 is one float32 per pixel, and `units` says what
|
||||||
|
// those floats measure.
|
||||||
|
auto depth_metadata = new_metadata();
|
||||||
|
depth_metadata->set_string("units", "mm");
|
||||||
|
set_intrinsics(*depth_metadata, frame.calib);
|
||||||
|
set_frame_stamps(*depth_metadata, frame);
|
||||||
|
|
||||||
|
send(dora_node, "depth",
|
||||||
|
arrow_c::image(width, height, "32FC1",
|
||||||
|
reinterpret_cast<const uint8_t*>(frame.depth.data()),
|
||||||
|
frame.depth.size() * sizeof(float)),
|
||||||
|
std::move(depth_metadata));
|
||||||
|
|
||||||
// --- point_cloud ---
|
// --- point_cloud ---
|
||||||
int pc_width = point_cloud_mat.getWidth();
|
if (frame.has_point_cloud) {
|
||||||
int pc_height = point_cloud_mat.getHeight();
|
// Unlike the two images, a cloud is a flat array of values: its
|
||||||
|
// grid shape is only in the metadata.
|
||||||
std::vector<float> pc_buf;
|
|
||||||
const uint8_t* pc_data_ptr = nullptr;
|
|
||||||
size_t pc_data_size = 0;
|
|
||||||
auto pc_metadata = new_metadata();
|
auto pc_metadata = new_metadata();
|
||||||
pc_metadata->set_string("dtype", "float32");
|
|
||||||
pc_metadata->set_string("channels", "XYZRGBA");
|
pc_metadata->set_string("channels", "XYZRGBA");
|
||||||
pc_metadata->set_string("units", "mm");
|
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<size_t>(hh * hw * 4));
|
|
||||||
const sl::float4* src = point_cloud_mat.getPtr<sl::float4>();
|
|
||||||
for (int y = 0; y < hh; y++) {
|
|
||||||
for (int x = 0; x < hw; x++) {
|
|
||||||
const sl::float4& pt = src[(y * 2) * pc_width + (x * 2)];
|
|
||||||
float* dst = pc_buf.data() + (y * hw + x) * 4;
|
|
||||||
dst[0] = pt.x; dst[1] = pt.y; dst[2] = pt.z; dst[3] = pt.w;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
pc_metadata->set_string("layout", "HWC");
|
pc_metadata->set_string("layout", "HWC");
|
||||||
pc_metadata->set_list_int("shape", make_shape(hh, hw, 4));
|
pc_metadata->set_list_int("shape",
|
||||||
pc_data_ptr = reinterpret_cast<const uint8_t*>(pc_buf.data());
|
make_vector_int({frame.pc_height, frame.pc_width, 4}));
|
||||||
pc_data_size = pc_buf.size() * sizeof(float);
|
set_intrinsics(*pc_metadata, frame.calib);
|
||||||
} else if (pc_mode == PcMode::ROI) {
|
set_frame_stamps(*pc_metadata, frame);
|
||||||
const auto& objs = detector->objects();
|
|
||||||
for (const auto& obj : objs) {
|
|
||||||
int x1 = std::max(0, static_cast<int>(obj.bounding_box_2d[0]));
|
|
||||||
int y1 = std::max(0, static_cast<int>(obj.bounding_box_2d[1]));
|
|
||||||
int x2 = std::min(pc_width - 1, static_cast<int>(obj.bounding_box_2d[4]));
|
|
||||||
int y2 = std::min(pc_height - 1, static_cast<int>(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<int>(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<const uint8_t*>(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<size_t>(pc_width * pc_height * sizeof(sl::float4));
|
|
||||||
pc_data_ptr = reinterpret_cast<const uint8_t*>(point_cloud_mat.getPtr<sl::float4>());
|
|
||||||
pc_metadata->set_string("layout", "HWC");
|
|
||||||
pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4));
|
|
||||||
}
|
|
||||||
|
|
||||||
rust::Slice<const uint8_t> pc_slice{pc_data_ptr, pc_data_size};
|
send(dora_node, "point_cloud",
|
||||||
auto pc_result = send_output_with_metadata(
|
arrow_c::float32(frame.point_cloud.data(), frame.point_cloud.size()),
|
||||||
dora_node.send_output, "point_cloud", pc_slice, std::move(pc_metadata));
|
std::move(pc_metadata));
|
||||||
if (!std::string(pc_result.error).empty()) {
|
|
||||||
std::cerr << "Error sending point_cloud: " << std::string(pc_result.error) << std::endl;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// --- camera_info ---
|
// --- camera_info ---
|
||||||
float camera_data[4] = {calib.fx, calib.fy, calib.cx, calib.cy};
|
if (frame.camera_info_first) {
|
||||||
rust::Slice<const uint8_t> cam_slice{
|
const float camera_data[4] = {frame.calib.fx, frame.calib.fy, frame.calib.cx,
|
||||||
reinterpret_cast<const uint8_t*>(camera_data), sizeof(camera_data)};
|
frame.calib.cy};
|
||||||
|
|
||||||
auto cam_metadata = new_metadata();
|
auto cam_metadata = new_metadata();
|
||||||
cam_metadata->set_string("dtype", "float32");
|
cam_metadata->set_list_float("distortion", make_distortion(frame.calib.distortion));
|
||||||
cam_metadata->set_string("layout", "C");
|
set_frame_stamps(*cam_metadata, frame);
|
||||||
cam_metadata->set_list_int("shape", make_vector_int({4}));
|
|
||||||
cam_metadata->set_list_float("distortion", make_distortion(calib.distortion));
|
|
||||||
|
|
||||||
auto cam_result = send_output_with_metadata(
|
send(dora_node, "camera_info", arrow_c::float32(camera_data, 4),
|
||||||
dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata));
|
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<int>(objects.size());
|
|
||||||
|
|
||||||
const size_t obj_size = obj_buffer.size() * sizeof(float);
|
|
||||||
rust::Slice<const uint8_t> obj_slice{
|
|
||||||
reinterpret_cast<const uint8_t*>(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;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,142 +0,0 @@
|
|||||||
// Standalone test for the OpenVocab detection engine.
|
|
||||||
// No dora framework, no live ZED camera — loads images from disk.
|
|
||||||
//
|
|
||||||
// Usage:
|
|
||||||
// bin/test_open_vocab [yolo_image] [yoloworld_image]
|
|
||||||
// yolo_image default: test/assets/yolo.png
|
|
||||||
// yoloworld_image default: test/assets/yoloworld.png
|
|
||||||
//
|
|
||||||
// Uses the same env vars as the main binary:
|
|
||||||
// ZED_ONNX_MODEL_PATH path to YOLO ONNX model (required)
|
|
||||||
// ZED_ONNX_LABELS comma-separated labels (required)
|
|
||||||
// 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_TEXT_ENCODER_PATH clip text encoder ONNX (open-vocab only)
|
|
||||||
// ZED_ONNX_CLIP_VOCAB clip_vocab.txt path (open-vocab only)
|
|
||||||
// ZED_ONNX_CLIP_MERGES clip_merges.txt path (open-vocab only)
|
|
||||||
// ZED_ONNX_WORLD_MODEL_PATH alternative ONNX model for the second image;
|
|
||||||
// falls back to ZED_ONNX_MODEL_PATH if not set
|
|
||||||
|
|
||||||
#include "engines/open_vocab.h"
|
|
||||||
#include "util/env.h"
|
|
||||||
|
|
||||||
#include <opencv2/opencv.hpp>
|
|
||||||
#include <sl/Camera.hpp>
|
|
||||||
|
|
||||||
#include <filesystem>
|
|
||||||
#include <iostream>
|
|
||||||
#include <sstream>
|
|
||||||
#include <string>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
namespace fs = std::filesystem;
|
|
||||||
|
|
||||||
namespace {
|
|
||||||
|
|
||||||
std::vector<std::string> parse_csv(const std::string& s) {
|
|
||||||
std::vector<std::string> 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<engines::DetectedObject>& objects,
|
|
||||||
const std::vector<std::string>& labels) {
|
|
||||||
for (const auto& obj : objects) {
|
|
||||||
int x1 = static_cast<int>(obj.bounding_box_2d[0]);
|
|
||||||
int y1 = static_cast<int>(obj.bounding_box_2d[1]);
|
|
||||||
int x2 = static_cast<int>(obj.bounding_box_2d[4]);
|
|
||||||
int y2 = static_cast<int>(obj.bounding_box_2d[5]);
|
|
||||||
|
|
||||||
std::string name = (obj.label_id >= 0 && obj.label_id < static_cast<int>(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);
|
|
||||||
std::string text = name + buf;
|
|
||||||
|
|
||||||
cv::rectangle(img, {x1, y1}, {x2, y2}, {0, 200, 0}, 2);
|
|
||||||
cv::putText(img, text, {x1, std::max(y1 - 6, 14)},
|
|
||||||
cv::FONT_HERSHEY_SIMPLEX, 0.55, {0, 200, 0}, 2, cv::LINE_AA);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool run_test(engines::OpenVocab& engine, const std::string& image_path) {
|
|
||||||
cv::Mat img = cv::imread(image_path);
|
|
||||||
if (img.empty()) {
|
|
||||||
std::cerr << "Error: cannot read image: " << image_path << std::endl;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
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<int>(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 true;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
int main(int argc, char* argv[]) {
|
|
||||||
std::string yolo_path = (argc > 1) ? argv[1] : "test/assets/yolo.png";
|
|
||||||
std::string yoloworld_path = (argc > 2) ? argv[2] : "test/assets/yoloworld.png";
|
|
||||||
|
|
||||||
auto config = engines::OpenVocabConfig::from_env();
|
|
||||||
|
|
||||||
// enable() ignores this parameter for OpenVocab
|
|
||||||
sl::Camera dummy_cam;
|
|
||||||
|
|
||||||
int failures = 0;
|
|
||||||
|
|
||||||
// --- Test 1: yolo image ---
|
|
||||||
{
|
|
||||||
std::cout << "\n=== Test 1: " << yolo_path << " ===" << std::endl;
|
|
||||||
engines::OpenVocab engine(config);
|
|
||||||
engine.enable(dummy_cam);
|
|
||||||
if (!run_test(engine, yolo_path)) ++failures;
|
|
||||||
}
|
|
||||||
|
|
||||||
// --- Test 2: yoloworld image (optionally with a different model) ---
|
|
||||||
{
|
|
||||||
std::cout << "\n=== Test 2: " << yoloworld_path << " ===" << std::endl;
|
|
||||||
engines::OpenVocabConfig config2 = config;
|
|
||||||
const char* world_model = std::getenv("ZED_ONNX_WORLD_MODEL_PATH");
|
|
||||||
if (world_model && *world_model)
|
|
||||||
config2.model_path = world_model;
|
|
||||||
|
|
||||||
engines::OpenVocab engine(config2);
|
|
||||||
engine.enable(dummy_cam);
|
|
||||||
if (!run_test(engine, yoloworld_path)) ++failures;
|
|
||||||
}
|
|
||||||
|
|
||||||
return failures > 0 ? 1 : 0;
|
|
||||||
}
|
|
||||||
@@ -1,226 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
// Minimal CLIP ViT-B/32 BPE tokenizer — header-only, C++17, no external deps.
|
|
||||||
//
|
|
||||||
// Loads clip_vocab.txt (token\tid, tab-separated) and clip_merges.txt
|
|
||||||
// (a b, space-separated BPE pairs) produced by export_clip_text_encoder.py.
|
|
||||||
// Returns 77 int64 token IDs compatible with clip_text_encoder.onnx.
|
|
||||||
|
|
||||||
#include <algorithm>
|
|
||||||
#include <climits>
|
|
||||||
#include <fstream>
|
|
||||||
#include <sstream>
|
|
||||||
#include <stdexcept>
|
|
||||||
#include <string>
|
|
||||||
#include <unordered_map>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
namespace util {
|
|
||||||
|
|
||||||
class ClipTokenizer {
|
|
||||||
public:
|
|
||||||
static constexpr int SEQ_LEN = 77;
|
|
||||||
static constexpr int64_t BOS = 49406;
|
|
||||||
static constexpr int64_t EOS = 49407;
|
|
||||||
static constexpr int64_t PAD = 0;
|
|
||||||
|
|
||||||
void load(const std::string& vocab_path, const std::string& merges_path) {
|
|
||||||
load_vocab(vocab_path);
|
|
||||||
load_merges(merges_path);
|
|
||||||
init_byte_encoder();
|
|
||||||
}
|
|
||||||
|
|
||||||
bool is_loaded() const { return !vocab_.empty(); }
|
|
||||||
|
|
||||||
// Tokenize a label string. Returns exactly SEQ_LEN int64 token IDs.
|
|
||||||
std::vector<int64_t> tokenize(const std::string& text) const {
|
|
||||||
std::vector<int64_t> ids;
|
|
||||||
ids.reserve(SEQ_LEN);
|
|
||||||
ids.push_back(BOS);
|
|
||||||
|
|
||||||
std::string lower = text;
|
|
||||||
std::transform(lower.begin(), lower.end(), lower.begin(),
|
|
||||||
[](unsigned char c) { return static_cast<char>(std::tolower(c)); });
|
|
||||||
|
|
||||||
// Split on whitespace — sufficient for typical detection label names
|
|
||||||
std::istringstream ss(lower);
|
|
||||||
std::string word;
|
|
||||||
while (ss >> word) {
|
|
||||||
if (static_cast<int>(ids.size()) >= SEQ_LEN - 1) break;
|
|
||||||
|
|
||||||
// Byte-encode the word via the GPT-2 byte encoder
|
|
||||||
std::string encoded;
|
|
||||||
encoded.reserve(word.size() * 3);
|
|
||||||
for (unsigned char c : word) {
|
|
||||||
auto it = byte_encoder_.find(c);
|
|
||||||
if (it != byte_encoder_.end())
|
|
||||||
encoded += utf8_of(it->second);
|
|
||||||
}
|
|
||||||
if (encoded.empty()) continue;
|
|
||||||
|
|
||||||
// Apply BPE
|
|
||||||
auto tokens = bpe_encode(encoded);
|
|
||||||
for (const auto& tok : tokens) {
|
|
||||||
if (static_cast<int>(ids.size()) >= SEQ_LEN - 1) break;
|
|
||||||
auto vit = vocab_.find(tok);
|
|
||||||
if (vit != vocab_.end())
|
|
||||||
ids.push_back(static_cast<int64_t>(vit->second));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
ids.push_back(EOS);
|
|
||||||
ids.resize(SEQ_LEN, PAD);
|
|
||||||
return ids;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
std::unordered_map<std::string, int> vocab_; // token string → id
|
|
||||||
std::unordered_map<std::string, int> merge_ranks_; // "a b" → rank
|
|
||||||
std::unordered_map<unsigned char, uint32_t> byte_encoder_; // byte → unicode codepoint
|
|
||||||
|
|
||||||
// -----------------------------------------------------------------
|
|
||||||
// Byte encoder — matches Python's bytes_to_unicode() in OpenAI CLIP
|
|
||||||
// -----------------------------------------------------------------
|
|
||||||
void init_byte_encoder() {
|
|
||||||
// Printable ASCII + Latin-1 supplement ranges that map to themselves
|
|
||||||
std::vector<int> bs;
|
|
||||||
for (int i = '!'; i <= '~'; i++) bs.push_back(i);
|
|
||||||
for (int i = 0xA1; i <= 0xAC; i++) bs.push_back(i);
|
|
||||||
for (int i = 0xAE; i <= 0xFF; i++) bs.push_back(i);
|
|
||||||
|
|
||||||
std::vector<uint32_t> cs;
|
|
||||||
cs.reserve(bs.size());
|
|
||||||
for (int b : bs) cs.push_back(static_cast<uint32_t>(b));
|
|
||||||
|
|
||||||
// Remaining bytes map to codepoints starting at U+0100
|
|
||||||
int n = 0;
|
|
||||||
for (int b = 0; b < 256; b++) {
|
|
||||||
if (std::find(bs.begin(), bs.end(), b) == bs.end()) {
|
|
||||||
bs.push_back(b);
|
|
||||||
cs.push_back(256u + static_cast<uint32_t>(n));
|
|
||||||
++n;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
byte_encoder_.clear();
|
|
||||||
for (size_t i = 0; i < bs.size(); i++)
|
|
||||||
byte_encoder_[static_cast<unsigned char>(bs[i])] = cs[i];
|
|
||||||
}
|
|
||||||
|
|
||||||
// Encode a unicode codepoint as a UTF-8 std::string
|
|
||||||
static std::string utf8_of(uint32_t cp) {
|
|
||||||
std::string s;
|
|
||||||
if (cp < 0x80u) {
|
|
||||||
s += static_cast<char>(cp);
|
|
||||||
} else if (cp < 0x800u) {
|
|
||||||
s += static_cast<char>(0xC0u | (cp >> 6));
|
|
||||||
s += static_cast<char>(0x80u | (cp & 0x3Fu));
|
|
||||||
} else if (cp < 0x10000u) {
|
|
||||||
s += static_cast<char>(0xE0u | (cp >> 12));
|
|
||||||
s += static_cast<char>(0x80u | ((cp >> 6) & 0x3Fu));
|
|
||||||
s += static_cast<char>(0x80u | (cp & 0x3Fu));
|
|
||||||
} else {
|
|
||||||
s += static_cast<char>(0xF0u | (cp >> 18));
|
|
||||||
s += static_cast<char>(0x80u | ((cp >> 12) & 0x3Fu));
|
|
||||||
s += static_cast<char>(0x80u | ((cp >> 6) & 0x3Fu));
|
|
||||||
s += static_cast<char>(0x80u | (cp & 0x3Fu));
|
|
||||||
}
|
|
||||||
return s;
|
|
||||||
}
|
|
||||||
|
|
||||||
// -----------------------------------------------------------------
|
|
||||||
// BPE
|
|
||||||
// -----------------------------------------------------------------
|
|
||||||
|
|
||||||
// Split a UTF-8 string into individual Unicode character substrings
|
|
||||||
static std::vector<std::string> split_utf8_chars(const std::string& s) {
|
|
||||||
std::vector<std::string> chars;
|
|
||||||
for (size_t i = 0; i < s.size(); ) {
|
|
||||||
unsigned char c = static_cast<unsigned char>(s[i]);
|
|
||||||
int len = (c < 0x80) ? 1 : (c < 0xE0) ? 2 : (c < 0xF0) ? 3 : 4;
|
|
||||||
chars.push_back(s.substr(i, len));
|
|
||||||
i += len;
|
|
||||||
}
|
|
||||||
return chars;
|
|
||||||
}
|
|
||||||
|
|
||||||
// BPE-encode a single byte-encoded word, returning the token strings
|
|
||||||
std::vector<std::string> bpe_encode(const std::string& word_str) const {
|
|
||||||
std::vector<std::string> word = split_utf8_chars(word_str);
|
|
||||||
if (word.size() <= 1) return word;
|
|
||||||
|
|
||||||
while (true) {
|
|
||||||
// Find the pair with the lowest merge rank
|
|
||||||
int best_rank = INT_MAX;
|
|
||||||
int best_i = -1;
|
|
||||||
for (int i = 0; i + 1 < static_cast<int>(word.size()); i++) {
|
|
||||||
auto it = merge_ranks_.find(word[i] + " " + word[i + 1]);
|
|
||||||
if (it != merge_ranks_.end() && it->second < best_rank) {
|
|
||||||
best_rank = it->second;
|
|
||||||
best_i = i;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (best_i < 0) break;
|
|
||||||
|
|
||||||
// Apply merge at best_i
|
|
||||||
std::string merged = word[best_i] + word[best_i + 1];
|
|
||||||
word.erase(word.begin() + best_i, word.begin() + best_i + 2);
|
|
||||||
word.insert(word.begin() + best_i, std::move(merged));
|
|
||||||
}
|
|
||||||
|
|
||||||
return word;
|
|
||||||
}
|
|
||||||
|
|
||||||
// -----------------------------------------------------------------
|
|
||||||
// File loading
|
|
||||||
// -----------------------------------------------------------------
|
|
||||||
|
|
||||||
void load_vocab(const std::string& path) {
|
|
||||||
std::ifstream f(path);
|
|
||||||
if (!f.is_open())
|
|
||||||
throw std::runtime_error("ClipTokenizer: cannot open vocab file: " + path);
|
|
||||||
|
|
||||||
std::string line;
|
|
||||||
while (std::getline(f, line)) {
|
|
||||||
auto tab = line.rfind('\t');
|
|
||||||
if (tab == std::string::npos) continue;
|
|
||||||
std::string token = unescape(line.substr(0, tab));
|
|
||||||
int id = std::stoi(line.substr(tab + 1));
|
|
||||||
vocab_[token] = id;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void load_merges(const std::string& path) {
|
|
||||||
std::ifstream f(path);
|
|
||||||
if (!f.is_open())
|
|
||||||
throw std::runtime_error("ClipTokenizer: cannot open merges file: " + path);
|
|
||||||
|
|
||||||
std::string line;
|
|
||||||
int rank = 0;
|
|
||||||
while (std::getline(f, line)) {
|
|
||||||
if (line.empty() || line[0] == '#') continue;
|
|
||||||
auto sp = line.find(' ');
|
|
||||||
if (sp == std::string::npos) continue;
|
|
||||||
merge_ranks_[line] = rank++; // key is "a b" as written in the file
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static std::string unescape(const std::string& s) {
|
|
||||||
std::string r;
|
|
||||||
r.reserve(s.size());
|
|
||||||
for (size_t i = 0; i < s.size(); i++) {
|
|
||||||
if (s[i] == '\\' && i + 1 < s.size()) {
|
|
||||||
switch (s[++i]) {
|
|
||||||
case '\\': r += '\\'; break;
|
|
||||||
case 'n': r += '\n'; break;
|
|
||||||
case 't': r += '\t'; break;
|
|
||||||
default: r += '\\'; r += s[i]; break;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
r += s[i];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return r;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace util
|
|
||||||
+174
-25
@@ -1,11 +1,12 @@
|
|||||||
#include "zed/camera.h"
|
#include "zed/camera.h"
|
||||||
#include "util/env.h"
|
#include "util/env.h"
|
||||||
|
|
||||||
#include <chrono>
|
#include <algorithm>
|
||||||
|
#include <cstdlib>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <iterator>
|
#include <iterator>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <thread>
|
#include <utility>
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
@@ -21,17 +22,30 @@ sl::RESOLUTION parse_resolution(int resolution) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const std::pair<const char*, sl::DEPTH_MODE> 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) {
|
sl::DEPTH_MODE parse_depth_mode(const std::string& mode) {
|
||||||
if (mode == "PERFORMANCE") {
|
for (const auto& entry : DEPTH_MODES) {
|
||||||
return sl::DEPTH_MODE::PERFORMANCE;
|
if (mode == entry.first) {
|
||||||
|
return entry.second;
|
||||||
}
|
}
|
||||||
if (mode == "QUALITY") {
|
|
||||||
return sl::DEPTH_MODE::QUALITY;
|
|
||||||
}
|
}
|
||||||
if (mode == "NONE") {
|
std::cerr << "Unknown ZED_DEPTH_MODE '" << mode << "'. Accepted values:";
|
||||||
return sl::DEPTH_MODE::NONE;
|
for (const auto& entry : DEPTH_MODES) {
|
||||||
|
std::cerr << " " << entry.first;
|
||||||
}
|
}
|
||||||
return sl::DEPTH_MODE::NEURAL;
|
std::cerr << std::endl;
|
||||||
|
std::exit(1);
|
||||||
}
|
}
|
||||||
|
|
||||||
sl::FLIP_MODE parse_flip(const std::string& mode) {
|
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;
|
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() {
|
void list_available_cameras() {
|
||||||
auto devices = sl::Camera::getDeviceList();
|
auto devices = sl::Camera::getDeviceList();
|
||||||
std::cout << "Detected ZED cameras: " << devices.size() << std::endl;
|
std::cout << "Detected ZED cameras: " << devices.size() << std::endl;
|
||||||
@@ -59,50 +91,112 @@ Config Config::from_env() {
|
|||||||
Config c;
|
Config c;
|
||||||
c.resolution = util::get_env_int("ZED_RESOLUTION", c.resolution);
|
c.resolution = util::get_env_int("ZED_RESOLUTION", c.resolution);
|
||||||
c.fps = util::get_env_int("ZED_FPS", c.fps);
|
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_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_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.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.flip = util::get_env_str("ZED_FLIP", c.flip);
|
||||||
c.warmup_frames = util::get_env_int("ZED_WARMUP_FRAMES", c.warmup_frames);
|
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;
|
return c;
|
||||||
}
|
}
|
||||||
|
|
||||||
Camera::Camera(const Config& config) : config_(config) {
|
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_resolution = parse_resolution(config_.resolution);
|
||||||
init_params_.camera_fps = config_.fps;
|
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_.coordinate_units = sl::UNIT::MILLIMETER;
|
||||||
init_params_.camera_image_flip = parse_flip(config_.flip);
|
init_params_.camera_image_flip = parse_flip(config_.flip);
|
||||||
init_params_.depth_minimum_distance = config_.depth_min_mm;
|
init_params_.depth_minimum_distance = config_.depth_min_mm;
|
||||||
init_params_.depth_maximum_distance = config_.depth_max_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_depth = true;
|
||||||
runtime_params_.enable_fill_mode = config_.depth_fill;
|
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() {
|
Camera::~Camera() {
|
||||||
close();
|
close();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Camera::open() {
|
bool Camera::open() {
|
||||||
list_available_cameras();
|
|
||||||
while (true) {
|
|
||||||
sl::ERROR_CODE err = zed_.open(init_params_);
|
sl::ERROR_CODE err = zed_.open(init_params_);
|
||||||
if (err == sl::ERROR_CODE::SUCCESS) {
|
if (err != sl::ERROR_CODE::SUCCESS) {
|
||||||
return;
|
|
||||||
}
|
|
||||||
std::cerr << "Failed to open ZED camera: " << sl::toString(err) << std::endl;
|
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() {
|
void Camera::close() {
|
||||||
|
recovering_ = false;
|
||||||
zed_.close();
|
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<int>(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() {
|
void Camera::warmup() {
|
||||||
for (int i = 0; i < config_.warmup_frames; i++) {
|
for (int i = 0; i < config_.warmup_frames; i++) {
|
||||||
zed_.grab(runtime_params_);
|
zed_.grab(runtime_params_);
|
||||||
@@ -110,11 +204,34 @@ void Camera::warmup() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool Camera::grab() {
|
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;
|
return false;
|
||||||
}
|
}
|
||||||
|
if (recovering_) {
|
||||||
|
recovering_ = false;
|
||||||
|
std::cout << "ZED camera link restored" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
zed_.retrieveImage(image_mat_, sl::VIEW::LEFT);
|
zed_.retrieveImage(image_mat_, sl::VIEW::LEFT);
|
||||||
|
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);
|
zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA);
|
||||||
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -135,16 +252,48 @@ Calibration Camera::calibration() const {
|
|||||||
return result;
|
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<sl::uchar4>();
|
||||||
|
|
||||||
|
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<size_t>(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<int64_t>(
|
||||||
|
zed_.getTimestamp(sl::TIME_REFERENCE::IMAGE).getNanoseconds());
|
||||||
|
}
|
||||||
|
|
||||||
sl::Mat& Camera::image() {
|
sl::Mat& Camera::image() {
|
||||||
return image_mat_;
|
return image_mat_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
sl::Mat& Camera::depth() {
|
||||||
|
return depth_mat_;
|
||||||
|
}
|
||||||
|
|
||||||
sl::Mat& Camera::point_cloud() {
|
sl::Mat& Camera::point_cloud() {
|
||||||
return point_cloud_mat_;
|
return point_cloud_mat_;
|
||||||
}
|
}
|
||||||
|
|
||||||
sl::Camera& Camera::handle() {
|
|
||||||
return zed_;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace zed
|
} // namespace zed
|
||||||
|
|||||||
+32
-8
@@ -1,27 +1,33 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <sl/Camera.hpp>
|
#include <sl/Camera.hpp>
|
||||||
|
#include <cstdint>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
namespace zed {
|
namespace zed {
|
||||||
|
|
||||||
|
enum class PcMode { FULL, HALF };
|
||||||
|
|
||||||
struct Config {
|
struct Config {
|
||||||
int resolution = 720;
|
int resolution = 1080;
|
||||||
int fps = 15;
|
int fps = 15;
|
||||||
std::string depth_mode = "NEURAL";
|
sl::DEPTH_MODE depth_mode = sl::DEPTH_MODE::NEURAL;
|
||||||
int depth_min_mm = 10;
|
int depth_min_mm = 200;
|
||||||
int depth_max_mm = 500;
|
int depth_max_mm = 600;
|
||||||
bool depth_fill = false;
|
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";
|
std::string flip = "ON";
|
||||||
int warmup_frames = 30;
|
int warmup_frames = 30;
|
||||||
int retry_interval_ms = 5000;
|
|
||||||
|
|
||||||
static Config from_env();
|
static Config from_env();
|
||||||
};
|
};
|
||||||
|
|
||||||
struct Calibration {
|
struct Calibration {
|
||||||
float fx, fy, cx, cy;
|
float fx = 0, fy = 0, cx = 0, cy = 0;
|
||||||
std::vector<double> distortion;
|
std::vector<double> distortion;
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -30,14 +36,26 @@ public:
|
|||||||
explicit Camera(const Config& config);
|
explicit Camera(const Config& config);
|
||||||
~Camera();
|
~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();
|
void close();
|
||||||
|
// Close the handle and reset the device over USB, where the model allows it.
|
||||||
|
void reset();
|
||||||
void warmup();
|
void warmup();
|
||||||
bool grab();
|
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;
|
Calibration calibration() const;
|
||||||
|
int64_t device_timestamp_ns();
|
||||||
sl::Mat& image();
|
sl::Mat& image();
|
||||||
|
sl::Mat& depth();
|
||||||
sl::Mat& point_cloud();
|
sl::Mat& point_cloud();
|
||||||
sl::Camera& handle();
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
Config config_;
|
Config config_;
|
||||||
@@ -45,7 +63,13 @@ private:
|
|||||||
sl::InitParameters init_params_;
|
sl::InitParameters init_params_;
|
||||||
sl::RuntimeParameters runtime_params_;
|
sl::RuntimeParameters runtime_params_;
|
||||||
sl::Mat image_mat_;
|
sl::Mat image_mat_;
|
||||||
|
sl::Mat depth_mat_;
|
||||||
sl::Mat point_cloud_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
|
} // namespace zed
|
||||||
|
|||||||
@@ -0,0 +1,302 @@
|
|||||||
|
#include "zed/capture.h"
|
||||||
|
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <chrono>
|
||||||
|
#include <cmath>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
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<size_t>(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<double>(Clock::now() - since).count();
|
||||||
|
}
|
||||||
|
|
||||||
|
int64_t host_timestamp_ns() {
|
||||||
|
return std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||||
|
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<float>& 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<sl::float1>();
|
||||||
|
|
||||||
|
out.resize(static_cast<size_t>(height) * static_cast<size_t>(width));
|
||||||
|
for (int y = 0; y < height; y++) {
|
||||||
|
const float* row = src + static_cast<size_t>(y) * step;
|
||||||
|
float* dst = out.data() + static_cast<size_t>(y) * static_cast<size_t>(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<std::mutex> lock(mutex_);
|
||||||
|
running_ = true;
|
||||||
|
}
|
||||||
|
thread_ = std::thread(&Capture::run, this);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Capture::stop() {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
running_ = false;
|
||||||
|
wake_.notify_all();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Capture::wait_exit(std::chrono::milliseconds timeout) {
|
||||||
|
std::unique_lock<std::mutex> lock(mutex_);
|
||||||
|
return exited_.wait_for(lock, timeout, [this] { return finished_; });
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Capture::take_frame(Frame& out) {
|
||||||
|
std::lock_guard<std::mutex> 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<std::mutex> lock(mutex_);
|
||||||
|
if (!state_changed_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
out = state_;
|
||||||
|
state_changed_ = false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Capture::set_state(const char* state) {
|
||||||
|
std::lock_guard<std::mutex> 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<std::mutex> 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<std::chrono::milliseconds>(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<sl::uchar1>(),
|
||||||
|
image_mat.getStep(sl::MEM::CPU) * sizeof(sl::uchar4));
|
||||||
|
frame.image_bgr.resize(static_cast<size_t>(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<sl::float4>();
|
||||||
|
|
||||||
|
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<size_t>(frame.pc_height) * frame.pc_width * 4);
|
||||||
|
|
||||||
|
for (int y = 0; y < frame.pc_height; y++) {
|
||||||
|
const sl::float4* row = src + static_cast<size_t>(y * stride) * pc_step;
|
||||||
|
float* dst = frame.point_cloud.data() + static_cast<size_t>(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<std::mutex> 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<std::mutex> 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<std::mutex> lock(mutex_);
|
||||||
|
finished_ = true;
|
||||||
|
exited_.notify_all();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace zed
|
||||||
@@ -0,0 +1,94 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "zed/camera.h"
|
||||||
|
|
||||||
|
#include <condition_variable>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <thread>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
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<uint8_t> image_bgr; // height * width * 3
|
||||||
|
std::vector<float> depth; // height * width, millimetres
|
||||||
|
bool has_point_cloud = false;
|
||||||
|
int pc_width = 0;
|
||||||
|
int pc_height = 0;
|
||||||
|
std::vector<float> 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
|
||||||
@@ -0,0 +1,36 @@
|
|||||||
|
// C entry points for tests/test_arrow.py.
|
||||||
|
//
|
||||||
|
// The Python side cannot call the C++ builders, so each one is wrapped here and
|
||||||
|
// the payload is passed in from the test rather than fixed in this file.
|
||||||
|
|
||||||
|
#include "arrow/array.h"
|
||||||
|
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
void hand_over(void* array, void* schema, const arrow_c::Exported& exported) {
|
||||||
|
// The caller owns the two structs, exactly as dora does: it reads them and
|
||||||
|
// is then responsible for releasing what they point at.
|
||||||
|
std::memcpy(array, &exported.array, sizeof(exported.array));
|
||||||
|
std::memcpy(schema, &exported.schema, sizeof(exported.schema));
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
|
||||||
|
void zed_export_image(void* array, void* schema, uint32_t width, uint32_t height,
|
||||||
|
const char* encoding, const uint8_t* data, size_t size) {
|
||||||
|
hand_over(array, schema, arrow_c::image(width, height, encoding, data, size));
|
||||||
|
}
|
||||||
|
|
||||||
|
void zed_export_float32(void* array, void* schema, const float* values, size_t count) {
|
||||||
|
hand_over(array, schema, arrow_c::float32(values, count));
|
||||||
|
}
|
||||||
|
|
||||||
|
void zed_export_string(void* array, void* schema, const char* text, size_t size) {
|
||||||
|
hand_over(array, schema, arrow_c::string(text, size));
|
||||||
|
}
|
||||||
|
|
||||||
|
} // extern "C"
|
||||||
@@ -0,0 +1,54 @@
|
|||||||
|
// Release has to free the whole tree: the array nodes, the schema nodes, and
|
||||||
|
// the storage their buffers point into. Nothing here asserts that directly -
|
||||||
|
// the sanitizers tests/run.sh builds this with are the assertion, and a node
|
||||||
|
// that leaks per frame is a node that dies on a long run.
|
||||||
|
|
||||||
|
#include "arrow/array.h"
|
||||||
|
|
||||||
|
#include <cassert>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
void release(arrow_c::Exported& exported) {
|
||||||
|
exported.array.release(&exported.array);
|
||||||
|
exported.schema.release(&exported.schema);
|
||||||
|
|
||||||
|
// A released struct marks itself, which is how a consumer knows not to
|
||||||
|
// release it twice.
|
||||||
|
assert(exported.array.release == nullptr);
|
||||||
|
assert(exported.schema.release == nullptr);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
int main() {
|
||||||
|
const std::vector<uint8_t> pixels(4 * 4 * 3, 7);
|
||||||
|
const std::vector<float> depth(4 * 4, 1500.0F);
|
||||||
|
const std::string state = "streaming";
|
||||||
|
|
||||||
|
for (int i = 0; i < 100; ++i) {
|
||||||
|
arrow_c::Exported image =
|
||||||
|
arrow_c::image(4, 4, "bgr8", pixels.data(), pixels.size());
|
||||||
|
assert(image.array.length == 1);
|
||||||
|
assert(image.array.n_children == 4);
|
||||||
|
release(image);
|
||||||
|
|
||||||
|
arrow_c::Exported depth_image =
|
||||||
|
arrow_c::image(4, 4, "32FC1", reinterpret_cast<const uint8_t*>(depth.data()),
|
||||||
|
depth.size() * sizeof(float));
|
||||||
|
release(depth_image);
|
||||||
|
|
||||||
|
arrow_c::Exported values = arrow_c::float32(depth.data(), depth.size());
|
||||||
|
assert(values.array.length == static_cast<int64_t>(depth.size()));
|
||||||
|
release(values);
|
||||||
|
|
||||||
|
arrow_c::Exported text = arrow_c::string(state.data(), state.size());
|
||||||
|
assert(text.array.length == 1);
|
||||||
|
release(text);
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
Executable
+29
@@ -0,0 +1,29 @@
|
|||||||
|
#!/usr/bin/env bash
|
||||||
|
# Build and run the payload tests.
|
||||||
|
#
|
||||||
|
# They cover src/arrow only, which is the part of the node that has no ZED SDK,
|
||||||
|
# no CUDA and no dora dependency - so this runs anywhere, and is what CI runs.
|
||||||
|
set -euo pipefail
|
||||||
|
|
||||||
|
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||||
|
REPO_DIR="$(dirname "$SCRIPT_DIR")"
|
||||||
|
OUT_DIR="${REPO_DIR}/build/tests"
|
||||||
|
|
||||||
|
mkdir -p "$OUT_DIR"
|
||||||
|
|
||||||
|
# Sanitized: the release callbacks have to free the whole exported tree, and a
|
||||||
|
# leak there would be one per frame.
|
||||||
|
g++ -std=c++17 -Wall -Wextra -Werror -g -fsanitize=address,undefined \
|
||||||
|
-I "${REPO_DIR}/src" \
|
||||||
|
-o "${OUT_DIR}/arrow_test" \
|
||||||
|
"${REPO_DIR}/src/arrow/array.cc" "${REPO_DIR}/tests/arrow_test.cc"
|
||||||
|
"${OUT_DIR}/arrow_test"
|
||||||
|
echo "ok arrow_test"
|
||||||
|
|
||||||
|
# Unsanitized: pyarrow is the consumer standing in for dora, and loads the same
|
||||||
|
# code as a shared library.
|
||||||
|
g++ -std=c++17 -Wall -Wextra -Werror -O2 -shared -fPIC \
|
||||||
|
-I "${REPO_DIR}/src" \
|
||||||
|
-o "${OUT_DIR}/libzed_arrow.so" \
|
||||||
|
"${REPO_DIR}/src/arrow/array.cc" "${REPO_DIR}/tests/arrow_export.cc"
|
||||||
|
uv run --no-project --with pyarrow python "${REPO_DIR}/tests/test_arrow.py"
|
||||||
@@ -0,0 +1,129 @@
|
|||||||
|
"""Round-trip every exported payload through a real Arrow consumer.
|
||||||
|
|
||||||
|
The node builds its Arrow arrays by hand against the C data interface rather
|
||||||
|
than linking Arrow C++, so this is what shows that what dora imports on the
|
||||||
|
other side of `send_arrow_output` is what went in. It needs neither a camera nor
|
||||||
|
the ZED SDK.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import ctypes
|
||||||
|
import pathlib
|
||||||
|
import sys
|
||||||
|
|
||||||
|
import pyarrow as pa
|
||||||
|
|
||||||
|
LIBRARY = pathlib.Path(__file__).resolve().parent.parent / "build" / "tests" / "libzed_arrow.so"
|
||||||
|
|
||||||
|
# ArrowArray is ten machine words and ArrowSchema nine, and both want the
|
||||||
|
# 8-byte alignment an array of uint64 gives.
|
||||||
|
WORDS = 16
|
||||||
|
# Index of the `release` field in ArrowArray: a consumer that took ownership
|
||||||
|
# zeroes it, which is how this test sees the move happen.
|
||||||
|
RELEASE = 8
|
||||||
|
|
||||||
|
IMAGE_TYPE = pa.struct([
|
||||||
|
("width", pa.uint32()),
|
||||||
|
("height", pa.uint32()),
|
||||||
|
("encoding", pa.utf8()),
|
||||||
|
("data", pa.large_binary()),
|
||||||
|
])
|
||||||
|
|
||||||
|
|
||||||
|
def load():
|
||||||
|
"""Load the test library and declare the argument types of its exports."""
|
||||||
|
library = ctypes.CDLL(str(LIBRARY))
|
||||||
|
|
||||||
|
library.zed_export_image.argtypes = [
|
||||||
|
ctypes.c_void_p, ctypes.c_void_p, ctypes.c_uint32, ctypes.c_uint32,
|
||||||
|
ctypes.c_char_p, ctypes.c_char_p, ctypes.c_size_t,
|
||||||
|
]
|
||||||
|
library.zed_export_float32.argtypes = [
|
||||||
|
ctypes.c_void_p, ctypes.c_void_p, ctypes.POINTER(ctypes.c_float), ctypes.c_size_t,
|
||||||
|
]
|
||||||
|
library.zed_export_string.argtypes = [
|
||||||
|
ctypes.c_void_p, ctypes.c_void_p, ctypes.c_char_p, ctypes.c_size_t,
|
||||||
|
]
|
||||||
|
return library
|
||||||
|
|
||||||
|
|
||||||
|
def structs():
|
||||||
|
"""Allocate the ArrowArray and ArrowSchema the producer fills in."""
|
||||||
|
return (ctypes.c_uint64 * WORDS)(), (ctypes.c_uint64 * WORDS)()
|
||||||
|
|
||||||
|
|
||||||
|
def imported(array, schema):
|
||||||
|
"""Import the pair as pyarrow does, taking ownership of both."""
|
||||||
|
result = pa.Array._import_from_c(ctypes.addressof(array), ctypes.addressof(schema))
|
||||||
|
assert array[RELEASE] == 0, "the consumer should have taken ownership of the array"
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def test_image(library):
|
||||||
|
"""An image row carries its own dimensions, encoding and bytes."""
|
||||||
|
array, schema = structs()
|
||||||
|
pixels = bytes(range(12))
|
||||||
|
|
||||||
|
library.zed_export_image(
|
||||||
|
ctypes.byref(array), ctypes.byref(schema), 2, 2, b"bgr8", pixels, len(pixels)
|
||||||
|
)
|
||||||
|
result = imported(array, schema)
|
||||||
|
|
||||||
|
assert result.type == IMAGE_TYPE, result.type
|
||||||
|
assert result.to_pylist() == [
|
||||||
|
{"width": 2, "height": 2, "encoding": "bgr8", "data": pixels}
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def test_depth(library):
|
||||||
|
"""A depth map is an image whose bytes are float32 millimetres."""
|
||||||
|
array, schema = structs()
|
||||||
|
millimetres = [1500.0, 0.0, 250.5, 4000.0]
|
||||||
|
payload = pa.array(millimetres, type=pa.float32()).buffers()[1].to_pybytes()
|
||||||
|
|
||||||
|
library.zed_export_image(
|
||||||
|
ctypes.byref(array), ctypes.byref(schema), 2, 2, b"32FC1", payload, len(payload)
|
||||||
|
)
|
||||||
|
row = imported(array, schema).to_pylist()[0]
|
||||||
|
|
||||||
|
assert row["encoding"] == "32FC1"
|
||||||
|
assert pa.Array.from_buffers(
|
||||||
|
pa.float32(), 4, [None, pa.py_buffer(row["data"])]
|
||||||
|
).to_pylist() == millimetres
|
||||||
|
|
||||||
|
|
||||||
|
def test_float32(library):
|
||||||
|
"""Intrinsics and the point cloud go out as plain float32 values."""
|
||||||
|
array, schema = structs()
|
||||||
|
values = (ctypes.c_float * 4)(700.5, 700.5, 640.0, 360.0)
|
||||||
|
|
||||||
|
library.zed_export_float32(ctypes.byref(array), ctypes.byref(schema), values, 4)
|
||||||
|
result = imported(array, schema)
|
||||||
|
|
||||||
|
assert result.type == pa.float32()
|
||||||
|
assert result.to_pylist() == [700.5, 700.5, 640.0, 360.0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_string(library):
|
||||||
|
"""The camera state goes out as one UTF-8 string."""
|
||||||
|
array, schema = structs()
|
||||||
|
|
||||||
|
library.zed_export_string(ctypes.byref(array), ctypes.byref(schema), b"streaming", 9)
|
||||||
|
result = imported(array, schema)
|
||||||
|
|
||||||
|
assert result.type == pa.utf8()
|
||||||
|
assert result.to_pylist() == ["streaming"]
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
"""Run every test, reporting the first failure."""
|
||||||
|
library = load()
|
||||||
|
|
||||||
|
for test in (test_image, test_depth, test_float32, test_string):
|
||||||
|
test(library)
|
||||||
|
print(f"ok {test.__name__}")
|
||||||
|
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
sys.exit(main())
|
||||||
Reference in New Issue
Block a user