24 Commits
Author SHA1 Message Date
cristhian a4cd493330 docs: shorten the README and correct how the tick is described
validate / validate (push) Successful in 58m47s
validate / test (push) Successful in 58m52s
validate / test (pull_request) Successful in 58m49s
validate / validate (pull_request) Successful in 58m48s
The README carried explanations that belong in AGENTS.md; it now follows
the shape of the sibling node repositories and is a quarter shorter.

The tick description was also wrong: it does not trigger a grab. Capture
is free-running at ZED_FPS and a tick forwards the newest frame there is,
so the tick sets the publish rate, and frames captured between two ticks
are dropped. The usage snippet listed four of the five outputs, which
read as an omission rather than the choice it is.
2026-09-15 10:52:12 -05:00
cristhian c1a7d70ba6 add typed Arrow payloads and a dora-image-view dataflow
dora-image-view takes std/media/v1/Image, and dora's compatibility graph
has no edge from std/core/v1/UInt8 to it, so wiring the two failed
`dora validate` whatever the node actually sent. Its flat-buffer fallback
would not have decoded these frames either: it reads width/height
metadata, and this node only published shape.

image and depth now go out as the Image struct, camera_info and
point_cloud as Float32 arrays and camera_state as a string, all built
against the Arrow C data interface in src/arrow so the node still links
no Arrow C++. Shape, dtype and encoding metadata are dropped where the
payload describes itself; intrinsics, frame_id and both timestamps are
unchanged. tests/run.sh covers the packing without a camera or the SDK,
round-tripping each array through pyarrow and rebuilding it under the
sanitizers.

Breaking: image_bgr is renamed to image and every payload changes shape.
dataflow_plot.yml is replaced by dataflow_view.yml, which wires the
viewer from a tag and repeats its contract in input_types so validate
checks both sides of the wiring.
2026-09-15 10:52:08 -05:00
cristhian d4c9f153c1 Merge pull request 'Split the opencv-plot viewer into its own dataflow' (#19) from split-plot-dataflow into main
validate / validate (push) Successful in 58m59s
Reviewed-on: #19
2026-09-08 17:23:12 +00:00
cesarandClaude Opus 5 483da05a98 refactor: split the opencv-plot viewer into its own dataflow
validate / validate (push) Successful in 59m2s
validate / validate (pull_request) Successful in 59m0s
dataflow.yml is now the camera alone, so running it no longer pulls
dora-hub over pip or needs a display. dataflow_plot.yml adds the viewer
on image_bgr for when the stream should be looked at.

dora 1.0 resolves an input's source only within the same descriptor, so
the camera node is declared in both files rather than shared, and the two
are run one at a time. CI validates both, which is what catches an output
rename that breaks only the viewer.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01A65jaxrPXMN9EhBDMmMWb2
2026-09-08 14:11:25 -03:00
cristhian 86b7a1a8f7 Merge pull request '#18: v0.2.0 pure camera source on dora 1.0' (#18) from v0.2.0-camera-source into main
validate / validate (push) Successful in 59m0s
2026-09-08 15:06:21 +00:00
cesarandClaude Opus 5 5822a870bb fix the camera blocking the dora event loop by moving it to its own thread
validate / validate (push) Successful in 59m1s
validate / validate (pull_request) Successful in 59m2s
Hardware testing showed the tick-driven recovery state machine did not work.
async_grab_camera_recovery does not make grab() prompt on a disconnected ZED-M
under SDK 5.3: grabs took ~3 s each while the link was down and then blocked
indefinitely. The event loop stopped turning, so Stop went unanswered, the
30 s patience timer and the backoff were never reached, and replugging the
camera did nothing. Measured: 122 s frozen, then dora stop needed 10 s and a
forced kill.

Move everything that can block onto a capture thread. zed::Capture owns the
camera and runs open, warmup, grab, blank and stall detection, backoff and
escalation; it packs each frame into owned buffers and hands the newest one to
the reader by swapping under a mutex. The dora event loop now only drains that
buffer and publishes, so it never calls into the SDK and stays responsive
however long the SDK blocks.

The newest frame wins and uncollected frames are dropped, which is what a
consumer wants from a camera source. Swapping rather than assigning returns the
previous frame's buffers to the capture thread, so steady state does not
reallocate. The point cloud is now copied out of the SDK buffer rather than
published in place, since the reader is another thread.

On exit the loop waits 2 s for the thread and then _Exit()s if it is still
inside the SDK, so Stop is always answered promptly.

This also removes the blocking warmup from the event loop: it stays a blocking
burst, which is harmless on the capture thread.

Verified on a ZED-M: unplug leaves the node responsive and logs the backoff
1, 2, 5 then 10 s; replug gives "camera recovered after 13 attempt(s)" and
publishing resumes without restarting the dataflow; dora stop with the camera
unplugged returns in 0.70 s with the thread joining gracefully.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c
2026-09-08 12:04:35 -03:00
cesarandClaude Opus 5 2f55ca3f74 docs: correct the output summaries and drop a duplicated paragraph
validate / validate (push) Successful in 59m1s
validate / validate (pull_request) Successful in 59m1s
The README and AGENTS intros still described the v0.1.0 output set. Also
removes a copy of the point-cloud rationale that ended up orphaned after the
recovery section landed above it, and narrows the timestamp section to the
frame outputs, since camera_state carries only the host clock.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c
2026-09-08 09:35:09 -03:00
cesarandClaude Opus 5 ee48948de9 add a dora-node.yml manifest and a dora validate CI gate
No source package is shared between node repositories, so the manifest and its
type URNs are the only contract a consumer has. Declaring them makes a wiring
mistake fail at validate time with the producer and consumer URNs, instead of
at runtime.

Every payload this node sends is a raw buffer whose shape and dtype live in the
metadata rather than an Arrow struct, so the URNs name the element type:
UInt8 for image_bgr, Float32 for depth, camera_info and point_cloud, String for
camera_state. tick is UInt64, matching how dora types timer inputs.

dataflow.yml now builds the node from the checkout with path: + build:. A
git:-sourced node does not pick up an adjacent manifest, so validating the
previous form checked nothing; with path: the CLI reports "injected 6 contract
type(s)" and a deliberately mismatched annotation exits non-zero under
strict_types. The README keeps the tag: form for consumers.

The workflow needs no camera and no ZED SDK, since validation never runs the
node. Also ignores the git/ and out/ working directories dora build creates,
and drops the ignore rules for the deleted test assets.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c
2026-09-08 09:34:12 -03:00
cesarandClaude Opus 5 d8956f269d fix camera recovery blocking the dora event loop
Camera::open() retried forever inside a fixed sleep, so a camera that did not
come back left the node stuck there: it stopped reading dora events and no
longer honoured Stop or AllInputsClosed.

Recovery is now a state machine over ticks. open() makes one attempt and
returns; the loop decides when to try again from a 1/2/5/10 s backoff deadline,
and nothing sleeps, so events keep being read while the camera is down. The
first attempt closes the handle; past that the device is wedged and the node
escalates to sl::Camera::reboot(), guarded to the ZED 2 and newer since it does
nothing on the ZED and ZED Mini. Reopening pins the serial so recovery gets the
same physical camera back.

Set async_grab_camera_recovery so a link hiccup returns CAMERA_REBOOTING
immediately instead of blocking the grab for ~17 s, and enable_image_validity_
check so the SDK flags frames a stalled camera hands back looking valid. Those
transient errors no longer drop the handle, since the SDK is already
reconnecting it.

An all-black frame now counts as a failed grab rather than being published, and
a camera delivering nothing usable for 5 s (30 s while the SDK reports it is
reconnecting) triggers recovery. A new camera_state output publishes
opening/streaming/recovering so a downstream node can tell "no frame yet" from
"camera lost".

ZED_RETRY_INTERVAL_SECONDS is replaced by the backoff.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c
2026-09-08 09:26:17 -03:00
cesarandClaude Opus 5 42c2b791fd make point_cloud optional and publish camera_info once per session
ZED_POINT_CLOUD, off by default, gates both retrieveMeasure(XYZRGBA) and the
point_cloud output; when off the measure is not retrieved at all. At 1080p a
full cloud is ~14.7 MB per frame and costs capture time whether or not anything
consumes it, and consumers that need 3D can back-project depth instead.

camera_info now goes out once per camera session rather than on every frame:
fx, fy, cx, cy and the distortion coefficients are fixed for a given camera and
resolution, so repeating them per frame adds no information. It rides the first
frame after the camera opens, and again after a reconnection, so it still
carries that frame's timestamps.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c
2026-09-08 09:21:11 -03:00
cesarandClaude Opus 5 617b8ceb50 add depth configuration parity with the reference driver
parse_depth_mode returned NEURAL for any unrecognised string, so a typo
silently changed depth quality, and NEURAL_PLUS and NEURAL_LIGHT could not be
selected at all. All six modes are now reachable and an unknown name exits
non-zero listing the accepted values.

Parsing moves into Config::from_env() and main reads the configuration before
init_dora_node(), so a bad value fails with its own message instead of after
the node has registered with the dataflow.

Add ZED_CONFIDENCE_THRESHOLD (60) and ZED_TEXTURE_CONFIDENCE_THRESHOLD (100),
which were never set, and change the defaults to 1080 / 200 mm / 600 mm to
match the reference driver perception was tuned against. The resolved settings
are logged at startup.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c
2026-09-08 09:20:01 -03:00
cesarandClaude Opus 5 15134dc12e add a depth output
Publish sl::MEASURE::DEPTH as float32 [H, W] millimetres on every frame that
carries image_bgr, with the intrinsics, frame_id and timestamps the other
outputs carry.

The map is copied out rather than published in place: sl::Mat rows can be
padded, so the buffer is not necessarily H * W floats. inf and nan are cleared
to 0 in the same pass, matching the reference driver — a hole is meaningful and
a consumer cannot back-project a non-finite value.

Consumers that need 3D can now back-project depth with the published
intrinsics instead of receiving a point cloud.

Also drops the stale objects entry from the dataflow outputs, missed when
detection was removed.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c
2026-09-08 09:17:36 -03:00
cesarandClaude Opus 5 dc7f9f5501 fix frame timestamps to use a comparable epoch
steady_clock's epoch is unspecified and typically counts from process start or
boot, so the published timestamp_ns could not be compared against another
node's timestamps, wall clock or a recording — and it recorded when the node
processed the frame rather than when the camera captured it.

Publish two keys on every output instead: device_timestamp_ns from
sl::Camera::getTimestamp(TIME_REFERENCE::IMAGE), and host_timestamp_ns from
system_clock. Both are Unix-epoch nanoseconds, set as dora timestamp
parameters. The SDK clock source is pinned to SYSTEM_CLOCK at construction,
since it is process-wide and decides the epoch of the device value.

Both are kept because the wall clock can step when NTP synchronises after boot
on a Jetson, while the device timestamp is what two nodes reading the same
frame can agree on.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c
2026-09-08 09:15:47 -03:00
cesarandClaude Opus 5 126dbf4f32 remove detection from the node
dora-zed publishes camera data only. The two engines under src/engines/, the
objects output, the ZED_ENGINE/ZED_DETECTION_*/ZED_ONNX_* variables and the
ZED_PC_MODE=ROI crop are gone, along with the standalone engine test and the
model export script.

This drops the unconditional ONNX Runtime requirement from CMakeLists, the
Dockerfile and the install scripts, so the node now configures on a machine
with no ONNX Runtime — which is what blocked the Jetson build, since there is
no official ARM64 GPU binary.

zed_native is dropped rather than kept: it is bound to the live camera handle
and cannot run in a node that receives images over a dataflow. The removed
code remains reachable at tag v0.1.0.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c
2026-09-08 09:12:40 -03:00
cesarandClaude Opus 5 a734692b60 update the build to dora 1.0 and a shell-free build command
dora 1.0 runs a node's build command directly rather than through a shell, so
the mkdir/cd/&& sequence in dataflow.yml could not work. Move it to
scripts/build.sh and invoke that instead.

Default CMAKE_BUILD_TYPE to Release so DoraTargets.cmake picks the release
cargo profile and the binary does not link debug dora bindings.

Pin the dataflow source with tag: — 1.0 replaced git_checkout with the
mutually exclusive tag/rev/branch.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c
2026-09-08 09:10:53 -03:00
cristhian 1dd1cefcc9 Merge pull request 'remove the YOLO-World engine and add a container build to verify it' (#17) from remove-yoloworld-engine into main
Reviewed-on: #17
2026-09-07 20:07:24 +00:00
cristhian f40e697b2a add a container build environment
Dockerfile provisions CUDA 12.8, ZED SDK 5.2, ONNX Runtime and Rust on
Ubuntu 24.04, the newest release Stereolabs ships a ZED SDK for, so the
node can be compiled and the build verified without installing the SDK
stack on the host.

The compile needs --gpus all: libsl_zed.so links against the CUDA driver
API, NVDEC and NVENC, which ship with the display driver rather than the
CUDA toolkit and so exist in no image. The container runtime mounts them
from the host, and only includes NVDEC/NVENC when the video capability
is requested.

Runs as the invoking user so build/ and bin/ are not left root-owned,
which needs the toolchain outside /root and the ZED SDK directory
readable to an arbitrary uid.
2026-09-07 14:33:58 -03:00
cristhian b4144dfba5 match the cargo profile to CMAKE_BUILD_TYPE
The dora bindings were always built with the debug profile and linked
from target/debug, so a release build still pulled in unoptimized arrow,
zenoh and opentelemetry code -- a 640 MB dora_zed against 48 MB once the
profile follows the build type.
2026-09-07 14:33:50 -03:00
cristhian d62f4fbb29 pin dora-rs to v1.0.1 instead of tracking main
Cloning main made the build non-reproducible and free to drift from the
dora runtime that launches the node. DORA_GIT_TAG can still pin a tag
explicitly, and an installed dora CLI continues to take precedence so
the bindings match the runtime, now with a warning when that CLI
predates the 1.0 this node targets.
2026-09-07 14:33:50 -03:00
cristhian 647be61b7b fix ONNX Runtime install layout so CMake can find it
The release tarball ships its CMake package files under lib/, but
onnxruntimeTargets.cmake resolves the library as <prefix>/lib64. Ubuntu
is multiarch, so CMake does not search lib64 for packages and ld.so does
not search /usr/local/lib64 at all, leaving find_package(onnxruntime)
failing on a stock install. Keep the libraries in lib64, move only the
package files to lib/cmake, and add lib64 to the linker path.
2026-09-07 14:33:33 -03:00
cristhian b16dfcd965 docs: clarify ONNX Runtime dependency and input handling
ONNX Runtime is a configure-time dependency for every build, not only
for the yolo_onnx engine. The event loop no longer inspects the input
id, so it grabs on any input rather than specifically on tick.
2026-09-07 14:30:01 -03:00
cristhian d6ae0f7fef remove dead split_labels helper
Its only caller was the deleted yoloworld_onnx.cc.
2026-09-07 14:29:52 -03:00
cristhian f1934c9b4a remove the YOLO-World open-vocabulary engine
Deletes the yoloworld_onnx engine, the CLIP BPE tokenizer, the standalone
test binary and both export scripts. Drops the update_vocabulary input and
the yoloworld_onnx/open_vocab dispatch from main.cc, along with the now
unused Engine::set_labels. Documentation follows.
2026-09-07 05:10:59 -03:00
cristhian aguilera 44eec29b83 refactor open_vocab into yolo_onnx and yoloworld_onnx engines
Split the monolithic open_vocab engine into two focused engines:
yolo_onnx (closed-vocabulary, any single-input YOLO ONNX) and
yoloworld_onnx (open-vocabulary YOLO-World with CLIP encoder).
Shared NMS/letterbox/fuse helpers extracted to yolo_helpers.h.
Add export_yolo.py script and separate test binaries for each engine.
2026-03-30 04:44:25 -03:00
36 changed files with 1736 additions and 1952 deletions
+5
View File
@@ -0,0 +1,5 @@
.git/
build/
bin/
tmp/
models/
+50
View File
@@ -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
View File
@@ -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/
+38 -21
View File
@@ -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
View File
@@ -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
View File
@@ -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
View File
@@ -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})
+156 -156
View File
@@ -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
View File
@@ -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
+46
View File
@@ -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
+66
View File
@@ -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
+14
View File
@@ -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)"
-105
View File
@@ -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()
-124
View File
@@ -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()
+1 -14
View File
@@ -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}"
+1 -20
View File
@@ -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}"
-22
View File
@@ -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."
+179
View File
@@ -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
+75
View File
@@ -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
-28
View File
@@ -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
-592
View File
@@ -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
-47
View File
@@ -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
-99
View File
@@ -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
-34
View File
@@ -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
View File
@@ -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;
} }
-142
View File
@@ -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;
}
-226
View File
@@ -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
View File
@@ -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
View File
@@ -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
+302
View File
@@ -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
+94
View File
@@ -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
View File
+36
View File
@@ -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"
+54
View File
@@ -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
View File
@@ -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"
+129
View File
@@ -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())