Compare commits
17
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
ff6a87f084 | ||
|
|
a4cd493330 | ||
|
|
c1a7d70ba6 | ||
|
|
d4c9f153c1 | ||
|
|
483da05a98 | ||
|
|
86b7a1a8f7 | ||
|
|
5822a870bb | ||
|
|
2f55ca3f74 | ||
|
|
ee48948de9 | ||
|
|
d8956f269d | ||
|
|
42c2b791fd | ||
|
|
617b8ceb50 | ||
|
|
15134dc12e | ||
|
|
dc7f9f5501 | ||
|
|
126dbf4f32 | ||
|
|
a734692b60 | ||
|
|
1dd1cefcc9 |
@@ -0,0 +1,50 @@
|
|||||||
|
# Two gates, neither of which needs a camera, the ZED SDK or CUDA: the dataflows
|
||||||
|
# are checked against the typed contract in dora-node.yml without running the
|
||||||
|
# node, and the payload packing is compiled and tested on its own.
|
||||||
|
name: validate
|
||||||
|
|
||||||
|
on: [push, pull_request]
|
||||||
|
|
||||||
|
env:
|
||||||
|
DORA_VERSION: v1.0.1
|
||||||
|
DORA_TARGET: x86_64-unknown-linux-gnu
|
||||||
|
|
||||||
|
jobs:
|
||||||
|
validate:
|
||||||
|
runs-on: ubuntu-latest
|
||||||
|
steps:
|
||||||
|
- uses: actions/checkout@v4
|
||||||
|
|
||||||
|
- name: Install the dora CLI
|
||||||
|
run: |
|
||||||
|
curl -fsSL -o dora-cli.tar.gz \
|
||||||
|
"https://github.com/dora-rs/dora/releases/download/${DORA_VERSION}/dora-cli-${DORA_TARGET}.tar.gz"
|
||||||
|
tar xzf dora-cli.tar.gz
|
||||||
|
mkdir -p "$HOME/.local/bin"
|
||||||
|
install -m 755 "dora-cli-${DORA_TARGET}/dora" "$HOME/.local/bin/dora"
|
||||||
|
echo "$HOME/.local/bin" >> "$GITHUB_PATH"
|
||||||
|
rm -rf dora-cli.tar.gz "dora-cli-${DORA_TARGET}"
|
||||||
|
|
||||||
|
- run: dora --version
|
||||||
|
|
||||||
|
# strict_types: true in each dataflow turns a type warning into a
|
||||||
|
# non-zero exit, so a mismatched annotation fails here rather than at
|
||||||
|
# runtime. The viewer file is checked too: it wires the same outputs, and
|
||||||
|
# a rename that breaks only the consumer should still fail the build.
|
||||||
|
- name: Validate the dataflows
|
||||||
|
run: |
|
||||||
|
dora validate dataflow.yml
|
||||||
|
dora validate dataflow_view.yml
|
||||||
|
|
||||||
|
test:
|
||||||
|
runs-on: ubuntu-latest
|
||||||
|
steps:
|
||||||
|
- uses: actions/checkout@v4
|
||||||
|
|
||||||
|
- name: Install uv
|
||||||
|
run: |
|
||||||
|
curl -LsSf https://astral.sh/uv/install.sh | sh
|
||||||
|
echo "$HOME/.local/bin" >> "$GITHUB_PATH"
|
||||||
|
|
||||||
|
- name: Test the payload packing
|
||||||
|
run: tests/run.sh
|
||||||
+4
-3
@@ -1,6 +1,7 @@
|
|||||||
build/
|
build/
|
||||||
bin/
|
bin/
|
||||||
tmp/
|
tmp/
|
||||||
test/assets/*.png
|
|
||||||
test/assets/*.jpg
|
# dora build working directories
|
||||||
test/assets/*_result.*
|
git/
|
||||||
|
out/
|
||||||
|
|||||||
@@ -4,17 +4,29 @@ 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.
|
||||||
|
|
||||||
@@ -24,33 +36,36 @@ For a build without installing the SDK stack on the host, see the `Dockerfile` (
|
|||||||
|
|
||||||
```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/yolo_helpers.h`** — Header-only shared helpers used by the ONNX engine: `RawDetection`, `nms()`, `letterbox()`, `fuse_bbox()`, `backproject()`. Lives in `engines::detail` namespace.
|
- **`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/engines/yolo_onnx.h/cc`** — `engines::YoloOnnx` / `YoloOnnxConfig`: closed-vocabulary detection for any single-input YOLO ONNX model (YOLOv8, YOLOv11, YOLOv5, etc.). Auto-detects output layout (`NC_FIRST` / `NC_LAST` / `NC_LAST_OBJ`) from tensor shape; override with `ZED_ONNX_OUTPUT_FORMAT`. Labels loaded from `ZED_ONNX_LABELS` or `ZED_ONNX_LABELS_FILE`.
|
|
||||||
- **`src/main.cc`** — Dora event loop. Selects engine via `ZED_ENGINE` env var (`yolo_onnx`/`onnx` or `native`), holds a `std::unique_ptr<engines::Engine>`. Parses `ZED_PC_MODE` (`FULL`/`HALF`/`ROI`) once before the loop. On any input event (intended to be driven by a tick timer; the id is not inspected): calls `camera.grab()` and `detector->detect()`, then sends `image_bgr`, `point_cloud`, `camera_info`, and `objects` outputs. Point cloud output varies by mode: `FULL` sends the raw `sl::Mat` pointer (no copy); `HALF` downsamples by 2 in each dimension; `ROI` emits only valid points within detected bounding boxes as a flat `[N, 4]` array. Handles reconnection on grab failure.
|
|
||||||
- **`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.
|
||||||
- **`Dockerfile`** — Ubuntu 24.04 x86-64 build environment (CUDA 12.8, ZED SDK 5.2, ONNX Runtime, Rust). No ARM64/Jetson image; build natively there. No camera needed; run the compile with `--gpus all`. Sets `NVIDIA_DRIVER_CAPABILITIES=compute,utility,video` — the `video` capability is what makes the container runtime mount NVDEC/NVENC, which `libsl_zed.so` links against. ONNX Runtime's CMake package files are relocated to `/usr/local/lib/cmake` because Ubuntu's multiarch CMake does not search `lib64`. Rust lives at `/usr/local/{rustup,cargo}` rather than `/root` so the build can run as the invoking user. `docker build -t dora-zed-build . && docker run --rm --gpus all --user "$(id -u):$(id -g)" -v "$PWD:/src" -v dora-zed-cargo:/usr/local/cargo/registry dora-zed-build`.
|
- **`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_yolo.py`** — PEP 723 uv script. Exports a standard YOLOv11 model to ONNX for closed-vocabulary detection. Run: `uv run scripts/export_yolo.py --model n --output-dir models/`.
|
- **`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_yolo_onnx.cc`** — Standalone C++ test binary (`bin/test_yolo_onnx`). Exercises `YoloOnnx` with an image (default `test/assets/yolo.png`). No dora and no live camera required. Uses the same env vars as the main binary. Saves an annotated result image alongside the input.
|
- **`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 `yolo_onnx` 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
-29
@@ -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/yolo_onnx.cc
|
|
||||||
${node_bridge}
|
${node_bridge}
|
||||||
)
|
)
|
||||||
add_dependencies(dora_zed Dora_cxx)
|
add_dependencies(dora_zed Dora_cxx)
|
||||||
@@ -38,29 +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 tests — no dora, no live camera required
|
|
||||||
|
|
||||||
add_executable(test_yolo_onnx
|
|
||||||
src/test_yolo_onnx.cc
|
|
||||||
src/engines/yolo_onnx.cc
|
|
||||||
)
|
|
||||||
target_include_directories(test_yolo_onnx PRIVATE
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/src
|
|
||||||
${ZED_INCLUDE_DIRS}
|
|
||||||
${OpenCV_INCLUDE_DIRS}
|
|
||||||
${CUDAToolkit_INCLUDE_DIRS}
|
|
||||||
${ONNXRUNTIME_INCLUDE_DIRS}
|
|
||||||
)
|
|
||||||
target_link_libraries(test_yolo_onnx PRIVATE
|
|
||||||
${ZED_LIBRARIES}
|
|
||||||
${OpenCV_LIBS}
|
|
||||||
${ONNXRUNTIME_LIBRARIES}
|
|
||||||
)
|
|
||||||
install(TARGETS test_yolo_onnx DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/bin)
|
|
||||||
|
|||||||
+1
-20
@@ -21,7 +21,6 @@
|
|||||||
FROM nvidia/cuda:12.8.1-devel-ubuntu24.04
|
FROM nvidia/cuda:12.8.1-devel-ubuntu24.04
|
||||||
|
|
||||||
ARG ZED_VERSION=5.2
|
ARG ZED_VERSION=5.2
|
||||||
ARG ORT_VERSION=1.24.4
|
|
||||||
|
|
||||||
ENV DEBIAN_FRONTEND=noninteractive
|
ENV DEBIAN_FRONTEND=noninteractive
|
||||||
|
|
||||||
@@ -51,25 +50,7 @@ RUN curl -fSL -o /tmp/ZED_SDK.run \
|
|||||||
&& chmod -R a+rX /usr/local/zed \
|
&& chmod -R a+rX /usr/local/zed \
|
||||||
&& rm -f /tmp/ZED_SDK.run
|
&& rm -f /tmp/ZED_SDK.run
|
||||||
|
|
||||||
# The tarball puts 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:
|
|
||||||
# keep the libraries in lib64, move only the package files to lib/cmake, and add
|
|
||||||
# lib64 to the linker path.
|
|
||||||
RUN curl -fSL -o /tmp/ort.tgz \
|
|
||||||
"https://github.com/microsoft/onnxruntime/releases/download/v${ORT_VERSION}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz" \
|
|
||||||
&& tar -xzf /tmp/ort.tgz -C /tmp \
|
|
||||||
&& mkdir -p /usr/local/include/onnxruntime /usr/local/lib64 \
|
|
||||||
&& cp -r /tmp/onnxruntime-linux-x64-gpu-${ORT_VERSION}/include/* /usr/local/include/onnxruntime/ \
|
|
||||||
&& cp -r /tmp/onnxruntime-linux-x64-gpu-${ORT_VERSION}/lib/* /usr/local/lib64/ \
|
|
||||||
&& mkdir -p /usr/local/lib/cmake \
|
|
||||||
&& mv /usr/local/lib64/cmake/onnxruntime /usr/local/lib/cmake/ \
|
|
||||||
&& rmdir /usr/local/lib64/cmake \
|
|
||||||
&& echo /usr/local/lib64 > /etc/ld.so.conf.d/onnxruntime.conf \
|
|
||||||
&& ldconfig \
|
|
||||||
&& rm -rf /tmp/ort.tgz /tmp/onnxruntime-linux-x64-gpu-${ORT_VERSION}
|
|
||||||
|
|
||||||
ENV NVIDIA_DRIVER_CAPABILITIES=compute,utility,video
|
ENV NVIDIA_DRIVER_CAPABILITIES=compute,utility,video
|
||||||
|
|
||||||
WORKDIR /src
|
WORKDIR /src
|
||||||
CMD ["bash", "-lc", "mkdir -p build && cd build && cmake .. && make -j$(nproc) install"]
|
CMD ["scripts/build.sh"]
|
||||||
|
|||||||
@@ -1,6 +1,8 @@
|
|||||||
# dora-zed
|
# dora-zed
|
||||||
|
|
||||||
Dora node for ZED stereo cameras. Publishes BGR images, point clouds, camera intrinsics, and 3D object detections.
|
Dora node for ZED stereo cameras. 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
|
## Usage
|
||||||
|
|
||||||
@@ -9,56 +11,169 @@ 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
|
||||||
git_checkout: v0.1.0
|
tag: v0.3.0
|
||||||
build: mkdir -p build && cd build && cmake .. && make install
|
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
|
||||||
```
|
```
|
||||||
|
|
||||||
See [`dataflow.yml`](dataflow.yml) for a complete runnable example with a camera display node.
|
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.
|
||||||
|
|
||||||
## Dependencies
|
### Runnable dataflows
|
||||||
|
|
||||||
| Dependency | Version | Notes |
|
| File | Contents | Run |
|
||||||
|---|---|---|
|
|---|---|---|
|
||||||
| [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 | Bundles OpenCV; default path `/usr/local/zed` |
|
| [`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 | Builds dora-rs C++ bindings |
|
|
||||||
| [ONNX Runtime](https://github.com/microsoft/onnxruntime/releases) | 1.20+ | Required at configure time; used at runtime only by `yolo_onnx` |
|
|
||||||
|
|
||||||
### Installation scripts
|
[view]: https://gitea.liria.cl/liria/dora-image-view
|
||||||
|
|
||||||
| Platform | Script |
|
`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.
|
||||||
|
|
||||||
|
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
|
||||||
|
them against [`dora-node.yml`](dora-node.yml) — the typed contract a consumer wires
|
||||||
|
against, and what [CI](.gitea/workflows/validate.yml) validates on every push.
|
||||||
|
|
||||||
|
## Inputs
|
||||||
|
|
||||||
|
| Input | Description |
|
||||||
|
|---|---|
|
||||||
|
| `tick` | Publishes the newest frame captured since the last tick, or nothing. Wire to `dora/timer/millis/N`. |
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
| Output | Payload | Metadata |
|
||||||
|
|---|---|---|
|
||||||
|
| `image` | `std/media/v1/Image` struct, `bgr8` | intrinsics, `frame_id`, timestamps |
|
||||||
|
| `depth` | `std/media/v1/Image` struct, `32FC1` | `units=mm`, intrinsics, `frame_id`, timestamps |
|
||||||
|
| `camera_info` | `float32` `[fx, fy, cx, cy]` | `distortion`, `frame_id`, timestamps |
|
||||||
|
| `point_cloud` | `float32` XYZRGBA, off by default | `shape`, `channels`, `units=mm`, intrinsics, timestamps |
|
||||||
|
| `camera_state` | `string` | `host_timestamp_ns` |
|
||||||
|
|
||||||
|
`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`.
|
||||||
|
|
||||||
|
`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.
|
||||||
|
|
||||||
|
Every output of a frame carries the same `frame_id` and both clocks as dora `timestamp`
|
||||||
|
metadata: `device_timestamp_ns`, when the camera captured the frame, and
|
||||||
|
`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.
|
||||||
|
|
||||||
|
### Camera state
|
||||||
|
|
||||||
|
`camera_state` lets a consumer tell a camera that has not delivered its first frame yet
|
||||||
|
from one that has been lost:
|
||||||
|
|
||||||
|
| State | Meaning |
|
||||||
|
|---|---|
|
||||||
|
| `opening` | The camera has not delivered a usable frame yet in this run |
|
||||||
|
| `streaming` | Frames are being published |
|
||||||
|
| `recovering` | The camera was streaming and has stopped; the node is 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
|
||||||
|
```
|
||||||
|
|
||||||
|
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
|
||||||
|
scripts/build.sh
|
||||||
|
```
|
||||||
|
|
||||||
|
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.
|
||||||
|
|
||||||
|
**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` |
|
| Ubuntu 24.04 x86_64 | `sudo scripts/install_linux_x64.sh` |
|
||||||
| NVIDIA Jetson (ARM64) | `sudo scripts/install_linux_arm.sh` |
|
| NVIDIA Jetson (ARM64) | `sudo scripts/install_linux_arm.sh` |
|
||||||
| Windows x64 (PowerShell admin) | `scripts/install_windows.ps1` |
|
| Windows x64 (PowerShell admin) | `scripts/install_windows.ps1`, plus the [ZED SDK][sdk] by hand |
|
||||||
|
|
||||||
> **Jetson:** No official ONNX Runtime GPU binary for ARM64. It is still needed to configure the build; only the `yolo_onnx` engine uses it at runtime. Requires [building from source](https://onnxruntime.ai/docs/build/eps.html#tensorrt).
|
[sdk]: https://www.stereolabs.com/developers/release
|
||||||
>
|
|
||||||
> **Windows:** Install ZED SDK manually from [stereolabs.com](https://www.stereolabs.com/developers/release), then pass the ONNX Runtime path: `cmake .. -DCMAKE_PREFIX_PATH=C:\onnxruntime`
|
|
||||||
|
|
||||||
## Local build
|
[`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
|
||||||
```bash
|
the [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html):
|
||||||
git clone ssh://git@gitea.liria.cl:5001/liria/dora-zed.git
|
|
||||||
cd dora-zed
|
|
||||||
mkdir -p build && cd build && cmake .. && make install
|
|
||||||
```
|
|
||||||
|
|
||||||
Binary installed to `bin/dora_zed`.
|
|
||||||
|
|
||||||
### Containerized build
|
|
||||||
|
|
||||||
[`Dockerfile`](Dockerfile) provides the full toolchain — CUDA, ZED SDK, ONNX Runtime and Rust — on Ubuntu 24.04, the newest release Stereolabs ships a ZED SDK for. No camera is needed.
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
docker build -t dora-zed-build .
|
docker build -t dora-zed-build .
|
||||||
@@ -66,126 +181,18 @@ docker run --rm --gpus all --user "$(id -u):$(id -g)" \
|
|||||||
-v "$PWD:/src" -v dora-zed-cargo:/usr/local/cargo/registry dora-zed-build
|
-v "$PWD:/src" -v dora-zed-cargo:/usr/local/cargo/registry dora-zed-build
|
||||||
```
|
```
|
||||||
|
|
||||||
The first run clones dora-rs and compiles it with Cargo; the named volume keeps that cache across runs.
|
It runs the same `scripts/build.sh`. The named volume keeps the Cargo cache across runs
|
||||||
`--user` keeps `build/` and `bin/` owned by you rather than root.
|
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.
|
||||||
|
|
||||||
x86-64 only. On Jetson, run [`scripts/install_linux_arm.sh`](scripts/install_linux_arm.sh) on the
|
## Test
|
||||||
device and build natively — there is no ARM64 image here.
|
|
||||||
|
|
||||||
Needs an NVIDIA driver and the
|
|
||||||
[NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html)
|
|
||||||
on the host, as the ZED SDK does anyway. The image sets
|
|
||||||
`NVIDIA_DRIVER_CAPABILITIES=compute,utility,video`: `libsl_zed.so` links against NVDEC
|
|
||||||
(`libnvcuvid`) and NVENC (`libnvidia-encode`), which the container runtime mounts from the host
|
|
||||||
only when `video` is in the capability list.
|
|
||||||
|
|
||||||
## Configuration
|
|
||||||
|
|
||||||
All behavior is controlled via environment variables.
|
|
||||||
|
|
||||||
### Camera
|
|
||||||
|
|
||||||
| Variable | Default | Description |
|
|
||||||
|---|---|---|
|
|
||||||
| `ZED_RESOLUTION` | `720` | `720`, `1080`, or `2000` |
|
|
||||||
| `ZED_FPS` | `15` | Frame rate |
|
|
||||||
| `ZED_DEPTH_MODE` | `NEURAL` | `PERFORMANCE`, `QUALITY`, `NEURAL`, or `NONE` |
|
|
||||||
| `ZED_DEPTH_MIN_MM` | `10` | Minimum depth (mm) |
|
|
||||||
| `ZED_DEPTH_MAX_MM` | `500` | Maximum depth (mm) |
|
|
||||||
| `ZED_DEPTH_FILL` | `false` | Enable depth fill mode |
|
|
||||||
| `ZED_FLIP` | `ON` | `ON` or `OFF` |
|
|
||||||
| `ZED_WARMUP_FRAMES` | `30` | Frames to discard on startup |
|
|
||||||
| `ZED_RETRY_INTERVAL_SECONDS` | `5` | Reconnection interval (s) |
|
|
||||||
|
|
||||||
### Detection engine
|
|
||||||
|
|
||||||
Set `ZED_ENGINE` to select the detection backend:
|
|
||||||
|
|
||||||
| `ZED_ENGINE` | Custom model? | Classes |
|
|
||||||
|---|---|---|
|
|
||||||
| `native` (default) | No — ZED SDK built-in TensorRT | Fixed: 7 categories |
|
|
||||||
| `yolo_onnx` (alias: `onnx`) | Yes — any single-input YOLO ONNX (v5/v8/v11) | Fixed labels from file or env var |
|
|
||||||
|
|
||||||
#### `native` engine
|
|
||||||
|
|
||||||
Uses ZED SDK's built-in TensorRT models — no model files to download or configure. `ZED_DETECTION_MODEL` selects the speed/accuracy variant, not a file path.
|
|
||||||
|
|
||||||
Detects 7 fixed categories: PERSON, VEHICLE, BAG, ANIMAL, ELECTRONICS, FRUIT_VEGETABLE, SPORT.
|
|
||||||
|
|
||||||
| Variable | Default | Description |
|
|
||||||
|---|---|---|
|
|
||||||
| `ZED_DETECTION_MODEL` | `MULTI_CLASS_BOX_MEDIUM` | `MULTI_CLASS_BOX_FAST`, `MULTI_CLASS_BOX_MEDIUM`, or `MULTI_CLASS_BOX_ACCURATE` |
|
|
||||||
| `ZED_DETECTION_CONFIDENCE` | `40` | Confidence threshold (0–100) |
|
|
||||||
| `ZED_DETECTION_TRACKING` | `false` | Enable object tracking |
|
|
||||||
|
|
||||||
#### `yolo_onnx` engine (closed-vocabulary)
|
|
||||||
|
|
||||||
For any single-input YOLO ONNX model with fixed class names (YOLOv8, YOLOv11, YOLOv5, custom-trained).
|
|
||||||
|
|
||||||
Export a YOLOv11 model once:
|
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
uv run scripts/export_yolo.py --model n --output-dir models/
|
tests/run.sh
|
||||||
```
|
```
|
||||||
|
|
||||||
Then run:
|
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
|
||||||
```bash
|
built again under the sanitizers to check that releasing one frees it. They need neither a
|
||||||
ZED_ENGINE=yolo_onnx \
|
camera, the ZED SDK nor dora, which is what lets CI run them; everything past the packing
|
||||||
ZED_ONNX_MODEL_PATH=models/yolo11n_i640.onnx \
|
is covered by running a dataflow against a real camera.
|
||||||
ZED_ONNX_LABELS_FILE=models/coco80.txt \
|
|
||||||
dora start dataflow.yml
|
|
||||||
```
|
|
||||||
|
|
||||||
**Required:**
|
|
||||||
|
|
||||||
| Variable | Description |
|
|
||||||
|---|---|
|
|
||||||
| `ZED_ONNX_MODEL_PATH` | Path to YOLO ONNX model |
|
|
||||||
| `ZED_ONNX_LABELS` or `ZED_ONNX_LABELS_FILE` | Class names (inline CSV or one-per-line file) |
|
|
||||||
|
|
||||||
**Optional:**
|
|
||||||
|
|
||||||
| Variable | Default | Description |
|
|
||||||
|---|---|---|
|
|
||||||
| `ZED_ONNX_CONFIDENCE` | `0.25` | Confidence threshold (0.0–1.0) |
|
|
||||||
| `ZED_ONNX_NMS_THRESHOLD` | `0.45` | NMS IoU threshold |
|
|
||||||
| `ZED_ONNX_INPUT_SIZE` | `640` | Model input size (px) |
|
|
||||||
| `ZED_ONNX_OUTPUT_FORMAT` | `auto` | Output layout: `auto`, `nc_first`/`yolov8`/`yolov11`, `nc_last`, `nc_last_obj`/`yolov5`. `auto` detects from tensor shape. |
|
|
||||||
|
|
||||||
### Point cloud
|
|
||||||
|
|
||||||
| `ZED_PC_MODE` | Shape | Description |
|
|
||||||
|---|---|---|
|
|
||||||
| `FULL` (default) | `[H, W, 4]` | Full resolution grid (~14.7 MB/frame at 720p) |
|
|
||||||
| `HALF` | `[H/2, W/2, 4]` | Stride-2 downsample (~4× smaller) |
|
|
||||||
| `ROI` | `[N, 4]` | Valid points within detected bounding boxes only |
|
|
||||||
|
|
||||||
## Inputs
|
|
||||||
|
|
||||||
| Input | Description |
|
|
||||||
|---|---|
|
|
||||||
| `tick` | Triggers a camera grab and detection. Connect to `dora/timer/millis/N`. |
|
|
||||||
|
|
||||||
## Outputs
|
|
||||||
|
|
||||||
| Output | Type | Description |
|
|
||||||
|---|---|---|
|
|
||||||
| `image_bgr` | `uint8 [H, W, 3]` | BGR image. Metadata: `shape`, `encoding=bgr8`, intrinsics, `timestamp_ns`. |
|
|
||||||
| `point_cloud` | `float32 [*, 4]` | XYZRGBA point cloud. Shape depends on `ZED_PC_MODE`. Metadata: `units=mm`, intrinsics. |
|
|
||||||
| `camera_info` | `float32 [4]` | `[fx, fy, cx, cy]`. Metadata: `distortion` coefficients. |
|
|
||||||
| `objects` | `float32 [N, 37]` | Detected objects. See below. |
|
|
||||||
|
|
||||||
### `objects` layout
|
|
||||||
|
|
||||||
Each of the N rows contains 37 floats:
|
|
||||||
|
|
||||||
| Index | Field | Description |
|
|
||||||
|---|---|---|
|
|
||||||
| 0 | `label_id` | Class index |
|
|
||||||
| 1 | `confidence` | Score (0–100) |
|
|
||||||
| 2–4 | `x, y, z` | 3D center (mm) |
|
|
||||||
| 5–12 | `bb2d` | 2D bounding box: 4 corners × (x, y) px |
|
|
||||||
| 13–36 | `bb3d` | 3D bounding box: 8 corners × (x, y, z) mm |
|
|
||||||
|
|
||||||
Metadata includes `engine`, `label_names`, `image_width`, `image_height`, and `timestamp_ns`. An empty array `[0, 37]` is sent when no objects are detected.
|
|
||||||
|
|||||||
+14
-12
@@ -1,18 +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
|
|
||||||
git_checkout: v0.1.0
|
|
||||||
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
|
||||||
outputs:
|
outputs:
|
||||||
- image_bgr
|
- image
|
||||||
- objects
|
- depth
|
||||||
|
- point_cloud
|
||||||
- id: plot
|
- camera_info
|
||||||
git: https://github.com/dora-rs/dora-hub.git
|
- camera_state
|
||||||
build: pip install ./node-hub/opencv-plot
|
|
||||||
path: opencv-plot
|
|
||||||
inputs:
|
|
||||||
image: zed-camera/image_bgr
|
|
||||||
|
|||||||
@@ -0,0 +1,46 @@
|
|||||||
|
# The camera into dora-image-view, and the worked example of wiring this node to
|
||||||
|
# a consumer.
|
||||||
|
#
|
||||||
|
# dora has no cross-dataflow wiring, so the camera node is declared here too
|
||||||
|
# rather than referenced — run this file or dataflow.yml, not both.
|
||||||
|
#
|
||||||
|
# Run it with `--uv`, which gives the viewer its own managed environment:
|
||||||
|
#
|
||||||
|
# dora run dataflow_view.yml --uv
|
||||||
|
#
|
||||||
|
# Fetching the viewer needs anonymous read access to its repository. dora-cli
|
||||||
|
# 1.0.1 fetches through libgit2 with no credential callback, so ssh:// cannot
|
||||||
|
# authenticate at all; on Gitea, marking the repository public is not enough
|
||||||
|
# either, since a repository inside a private organization answers an anonymous
|
||||||
|
# clone with 401.
|
||||||
|
strict_types: true
|
||||||
|
|
||||||
|
nodes:
|
||||||
|
- id: zed-camera
|
||||||
|
path: bin/dora_zed
|
||||||
|
build: scripts/build.sh
|
||||||
|
inputs:
|
||||||
|
tick: dora/timer/millis/100
|
||||||
|
outputs:
|
||||||
|
- image
|
||||||
|
- depth
|
||||||
|
- point_cloud
|
||||||
|
- camera_info
|
||||||
|
- camera_state
|
||||||
|
|
||||||
|
- id: view
|
||||||
|
git: https://gitea.liria.cl/liria/dora-image-view.git
|
||||||
|
tag: v0.1.0
|
||||||
|
build: uv pip install .
|
||||||
|
path: dora-image-view
|
||||||
|
inputs:
|
||||||
|
image: zed-camera/image
|
||||||
|
depth: zed-camera/depth
|
||||||
|
camera_info: zed-camera/camera_info
|
||||||
|
# The viewer's own manifest says this, but a `git:` node's manifest is not
|
||||||
|
# read at validate time — only at build. Repeating it here is what makes the
|
||||||
|
# two contracts checked against each other now rather than at runtime.
|
||||||
|
input_types:
|
||||||
|
image: std/media/v1/Image
|
||||||
|
depth: std/media/v1/Image
|
||||||
|
camera_info: std/core/v1/Float32
|
||||||
@@ -0,0 +1,66 @@
|
|||||||
|
# Node manifest: the typed contract a consumer wires against.
|
||||||
|
#
|
||||||
|
# No source package is shared between node repositories, so this file and its
|
||||||
|
# type URNs are the only contract a consumer has. `dora validate` checks a
|
||||||
|
# dataflow against it without running the node, so CI needs no camera and no
|
||||||
|
# ZED SDK.
|
||||||
|
apiVersion: 1
|
||||||
|
name: dora-zed
|
||||||
|
namespace: liria
|
||||||
|
description: ZED stereo camera source publishing an image, a depth map, intrinsics and an optional point cloud
|
||||||
|
categories: [sensor]
|
||||||
|
keywords: [zed, stereolabs, camera, depth, stereo, jetson]
|
||||||
|
|
||||||
|
runtime: cpp
|
||||||
|
entrypoint: bin/dora_zed
|
||||||
|
build: scripts/build.sh
|
||||||
|
dora: ">=1.0"
|
||||||
|
|
||||||
|
inputs:
|
||||||
|
tick:
|
||||||
|
type: std/core/v1/UInt64
|
||||||
|
required: true
|
||||||
|
description: >-
|
||||||
|
Publishes the newest frame the capture thread has grabbed, or nothing if
|
||||||
|
none arrived since the last tick. It does not drive the camera: capture is
|
||||||
|
free-running at ZED_FPS, so this sets the publish rate and anything
|
||||||
|
captured between two ticks is dropped. Connect to dora/timer/millis/N.
|
||||||
|
|
||||||
|
outputs:
|
||||||
|
image:
|
||||||
|
type: std/media/v1/Image
|
||||||
|
description: >-
|
||||||
|
Captured frame as an Arrow struct {width, height, encoding, data}. The
|
||||||
|
encoding is always bgr8, three bytes per pixel.
|
||||||
|
depth:
|
||||||
|
type: std/media/v1/Image
|
||||||
|
description: >-
|
||||||
|
Depth map as an Arrow struct {width, height, encoding, data}, one channel
|
||||||
|
per pixel -- dora has no depth type, and a depth map is an image whose
|
||||||
|
pixels are distances, which is how ROS spells it too. The encoding is
|
||||||
|
32FC1 and the `units` metadata is mm. Missing depth is 0, never inf or
|
||||||
|
nan.
|
||||||
|
camera_info:
|
||||||
|
type: std/core/v1/Float32
|
||||||
|
description: Intrinsics [fx, fy, cx, cy], distortion in the metadata. Published once per camera session.
|
||||||
|
point_cloud:
|
||||||
|
type: std/core/v1/Float32
|
||||||
|
description: XYZRGBA point cloud, shape in the metadata. Published only when ZED_POINT_CLOUD is on.
|
||||||
|
camera_state:
|
||||||
|
type: std/core/v1/String
|
||||||
|
description: One of opening, streaming, recovering. Published on every change.
|
||||||
|
|
||||||
|
example: |
|
||||||
|
- id: zed-camera
|
||||||
|
git: https://gitea.liria.cl/liria/dora-zed.git
|
||||||
|
tag: v0.3.0
|
||||||
|
build: scripts/build.sh
|
||||||
|
path: bin/dora_zed
|
||||||
|
inputs:
|
||||||
|
tick: dora/timer/millis/100
|
||||||
|
outputs:
|
||||||
|
- image
|
||||||
|
- depth
|
||||||
|
- point_cloud
|
||||||
|
- camera_info
|
||||||
|
- camera_state
|
||||||
Executable
+14
@@ -0,0 +1,14 @@
|
|||||||
|
#!/usr/bin/env bash
|
||||||
|
# Configure, compile and install the node into bin/.
|
||||||
|
#
|
||||||
|
# dora 1.0 runs a node's `build:` command directly rather than through a shell,
|
||||||
|
# so `cd`, `&&` and command substitution are not available there. The whole
|
||||||
|
# sequence lives here instead, and the dataflow only invokes this script.
|
||||||
|
set -euo pipefail
|
||||||
|
|
||||||
|
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||||
|
REPO_DIR="$(dirname "$SCRIPT_DIR")"
|
||||||
|
BUILD_DIR="${REPO_DIR}/build"
|
||||||
|
|
||||||
|
cmake -S "$REPO_DIR" -B "$BUILD_DIR" -DCMAKE_BUILD_TYPE=Release
|
||||||
|
cmake --build "$BUILD_DIR" --target install --parallel "$(nproc)"
|
||||||
@@ -1,111 +0,0 @@
|
|||||||
# /// script
|
|
||||||
# requires-python = ">=3.10"
|
|
||||||
# dependencies = ["ultralytics>=8.3"]
|
|
||||||
# ///
|
|
||||||
"""Export a standard YOLOv11 model to ONNX for closed-vocabulary detection.
|
|
||||||
|
|
||||||
The exported model has one input:
|
|
||||||
images (1, 3, imgsz, imgsz) float32
|
|
||||||
|
|
||||||
Class names are fixed at training time. Provide them at runtime via
|
|
||||||
ZED_ONNX_LABELS or ZED_ONNX_LABELS_FILE.
|
|
||||||
|
|
||||||
Usage:
|
|
||||||
uv run scripts/export_yolo.py
|
|
||||||
uv run scripts/export_yolo.py --model s --output-dir models/
|
|
||||||
uv run scripts/export_yolo.py --model m --imgsz 1280
|
|
||||||
"""
|
|
||||||
|
|
||||||
import argparse
|
|
||||||
import shutil
|
|
||||||
import sys
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
MODELS = {
|
|
||||||
"n": "yolo11n",
|
|
||||||
"s": "yolo11s",
|
|
||||||
"m": "yolo11m",
|
|
||||||
"l": "yolo11l",
|
|
||||||
"x": "yolo11x",
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def parse_args():
|
|
||||||
p = argparse.ArgumentParser(
|
|
||||||
description="Export YOLOv11 to ONNX for closed-vocabulary detection"
|
|
||||||
)
|
|
||||||
p.add_argument(
|
|
||||||
"--model", "-m",
|
|
||||||
choices=list(MODELS),
|
|
||||||
default="n",
|
|
||||||
help="Model size: n (~6 MB), s (~18 MB), m (~38 MB), l (~49 MB), x (~109 MB). Default: n",
|
|
||||||
)
|
|
||||||
p.add_argument(
|
|
||||||
"--imgsz",
|
|
||||||
type=int,
|
|
||||||
default=640,
|
|
||||||
help="Input image size in pixels (square). Default: 640",
|
|
||||||
)
|
|
||||||
p.add_argument(
|
|
||||||
"--output-dir", "-o",
|
|
||||||
default=".",
|
|
||||||
help="Output directory for the .onnx file. Default: current directory",
|
|
||||||
)
|
|
||||||
return p.parse_args()
|
|
||||||
|
|
||||||
|
|
||||||
def verify_output(onnx_path: Path):
|
|
||||||
try:
|
|
||||||
import onnx
|
|
||||||
model = onnx.load(str(onnx_path))
|
|
||||||
inputs = [(i.name, [d.dim_value for d in i.type.tensor_type.shape.dim])
|
|
||||||
for i in model.graph.input]
|
|
||||||
outputs = [(o.name, [d.dim_value for d in o.type.tensor_type.shape.dim])
|
|
||||||
for o in model.graph.output]
|
|
||||||
print(f" Inputs: {inputs}")
|
|
||||||
print(f" Outputs: {outputs}")
|
|
||||||
except ImportError:
|
|
||||||
print(" (install 'onnx' to verify model structure)")
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
|
||||||
args = parse_args()
|
|
||||||
|
|
||||||
try:
|
|
||||||
from ultralytics import YOLO
|
|
||||||
except ImportError:
|
|
||||||
print("Error: ultralytics not installed. Run: uv run scripts/export_yolo.py", file=sys.stderr)
|
|
||||||
sys.exit(1)
|
|
||||||
|
|
||||||
model_name = MODELS[args.model]
|
|
||||||
out_dir = Path(args.output_dir)
|
|
||||||
out_dir.mkdir(parents=True, exist_ok=True)
|
|
||||||
out_name = f"{model_name}_i{args.imgsz}.onnx"
|
|
||||||
dst = out_dir / out_name
|
|
||||||
|
|
||||||
print(f"Model: {model_name}.pt")
|
|
||||||
print(f"Input size: {args.imgsz}")
|
|
||||||
print(f"Output: {dst}")
|
|
||||||
print()
|
|
||||||
|
|
||||||
model = YOLO(f"{model_name}.pt")
|
|
||||||
exported = Path(model.export(format="onnx", imgsz=args.imgsz))
|
|
||||||
|
|
||||||
if exported.resolve() != dst.resolve():
|
|
||||||
shutil.move(str(exported), str(dst))
|
|
||||||
|
|
||||||
print(f"\nVerifying exported model...")
|
|
||||||
verify_output(dst)
|
|
||||||
|
|
||||||
print(f"\nExported: {dst.resolve()}")
|
|
||||||
print(f"\nRun with (COCO labels example):")
|
|
||||||
print(f" ZED_ENGINE=onnx \\")
|
|
||||||
print(f" ZED_ONNX_MODEL_PATH={dst.resolve()} \\")
|
|
||||||
print(f" ZED_ONNX_LABELS_FILE=models/coco80.txt \\")
|
|
||||||
print(f" dora start dataflow.yml")
|
|
||||||
print(f"\nOr with inline labels:")
|
|
||||||
print(f" ZED_ONNX_LABELS=\"person,car,chair\" \\")
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
|
||||||
main()
|
|
||||||
@@ -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.
|
|
||||||
# ONNX Runtime is still needed to configure the build; only the yolo_onnx
|
|
||||||
# engine uses it at runtime. Build it from source:
|
|
||||||
# https://onnxruntime.ai/docs/build/eps.html#tensorrt
|
|
||||||
set -euo pipefail
|
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 " Needed to configure the build; only yolo_onnx uses it at runtime."
|
|
||||||
echo " Build from source:"
|
|
||||||
echo " https://onnxruntime.ai/docs/build/eps.html#tensorrt"
|
|
||||||
echo ""
|
|
||||||
|
|
||||||
echo ">>> Done. You can now run: mkdir -p build && cd build && cmake .. && make install"
|
|
||||||
echo ">>> Downloaded files are in ${TMP_DIR}"
|
echo ">>> Downloaded files are in ${TMP_DIR}"
|
||||||
|
|||||||
@@ -6,7 +6,6 @@
|
|||||||
set -euo pipefail
|
set -euo pipefail
|
||||||
|
|
||||||
ZED_VERSION="5.2"
|
ZED_VERSION="5.2"
|
||||||
ORT_VERSION="1.24.4"
|
|
||||||
|
|
||||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||||
TMP_DIR="${SCRIPT_DIR}/../tmp"
|
TMP_DIR="${SCRIPT_DIR}/../tmp"
|
||||||
@@ -34,32 +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/lib/cmake/onnxruntime ]; then
|
|
||||||
echo ">>> ONNX Runtime already installed — skipping."
|
|
||||||
else
|
|
||||||
echo ">>> Downloading ONNX Runtime ${ORT_VERSION} GPU..."
|
|
||||||
download \
|
|
||||||
"https://github.com/microsoft/onnxruntime/releases/download/v${ORT_VERSION}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz" \
|
|
||||||
"${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz"
|
|
||||||
echo ">>> Extracting..."
|
|
||||||
tar -xzf "${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}.tgz" -C "${TMP_DIR}"
|
|
||||||
echo ">>> Installing ONNX Runtime to /usr/local..."
|
|
||||||
sudo mkdir -p /usr/local/include/onnxruntime
|
|
||||||
sudo cp -r "${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}/include/"* /usr/local/include/onnxruntime/
|
|
||||||
sudo mkdir -p /usr/local/lib64
|
|
||||||
sudo cp -r "${TMP_DIR}/onnxruntime-linux-x64-gpu-${ORT_VERSION}/lib/"* /usr/local/lib64/
|
|
||||||
# onnxruntimeTargets.cmake resolves the library as <prefix>/lib64, but Ubuntu
|
|
||||||
# is multiarch: CMake does not search lib64 for packages and ld.so does not
|
|
||||||
# search /usr/local/lib64 at all. Move only the package files to lib/cmake
|
|
||||||
# and put lib64 on the linker path.
|
|
||||||
sudo mkdir -p /usr/local/lib/cmake
|
|
||||||
sudo rm -rf /usr/local/lib/cmake/onnxruntime
|
|
||||||
sudo mv /usr/local/lib64/cmake/onnxruntime /usr/local/lib/cmake/
|
|
||||||
sudo rmdir /usr/local/lib64/cmake
|
|
||||||
echo /usr/local/lib64 | sudo tee /etc/ld.so.conf.d/onnxruntime.conf > /dev/null
|
|
||||||
sudo ldconfig
|
|
||||||
fi
|
|
||||||
|
|
||||||
echo ">>> Done. You can now run: mkdir -p build && cd build && cmake .. && make install"
|
|
||||||
echo ">>> Downloaded files are in ${TMP_DIR}"
|
echo ">>> Downloaded files are in ${TMP_DIR}"
|
||||||
|
|||||||
@@ -10,10 +10,6 @@
|
|||||||
$ErrorActionPreference = "Stop"
|
$ErrorActionPreference = "Stop"
|
||||||
|
|
||||||
$ZED_INSTALL_DIR = "C:\Program Files (x86)\ZED SDK"
|
$ZED_INSTALL_DIR = "C:\Program Files (x86)\ZED SDK"
|
||||||
$ORT_VERSION = "1.24.4"
|
|
||||||
$ORT_NAME = "onnxruntime-win-x64-gpu_cuda13-$ORT_VERSION"
|
|
||||||
$ORT_URL = "https://github.com/microsoft/onnxruntime/releases/download/v$ORT_VERSION/$ORT_NAME.zip"
|
|
||||||
$ORT_DEST = "C:\onnxruntime"
|
|
||||||
|
|
||||||
# --- ZED SDK ---
|
# --- ZED SDK ---
|
||||||
if (Test-Path $ZED_INSTALL_DIR) {
|
if (Test-Path $ZED_INSTALL_DIR) {
|
||||||
@@ -25,23 +21,5 @@ if (Test-Path $ZED_INSTALL_DIR) {
|
|||||||
Write-Host ""
|
Write-Host ""
|
||||||
}
|
}
|
||||||
|
|
||||||
# --- ONNX Runtime ---
|
|
||||||
if (Test-Path "$ORT_DEST\lib\onnxruntime.dll") {
|
|
||||||
Write-Host ">>> ONNX Runtime already installed at '$ORT_DEST' — skipping."
|
|
||||||
} else {
|
|
||||||
Write-Host ">>> Downloading ONNX Runtime $ORT_VERSION GPU (CUDA 13)..."
|
|
||||||
$tmp = "$env:TEMP\$ORT_NAME.zip"
|
|
||||||
Invoke-WebRequest -Uri $ORT_URL -OutFile $tmp
|
|
||||||
|
|
||||||
Write-Host ">>> Extracting to $ORT_DEST..."
|
|
||||||
Expand-Archive -Path $tmp -DestinationPath $env:TEMP -Force
|
|
||||||
if (Test-Path $ORT_DEST) { Remove-Item $ORT_DEST -Recurse -Force }
|
|
||||||
Move-Item "$env:TEMP\$ORT_NAME" $ORT_DEST
|
|
||||||
Remove-Item $tmp
|
|
||||||
|
|
||||||
Write-Host ">>> ONNX Runtime installed to $ORT_DEST"
|
|
||||||
Write-Host " Pass it to CMake: cmake .. -DCMAKE_PREFIX_PATH=C:\onnxruntime"
|
|
||||||
}
|
|
||||||
|
|
||||||
Write-Host ""
|
Write-Host ""
|
||||||
Write-Host ">>> Done."
|
Write-Host ">>> Done."
|
||||||
|
|||||||
@@ -0,0 +1,179 @@
|
|||||||
|
#include "arrow/array.h"
|
||||||
|
|
||||||
|
#include <cstring>
|
||||||
|
#include <string>
|
||||||
|
#include <utility>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace arrow_c {
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
// Producer-owned storage for one schema node. The struct's `format`, `name` and
|
||||||
|
// `children` pointers point in here, which is why it outlives the call that
|
||||||
|
// built it and is freed by the release callback rather than by the builder.
|
||||||
|
struct SchemaData {
|
||||||
|
std::string format;
|
||||||
|
std::string name;
|
||||||
|
std::vector<ArrowSchema*> children;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Producer-owned storage for one array node. `owned` holds the bytes this
|
||||||
|
// node's own buffers point into - offset arrays, and scalars that have no
|
||||||
|
// storage elsewhere. The payload itself is borrowed from the caller.
|
||||||
|
struct ArrayData {
|
||||||
|
std::vector<uint8_t> owned;
|
||||||
|
std::vector<const void*> buffers;
|
||||||
|
std::vector<ArrowArray*> children;
|
||||||
|
};
|
||||||
|
|
||||||
|
// A node's release callback releases its children before freeing itself, so a
|
||||||
|
// consumer that only releases the root - which is all the interface requires -
|
||||||
|
// still frees the whole tree.
|
||||||
|
void release_schema(ArrowSchema* schema) {
|
||||||
|
auto* data = static_cast<SchemaData*>(schema->private_data);
|
||||||
|
|
||||||
|
for (ArrowSchema* child : data->children) {
|
||||||
|
if (child->release != nullptr) {
|
||||||
|
child->release(child);
|
||||||
|
}
|
||||||
|
delete child;
|
||||||
|
}
|
||||||
|
|
||||||
|
delete data;
|
||||||
|
schema->release = nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
void release_array(ArrowArray* array) {
|
||||||
|
auto* data = static_cast<ArrayData*>(array->private_data);
|
||||||
|
|
||||||
|
for (ArrowArray* child : data->children) {
|
||||||
|
if (child->release != nullptr) {
|
||||||
|
child->release(child);
|
||||||
|
}
|
||||||
|
delete child;
|
||||||
|
}
|
||||||
|
|
||||||
|
delete data;
|
||||||
|
array->release = nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_schema(ArrowSchema& schema, const char* format, const char* name,
|
||||||
|
std::vector<ArrowSchema*> children) {
|
||||||
|
auto* data = new SchemaData{format, name, std::move(children)};
|
||||||
|
|
||||||
|
schema.format = data->format.c_str();
|
||||||
|
schema.name = data->name.c_str();
|
||||||
|
schema.metadata = nullptr;
|
||||||
|
// Nullable everywhere, which is what pyarrow's defaults produce: the same
|
||||||
|
// struct built by a Python node has to compare equal to this one.
|
||||||
|
schema.flags = ARROW_FLAG_NULLABLE;
|
||||||
|
schema.n_children = static_cast<int64_t>(data->children.size());
|
||||||
|
schema.children = data->children.empty() ? nullptr : data->children.data();
|
||||||
|
schema.dictionary = nullptr;
|
||||||
|
schema.release = release_schema;
|
||||||
|
schema.private_data = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
// `buffers` may point into `owned`: moving a vector keeps its heap block, so
|
||||||
|
// pointers taken before the move stay valid after it.
|
||||||
|
void set_array(ArrowArray& array, int64_t length, std::vector<uint8_t> owned,
|
||||||
|
std::vector<const void*> buffers, std::vector<ArrowArray*> children) {
|
||||||
|
auto* data = new ArrayData{std::move(owned), std::move(buffers), std::move(children)};
|
||||||
|
|
||||||
|
array.length = length;
|
||||||
|
// Nothing this node sends is null, so the validity bitmap - the first
|
||||||
|
// buffer of every node below - is left out entirely.
|
||||||
|
array.null_count = 0;
|
||||||
|
array.offset = 0;
|
||||||
|
array.n_buffers = static_cast<int64_t>(data->buffers.size());
|
||||||
|
array.n_children = static_cast<int64_t>(data->children.size());
|
||||||
|
array.buffers = data->buffers.data();
|
||||||
|
array.children = data->children.empty() ? nullptr : data->children.data();
|
||||||
|
array.dictionary = nullptr;
|
||||||
|
array.release = release_array;
|
||||||
|
array.private_data = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
// One field of a struct, owned by its parent.
|
||||||
|
struct Child {
|
||||||
|
ArrowSchema* schema;
|
||||||
|
ArrowArray* array;
|
||||||
|
};
|
||||||
|
|
||||||
|
Child new_child() {
|
||||||
|
return {new ArrowSchema{}, new ArrowArray{}};
|
||||||
|
}
|
||||||
|
|
||||||
|
void fill_uint32(ArrowSchema& schema, ArrowArray& array, const char* name, uint32_t value) {
|
||||||
|
std::vector<uint8_t> owned(sizeof(value));
|
||||||
|
std::memcpy(owned.data(), &value, sizeof(value));
|
||||||
|
const void* values = owned.data();
|
||||||
|
|
||||||
|
set_schema(schema, "I", name, {});
|
||||||
|
set_array(array, 1, std::move(owned), {nullptr, values}, {});
|
||||||
|
}
|
||||||
|
|
||||||
|
// One string, offsets and characters in a single allocation.
|
||||||
|
void fill_utf8(ArrowSchema& schema, ArrowArray& array, const char* name, const char* text,
|
||||||
|
size_t size) {
|
||||||
|
const int32_t offsets[2] = {0, static_cast<int32_t>(size)};
|
||||||
|
|
||||||
|
std::vector<uint8_t> owned(sizeof(offsets) + size);
|
||||||
|
std::memcpy(owned.data(), offsets, sizeof(offsets));
|
||||||
|
std::memcpy(owned.data() + sizeof(offsets), text, size);
|
||||||
|
const void* offsets_buffer = owned.data();
|
||||||
|
const void* values_buffer = owned.data() + sizeof(offsets);
|
||||||
|
|
||||||
|
set_schema(schema, "u", name, {});
|
||||||
|
set_array(array, 1, std::move(owned), {nullptr, offsets_buffer, values_buffer}, {});
|
||||||
|
}
|
||||||
|
|
||||||
|
// One byte string. Only the offsets are owned; the bytes are the caller's.
|
||||||
|
void fill_large_binary(ArrowSchema& schema, ArrowArray& array, const char* name,
|
||||||
|
const uint8_t* data, size_t size) {
|
||||||
|
const int64_t offsets[2] = {0, static_cast<int64_t>(size)};
|
||||||
|
|
||||||
|
std::vector<uint8_t> owned(sizeof(offsets));
|
||||||
|
std::memcpy(owned.data(), offsets, sizeof(offsets));
|
||||||
|
const void* offsets_buffer = owned.data();
|
||||||
|
|
||||||
|
set_schema(schema, "Z", name, {});
|
||||||
|
set_array(array, 1, std::move(owned), {nullptr, offsets_buffer, data}, {});
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
Exported image(uint32_t width, uint32_t height, const char* encoding, const uint8_t* data,
|
||||||
|
size_t size) {
|
||||||
|
Child w = new_child();
|
||||||
|
fill_uint32(*w.schema, *w.array, "width", width);
|
||||||
|
|
||||||
|
Child h = new_child();
|
||||||
|
fill_uint32(*h.schema, *h.array, "height", height);
|
||||||
|
|
||||||
|
Child e = new_child();
|
||||||
|
fill_utf8(*e.schema, *e.array, "encoding", encoding, std::strlen(encoding));
|
||||||
|
|
||||||
|
Child d = new_child();
|
||||||
|
fill_large_binary(*d.schema, *d.array, "data", data, size);
|
||||||
|
|
||||||
|
Exported out;
|
||||||
|
set_schema(out.schema, "+s", "", {w.schema, h.schema, e.schema, d.schema});
|
||||||
|
set_array(out.array, 1, {}, {nullptr}, {w.array, h.array, e.array, d.array});
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
Exported float32(const float* values, size_t count) {
|
||||||
|
Exported out;
|
||||||
|
set_schema(out.schema, "f", "", {});
|
||||||
|
set_array(out.array, static_cast<int64_t>(count), {}, {nullptr, values}, {});
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
Exported string(const char* text, size_t size) {
|
||||||
|
Exported out;
|
||||||
|
fill_utf8(out.schema, out.array, "", text, size);
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace arrow_c
|
||||||
@@ -0,0 +1,75 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <cstddef>
|
||||||
|
#include <cstdint>
|
||||||
|
|
||||||
|
// The Arrow C data interface, verbatim from the Arrow specification.
|
||||||
|
//
|
||||||
|
// These two structs are the whole ABI dora's send_arrow_output takes, so
|
||||||
|
// defining them here is what lets this node publish typed Arrow arrays without
|
||||||
|
// linking Arrow C++ - a heavy dependency for a producer that exports one array
|
||||||
|
// per output and never reads one.
|
||||||
|
#ifndef ARROW_C_DATA_INTERFACE
|
||||||
|
#define ARROW_C_DATA_INTERFACE
|
||||||
|
|
||||||
|
#define ARROW_FLAG_DICTIONARY_ORDERED 1
|
||||||
|
#define ARROW_FLAG_NULLABLE 2
|
||||||
|
#define ARROW_FLAG_MAP_KEYS_SORTED 4
|
||||||
|
|
||||||
|
struct ArrowSchema {
|
||||||
|
const char* format;
|
||||||
|
const char* name;
|
||||||
|
const char* metadata;
|
||||||
|
int64_t flags;
|
||||||
|
int64_t n_children;
|
||||||
|
struct ArrowSchema** children;
|
||||||
|
struct ArrowSchema* dictionary;
|
||||||
|
|
||||||
|
void (*release)(struct ArrowSchema*);
|
||||||
|
void* private_data;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct ArrowArray {
|
||||||
|
int64_t length;
|
||||||
|
int64_t null_count;
|
||||||
|
int64_t offset;
|
||||||
|
int64_t n_buffers;
|
||||||
|
int64_t n_children;
|
||||||
|
const void** buffers;
|
||||||
|
struct ArrowArray** children;
|
||||||
|
struct ArrowArray* dictionary;
|
||||||
|
|
||||||
|
void (*release)(struct ArrowArray*);
|
||||||
|
void* private_data;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // ARROW_C_DATA_INTERFACE
|
||||||
|
|
||||||
|
namespace arrow_c {
|
||||||
|
|
||||||
|
// One array with the schema describing it.
|
||||||
|
//
|
||||||
|
// dora moves both structs out of here and calls their release callbacks once it
|
||||||
|
// has copied the payload into shared memory, so an Exported is built, sent
|
||||||
|
// once, and never touched again.
|
||||||
|
struct Exported {
|
||||||
|
ArrowArray array{};
|
||||||
|
ArrowSchema schema{};
|
||||||
|
};
|
||||||
|
|
||||||
|
// std/media/v1/Image: one row of {width, height, encoding, data}.
|
||||||
|
//
|
||||||
|
// The payload is borrowed rather than copied, so it has to stay valid until the
|
||||||
|
// send returns. A capture-thread frame buffer does: the event loop holds the
|
||||||
|
// frame across the call.
|
||||||
|
Exported image(uint32_t width, uint32_t height, const char* encoding, const uint8_t* data,
|
||||||
|
size_t size);
|
||||||
|
|
||||||
|
// std/core/v1/Float32: `count` values, borrowed on the same terms as image().
|
||||||
|
Exported float32(const float* values, size_t count);
|
||||||
|
|
||||||
|
// std/core/v1/String: one UTF-8 string. Copied, since every string this node
|
||||||
|
// sends is a short state name.
|
||||||
|
Exported string(const char* text, size_t size);
|
||||||
|
|
||||||
|
} // namespace arrow_c
|
||||||
@@ -1,27 +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 const std::vector<DetectedObject>& objects() const = 0;
|
|
||||||
virtual std::string label_names() const = 0;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace engines
|
|
||||||
@@ -1,136 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include <opencv2/opencv.hpp>
|
|
||||||
#include <sl/Camera.hpp>
|
|
||||||
|
|
||||||
#include <algorithm>
|
|
||||||
#include <cmath>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
namespace engines::detail {
|
|
||||||
|
|
||||||
// --- NMS ---
|
|
||||||
|
|
||||||
struct RawDetection {
|
|
||||||
float x1, y1, x2, y2;
|
|
||||||
float score;
|
|
||||||
int class_id;
|
|
||||||
};
|
|
||||||
|
|
||||||
inline float iou(const RawDetection& a, const RawDetection& b) {
|
|
||||||
float inter_x1 = std::max(a.x1, b.x1);
|
|
||||||
float inter_y1 = std::max(a.y1, b.y1);
|
|
||||||
float inter_x2 = std::min(a.x2, b.x2);
|
|
||||||
float inter_y2 = std::min(a.y2, b.y2);
|
|
||||||
float inter_w = std::max(0.0f, inter_x2 - inter_x1);
|
|
||||||
float inter_h = std::max(0.0f, inter_y2 - inter_y1);
|
|
||||||
float inter_area = inter_w * inter_h;
|
|
||||||
float area_a = (a.x2 - a.x1) * (a.y2 - a.y1);
|
|
||||||
float area_b = (b.x2 - b.x1) * (b.y2 - b.y1);
|
|
||||||
float union_area = area_a + area_b - inter_area;
|
|
||||||
return union_area > 0.0f ? inter_area / union_area : 0.0f;
|
|
||||||
}
|
|
||||||
|
|
||||||
inline std::vector<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 ---
|
|
||||||
|
|
||||||
inline 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;
|
|
||||||
};
|
|
||||||
|
|
||||||
inline 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
inline void backproject(float u, float v, float z,
|
|
||||||
float fx, float fy, float cx, float cy,
|
|
||||||
float& X, float& Y, float& Z) {
|
|
||||||
Z = z;
|
|
||||||
X = (u - cx) * z / fx;
|
|
||||||
Y = (v - cy) * z / fy;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace engines::detail
|
|
||||||
@@ -1,369 +0,0 @@
|
|||||||
#include "engines/yolo_onnx.h"
|
|
||||||
#include "engines/yolo_helpers.h"
|
|
||||||
#include "util/env.h"
|
|
||||||
|
|
||||||
#include <onnxruntime_cxx_api.h>
|
|
||||||
#include <opencv2/opencv.hpp>
|
|
||||||
#include <sl/Camera.hpp>
|
|
||||||
|
|
||||||
#include <cmath>
|
|
||||||
#include <fstream>
|
|
||||||
#include <iostream>
|
|
||||||
#include <string>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
namespace engines {
|
|
||||||
|
|
||||||
using namespace detail;
|
|
||||||
|
|
||||||
// --- Impl ---
|
|
||||||
|
|
||||||
struct YoloOnnx::Impl {
|
|
||||||
Ort::Env env{ORT_LOGGING_LEVEL_WARNING, "yolo_onnx"};
|
|
||||||
Ort::SessionOptions session_options;
|
|
||||||
std::unique_ptr<Ort::Session> session;
|
|
||||||
Ort::AllocatorWithDefaultOptions allocator;
|
|
||||||
|
|
||||||
std::string input_name;
|
|
||||||
std::string output_name;
|
|
||||||
int num_classes = 0;
|
|
||||||
YoloOutputLayout resolved_layout = YoloOutputLayout::NC_FIRST;
|
|
||||||
|
|
||||||
std::vector<RawDetection> run_inference(const YoloOnnxConfig& config,
|
|
||||||
const cv::Mat& image_bgr);
|
|
||||||
};
|
|
||||||
|
|
||||||
// --- Impl::run_inference ---
|
|
||||||
|
|
||||||
std::vector<RawDetection> YoloOnnx::Impl::run_inference(
|
|
||||||
const YoloOnnxConfig& config, const cv::Mat& image_bgr)
|
|
||||||
{
|
|
||||||
if (!session) return {};
|
|
||||||
|
|
||||||
int orig_w = image_bgr.cols;
|
|
||||||
int orig_h = image_bgr.rows;
|
|
||||||
|
|
||||||
cv::Mat rgb;
|
|
||||||
cv::cvtColor(image_bgr, rgb, cv::COLOR_BGR2RGB);
|
|
||||||
|
|
||||||
float scale, pad_x, pad_y;
|
|
||||||
cv::Mat lb = letterbox(rgb, config.input_size, scale, pad_x, pad_y);
|
|
||||||
|
|
||||||
cv::Mat lb_float;
|
|
||||||
lb.convertTo(lb_float, CV_32F, 1.0f / 255.0f);
|
|
||||||
|
|
||||||
int sz = config.input_size;
|
|
||||||
std::vector<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* input_name_cstr = input_name.c_str();
|
|
||||||
const char* output_name_cstr = output_name.c_str();
|
|
||||||
|
|
||||||
std::vector<Ort::Value> outputs;
|
|
||||||
try {
|
|
||||||
outputs = session->Run(
|
|
||||||
Ort::RunOptions{nullptr},
|
|
||||||
&input_name_cstr, &image_tensor, 1,
|
|
||||||
&output_name_cstr, 1);
|
|
||||||
} catch (const Ort::Exception& e) {
|
|
||||||
std::cerr << "YoloOnnx: inference failed: " << e.what() << std::endl;
|
|
||||||
return {};
|
|
||||||
}
|
|
||||||
|
|
||||||
auto out_shape = outputs[0].GetTensorTypeAndShapeInfo().GetShape();
|
|
||||||
if (out_shape.size() < 3) return {};
|
|
||||||
|
|
||||||
const float* out = outputs[0].GetTensorData<float>();
|
|
||||||
int nc = num_classes;
|
|
||||||
if (nc <= 0) return {};
|
|
||||||
|
|
||||||
std::vector<RawDetection> candidates;
|
|
||||||
|
|
||||||
if (resolved_layout == YoloOutputLayout::NC_FIRST) {
|
|
||||||
// (1, 4+N, anchors) — YOLOv8, YOLOv11
|
|
||||||
int num_anchors = static_cast<int>(out_shape[2]);
|
|
||||||
for (int a = 0; a < num_anchors; a++) {
|
|
||||||
float cx_lb = out[0 * num_anchors + a];
|
|
||||||
float cy_lb = out[1 * num_anchors + a];
|
|
||||||
float w_lb = out[2 * num_anchors + a];
|
|
||||||
float h_lb = out[3 * num_anchors + a];
|
|
||||||
|
|
||||||
float best_score = 0.0f;
|
|
||||||
int best_class = 0;
|
|
||||||
for (int c = 0; c < nc; c++) {
|
|
||||||
float s = out[(4 + c) * num_anchors + a];
|
|
||||||
if (s > best_score) { best_score = s; best_class = c; }
|
|
||||||
}
|
|
||||||
if (best_score < config.confidence_threshold) continue;
|
|
||||||
|
|
||||||
float x1_lb = cx_lb - w_lb / 2.0f;
|
|
||||||
float y1_lb = cy_lb - h_lb / 2.0f;
|
|
||||||
float x2_lb = cx_lb + w_lb / 2.0f;
|
|
||||||
float y2_lb = cy_lb + h_lb / 2.0f;
|
|
||||||
|
|
||||||
float x1 = std::max(0.0f, std::min((x1_lb - pad_x) / scale, static_cast<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});
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
// (1, anchors, 4+N) or (1, anchors, 5+N)
|
|
||||||
int num_anchors = static_cast<int>(out_shape[1]);
|
|
||||||
int row_stride = static_cast<int>(out_shape[2]);
|
|
||||||
int cls_offset = (resolved_layout == YoloOutputLayout::NC_LAST_OBJ) ? 5 : 4;
|
|
||||||
|
|
||||||
for (int a = 0; a < num_anchors; a++) {
|
|
||||||
const float* row = out + a * row_stride;
|
|
||||||
float cx_lb = row[0], cy_lb = row[1], w_lb = row[2], h_lb = row[3];
|
|
||||||
float obj_conf = (resolved_layout == YoloOutputLayout::NC_LAST_OBJ) ? row[4] : 1.0f;
|
|
||||||
|
|
||||||
float best_score = 0.0f;
|
|
||||||
int best_class = 0;
|
|
||||||
for (int c = 0; c < nc; c++) {
|
|
||||||
float s = obj_conf * row[cls_offset + c];
|
|
||||||
if (s > best_score) { best_score = s; best_class = c; }
|
|
||||||
}
|
|
||||||
if (best_score < config.confidence_threshold) continue;
|
|
||||||
|
|
||||||
float x1_lb = cx_lb - w_lb / 2.0f;
|
|
||||||
float y1_lb = cy_lb - h_lb / 2.0f;
|
|
||||||
float x2_lb = cx_lb + w_lb / 2.0f;
|
|
||||||
float y2_lb = cy_lb + h_lb / 2.0f;
|
|
||||||
|
|
||||||
float x1 = std::max(0.0f, std::min((x1_lb - pad_x) / scale, static_cast<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 ---
|
|
||||||
|
|
||||||
YoloOnnxConfig YoloOnnxConfig::from_env() {
|
|
||||||
YoloOnnxConfig c;
|
|
||||||
c.model_path = util::get_env_str("ZED_ONNX_MODEL_PATH", c.model_path);
|
|
||||||
c.confidence_threshold = util::get_env_float("ZED_ONNX_CONFIDENCE", c.confidence_threshold);
|
|
||||||
c.nms_threshold = util::get_env_float("ZED_ONNX_NMS_THRESHOLD", c.nms_threshold);
|
|
||||||
c.input_size = util::get_env_int("ZED_ONNX_INPUT_SIZE", c.input_size);
|
|
||||||
c.labels = util::get_env_str("ZED_ONNX_LABELS", c.labels);
|
|
||||||
c.labels_file = util::get_env_str("ZED_ONNX_LABELS_FILE", c.labels_file);
|
|
||||||
|
|
||||||
std::string fmt = util::get_env_str("ZED_ONNX_OUTPUT_FORMAT", "");
|
|
||||||
if (fmt == "nc_first" || fmt == "yolov8" || fmt == "yolov11")
|
|
||||||
c.output_layout = YoloOutputLayout::NC_FIRST;
|
|
||||||
else if (fmt == "nc_last")
|
|
||||||
c.output_layout = YoloOutputLayout::NC_LAST;
|
|
||||||
else if (fmt == "nc_last_obj" || fmt == "yolov5")
|
|
||||||
c.output_layout = YoloOutputLayout::NC_LAST_OBJ;
|
|
||||||
|
|
||||||
return c;
|
|
||||||
}
|
|
||||||
|
|
||||||
// --- YoloOnnx ---
|
|
||||||
|
|
||||||
YoloOnnx::YoloOnnx(const YoloOnnxConfig& config)
|
|
||||||
: config_(config), impl_(std::make_unique<Impl>()) {}
|
|
||||||
|
|
||||||
YoloOnnx::~YoloOnnx() = default;
|
|
||||||
|
|
||||||
void YoloOnnx::enable(sl::Camera& /* camera */) {
|
|
||||||
if (config_.model_path.empty()) {
|
|
||||||
std::cerr << "YoloOnnx: ZED_ONNX_MODEL_PATH is not set." << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
OrtCUDAProviderOptions cuda_opts{};
|
|
||||||
cuda_opts.device_id = 0;
|
|
||||||
try {
|
|
||||||
impl_->session_options.AppendExecutionProvider_CUDA(cuda_opts);
|
|
||||||
} catch (const Ort::Exception& e) {
|
|
||||||
std::cerr << "YoloOnnx: CUDA unavailable (" << e.what() << "), using CPU." << std::endl;
|
|
||||||
}
|
|
||||||
impl_->session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL);
|
|
||||||
|
|
||||||
std::cout << "YoloOnnx: loading model (" << config_.model_path << ")..." << std::endl;
|
|
||||||
try {
|
|
||||||
impl_->session = std::make_unique<Ort::Session>(
|
|
||||||
impl_->env, config_.model_path.c_str(), impl_->session_options);
|
|
||||||
} catch (const Ort::Exception& e) {
|
|
||||||
std::cerr << "YoloOnnx: failed to load model: " << e.what() << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
impl_->input_name = impl_->session->GetInputNameAllocated(0, impl_->allocator).get();
|
|
||||||
impl_->output_name = impl_->session->GetOutputNameAllocated(0, impl_->allocator).get();
|
|
||||||
|
|
||||||
size_t num_inputs = impl_->session->GetInputCount();
|
|
||||||
if (num_inputs >= 2) {
|
|
||||||
std::cerr << "YoloOnnx: model has " << num_inputs << " inputs — "
|
|
||||||
"this engine only supports single-input models."
|
|
||||||
<< std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
auto out_info = impl_->session->GetOutputTypeInfo(0).GetTensorTypeAndShapeInfo();
|
|
||||||
auto shape = out_info.GetShape();
|
|
||||||
if (shape.size() == 3) {
|
|
||||||
int64_t dim1 = shape[1];
|
|
||||||
int64_t dim2 = shape[2];
|
|
||||||
|
|
||||||
YoloOutputLayout resolved;
|
|
||||||
if (config_.output_layout == YoloOutputLayout::AUTO) {
|
|
||||||
// dim1 < dim2 → NC_FIRST (e.g. 84 vs 8400 for YOLOv8/11)
|
|
||||||
// dim1 > dim2 → NC_LAST_OBJ (e.g. 25200 vs 85 for YOLOv5)
|
|
||||||
resolved = (dim1 < dim2) ? YoloOutputLayout::NC_FIRST
|
|
||||||
: YoloOutputLayout::NC_LAST_OBJ;
|
|
||||||
} else {
|
|
||||||
resolved = config_.output_layout;
|
|
||||||
}
|
|
||||||
impl_->resolved_layout = resolved;
|
|
||||||
|
|
||||||
if (resolved == YoloOutputLayout::NC_FIRST) {
|
|
||||||
impl_->num_classes = static_cast<int>(dim1) - 4;
|
|
||||||
} else if (resolved == YoloOutputLayout::NC_LAST) {
|
|
||||||
impl_->num_classes = static_cast<int>(dim2) - 4;
|
|
||||||
} else { // NC_LAST_OBJ
|
|
||||||
impl_->num_classes = static_cast<int>(dim2) - 5;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!config_.labels_file.empty() && config_.labels.empty()) {
|
|
||||||
std::ifstream lf(config_.labels_file);
|
|
||||||
if (!lf) {
|
|
||||||
std::cerr << "YoloOnnx: cannot open ZED_ONNX_LABELS_FILE: "
|
|
||||||
<< config_.labels_file << std::endl;
|
|
||||||
} else {
|
|
||||||
std::string line, joined;
|
|
||||||
while (std::getline(lf, line)) {
|
|
||||||
size_t l = line.find_first_not_of(" \t\r");
|
|
||||||
size_t r = line.find_last_not_of(" \t\r");
|
|
||||||
if (l == std::string::npos) continue;
|
|
||||||
if (!joined.empty()) joined += ',';
|
|
||||||
joined += line.substr(l, r - l + 1);
|
|
||||||
}
|
|
||||||
if (!joined.empty()) {
|
|
||||||
config_.labels = joined;
|
|
||||||
std::cout << "YoloOnnx: loaded labels from file ("
|
|
||||||
<< config_.labels_file << ")" << std::endl;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
std::cout << "YoloOnnx: model loaded."
|
|
||||||
<< " Classes=" << impl_->num_classes
|
|
||||||
<< " Layout=" << (impl_->resolved_layout == YoloOutputLayout::NC_FIRST ? "NC_FIRST"
|
|
||||||
: impl_->resolved_layout == YoloOutputLayout::NC_LAST ? "NC_LAST"
|
|
||||||
: "NC_LAST_OBJ")
|
|
||||||
<< std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
void YoloOnnx::detect(sl::Camera& camera) {
|
|
||||||
detected_.clear();
|
|
||||||
if (!impl_->session) return;
|
|
||||||
|
|
||||||
sl::Mat img_mat;
|
|
||||||
camera.retrieveImage(img_mat, sl::VIEW::LEFT);
|
|
||||||
|
|
||||||
sl::Mat pc_mat;
|
|
||||||
camera.retrieveMeasure(pc_mat, sl::MEASURE::XYZRGBA);
|
|
||||||
|
|
||||||
sl::CameraInformation info = camera.getCameraInformation();
|
|
||||||
auto calib = info.camera_configuration.calibration_parameters.left_cam;
|
|
||||||
float fx = calib.fx, fy = calib.fy, cx = calib.cx, cy = calib.cy;
|
|
||||||
|
|
||||||
int orig_w = static_cast<int>(img_mat.getWidth());
|
|
||||||
int orig_h = static_cast<int>(img_mat.getHeight());
|
|
||||||
|
|
||||||
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);
|
|
||||||
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void YoloOnnx::detect(const cv::Mat& image_bgr) {
|
|
||||||
detected_.clear();
|
|
||||||
if (!impl_->session) return;
|
|
||||||
|
|
||||||
auto kept = impl_->run_inference(config_, image_bgr);
|
|
||||||
|
|
||||||
for (const auto& det : kept) {
|
|
||||||
DetectedObject obj{};
|
|
||||||
obj.label_id = det.class_id;
|
|
||||||
obj.confidence = det.score * 100.0f;
|
|
||||||
obj.bounding_box_2d[0] = det.x1; obj.bounding_box_2d[1] = det.y1;
|
|
||||||
obj.bounding_box_2d[2] = det.x2; obj.bounding_box_2d[3] = det.y1;
|
|
||||||
obj.bounding_box_2d[4] = det.x2; obj.bounding_box_2d[5] = det.y2;
|
|
||||||
obj.bounding_box_2d[6] = det.x1; obj.bounding_box_2d[7] = det.y2;
|
|
||||||
detected_.push_back(obj);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const std::vector<DetectedObject>& YoloOnnx::objects() const {
|
|
||||||
return detected_;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::string YoloOnnx::label_names() const {
|
|
||||||
return config_.labels;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace engines
|
|
||||||
@@ -1,48 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "engines/engine.h"
|
|
||||||
|
|
||||||
#include <opencv2/core.hpp>
|
|
||||||
#include <memory>
|
|
||||||
#include <string>
|
|
||||||
|
|
||||||
namespace engines {
|
|
||||||
|
|
||||||
enum class YoloOutputLayout {
|
|
||||||
AUTO, // detect from output tensor shape at load time
|
|
||||||
NC_FIRST, // (1, 4+N, anchors) — YOLOv8, YOLOv11
|
|
||||||
NC_LAST, // (1, anchors, 4+N) — rows-first, no objectness
|
|
||||||
NC_LAST_OBJ // (1, anchors, 5+N) — YOLOv5, rows-first with objectness
|
|
||||||
};
|
|
||||||
|
|
||||||
struct YoloOnnxConfig {
|
|
||||||
std::string model_path; // ZED_ONNX_MODEL_PATH (required)
|
|
||||||
float confidence_threshold = 0.25f; // ZED_ONNX_CONFIDENCE
|
|
||||||
float nms_threshold = 0.45f; // ZED_ONNX_NMS_THRESHOLD
|
|
||||||
int input_size = 640; // ZED_ONNX_INPUT_SIZE
|
|
||||||
std::string labels; // ZED_ONNX_LABELS (comma-separated)
|
|
||||||
std::string labels_file; // ZED_ONNX_LABELS_FILE (one class name per line)
|
|
||||||
YoloOutputLayout output_layout = YoloOutputLayout::AUTO;
|
|
||||||
|
|
||||||
static YoloOnnxConfig from_env();
|
|
||||||
};
|
|
||||||
|
|
||||||
class YoloOnnx : public Engine {
|
|
||||||
public:
|
|
||||||
explicit YoloOnnx(const YoloOnnxConfig& config);
|
|
||||||
~YoloOnnx() override;
|
|
||||||
|
|
||||||
void enable(sl::Camera& camera) override;
|
|
||||||
void detect(sl::Camera& camera) override;
|
|
||||||
void detect(const cv::Mat& image_bgr); // image-only; no 3D fusion
|
|
||||||
const std::vector<DetectedObject>& objects() const override;
|
|
||||||
std::string label_names() const override;
|
|
||||||
|
|
||||||
private:
|
|
||||||
YoloOnnxConfig config_;
|
|
||||||
struct Impl;
|
|
||||||
std::unique_ptr<Impl> impl_;
|
|
||||||
std::vector<DetectedObject> detected_;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace engines
|
|
||||||
@@ -1,99 +0,0 @@
|
|||||||
#include "engines/zed_native.h"
|
|
||||||
#include "util/env.h"
|
|
||||||
|
|
||||||
#include <iostream>
|
|
||||||
#include <string>
|
|
||||||
|
|
||||||
namespace {
|
|
||||||
|
|
||||||
sl::OBJECT_DETECTION_MODEL parse_detection_model(const std::string& model) {
|
|
||||||
if (model == "MULTI_CLASS_BOX_FAST") {
|
|
||||||
return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST;
|
|
||||||
}
|
|
||||||
if (model == "MULTI_CLASS_BOX_ACCURATE") {
|
|
||||||
return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE;
|
|
||||||
}
|
|
||||||
return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
namespace engines {
|
|
||||||
|
|
||||||
DetectionConfig DetectionConfig::from_env() {
|
|
||||||
DetectionConfig c;
|
|
||||||
c.model = util::get_env_str("ZED_DETECTION_MODEL", c.model);
|
|
||||||
c.confidence_threshold = util::get_env_float("ZED_DETECTION_CONFIDENCE", c.confidence_threshold);
|
|
||||||
c.tracking = util::get_env_bool("ZED_DETECTION_TRACKING", c.tracking);
|
|
||||||
return c;
|
|
||||||
}
|
|
||||||
|
|
||||||
ZedNative::ZedNative(const DetectionConfig& config) : config_(config) {
|
|
||||||
det_params_.detection_model = parse_detection_model(config_.model);
|
|
||||||
det_params_.enable_tracking = config_.tracking;
|
|
||||||
det_params_.enable_segmentation = false;
|
|
||||||
|
|
||||||
det_rt_params_.detection_confidence_threshold = config_.confidence_threshold;
|
|
||||||
}
|
|
||||||
|
|
||||||
void ZedNative::enable(sl::Camera& camera) {
|
|
||||||
if (config_.tracking) {
|
|
||||||
sl::PositionalTrackingParameters tracking_params;
|
|
||||||
sl::ERROR_CODE err = camera.enablePositionalTracking(tracking_params);
|
|
||||||
if (err != sl::ERROR_CODE::SUCCESS) {
|
|
||||||
std::cerr << "Failed to enable positional tracking: "
|
|
||||||
<< sl::toString(err) << std::endl;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
std::cout << "Object Detection: loading model (" << config_.model << ")..." << std::endl;
|
|
||||||
sl::ERROR_CODE err = camera.enableObjectDetection(det_params_);
|
|
||||||
if (err != sl::ERROR_CODE::SUCCESS) {
|
|
||||||
std::cerr << "Failed to enable object detection: "
|
|
||||||
<< sl::toString(err) << std::endl;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
std::cout << "Object Detection: model loaded." << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
void ZedNative::detect(sl::Camera& camera) {
|
|
||||||
detected_.clear();
|
|
||||||
|
|
||||||
sl::ERROR_CODE err = camera.retrieveObjects(raw_objects_, det_rt_params_);
|
|
||||||
if (err != sl::ERROR_CODE::SUCCESS) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
for (const auto& obj : raw_objects_.object_list) {
|
|
||||||
DetectedObject det{};
|
|
||||||
det.label_id = static_cast<int>(obj.label);
|
|
||||||
det.confidence = obj.confidence;
|
|
||||||
|
|
||||||
det.position[0] = obj.position.x;
|
|
||||||
det.position[1] = obj.position.y;
|
|
||||||
det.position[2] = obj.position.z;
|
|
||||||
|
|
||||||
for (int i = 0; i < 4 && i < static_cast<int>(obj.bounding_box_2d.size()); i++) {
|
|
||||||
det.bounding_box_2d[i * 2 + 0] = obj.bounding_box_2d[i].x;
|
|
||||||
det.bounding_box_2d[i * 2 + 1] = obj.bounding_box_2d[i].y;
|
|
||||||
}
|
|
||||||
|
|
||||||
for (int i = 0; i < 8 && i < static_cast<int>(obj.bounding_box.size()); i++) {
|
|
||||||
det.bounding_box_3d[i * 3 + 0] = obj.bounding_box[i].x;
|
|
||||||
det.bounding_box_3d[i * 3 + 1] = obj.bounding_box[i].y;
|
|
||||||
det.bounding_box_3d[i * 3 + 2] = obj.bounding_box[i].z;
|
|
||||||
}
|
|
||||||
|
|
||||||
detected_.push_back(det);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
const std::vector<DetectedObject>& ZedNative::objects() const {
|
|
||||||
return detected_;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::string ZedNative::label_names() const {
|
|
||||||
return "PERSON,VEHICLE,BAG,ANIMAL,ELECTRONICS,FRUIT_VEGETABLE,SPORT";
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace engines
|
|
||||||
@@ -1,34 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include "engines/engine.h"
|
|
||||||
|
|
||||||
#include <string>
|
|
||||||
|
|
||||||
namespace engines {
|
|
||||||
|
|
||||||
struct DetectionConfig {
|
|
||||||
std::string model = "MULTI_CLASS_BOX_MEDIUM";
|
|
||||||
float confidence_threshold = 40.0f;
|
|
||||||
bool tracking = false;
|
|
||||||
|
|
||||||
static DetectionConfig from_env();
|
|
||||||
};
|
|
||||||
|
|
||||||
class ZedNative : public Engine {
|
|
||||||
public:
|
|
||||||
explicit ZedNative(const DetectionConfig& config);
|
|
||||||
|
|
||||||
void enable(sl::Camera& camera) override;
|
|
||||||
void detect(sl::Camera& camera) override;
|
|
||||||
const std::vector<DetectedObject>& objects() const override;
|
|
||||||
std::string label_names() const override;
|
|
||||||
|
|
||||||
private:
|
|
||||||
DetectionConfig config_;
|
|
||||||
sl::ObjectDetectionParameters det_params_;
|
|
||||||
sl::ObjectDetectionRuntimeParameters det_rt_params_;
|
|
||||||
sl::Objects raw_objects_;
|
|
||||||
std::vector<DetectedObject> detected_;
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace engines
|
|
||||||
+122
-212
@@ -1,32 +1,25 @@
|
|||||||
#include "engines/engine.h"
|
#include "arrow/array.h"
|
||||||
#include "engines/yolo_onnx.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));
|
|
||||||
|
// 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;
|
||||||
}
|
}
|
||||||
return buffer;
|
}
|
||||||
|
|
||||||
|
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 == "yolo_onnx" || engine_name == "onnx") {
|
|
||||||
auto cfg = engines::YoloOnnxConfig::from_env();
|
|
||||||
detector = std::make_unique<engines::YoloOnnx>(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,180 +100,91 @@ int main() {
|
|||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!camera.grab()) {
|
std::string state;
|
||||||
std::cerr << "Grab failed; attempting to reconnect..." << std::endl;
|
if (capture.take_state_change(state)) {
|
||||||
camera.close();
|
auto state_metadata = new_metadata();
|
||||||
camera.open();
|
state_metadata->set_timestamp("host_timestamp_ns", host_timestamp_ns());
|
||||||
calib = camera.calibration();
|
|
||||||
detector->enable(camera.handle());
|
send(dora_node, "camera_state", arrow_c::string(state.data(), state.size()),
|
||||||
|
std::move(state_metadata));
|
||||||
|
}
|
||||||
|
|
||||||
|
// No new frame since the last tick: the camera is slower than the timer,
|
||||||
|
// or it is down and the capture thread is recovering it.
|
||||||
|
if (!capture.take_frame(frame)) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
detector->detect(camera.handle());
|
auto width = static_cast<uint32_t>(frame.width);
|
||||||
|
auto height = static_cast<uint32_t>(frame.height);
|
||||||
sl::Mat& image_mat = camera.image();
|
|
||||||
sl::Mat& point_cloud_mat = camera.point_cloud();
|
|
||||||
|
|
||||||
int width = image_mat.getWidth();
|
|
||||||
int height = image_mat.getHeight();
|
|
||||||
|
|
||||||
int64_t current_frame_id = frame_id++;
|
|
||||||
int64_t timestamp_ns = static_cast<int64_t>(
|
|
||||||
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
|
||||||
std::chrono::steady_clock::now().time_since_epoch()
|
|
||||||
).count()
|
|
||||||
);
|
|
||||||
|
|
||||||
// --- image_bgr ---
|
|
||||||
cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr<sl::uchar1>());
|
|
||||||
cv::Mat image_bgr;
|
|
||||||
cv::cvtColor(image_rgba, image_bgr, cv::COLOR_BGRA2BGR);
|
|
||||||
|
|
||||||
const auto image_size = static_cast<size_t>(image_bgr.total() * image_bgr.elemSize());
|
|
||||||
rust::Slice<const uint8_t> image_slice{
|
|
||||||
reinterpret_cast<const uint8_t*>(image_bgr.data), image_size};
|
|
||||||
|
|
||||||
|
// --- image ---
|
||||||
|
// The payload carries its own width, height and encoding, so none of
|
||||||
|
// that travels as metadata: what is left is what describes the frame
|
||||||
|
// rather than the picture.
|
||||||
auto image_metadata = new_metadata();
|
auto image_metadata = new_metadata();
|
||||||
image_metadata->set_string("dtype", "uint8");
|
set_intrinsics(*image_metadata, frame.calib);
|
||||||
image_metadata->set_string("encoding", "bgr8");
|
set_frame_stamps(*image_metadata, frame);
|
||||||
image_metadata->set_string("layout", "HWC");
|
|
||||||
image_metadata->set_list_int("shape", make_shape(height, width, 3));
|
|
||||||
image_metadata->set_float("fx", calib.fx);
|
|
||||||
image_metadata->set_float("fy", calib.fy);
|
|
||||||
image_metadata->set_float("cx", calib.cx);
|
|
||||||
image_metadata->set_float("cy", calib.cy);
|
|
||||||
image_metadata->set_list_float("distortion", make_distortion(calib.distortion));
|
|
||||||
image_metadata->set_int("frame_id", current_frame_id);
|
|
||||||
image_metadata->set_int("timestamp_ns", timestamp_ns);
|
|
||||||
|
|
||||||
auto image_result = send_output_with_metadata(
|
send(dora_node, "image",
|
||||||
dora_node.send_output, "image_bgr", image_slice, std::move(image_metadata));
|
arrow_c::image(width, height, "bgr8", frame.image_bgr.data(), frame.image_bgr.size()),
|
||||||
if (!std::string(image_result.error).empty()) {
|
std::move(image_metadata));
|
||||||
std::cerr << "Error sending image_bgr: " << std::string(image_result.error) << std::endl;
|
|
||||||
}
|
// --- depth ---
|
||||||
|
// A depth map is an image whose pixels are distances, which is how ROS
|
||||||
|
// spells it too: 32FC1 is one float32 per pixel, and `units` says what
|
||||||
|
// those floats measure.
|
||||||
|
auto depth_metadata = new_metadata();
|
||||||
|
depth_metadata->set_string("units", "mm");
|
||||||
|
set_intrinsics(*depth_metadata, frame.calib);
|
||||||
|
set_frame_stamps(*depth_metadata, frame);
|
||||||
|
|
||||||
|
send(dora_node, "depth",
|
||||||
|
arrow_c::image(width, height, "32FC1",
|
||||||
|
reinterpret_cast<const uint8_t*>(frame.depth.data()),
|
||||||
|
frame.depth.size() * sizeof(float)),
|
||||||
|
std::move(depth_metadata));
|
||||||
|
|
||||||
// --- point_cloud ---
|
// --- point_cloud ---
|
||||||
int pc_width = point_cloud_mat.getWidth();
|
if (frame.has_point_cloud) {
|
||||||
int pc_height = point_cloud_mat.getHeight();
|
// Unlike the two images, a cloud is a flat array of values: its
|
||||||
|
// grid shape is only in the metadata.
|
||||||
std::vector<float> pc_buf;
|
|
||||||
const uint8_t* pc_data_ptr = nullptr;
|
|
||||||
size_t pc_data_size = 0;
|
|
||||||
auto pc_metadata = new_metadata();
|
auto pc_metadata = new_metadata();
|
||||||
pc_metadata->set_string("dtype", "float32");
|
|
||||||
pc_metadata->set_string("channels", "XYZRGBA");
|
pc_metadata->set_string("channels", "XYZRGBA");
|
||||||
pc_metadata->set_string("units", "mm");
|
pc_metadata->set_string("units", "mm");
|
||||||
pc_metadata->set_float("fx", calib.fx);
|
|
||||||
pc_metadata->set_float("fy", calib.fy);
|
|
||||||
pc_metadata->set_float("cx", calib.cx);
|
|
||||||
pc_metadata->set_float("cy", calib.cy);
|
|
||||||
pc_metadata->set_list_float("distortion", make_distortion(calib.distortion));
|
|
||||||
|
|
||||||
if (pc_mode == PcMode::HALF) {
|
|
||||||
int hw = pc_width / 2, hh = pc_height / 2;
|
|
||||||
pc_buf.resize(static_cast<size_t>(hh * hw * 4));
|
|
||||||
const sl::float4* src = point_cloud_mat.getPtr<sl::float4>();
|
|
||||||
for (int y = 0; y < hh; y++) {
|
|
||||||
for (int x = 0; x < hw; x++) {
|
|
||||||
const sl::float4& pt = src[(y * 2) * pc_width + (x * 2)];
|
|
||||||
float* dst = pc_buf.data() + (y * hw + x) * 4;
|
|
||||||
dst[0] = pt.x; dst[1] = pt.y; dst[2] = pt.z; dst[3] = pt.w;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
pc_metadata->set_string("layout", "HWC");
|
pc_metadata->set_string("layout", "HWC");
|
||||||
pc_metadata->set_list_int("shape", make_shape(hh, hw, 4));
|
pc_metadata->set_list_int("shape",
|
||||||
pc_data_ptr = reinterpret_cast<const uint8_t*>(pc_buf.data());
|
make_vector_int({frame.pc_height, frame.pc_width, 4}));
|
||||||
pc_data_size = pc_buf.size() * sizeof(float);
|
set_intrinsics(*pc_metadata, frame.calib);
|
||||||
} else if (pc_mode == PcMode::ROI) {
|
set_frame_stamps(*pc_metadata, frame);
|
||||||
const auto& objs = detector->objects();
|
|
||||||
for (const auto& obj : objs) {
|
|
||||||
int x1 = std::max(0, static_cast<int>(obj.bounding_box_2d[0]));
|
|
||||||
int y1 = std::max(0, static_cast<int>(obj.bounding_box_2d[1]));
|
|
||||||
int x2 = std::min(pc_width - 1, static_cast<int>(obj.bounding_box_2d[4]));
|
|
||||||
int y2 = std::min(pc_height - 1, static_cast<int>(obj.bounding_box_2d[5]));
|
|
||||||
for (int y = y1; y <= y2; y++) {
|
|
||||||
for (int x = x1; x <= x2; x++) {
|
|
||||||
sl::float4 pt;
|
|
||||||
point_cloud_mat.getValue(x, y, &pt);
|
|
||||||
if (!std::isnan(pt.x) && !std::isinf(pt.x)) {
|
|
||||||
pc_buf.push_back(pt.x); pc_buf.push_back(pt.y);
|
|
||||||
pc_buf.push_back(pt.z); pc_buf.push_back(pt.w);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
int N = static_cast<int>(pc_buf.size() / 4);
|
|
||||||
pc_metadata->set_string("layout", "NC");
|
|
||||||
pc_metadata->set_string("mode", "roi");
|
|
||||||
pc_metadata->set_list_int("shape", make_vector_int({N, 4}));
|
|
||||||
pc_data_ptr = reinterpret_cast<const uint8_t*>(pc_buf.data());
|
|
||||||
pc_data_size = pc_buf.size() * sizeof(float);
|
|
||||||
} else {
|
|
||||||
// FULL — send raw sl::Mat pointer directly (no copy)
|
|
||||||
pc_data_size = static_cast<size_t>(pc_width * pc_height * sizeof(sl::float4));
|
|
||||||
pc_data_ptr = reinterpret_cast<const uint8_t*>(point_cloud_mat.getPtr<sl::float4>());
|
|
||||||
pc_metadata->set_string("layout", "HWC");
|
|
||||||
pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4));
|
|
||||||
}
|
|
||||||
|
|
||||||
rust::Slice<const uint8_t> pc_slice{pc_data_ptr, pc_data_size};
|
send(dora_node, "point_cloud",
|
||||||
auto pc_result = send_output_with_metadata(
|
arrow_c::float32(frame.point_cloud.data(), frame.point_cloud.size()),
|
||||||
dora_node.send_output, "point_cloud", pc_slice, std::move(pc_metadata));
|
std::move(pc_metadata));
|
||||||
if (!std::string(pc_result.error).empty()) {
|
|
||||||
std::cerr << "Error sending point_cloud: " << std::string(pc_result.error) << std::endl;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// --- camera_info ---
|
// --- camera_info ---
|
||||||
float camera_data[4] = {calib.fx, calib.fy, calib.cx, calib.cy};
|
if (frame.camera_info_first) {
|
||||||
rust::Slice<const uint8_t> cam_slice{
|
const float camera_data[4] = {frame.calib.fx, frame.calib.fy, frame.calib.cx,
|
||||||
reinterpret_cast<const uint8_t*>(camera_data), sizeof(camera_data)};
|
frame.calib.cy};
|
||||||
|
|
||||||
auto cam_metadata = new_metadata();
|
auto cam_metadata = new_metadata();
|
||||||
cam_metadata->set_string("dtype", "float32");
|
cam_metadata->set_list_float("distortion", make_distortion(frame.calib.distortion));
|
||||||
cam_metadata->set_string("layout", "C");
|
set_frame_stamps(*cam_metadata, frame);
|
||||||
cam_metadata->set_list_int("shape", make_vector_int({4}));
|
|
||||||
cam_metadata->set_list_float("distortion", make_distortion(calib.distortion));
|
|
||||||
|
|
||||||
auto cam_result = send_output_with_metadata(
|
send(dora_node, "camera_info", arrow_c::float32(camera_data, 4),
|
||||||
dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata));
|
std::move(cam_metadata));
|
||||||
if (!std::string(cam_result.error).empty()) {
|
|
||||||
std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
// --- objects ---
|
|
||||||
const auto& objects = detector->objects();
|
|
||||||
auto obj_buffer = serialize_objects(objects);
|
|
||||||
int num_objects = static_cast<int>(objects.size());
|
|
||||||
|
|
||||||
const size_t obj_size = obj_buffer.size() * sizeof(float);
|
|
||||||
rust::Slice<const uint8_t> obj_slice{
|
|
||||||
reinterpret_cast<const uint8_t*>(obj_buffer.data()), obj_size};
|
|
||||||
|
|
||||||
auto obj_metadata = new_metadata();
|
|
||||||
obj_metadata->set_string("dtype", "float32");
|
|
||||||
obj_metadata->set_string("layout", "RC");
|
|
||||||
obj_metadata->set_string("units", "mm");
|
|
||||||
obj_metadata->set_list_int("shape", make_vector_int({num_objects, FIELDS_PER_OBJECT}));
|
|
||||||
obj_metadata->set_string("fields",
|
|
||||||
"label_id,confidence,x,y,z,"
|
|
||||||
"bb2d_x0,bb2d_y0,bb2d_x1,bb2d_y1,bb2d_x2,bb2d_y2,bb2d_x3,bb2d_y3,"
|
|
||||||
"bb3d_x0,bb3d_y0,bb3d_z0,bb3d_x1,bb3d_y1,bb3d_z1,"
|
|
||||||
"bb3d_x2,bb3d_y2,bb3d_z2,bb3d_x3,bb3d_y3,bb3d_z3,"
|
|
||||||
"bb3d_x4,bb3d_y4,bb3d_z4,bb3d_x5,bb3d_y5,bb3d_z5,"
|
|
||||||
"bb3d_x6,bb3d_y6,bb3d_z6,bb3d_x7,bb3d_y7,bb3d_z7");
|
|
||||||
obj_metadata->set_string("label_names", detector->label_names());
|
|
||||||
obj_metadata->set_string("engine", engine_name);
|
|
||||||
obj_metadata->set_int("frame_id", current_frame_id);
|
|
||||||
obj_metadata->set_int("timestamp_ns", timestamp_ns);
|
|
||||||
obj_metadata->set_int("image_width", width);
|
|
||||||
obj_metadata->set_int("image_height", height);
|
|
||||||
|
|
||||||
auto obj_result = send_output_with_metadata(
|
|
||||||
dora_node.send_output, "objects", obj_slice, std::move(obj_metadata));
|
|
||||||
if (!std::string(obj_result.error).empty()) {
|
|
||||||
std::cerr << "Error sending objects: " << std::string(obj_result.error) << std::endl;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
capture.stop();
|
||||||
|
if (!capture.wait_exit(std::chrono::seconds(2))) {
|
||||||
|
// The capture thread is inside a blocking SDK call, where nothing can
|
||||||
|
// reach it. Leave rather than let dora wait out its grace period and kill
|
||||||
|
// the node: the frames are gone either way, and Stop has been answered.
|
||||||
|
std::cerr << "capture thread still in the SDK; exiting anyway" << std::endl;
|
||||||
|
std::cout.flush();
|
||||||
|
std::_Exit(0);
|
||||||
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,113 +0,0 @@
|
|||||||
// Standalone test for the YoloOnnx (closed-vocabulary) detection engine.
|
|
||||||
// No dora framework, no live ZED camera — loads an image from disk.
|
|
||||||
//
|
|
||||||
// Usage:
|
|
||||||
// bin/test_yolo_onnx [image_path]
|
|
||||||
// image_path default: test/assets/yolo.png
|
|
||||||
//
|
|
||||||
// Required env vars:
|
|
||||||
// ZED_ONNX_MODEL_PATH path to YOLO ONNX model
|
|
||||||
//
|
|
||||||
// Optional env vars:
|
|
||||||
// ZED_ONNX_LABELS comma-separated class names
|
|
||||||
// ZED_ONNX_LABELS_FILE path to newline-separated class names file
|
|
||||||
// ZED_ONNX_CONFIDENCE detection threshold (default 0.25)
|
|
||||||
// ZED_ONNX_NMS_THRESHOLD NMS IoU threshold (default 0.45)
|
|
||||||
// ZED_ONNX_INPUT_SIZE model input size (default 640)
|
|
||||||
// ZED_ONNX_OUTPUT_FORMAT nc_first|nc_last|nc_last_obj|yolov5|yolov8|yolov11|auto
|
|
||||||
|
|
||||||
#include "engines/yolo_onnx.h"
|
|
||||||
#include "util/env.h"
|
|
||||||
|
|
||||||
#include <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);
|
|
||||||
|
|
||||||
cv::rectangle(img, {x1, y1}, {x2, y2}, {0, 200, 0}, 2);
|
|
||||||
cv::putText(img, name + buf, {x1, std::max(y1 - 6, 14)},
|
|
||||||
cv::FONT_HERSHEY_SIMPLEX, 0.55, {0, 200, 0}, 2, cv::LINE_AA);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
int main(int argc, char* argv[]) {
|
|
||||||
std::string image_path = (argc > 1) ? argv[1] : "test/assets/yolo.png";
|
|
||||||
|
|
||||||
auto config = engines::YoloOnnxConfig::from_env();
|
|
||||||
|
|
||||||
sl::Camera dummy_cam;
|
|
||||||
engines::YoloOnnx engine(config);
|
|
||||||
engine.enable(dummy_cam);
|
|
||||||
|
|
||||||
cv::Mat img = cv::imread(image_path);
|
|
||||||
if (img.empty()) {
|
|
||||||
std::cerr << "Error: cannot read image: " << image_path << std::endl;
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
engine.detect(img);
|
|
||||||
const auto& objects = engine.objects();
|
|
||||||
auto labels = parse_csv(engine.label_names());
|
|
||||||
|
|
||||||
std::cout << "[" << image_path << "] " << objects.size() << " detection(s):" << std::endl;
|
|
||||||
for (const auto& obj : objects) {
|
|
||||||
std::string name = (obj.label_id >= 0 && obj.label_id < static_cast<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 0;
|
|
||||||
}
|
|
||||||
+174
-25
@@ -1,11 +1,12 @@
|
|||||||
#include "zed/camera.h"
|
#include "zed/camera.h"
|
||||||
#include "util/env.h"
|
#include "util/env.h"
|
||||||
|
|
||||||
#include <chrono>
|
#include <algorithm>
|
||||||
|
#include <cstdlib>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <iterator>
|
#include <iterator>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <thread>
|
#include <utility>
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
@@ -21,17 +22,30 @@ sl::RESOLUTION parse_resolution(int resolution) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const std::pair<const char*, sl::DEPTH_MODE> DEPTH_MODES[] = {
|
||||||
|
{"NEURAL", sl::DEPTH_MODE::NEURAL},
|
||||||
|
{"NEURAL_PLUS", sl::DEPTH_MODE::NEURAL_PLUS},
|
||||||
|
{"NEURAL_LIGHT", sl::DEPTH_MODE::NEURAL_LIGHT},
|
||||||
|
{"PERFORMANCE", sl::DEPTH_MODE::PERFORMANCE},
|
||||||
|
{"QUALITY", sl::DEPTH_MODE::QUALITY},
|
||||||
|
{"NONE", sl::DEPTH_MODE::NONE},
|
||||||
|
};
|
||||||
|
|
||||||
|
// A typo here silently changes depth quality if it falls back to a default, and
|
||||||
|
// the resulting depth is wrong in a way that only shows up downstream, so an
|
||||||
|
// unrecognised name stops the node instead.
|
||||||
sl::DEPTH_MODE parse_depth_mode(const std::string& mode) {
|
sl::DEPTH_MODE parse_depth_mode(const std::string& mode) {
|
||||||
if (mode == "PERFORMANCE") {
|
for (const auto& entry : DEPTH_MODES) {
|
||||||
return sl::DEPTH_MODE::PERFORMANCE;
|
if (mode == entry.first) {
|
||||||
|
return entry.second;
|
||||||
}
|
}
|
||||||
if (mode == "QUALITY") {
|
|
||||||
return sl::DEPTH_MODE::QUALITY;
|
|
||||||
}
|
}
|
||||||
if (mode == "NONE") {
|
std::cerr << "Unknown ZED_DEPTH_MODE '" << mode << "'. Accepted values:";
|
||||||
return sl::DEPTH_MODE::NONE;
|
for (const auto& entry : DEPTH_MODES) {
|
||||||
|
std::cerr << " " << entry.first;
|
||||||
}
|
}
|
||||||
return sl::DEPTH_MODE::NEURAL;
|
std::cerr << std::endl;
|
||||||
|
std::exit(1);
|
||||||
}
|
}
|
||||||
|
|
||||||
sl::FLIP_MODE parse_flip(const std::string& mode) {
|
sl::FLIP_MODE parse_flip(const std::string& mode) {
|
||||||
@@ -41,6 +55,24 @@ sl::FLIP_MODE parse_flip(const std::string& mode) {
|
|||||||
return sl::FLIP_MODE::ON;
|
return sl::FLIP_MODE::ON;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// A blank frame is checked on a strided sample rather than the full image, which
|
||||||
|
// is just as conclusive and costs nothing at frame rate.
|
||||||
|
constexpr int BLANK_FRAME_STRIDE = 16;
|
||||||
|
|
||||||
|
// sl::Camera::reboot() reaches the ZED 2 and newer. On the first-generation ZED
|
||||||
|
// and the ZED Mini it returns without touching the device, so calling it there
|
||||||
|
// only costs time and fills the log.
|
||||||
|
bool model_supports_reboot(sl::MODEL model) {
|
||||||
|
return model != sl::MODEL::ZED && model != sl::MODEL::ZED_M;
|
||||||
|
}
|
||||||
|
|
||||||
|
// The SDK reconnects a camera whose link hiccuped by itself. Dropping the handle
|
||||||
|
// over one of these takes the recovery away from the SDK and hands it to us,
|
||||||
|
// which is slower and, on a device the kernel is about to suspend, often fatal.
|
||||||
|
bool is_transient_grab_error(sl::ERROR_CODE err) {
|
||||||
|
return err == sl::ERROR_CODE::CAMERA_REBOOTING || err == sl::ERROR_CODE::CORRUPTED_FRAME;
|
||||||
|
}
|
||||||
|
|
||||||
void list_available_cameras() {
|
void list_available_cameras() {
|
||||||
auto devices = sl::Camera::getDeviceList();
|
auto devices = sl::Camera::getDeviceList();
|
||||||
std::cout << "Detected ZED cameras: " << devices.size() << std::endl;
|
std::cout << "Detected ZED cameras: " << devices.size() << std::endl;
|
||||||
@@ -59,50 +91,112 @@ Config Config::from_env() {
|
|||||||
Config c;
|
Config c;
|
||||||
c.resolution = util::get_env_int("ZED_RESOLUTION", c.resolution);
|
c.resolution = util::get_env_int("ZED_RESOLUTION", c.resolution);
|
||||||
c.fps = util::get_env_int("ZED_FPS", c.fps);
|
c.fps = util::get_env_int("ZED_FPS", c.fps);
|
||||||
c.depth_mode = util::get_env_str("ZED_DEPTH_MODE", c.depth_mode);
|
c.depth_mode = parse_depth_mode(util::get_env_str("ZED_DEPTH_MODE", "NEURAL"));
|
||||||
c.depth_min_mm = util::get_env_int("ZED_DEPTH_MIN_MM", c.depth_min_mm);
|
c.depth_min_mm = util::get_env_int("ZED_DEPTH_MIN_MM", c.depth_min_mm);
|
||||||
c.depth_max_mm = util::get_env_int("ZED_DEPTH_MAX_MM", c.depth_max_mm);
|
c.depth_max_mm = util::get_env_int("ZED_DEPTH_MAX_MM", c.depth_max_mm);
|
||||||
c.depth_fill = util::get_env_bool("ZED_DEPTH_FILL", c.depth_fill);
|
c.depth_fill = util::get_env_bool("ZED_DEPTH_FILL", c.depth_fill);
|
||||||
|
c.point_cloud = util::get_env_bool("ZED_POINT_CLOUD", c.point_cloud);
|
||||||
|
c.pc_mode = util::get_env_str("ZED_PC_MODE", "FULL") == "HALF" ? PcMode::HALF : PcMode::FULL;
|
||||||
|
c.confidence_threshold = util::get_env_int("ZED_CONFIDENCE_THRESHOLD", c.confidence_threshold);
|
||||||
|
c.texture_confidence_threshold =
|
||||||
|
util::get_env_int("ZED_TEXTURE_CONFIDENCE_THRESHOLD", c.texture_confidence_threshold);
|
||||||
c.flip = util::get_env_str("ZED_FLIP", c.flip);
|
c.flip = util::get_env_str("ZED_FLIP", c.flip);
|
||||||
c.warmup_frames = util::get_env_int("ZED_WARMUP_FRAMES", c.warmup_frames);
|
c.warmup_frames = util::get_env_int("ZED_WARMUP_FRAMES", c.warmup_frames);
|
||||||
int retry_seconds = util::get_env_int("ZED_RETRY_INTERVAL_SECONDS", 5);
|
|
||||||
c.retry_interval_ms = retry_seconds * 1000;
|
|
||||||
return c;
|
return c;
|
||||||
}
|
}
|
||||||
|
|
||||||
Camera::Camera(const Config& config) : config_(config) {
|
Camera::Camera(const Config& config) : config_(config) {
|
||||||
|
// Process-wide, and must be set before any camera is opened. Without it the
|
||||||
|
// epoch of the timestamps published alongside every frame depends on the SDK
|
||||||
|
// default rather than on anything this node states.
|
||||||
|
sl::setTimestampClock(sl::TIMESTAMP_CLOCK::SYSTEM_CLOCK);
|
||||||
|
|
||||||
init_params_.camera_resolution = parse_resolution(config_.resolution);
|
init_params_.camera_resolution = parse_resolution(config_.resolution);
|
||||||
init_params_.camera_fps = config_.fps;
|
init_params_.camera_fps = config_.fps;
|
||||||
init_params_.depth_mode = parse_depth_mode(config_.depth_mode);
|
init_params_.depth_mode = config_.depth_mode;
|
||||||
init_params_.coordinate_units = sl::UNIT::MILLIMETER;
|
init_params_.coordinate_units = sl::UNIT::MILLIMETER;
|
||||||
init_params_.camera_image_flip = parse_flip(config_.flip);
|
init_params_.camera_image_flip = parse_flip(config_.flip);
|
||||||
init_params_.depth_minimum_distance = config_.depth_min_mm;
|
init_params_.depth_minimum_distance = config_.depth_min_mm;
|
||||||
init_params_.depth_maximum_distance = config_.depth_max_mm;
|
init_params_.depth_maximum_distance = config_.depth_max_mm;
|
||||||
|
// Recover a hiccuping link inside the SDK rather than out here. Without this
|
||||||
|
// a grab blocks for the roughly 17 s the SDK spends reconnecting, freezing the
|
||||||
|
// dora event loop; with it the grab returns CAMERA_REBOOTING straight away and
|
||||||
|
// the SDK reconnects in the background, holding the device open meanwhile.
|
||||||
|
init_params_.async_grab_camera_recovery = true;
|
||||||
|
// Let the SDK flag the frames a stalled camera hands back looking valid.
|
||||||
|
init_params_.enable_image_validity_check = 1;
|
||||||
|
|
||||||
runtime_params_.enable_depth = true;
|
runtime_params_.enable_depth = true;
|
||||||
runtime_params_.enable_fill_mode = config_.depth_fill;
|
runtime_params_.enable_fill_mode = config_.depth_fill;
|
||||||
|
// The two thresholds discard the depth the SDK is least certain of, which is
|
||||||
|
// the depth at object edges: those pixels straddle the object and whatever is
|
||||||
|
// behind it, and belong to neither.
|
||||||
|
runtime_params_.confidence_threshold = config_.confidence_threshold;
|
||||||
|
runtime_params_.texture_confidence_threshold = config_.texture_confidence_threshold;
|
||||||
|
|
||||||
|
std::cout << "ZED depth: mode=" << sl::toString(init_params_.depth_mode)
|
||||||
|
<< ", range=" << config_.depth_min_mm << "-" << config_.depth_max_mm << " mm"
|
||||||
|
<< ", confidence=" << config_.confidence_threshold
|
||||||
|
<< ", texture_confidence=" << config_.texture_confidence_threshold
|
||||||
|
<< ", fill=" << (config_.depth_fill ? "on" : "off") << std::endl;
|
||||||
|
|
||||||
|
list_available_cameras();
|
||||||
}
|
}
|
||||||
|
|
||||||
Camera::~Camera() {
|
Camera::~Camera() {
|
||||||
close();
|
close();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Camera::open() {
|
bool Camera::open() {
|
||||||
list_available_cameras();
|
|
||||||
while (true) {
|
|
||||||
sl::ERROR_CODE err = zed_.open(init_params_);
|
sl::ERROR_CODE err = zed_.open(init_params_);
|
||||||
if (err == sl::ERROR_CODE::SUCCESS) {
|
if (err != sl::ERROR_CODE::SUCCESS) {
|
||||||
return;
|
|
||||||
}
|
|
||||||
std::cerr << "Failed to open ZED camera: " << sl::toString(err) << std::endl;
|
std::cerr << "Failed to open ZED camera: " << sl::toString(err) << std::endl;
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(config_.retry_interval_ms));
|
return false;
|
||||||
}
|
}
|
||||||
|
recovering_ = false;
|
||||||
|
|
||||||
|
sl::CameraInformation info = zed_.getCameraInformation();
|
||||||
|
serial_number_ = info.serial_number;
|
||||||
|
model_ = info.camera_model;
|
||||||
|
// After the first open, reopen the same physical camera. Recovery runs while
|
||||||
|
// the device may be mid-re-enumeration, and picking whichever ZED answers
|
||||||
|
// first is not the same thing as picking ours.
|
||||||
|
init_params_.input.setFromSerialNumber(serial_number_);
|
||||||
|
|
||||||
|
std::cout << "ZED camera opened (" << sl::toString(model_) << ", SN " << serial_number_ << ")"
|
||||||
|
<< std::endl;
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Camera::close() {
|
void Camera::close() {
|
||||||
|
recovering_ = false;
|
||||||
zed_.close();
|
zed_.close();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Camera::reset() {
|
||||||
|
close();
|
||||||
|
if (serial_number_ == 0 || !model_supports_reboot(model_)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
// The ZED can stop delivering frames while staying enumerated: the video
|
||||||
|
// endpoint halts, grabs fail, and reopening returns CAMERA FAILED TO SETUP
|
||||||
|
// because the endpoint is still halted. A device reset clears that, and
|
||||||
|
// reboot() is the SDK's way to ask for one without unplugging the camera.
|
||||||
|
// The device re-enumerates afterwards, so the caller's backoff before the
|
||||||
|
// next open is what lets it settle.
|
||||||
|
sl::ERROR_CODE err = sl::Camera::reboot(static_cast<int>(serial_number_), true);
|
||||||
|
std::cout << "reset ZED camera (SN " << serial_number_ << "): " << sl::toString(err)
|
||||||
|
<< std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Camera::is_open() const {
|
||||||
|
return zed_.isOpened();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Camera::recovering() const {
|
||||||
|
return recovering_;
|
||||||
|
}
|
||||||
|
|
||||||
void Camera::warmup() {
|
void Camera::warmup() {
|
||||||
for (int i = 0; i < config_.warmup_frames; i++) {
|
for (int i = 0; i < config_.warmup_frames; i++) {
|
||||||
zed_.grab(runtime_params_);
|
zed_.grab(runtime_params_);
|
||||||
@@ -110,11 +204,34 @@ void Camera::warmup() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool Camera::grab() {
|
bool Camera::grab() {
|
||||||
if (zed_.grab(runtime_params_) != sl::ERROR_CODE::SUCCESS) {
|
sl::ERROR_CODE err = zed_.grab(runtime_params_);
|
||||||
|
if (err != sl::ERROR_CODE::SUCCESS) {
|
||||||
|
if (is_transient_grab_error(err)) {
|
||||||
|
// Every grab repeats this for as long as the reconnect takes, so only
|
||||||
|
// the first one is worth a line in the log.
|
||||||
|
if (!recovering_) {
|
||||||
|
recovering_ = true;
|
||||||
|
std::cerr << "ZED camera link dropped (" << sl::toString(err)
|
||||||
|
<< "); the SDK is reconnecting it" << std::endl;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
recovering_ = false;
|
||||||
|
std::cerr << "Failed to grab frame: " << sl::toString(err) << std::endl;
|
||||||
|
}
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
if (recovering_) {
|
||||||
|
recovering_ = false;
|
||||||
|
std::cout << "ZED camera link restored" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
zed_.retrieveImage(image_mat_, sl::VIEW::LEFT);
|
zed_.retrieveImage(image_mat_, sl::VIEW::LEFT);
|
||||||
|
zed_.retrieveMeasure(depth_mat_, sl::MEASURE::DEPTH);
|
||||||
|
// At 1080p a full XYZRGBA cloud is ~14.7 MB per frame, and retrieving it
|
||||||
|
// costs capture time whether or not anything consumes it.
|
||||||
|
if (config_.point_cloud) {
|
||||||
zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA);
|
zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA);
|
||||||
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -135,16 +252,48 @@ Calibration Camera::calibration() const {
|
|||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool Camera::image_is_blank() {
|
||||||
|
const int height = image_mat_.getHeight();
|
||||||
|
const int width = image_mat_.getWidth();
|
||||||
|
if (height == 0 || width == 0) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
const size_t step = image_mat_.getStep(sl::MEM::CPU);
|
||||||
|
const sl::uchar4* src = image_mat_.getPtr<sl::uchar4>();
|
||||||
|
|
||||||
|
unsigned char lowest[3] = {255, 255, 255};
|
||||||
|
unsigned char highest[3] = {0, 0, 0};
|
||||||
|
for (int y = 0; y < height; y += BLANK_FRAME_STRIDE) {
|
||||||
|
const sl::uchar4* row = src + static_cast<size_t>(y) * step;
|
||||||
|
for (int x = 0; x < width; x += BLANK_FRAME_STRIDE) {
|
||||||
|
const unsigned char channels[3] = {row[x].x, row[x].y, row[x].z};
|
||||||
|
for (int c = 0; c < 3; c++) {
|
||||||
|
lowest[c] = std::min(lowest[c], channels[c]);
|
||||||
|
highest[c] = std::max(highest[c], channels[c]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// Sensor noise makes a single-colour frame impossible for a live camera, so
|
||||||
|
// this tells a dead device apart from any real scene.
|
||||||
|
return lowest[0] == highest[0] && lowest[1] == highest[1] && lowest[2] == highest[2];
|
||||||
|
}
|
||||||
|
|
||||||
|
int64_t Camera::device_timestamp_ns() {
|
||||||
|
return static_cast<int64_t>(
|
||||||
|
zed_.getTimestamp(sl::TIME_REFERENCE::IMAGE).getNanoseconds());
|
||||||
|
}
|
||||||
|
|
||||||
sl::Mat& Camera::image() {
|
sl::Mat& Camera::image() {
|
||||||
return image_mat_;
|
return image_mat_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
sl::Mat& Camera::depth() {
|
||||||
|
return depth_mat_;
|
||||||
|
}
|
||||||
|
|
||||||
sl::Mat& Camera::point_cloud() {
|
sl::Mat& Camera::point_cloud() {
|
||||||
return point_cloud_mat_;
|
return point_cloud_mat_;
|
||||||
}
|
}
|
||||||
|
|
||||||
sl::Camera& Camera::handle() {
|
|
||||||
return zed_;
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace zed
|
} // namespace zed
|
||||||
|
|||||||
+32
-8
@@ -1,27 +1,33 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <sl/Camera.hpp>
|
#include <sl/Camera.hpp>
|
||||||
|
#include <cstdint>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
namespace zed {
|
namespace zed {
|
||||||
|
|
||||||
|
enum class PcMode { FULL, HALF };
|
||||||
|
|
||||||
struct Config {
|
struct Config {
|
||||||
int resolution = 720;
|
int resolution = 1080;
|
||||||
int fps = 15;
|
int fps = 15;
|
||||||
std::string depth_mode = "NEURAL";
|
sl::DEPTH_MODE depth_mode = sl::DEPTH_MODE::NEURAL;
|
||||||
int depth_min_mm = 10;
|
int depth_min_mm = 200;
|
||||||
int depth_max_mm = 500;
|
int depth_max_mm = 600;
|
||||||
bool depth_fill = false;
|
bool depth_fill = false;
|
||||||
|
bool point_cloud = false;
|
||||||
|
PcMode pc_mode = PcMode::FULL;
|
||||||
|
int confidence_threshold = 60;
|
||||||
|
int texture_confidence_threshold = 100;
|
||||||
std::string flip = "ON";
|
std::string flip = "ON";
|
||||||
int warmup_frames = 30;
|
int warmup_frames = 30;
|
||||||
int retry_interval_ms = 5000;
|
|
||||||
|
|
||||||
static Config from_env();
|
static Config from_env();
|
||||||
};
|
};
|
||||||
|
|
||||||
struct Calibration {
|
struct Calibration {
|
||||||
float fx, fy, cx, cy;
|
float fx = 0, fy = 0, cx = 0, cy = 0;
|
||||||
std::vector<double> distortion;
|
std::vector<double> distortion;
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -30,14 +36,26 @@ public:
|
|||||||
explicit Camera(const Config& config);
|
explicit Camera(const Config& config);
|
||||||
~Camera();
|
~Camera();
|
||||||
|
|
||||||
void open();
|
// One attempt, returning whether it succeeded. The caller decides when to try
|
||||||
|
// again, so a camera that is down never blocks the dora event loop.
|
||||||
|
bool open();
|
||||||
void close();
|
void close();
|
||||||
|
// Close the handle and reset the device over USB, where the model allows it.
|
||||||
|
void reset();
|
||||||
void warmup();
|
void warmup();
|
||||||
bool grab();
|
bool grab();
|
||||||
|
bool is_open() const;
|
||||||
|
// True while the SDK is reconnecting the camera after a link hiccup.
|
||||||
|
bool recovering() const;
|
||||||
|
// True when the last grabbed frame carries no picture at all. A camera whose
|
||||||
|
// link has dropped does not necessarily fail its grabs: it can keep reporting
|
||||||
|
// success and hand back a uniform frame.
|
||||||
|
bool image_is_blank();
|
||||||
Calibration calibration() const;
|
Calibration calibration() const;
|
||||||
|
int64_t device_timestamp_ns();
|
||||||
sl::Mat& image();
|
sl::Mat& image();
|
||||||
|
sl::Mat& depth();
|
||||||
sl::Mat& point_cloud();
|
sl::Mat& point_cloud();
|
||||||
sl::Camera& handle();
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
Config config_;
|
Config config_;
|
||||||
@@ -45,7 +63,13 @@ private:
|
|||||||
sl::InitParameters init_params_;
|
sl::InitParameters init_params_;
|
||||||
sl::RuntimeParameters runtime_params_;
|
sl::RuntimeParameters runtime_params_;
|
||||||
sl::Mat image_mat_;
|
sl::Mat image_mat_;
|
||||||
|
sl::Mat depth_mat_;
|
||||||
sl::Mat point_cloud_mat_;
|
sl::Mat point_cloud_mat_;
|
||||||
|
// Read off the device once it is open and kept afterwards: resetting a wedged
|
||||||
|
// camera needs the serial and the model, and by then the handle is gone.
|
||||||
|
unsigned int serial_number_ = 0;
|
||||||
|
sl::MODEL model_ = sl::MODEL::LAST;
|
||||||
|
bool recovering_ = false;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace zed
|
} // namespace zed
|
||||||
|
|||||||
@@ -0,0 +1,302 @@
|
|||||||
|
#include "zed/capture.h"
|
||||||
|
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <chrono>
|
||||||
|
#include <cmath>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
using Clock = std::chrono::steady_clock;
|
||||||
|
|
||||||
|
// Going this long without a usable frame means the camera is wedged rather than
|
||||||
|
// dropping the odd frame. Measured in elapsed time rather than in failed grabs,
|
||||||
|
// because a stalling camera can spend seconds inside a single grab.
|
||||||
|
constexpr double STALL_TIMEOUT_S = 5.0;
|
||||||
|
|
||||||
|
// How long the SDK is left to reconnect the camera on its own before the loop
|
||||||
|
// stops waiting on it and rebuilds the camera anyway. A driver that says it is
|
||||||
|
// recovering is usually right, but it has no way to tell us it has given up.
|
||||||
|
constexpr double DRIVER_RECOVERY_TIMEOUT_S = 30.0;
|
||||||
|
|
||||||
|
// How long to wait between grabs while the camera is not delivering.
|
||||||
|
constexpr std::chrono::milliseconds GRAB_RETRY{500};
|
||||||
|
|
||||||
|
// How long to wait before each reopen: one entry per attempt, the last repeated
|
||||||
|
// for every attempt beyond it.
|
||||||
|
constexpr int REOPEN_BACKOFF_S[] = {1, 2, 5, 10};
|
||||||
|
|
||||||
|
std::chrono::seconds reopen_backoff(int attempt) {
|
||||||
|
const size_t index = std::min(static_cast<size_t>(attempt), std::size(REOPEN_BACKOFF_S)) - 1;
|
||||||
|
return std::chrono::seconds(REOPEN_BACKOFF_S[index]);
|
||||||
|
}
|
||||||
|
|
||||||
|
double seconds_since(Clock::time_point since) {
|
||||||
|
return std::chrono::duration<double>(Clock::now() - since).count();
|
||||||
|
}
|
||||||
|
|
||||||
|
int64_t host_timestamp_ns() {
|
||||||
|
return std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||||
|
std::chrono::system_clock::now().time_since_epoch())
|
||||||
|
.count();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Copy the depth map into a tightly packed [H, W] buffer of millimetres.
|
||||||
|
//
|
||||||
|
// The rows of an sl::Mat can be padded, so the buffer is not necessarily H * W
|
||||||
|
// floats. inf and nan mark depth the SDK could not measure; they are cleared to
|
||||||
|
// 0 here, as the reference driver does, because a hole is meaningful and a
|
||||||
|
// consumer cannot back-project a non-finite value.
|
||||||
|
void pack_depth(sl::Mat& depth_mat, std::vector<float>& out) {
|
||||||
|
const int height = depth_mat.getHeight();
|
||||||
|
const int width = depth_mat.getWidth();
|
||||||
|
const size_t step = depth_mat.getStep(sl::MEM::CPU);
|
||||||
|
const float* src = depth_mat.getPtr<sl::float1>();
|
||||||
|
|
||||||
|
out.resize(static_cast<size_t>(height) * static_cast<size_t>(width));
|
||||||
|
for (int y = 0; y < height; y++) {
|
||||||
|
const float* row = src + static_cast<size_t>(y) * step;
|
||||||
|
float* dst = out.data() + static_cast<size_t>(y) * static_cast<size_t>(width);
|
||||||
|
for (int x = 0; x < width; x++) {
|
||||||
|
dst[x] = std::isfinite(row[x]) ? row[x] : 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
namespace zed {
|
||||||
|
|
||||||
|
Capture::Capture(const Config& config) : config_(config), camera_(config) {}
|
||||||
|
|
||||||
|
Capture::~Capture() {
|
||||||
|
stop();
|
||||||
|
if (thread_.joinable()) {
|
||||||
|
// The thread may still be inside the SDK. Detaching leaks it for the few
|
||||||
|
// moments the process has left, which is preferable to blocking here.
|
||||||
|
if (wait_exit(std::chrono::seconds(1))) {
|
||||||
|
thread_.join();
|
||||||
|
} else {
|
||||||
|
thread_.detach();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Capture::start() {
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
running_ = true;
|
||||||
|
}
|
||||||
|
thread_ = std::thread(&Capture::run, this);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Capture::stop() {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
running_ = false;
|
||||||
|
wake_.notify_all();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Capture::wait_exit(std::chrono::milliseconds timeout) {
|
||||||
|
std::unique_lock<std::mutex> lock(mutex_);
|
||||||
|
return exited_.wait_for(lock, timeout, [this] { return finished_; });
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Capture::take_frame(Frame& out) {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
if (!has_frame_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
std::swap(out, latest_);
|
||||||
|
has_frame_ = false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Capture::take_state_change(std::string& out) {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
if (!state_changed_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
out = state_;
|
||||||
|
state_changed_ = false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Capture::set_state(const char* state) {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
if (state_ == state) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
state_ = state;
|
||||||
|
state_changed_ = true;
|
||||||
|
std::cout << "camera state: " << state_ << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Capture::sleep_interruptible(std::chrono::milliseconds duration) {
|
||||||
|
std::unique_lock<std::mutex> lock(mutex_);
|
||||||
|
wake_.wait_for(lock, duration, [this] { return !running_; });
|
||||||
|
}
|
||||||
|
|
||||||
|
void Capture::begin_recovery(int attempt) {
|
||||||
|
// The first attempt only closes the handle, which is enough when the driver
|
||||||
|
// rather than the device lost its way; past that the device itself is wedged
|
||||||
|
// and nothing short of resetting it will bring the frames back.
|
||||||
|
if (attempt <= 1) {
|
||||||
|
camera_.close();
|
||||||
|
} else {
|
||||||
|
camera_.reset();
|
||||||
|
}
|
||||||
|
const auto wait = reopen_backoff(attempt);
|
||||||
|
std::cerr << "recovering camera (attempt " << attempt << "); next open in " << wait.count()
|
||||||
|
<< "s" << std::endl;
|
||||||
|
sleep_interruptible(std::chrono::duration_cast<std::chrono::milliseconds>(wait));
|
||||||
|
}
|
||||||
|
|
||||||
|
void Capture::pack_frame(Frame& frame, int64_t frame_id, bool camera_info_first) {
|
||||||
|
sl::Mat& image_mat = camera_.image();
|
||||||
|
const int width = image_mat.getWidth();
|
||||||
|
const int height = image_mat.getHeight();
|
||||||
|
|
||||||
|
frame.frame_id = frame_id;
|
||||||
|
frame.device_ns = camera_.device_timestamp_ns();
|
||||||
|
frame.host_ns = host_timestamp_ns();
|
||||||
|
frame.width = width;
|
||||||
|
frame.height = height;
|
||||||
|
frame.calib = camera_.calibration();
|
||||||
|
frame.camera_info_first = camera_info_first;
|
||||||
|
|
||||||
|
cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr<sl::uchar1>(),
|
||||||
|
image_mat.getStep(sl::MEM::CPU) * sizeof(sl::uchar4));
|
||||||
|
frame.image_bgr.resize(static_cast<size_t>(height) * width * 3);
|
||||||
|
cv::Mat image_bgr(height, width, CV_8UC3, frame.image_bgr.data());
|
||||||
|
cv::cvtColor(image_rgba, image_bgr, cv::COLOR_BGRA2BGR);
|
||||||
|
|
||||||
|
pack_depth(camera_.depth(), frame.depth);
|
||||||
|
|
||||||
|
frame.has_point_cloud = config_.point_cloud;
|
||||||
|
if (!frame.has_point_cloud) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Unlike the image and depth, the cloud used to be published straight from
|
||||||
|
// the SDK buffer. It has to be copied now that the reader is another thread.
|
||||||
|
sl::Mat& pc_mat = camera_.point_cloud();
|
||||||
|
const int pc_width = pc_mat.getWidth();
|
||||||
|
const int pc_height = pc_mat.getHeight();
|
||||||
|
const size_t pc_step = pc_mat.getStep(sl::MEM::CPU);
|
||||||
|
const sl::float4* src = pc_mat.getPtr<sl::float4>();
|
||||||
|
|
||||||
|
const int stride = config_.pc_mode == PcMode::HALF ? 2 : 1;
|
||||||
|
frame.pc_width = pc_width / stride;
|
||||||
|
frame.pc_height = pc_height / stride;
|
||||||
|
frame.point_cloud.resize(static_cast<size_t>(frame.pc_height) * frame.pc_width * 4);
|
||||||
|
|
||||||
|
for (int y = 0; y < frame.pc_height; y++) {
|
||||||
|
const sl::float4* row = src + static_cast<size_t>(y * stride) * pc_step;
|
||||||
|
float* dst = frame.point_cloud.data() + static_cast<size_t>(y) * frame.pc_width * 4;
|
||||||
|
for (int x = 0; x < frame.pc_width; x++) {
|
||||||
|
const sl::float4& point = row[x * stride];
|
||||||
|
dst[x * 4 + 0] = point.x;
|
||||||
|
dst[x * 4 + 1] = point.y;
|
||||||
|
dst[x * 4 + 2] = point.z;
|
||||||
|
dst[x * 4 + 3] = point.w;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Capture::run() {
|
||||||
|
int recovery_attempts = 0;
|
||||||
|
bool streamed_once = false;
|
||||||
|
bool stall_reported = false;
|
||||||
|
bool camera_info_pending = false;
|
||||||
|
auto last_good_frame_at = Clock::now();
|
||||||
|
int64_t frame_id = 0;
|
||||||
|
|
||||||
|
set_state("opening");
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
if (!running_) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const char* down_state = streamed_once ? "recovering" : "opening";
|
||||||
|
|
||||||
|
if (!camera_.is_open()) {
|
||||||
|
set_state(down_state);
|
||||||
|
if (!camera_.open()) {
|
||||||
|
recovery_attempts++;
|
||||||
|
begin_recovery(recovery_attempts);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
// Discard the first frames so auto-exposure settles. This blocks, and
|
||||||
|
// on this thread that is fine.
|
||||||
|
camera_.warmup();
|
||||||
|
camera_info_pending = true;
|
||||||
|
// Opening the ZED takes seconds. Start the stall clock from here so
|
||||||
|
// that cost is never mistaken for a stalled stream.
|
||||||
|
last_good_frame_at = Clock::now();
|
||||||
|
// recovery_attempts is deliberately not cleared here. A wedged device
|
||||||
|
// can open and still deliver nothing; clearing on open would restart
|
||||||
|
// the escalation ladder every cycle and never reach the reset.
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// A blank frame counts as a failed grab: a camera whose link has dropped
|
||||||
|
// does not necessarily fail its grabs, it can keep reporting success and
|
||||||
|
// hand back a uniform frame with an empty depth map.
|
||||||
|
if (!camera_.grab() || camera_.image_is_blank()) {
|
||||||
|
set_state(down_state);
|
||||||
|
const double stalled_for = seconds_since(last_good_frame_at);
|
||||||
|
// The SDK holds the camera open while it reconnects a dropped link.
|
||||||
|
// Taking the handle away mid-reconnect aborts that and leaves us to
|
||||||
|
// redo it the slow way, so it gets longer than a plain stall.
|
||||||
|
const double patience =
|
||||||
|
camera_.recovering() ? DRIVER_RECOVERY_TIMEOUT_S : STALL_TIMEOUT_S;
|
||||||
|
if (stalled_for < patience) {
|
||||||
|
sleep_interruptible(GRAB_RETRY);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (!stall_reported) {
|
||||||
|
stall_reported = true;
|
||||||
|
std::cerr << "camera delivered no usable frame for " << stalled_for << "s"
|
||||||
|
<< std::endl;
|
||||||
|
}
|
||||||
|
recovery_attempts++;
|
||||||
|
begin_recovery(recovery_attempts);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (stall_reported) {
|
||||||
|
std::cout << "camera recovered after " << recovery_attempts << " attempt(s)"
|
||||||
|
<< std::endl;
|
||||||
|
stall_reported = false;
|
||||||
|
}
|
||||||
|
last_good_frame_at = Clock::now();
|
||||||
|
recovery_attempts = 0;
|
||||||
|
streamed_once = true;
|
||||||
|
|
||||||
|
pack_frame(scratch_, frame_id++, camera_info_pending);
|
||||||
|
camera_info_pending = false;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
// Swapping rather than assigning hands the previous frame's buffers
|
||||||
|
// back, so steady-state capture does not reallocate.
|
||||||
|
std::swap(latest_, scratch_);
|
||||||
|
has_frame_ = true;
|
||||||
|
}
|
||||||
|
set_state("streaming");
|
||||||
|
}
|
||||||
|
|
||||||
|
camera_.close();
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
finished_ = true;
|
||||||
|
exited_.notify_all();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace zed
|
||||||
@@ -0,0 +1,94 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "zed/camera.h"
|
||||||
|
|
||||||
|
#include <condition_variable>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <thread>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace zed {
|
||||||
|
|
||||||
|
// One captured frame, owning its buffers.
|
||||||
|
//
|
||||||
|
// The SDK reuses its own buffers on the next grab, so nothing here can point
|
||||||
|
// into an sl::Mat: the capture thread copies every payload out before handing
|
||||||
|
// the frame over.
|
||||||
|
struct Frame {
|
||||||
|
int64_t frame_id = 0;
|
||||||
|
int64_t device_ns = 0;
|
||||||
|
int64_t host_ns = 0;
|
||||||
|
int width = 0;
|
||||||
|
int height = 0;
|
||||||
|
std::vector<uint8_t> image_bgr; // height * width * 3
|
||||||
|
std::vector<float> depth; // height * width, millimetres
|
||||||
|
bool has_point_cloud = false;
|
||||||
|
int pc_width = 0;
|
||||||
|
int pc_height = 0;
|
||||||
|
std::vector<float> point_cloud; // pc_height * pc_width * 4
|
||||||
|
Calibration calib;
|
||||||
|
// Set on the first frame of a camera session, so camera_info is published
|
||||||
|
// once per session rather than per frame.
|
||||||
|
bool camera_info_first = false;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Owns the camera on its own thread and hands the newest frame to the reader.
|
||||||
|
//
|
||||||
|
// The ZED SDK can block for an unbounded time inside grab() and open() — a
|
||||||
|
// disconnected camera has been measured blocking indefinitely, even with
|
||||||
|
// InitParameters::async_grab_camera_recovery set. Driving the camera from the
|
||||||
|
// dora event loop therefore froze the node: it stopped reading events, so Stop
|
||||||
|
// went unanswered and the recovery logic itself was never reached. Everything
|
||||||
|
// that can block lives on this thread instead, and the event loop only ever
|
||||||
|
// touches the buffer below.
|
||||||
|
class Capture {
|
||||||
|
public:
|
||||||
|
explicit Capture(const Config& config);
|
||||||
|
~Capture();
|
||||||
|
|
||||||
|
void start();
|
||||||
|
// Ask the thread to finish. Returns immediately: the thread may be inside a
|
||||||
|
// blocking SDK call and cannot be interrupted there.
|
||||||
|
void stop();
|
||||||
|
// Wait for the thread to actually leave, up to timeout. False means it is
|
||||||
|
// still stuck in the SDK.
|
||||||
|
bool wait_exit(std::chrono::milliseconds timeout);
|
||||||
|
|
||||||
|
// Move the newest frame out, if one arrived since the last call. Frames that
|
||||||
|
// are not collected are overwritten: the reader always wants the latest.
|
||||||
|
bool take_frame(Frame& out);
|
||||||
|
// Report the camera state, if it changed since the last call.
|
||||||
|
bool take_state_change(std::string& out);
|
||||||
|
|
||||||
|
private:
|
||||||
|
void run();
|
||||||
|
void set_state(const char* state);
|
||||||
|
// Release the camera and wait out the backoff for this attempt. The wait is
|
||||||
|
// interruptible, so stop() is honoured while the camera is down.
|
||||||
|
void begin_recovery(int attempt);
|
||||||
|
void sleep_interruptible(std::chrono::milliseconds duration);
|
||||||
|
void pack_frame(Frame& frame, int64_t frame_id, bool camera_info_first);
|
||||||
|
|
||||||
|
Config config_;
|
||||||
|
Camera camera_;
|
||||||
|
std::thread thread_;
|
||||||
|
|
||||||
|
std::mutex mutex_;
|
||||||
|
std::condition_variable wake_;
|
||||||
|
std::condition_variable exited_;
|
||||||
|
bool running_ = false;
|
||||||
|
bool finished_ = false;
|
||||||
|
|
||||||
|
Frame latest_;
|
||||||
|
// Holds the previously published frame once the reader has taken the newest
|
||||||
|
// one, so its buffers are reused instead of reallocated every frame.
|
||||||
|
Frame scratch_;
|
||||||
|
bool has_frame_ = false;
|
||||||
|
|
||||||
|
std::string state_;
|
||||||
|
bool state_changed_ = false;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace zed
|
||||||
@@ -0,0 +1,36 @@
|
|||||||
|
// C entry points for tests/test_arrow.py.
|
||||||
|
//
|
||||||
|
// The Python side cannot call the C++ builders, so each one is wrapped here and
|
||||||
|
// the payload is passed in from the test rather than fixed in this file.
|
||||||
|
|
||||||
|
#include "arrow/array.h"
|
||||||
|
|
||||||
|
#include <cstring>
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
void hand_over(void* array, void* schema, const arrow_c::Exported& exported) {
|
||||||
|
// The caller owns the two structs, exactly as dora does: it reads them and
|
||||||
|
// is then responsible for releasing what they point at.
|
||||||
|
std::memcpy(array, &exported.array, sizeof(exported.array));
|
||||||
|
std::memcpy(schema, &exported.schema, sizeof(exported.schema));
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
|
||||||
|
void zed_export_image(void* array, void* schema, uint32_t width, uint32_t height,
|
||||||
|
const char* encoding, const uint8_t* data, size_t size) {
|
||||||
|
hand_over(array, schema, arrow_c::image(width, height, encoding, data, size));
|
||||||
|
}
|
||||||
|
|
||||||
|
void zed_export_float32(void* array, void* schema, const float* values, size_t count) {
|
||||||
|
hand_over(array, schema, arrow_c::float32(values, count));
|
||||||
|
}
|
||||||
|
|
||||||
|
void zed_export_string(void* array, void* schema, const char* text, size_t size) {
|
||||||
|
hand_over(array, schema, arrow_c::string(text, size));
|
||||||
|
}
|
||||||
|
|
||||||
|
} // extern "C"
|
||||||
@@ -0,0 +1,54 @@
|
|||||||
|
// Release has to free the whole tree: the array nodes, the schema nodes, and
|
||||||
|
// the storage their buffers point into. Nothing here asserts that directly -
|
||||||
|
// the sanitizers tests/run.sh builds this with are the assertion, and a node
|
||||||
|
// that leaks per frame is a node that dies on a long run.
|
||||||
|
|
||||||
|
#include "arrow/array.h"
|
||||||
|
|
||||||
|
#include <cassert>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
void release(arrow_c::Exported& exported) {
|
||||||
|
exported.array.release(&exported.array);
|
||||||
|
exported.schema.release(&exported.schema);
|
||||||
|
|
||||||
|
// A released struct marks itself, which is how a consumer knows not to
|
||||||
|
// release it twice.
|
||||||
|
assert(exported.array.release == nullptr);
|
||||||
|
assert(exported.schema.release == nullptr);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
int main() {
|
||||||
|
const std::vector<uint8_t> pixels(4 * 4 * 3, 7);
|
||||||
|
const std::vector<float> depth(4 * 4, 1500.0F);
|
||||||
|
const std::string state = "streaming";
|
||||||
|
|
||||||
|
for (int i = 0; i < 100; ++i) {
|
||||||
|
arrow_c::Exported image =
|
||||||
|
arrow_c::image(4, 4, "bgr8", pixels.data(), pixels.size());
|
||||||
|
assert(image.array.length == 1);
|
||||||
|
assert(image.array.n_children == 4);
|
||||||
|
release(image);
|
||||||
|
|
||||||
|
arrow_c::Exported depth_image =
|
||||||
|
arrow_c::image(4, 4, "32FC1", reinterpret_cast<const uint8_t*>(depth.data()),
|
||||||
|
depth.size() * sizeof(float));
|
||||||
|
release(depth_image);
|
||||||
|
|
||||||
|
arrow_c::Exported values = arrow_c::float32(depth.data(), depth.size());
|
||||||
|
assert(values.array.length == static_cast<int64_t>(depth.size()));
|
||||||
|
release(values);
|
||||||
|
|
||||||
|
arrow_c::Exported text = arrow_c::string(state.data(), state.size());
|
||||||
|
assert(text.array.length == 1);
|
||||||
|
release(text);
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
Executable
+29
@@ -0,0 +1,29 @@
|
|||||||
|
#!/usr/bin/env bash
|
||||||
|
# Build and run the payload tests.
|
||||||
|
#
|
||||||
|
# They cover src/arrow only, which is the part of the node that has no ZED SDK,
|
||||||
|
# no CUDA and no dora dependency - so this runs anywhere, and is what CI runs.
|
||||||
|
set -euo pipefail
|
||||||
|
|
||||||
|
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||||
|
REPO_DIR="$(dirname "$SCRIPT_DIR")"
|
||||||
|
OUT_DIR="${REPO_DIR}/build/tests"
|
||||||
|
|
||||||
|
mkdir -p "$OUT_DIR"
|
||||||
|
|
||||||
|
# Sanitized: the release callbacks have to free the whole exported tree, and a
|
||||||
|
# leak there would be one per frame.
|
||||||
|
g++ -std=c++17 -Wall -Wextra -Werror -g -fsanitize=address,undefined \
|
||||||
|
-I "${REPO_DIR}/src" \
|
||||||
|
-o "${OUT_DIR}/arrow_test" \
|
||||||
|
"${REPO_DIR}/src/arrow/array.cc" "${REPO_DIR}/tests/arrow_test.cc"
|
||||||
|
"${OUT_DIR}/arrow_test"
|
||||||
|
echo "ok arrow_test"
|
||||||
|
|
||||||
|
# Unsanitized: pyarrow is the consumer standing in for dora, and loads the same
|
||||||
|
# code as a shared library.
|
||||||
|
g++ -std=c++17 -Wall -Wextra -Werror -O2 -shared -fPIC \
|
||||||
|
-I "${REPO_DIR}/src" \
|
||||||
|
-o "${OUT_DIR}/libzed_arrow.so" \
|
||||||
|
"${REPO_DIR}/src/arrow/array.cc" "${REPO_DIR}/tests/arrow_export.cc"
|
||||||
|
uv run --no-project --with pyarrow python "${REPO_DIR}/tests/test_arrow.py"
|
||||||
@@ -0,0 +1,129 @@
|
|||||||
|
"""Round-trip every exported payload through a real Arrow consumer.
|
||||||
|
|
||||||
|
The node builds its Arrow arrays by hand against the C data interface rather
|
||||||
|
than linking Arrow C++, so this is what shows that what dora imports on the
|
||||||
|
other side of `send_arrow_output` is what went in. It needs neither a camera nor
|
||||||
|
the ZED SDK.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import ctypes
|
||||||
|
import pathlib
|
||||||
|
import sys
|
||||||
|
|
||||||
|
import pyarrow as pa
|
||||||
|
|
||||||
|
LIBRARY = pathlib.Path(__file__).resolve().parent.parent / "build" / "tests" / "libzed_arrow.so"
|
||||||
|
|
||||||
|
# ArrowArray is ten machine words and ArrowSchema nine, and both want the
|
||||||
|
# 8-byte alignment an array of uint64 gives.
|
||||||
|
WORDS = 16
|
||||||
|
# Index of the `release` field in ArrowArray: a consumer that took ownership
|
||||||
|
# zeroes it, which is how this test sees the move happen.
|
||||||
|
RELEASE = 8
|
||||||
|
|
||||||
|
IMAGE_TYPE = pa.struct([
|
||||||
|
("width", pa.uint32()),
|
||||||
|
("height", pa.uint32()),
|
||||||
|
("encoding", pa.utf8()),
|
||||||
|
("data", pa.large_binary()),
|
||||||
|
])
|
||||||
|
|
||||||
|
|
||||||
|
def load():
|
||||||
|
"""Load the test library and declare the argument types of its exports."""
|
||||||
|
library = ctypes.CDLL(str(LIBRARY))
|
||||||
|
|
||||||
|
library.zed_export_image.argtypes = [
|
||||||
|
ctypes.c_void_p, ctypes.c_void_p, ctypes.c_uint32, ctypes.c_uint32,
|
||||||
|
ctypes.c_char_p, ctypes.c_char_p, ctypes.c_size_t,
|
||||||
|
]
|
||||||
|
library.zed_export_float32.argtypes = [
|
||||||
|
ctypes.c_void_p, ctypes.c_void_p, ctypes.POINTER(ctypes.c_float), ctypes.c_size_t,
|
||||||
|
]
|
||||||
|
library.zed_export_string.argtypes = [
|
||||||
|
ctypes.c_void_p, ctypes.c_void_p, ctypes.c_char_p, ctypes.c_size_t,
|
||||||
|
]
|
||||||
|
return library
|
||||||
|
|
||||||
|
|
||||||
|
def structs():
|
||||||
|
"""Allocate the ArrowArray and ArrowSchema the producer fills in."""
|
||||||
|
return (ctypes.c_uint64 * WORDS)(), (ctypes.c_uint64 * WORDS)()
|
||||||
|
|
||||||
|
|
||||||
|
def imported(array, schema):
|
||||||
|
"""Import the pair as pyarrow does, taking ownership of both."""
|
||||||
|
result = pa.Array._import_from_c(ctypes.addressof(array), ctypes.addressof(schema))
|
||||||
|
assert array[RELEASE] == 0, "the consumer should have taken ownership of the array"
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def test_image(library):
|
||||||
|
"""An image row carries its own dimensions, encoding and bytes."""
|
||||||
|
array, schema = structs()
|
||||||
|
pixels = bytes(range(12))
|
||||||
|
|
||||||
|
library.zed_export_image(
|
||||||
|
ctypes.byref(array), ctypes.byref(schema), 2, 2, b"bgr8", pixels, len(pixels)
|
||||||
|
)
|
||||||
|
result = imported(array, schema)
|
||||||
|
|
||||||
|
assert result.type == IMAGE_TYPE, result.type
|
||||||
|
assert result.to_pylist() == [
|
||||||
|
{"width": 2, "height": 2, "encoding": "bgr8", "data": pixels}
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def test_depth(library):
|
||||||
|
"""A depth map is an image whose bytes are float32 millimetres."""
|
||||||
|
array, schema = structs()
|
||||||
|
millimetres = [1500.0, 0.0, 250.5, 4000.0]
|
||||||
|
payload = pa.array(millimetres, type=pa.float32()).buffers()[1].to_pybytes()
|
||||||
|
|
||||||
|
library.zed_export_image(
|
||||||
|
ctypes.byref(array), ctypes.byref(schema), 2, 2, b"32FC1", payload, len(payload)
|
||||||
|
)
|
||||||
|
row = imported(array, schema).to_pylist()[0]
|
||||||
|
|
||||||
|
assert row["encoding"] == "32FC1"
|
||||||
|
assert pa.Array.from_buffers(
|
||||||
|
pa.float32(), 4, [None, pa.py_buffer(row["data"])]
|
||||||
|
).to_pylist() == millimetres
|
||||||
|
|
||||||
|
|
||||||
|
def test_float32(library):
|
||||||
|
"""Intrinsics and the point cloud go out as plain float32 values."""
|
||||||
|
array, schema = structs()
|
||||||
|
values = (ctypes.c_float * 4)(700.5, 700.5, 640.0, 360.0)
|
||||||
|
|
||||||
|
library.zed_export_float32(ctypes.byref(array), ctypes.byref(schema), values, 4)
|
||||||
|
result = imported(array, schema)
|
||||||
|
|
||||||
|
assert result.type == pa.float32()
|
||||||
|
assert result.to_pylist() == [700.5, 700.5, 640.0, 360.0]
|
||||||
|
|
||||||
|
|
||||||
|
def test_string(library):
|
||||||
|
"""The camera state goes out as one UTF-8 string."""
|
||||||
|
array, schema = structs()
|
||||||
|
|
||||||
|
library.zed_export_string(ctypes.byref(array), ctypes.byref(schema), b"streaming", 9)
|
||||||
|
result = imported(array, schema)
|
||||||
|
|
||||||
|
assert result.type == pa.utf8()
|
||||||
|
assert result.to_pylist() == ["streaming"]
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
"""Run every test, reporting the first failure."""
|
||||||
|
library = load()
|
||||||
|
|
||||||
|
for test in (test_image, test_depth, test_float32, test_string):
|
||||||
|
test(library)
|
||||||
|
print(f"ok {test.__name__}")
|
||||||
|
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
sys.exit(main())
|
||||||
Reference in New Issue
Block a user