diff --git a/AGENTS.md b/AGENTS.md index 286d639..b9d0158 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -4,7 +4,7 @@ This file provides guidance to AI coding assistants when working with code in th ## What this is -A dora-rs C++ node that interfaces with ZED stereo cameras. On each tick it grabs a frame and publishes three outputs over the dora dataflow: a BGR image, an XYZRGBA point cloud, and camera intrinsics. Camera management lives in `src/zed/` and the dora event loop in `src/main.cc`. +A dora-rs C++ node that interfaces with ZED stereo cameras. On each tick it grabs a frame, 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`. ## Build @@ -25,22 +25,29 @@ dora up dora start dataflow.yml ``` -Camera behavior is configured entirely through environment variables (see README.md for the full list: `ZED_RESOLUTION`, `ZED_FPS`, `ZED_DEPTH_MODE`, etc.). +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.). ## Architecture -- **`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. -- **`src/zed/camera.cc`** — Implementation. Env-variable parsing helpers and SDK enum parsers live in an anonymous namespace. -- **`src/main.cc`** — Dora event loop. Receives `tick` inputs, calls `camera.grab()`, and sends `image_bgr`, `point_cloud`, and `camera_info` outputs with metadata. Handles reconnection on grab failure. +- **`src/util/env.h`** — Shared inline env-variable parsing helpers (`get_env_int`, `get_env_str`, `get_env_bool`, `get_env_float`). +- **`src/zed/camera.h`** — `zed::Config`, `zed::Calibration`, and `zed::Camera` class. Camera wraps the ZED SDK lifecycle: open (with retry loop), warmup, grab (image + point cloud), calibration, and close. Exposes `handle()` for engine access. +- **`src/zed/camera.cc`** — Implementation. SDK enum parsers live in an anonymous namespace. +- **`src/engines/zed_native.h`** — `engines::DetectionConfig`, `engines::DetectedObject`, and `engines::ZedNative` class. Wraps ZED SDK's TensorRT-optimized object detection module. +- **`src/engines/zed_native.cc`** — Implementation. Enables detection on the camera, retrieves objects per frame, converts to `DetectedObject` structs. +- **`src/main.cc`** — Dora event loop. Receives `tick` inputs, calls `camera.grab()` and `detector.detect()`, then sends `image_bgr`, `point_cloud`, `camera_info`, and `objects` outputs with shared `frame_id`. 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. - **`dataflow.yml`** — Example dora dataflow definition using the `git:` source format. ## Key APIs - **dora C++ API** (``): `init_dora_node()`, `dora_node.events->next()`, `event_type()`, `send_output_with_metadata()`, `new_metadata()`. -- **ZED SDK** (``): `sl::Camera`, `sl::InitParameters`, `sl::RuntimeParameters`, `sl::Mat`. +- **ZED SDK** (``): `sl::Camera`, `sl::InitParameters`, `sl::RuntimeParameters`, `sl::Mat`, `sl::ObjectDetectionParameters`, `sl::Objects`. - Output data is sent as raw byte slices (`rust::Slice`) with typed metadata (shape, dtype, encoding). +## Tools + +- **Issue tracker:** Gitea. Use the Gitea MCP tools (`mcp__gitea__*`) to read and manage issues, pull requests, and other repository operations. + ## Rules - Do not mention AI tools in commit messages or documentation. diff --git a/CMakeLists.txt b/CMakeLists.txt index 3860e55..e59b311 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(OpenCV REQUIRED) link_directories(${dora_link_dirs}) link_directories(${ZED_LIBRARY_DIR}) -add_executable(dora_zed src/main.cc src/zed/camera.cc ${node_bridge}) +add_executable(dora_zed src/main.cc src/zed/camera.cc src/engines/zed_native.cc ${node_bridge}) add_dependencies(dora_zed Dora_cxx) target_include_directories( diff --git a/README.md b/README.md index 671baa0..49c99ea 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # dora-zed -Dora C++ node for ZED stereo cameras. Captures frames and publishes BGR images, XYZRGBA point clouds, and camera intrinsics. +Dora C++ node for ZED stereo cameras. Captures frames and publishes BGR images, XYZRGBA point clouds, camera intrinsics, and 3D object detections. ## Usage via `git:` in a dataflow @@ -16,6 +16,7 @@ nodes: - image_bgr - point_cloud - camera_info + - objects ``` Then run: @@ -61,6 +62,9 @@ The binary is installed to `bin/dora_zed` relative to the repository root. | `ZED_FLIP` | `ON` | Image flip: `ON` or `OFF` | | `ZED_WARMUP_FRAMES` | `30` | Number of warmup frames to discard | | `ZED_RETRY_INTERVAL_SECONDS` | `5` | Seconds between reconnection attempts | +| `ZED_DETECTION_MODEL` | `MULTI_CLASS_BOX_MEDIUM` | Detection model: `MULTI_CLASS_BOX_FAST`, `MULTI_CLASS_BOX_MEDIUM`, or `MULTI_CLASS_BOX_ACCURATE` | +| `ZED_DETECTION_CONFIDENCE` | `40` | Detection confidence threshold (0-100) | +| `ZED_DETECTION_TRACKING` | `false` | Enable object tracking across frames (enables positional tracking) | ## Outputs @@ -72,3 +76,16 @@ XYZRGBA point cloud as float32 bytes. Metadata includes `shape` (HxWx4), `units` ### `camera_info` Four float32 values: `[fx, fy, cx, cy]`. Metadata includes `distortion` coefficients. + +### `objects` +Detected objects as a flat float32 array with shape `[num_objects, 37]`. Each row contains 37 floats: + +| Index | Field | Description | +|---|---|---| +| 0 | `label_id` | Object class (0=PERSON, 1=VEHICLE, 2=BAG, 3=ANIMAL, 4=ELECTRONICS, 5=FRUIT_VEGETABLE, 6=SPORT) | +| 1 | `confidence` | Detection confidence (0-100) | +| 2-4 | `x, y, z` | 3D center position in mm | +| 5-12 | `bb2d_*` | 2D bounding box: 4 corners × 2 pixel coords | +| 13-36 | `bb3d_*` | 3D bounding box: 8 corners × 3 world coords (mm) | + +Metadata includes `frame_id` and `timestamp_ns` matching the corresponding `image_bgr`, plus `image_width` and `image_height` for interpreting 2D bounding box pixel coordinates. When no objects are detected, an empty array with shape `[0, 37]` is sent. diff --git a/dataflow.yml b/dataflow.yml index 3d8faaf..fecd9a3 100644 --- a/dataflow.yml +++ b/dataflow.yml @@ -9,3 +9,4 @@ nodes: - image_bgr - point_cloud - camera_info + - objects diff --git a/src/engines/zed_native.cc b/src/engines/zed_native.cc new file mode 100644 index 0000000..d475e01 --- /dev/null +++ b/src/engines/zed_native.cc @@ -0,0 +1,95 @@ +#include "engines/zed_native.h" +#include "util/env.h" + +#include +#include + +namespace { + +sl::OBJECT_DETECTION_MODEL parse_detection_model(const std::string& model) { + if (model == "MULTI_CLASS_BOX_FAST") { + return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_FAST; + } + if (model == "MULTI_CLASS_BOX_ACCURATE") { + return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_ACCURATE; + } + return sl::OBJECT_DETECTION_MODEL::MULTI_CLASS_BOX_MEDIUM; +} + +} // namespace + +namespace engines { + +DetectionConfig DetectionConfig::from_env() { + DetectionConfig c; + c.model = util::get_env_str("ZED_DETECTION_MODEL", c.model); + c.confidence_threshold = util::get_env_float("ZED_DETECTION_CONFIDENCE", c.confidence_threshold); + c.tracking = util::get_env_bool("ZED_DETECTION_TRACKING", c.tracking); + return c; +} + +ZedNative::ZedNative(const DetectionConfig& config) : config_(config) { + det_params_.detection_model = parse_detection_model(config_.model); + det_params_.enable_tracking = config_.tracking; + det_params_.enable_segmentation = false; + + det_rt_params_.detection_confidence_threshold = config_.confidence_threshold; +} + +void ZedNative::enable(sl::Camera& camera) { + if (config_.tracking) { + sl::PositionalTrackingParameters tracking_params; + sl::ERROR_CODE err = camera.enablePositionalTracking(tracking_params); + if (err != sl::ERROR_CODE::SUCCESS) { + std::cerr << "Failed to enable positional tracking: " + << sl::toString(err) << std::endl; + } + } + + std::cout << "Object Detection: loading model (" << config_.model << ")..." << std::endl; + sl::ERROR_CODE err = camera.enableObjectDetection(det_params_); + if (err != sl::ERROR_CODE::SUCCESS) { + std::cerr << "Failed to enable object detection: " + << sl::toString(err) << std::endl; + return; + } + std::cout << "Object Detection: model loaded." << std::endl; +} + +void ZedNative::detect(sl::Camera& camera) { + detected_.clear(); + + sl::ERROR_CODE err = camera.retrieveObjects(raw_objects_, det_rt_params_); + if (err != sl::ERROR_CODE::SUCCESS) { + return; + } + + for (const auto& obj : raw_objects_.object_list) { + DetectedObject det{}; + det.label_id = static_cast(obj.label); + det.confidence = obj.confidence; + + det.position[0] = obj.position.x; + det.position[1] = obj.position.y; + det.position[2] = obj.position.z; + + for (int i = 0; i < 4 && i < static_cast(obj.bounding_box_2d.size()); i++) { + det.bounding_box_2d[i * 2 + 0] = obj.bounding_box_2d[i].x; + det.bounding_box_2d[i * 2 + 1] = obj.bounding_box_2d[i].y; + } + + for (int i = 0; i < 8 && i < static_cast(obj.bounding_box.size()); i++) { + det.bounding_box_3d[i * 3 + 0] = obj.bounding_box[i].x; + det.bounding_box_3d[i * 3 + 1] = obj.bounding_box[i].y; + det.bounding_box_3d[i * 3 + 2] = obj.bounding_box[i].z; + } + + detected_.push_back(det); + } +} + +const std::vector& ZedNative::objects() const { + return detected_; +} + +} // namespace engines diff --git a/src/engines/zed_native.h b/src/engines/zed_native.h new file mode 100644 index 0000000..92db3dd --- /dev/null +++ b/src/engines/zed_native.h @@ -0,0 +1,41 @@ +#pragma once + +#include +#include +#include + +namespace engines { + +struct DetectionConfig { + std::string model = "MULTI_CLASS_BOX_MEDIUM"; + float confidence_threshold = 40.0f; + bool tracking = false; + + static DetectionConfig from_env(); +}; + +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 ZedNative { +public: + explicit ZedNative(const DetectionConfig& config); + + void enable(sl::Camera& camera); + void detect(sl::Camera& camera); + const std::vector& objects() const; + +private: + DetectionConfig config_; + sl::ObjectDetectionParameters det_params_; + sl::ObjectDetectionRuntimeParameters det_rt_params_; + sl::Objects raw_objects_; + std::vector detected_; +}; + +} // namespace engines diff --git a/src/main.cc b/src/main.cc index 1c781da..50e1dbc 100644 --- a/src/main.cc +++ b/src/main.cc @@ -1,15 +1,20 @@ +#include "engines/zed_native.h" #include "zed/camera.h" #include #include #include +#include #include #include #include +#include namespace { +constexpr int FIELDS_PER_OBJECT = 37; + rust::Vec make_shape(int height, int width, int channels) { rust::Vec shape; shape.push_back(height); @@ -34,6 +39,22 @@ rust::Vec make_vector_int(std::initializer_list values) { return out; } +std::vector serialize_objects(const std::vector& objects) { + std::vector buffer(objects.size() * FIELDS_PER_OBJECT); + for (size_t i = 0; i < objects.size(); i++) { + float* row = buffer.data() + i * FIELDS_PER_OBJECT; + const auto& obj = objects[i]; + row[0] = static_cast(obj.label_id); + row[1] = obj.confidence; + row[2] = obj.position[0]; + row[3] = obj.position[1]; + row[4] = obj.position[2]; + std::memcpy(row + 5, obj.bounding_box_2d, sizeof(obj.bounding_box_2d)); + std::memcpy(row + 13, obj.bounding_box_3d, sizeof(obj.bounding_box_3d)); + } + return buffer; +} + } // namespace int main() { @@ -45,6 +66,10 @@ int main() { camera.warmup(); auto calib = camera.calibration(); + auto det_config = engines::DetectionConfig::from_env(); + engines::ZedNative detector(det_config); + detector.enable(camera.handle()); + int64_t frame_id = 0; while (true) { auto event = dora_node.events->next(); @@ -66,15 +91,26 @@ int main() { camera.close(); camera.open(); calib = camera.calibration(); + detector.enable(camera.handle()); continue; } + detector.detect(camera.handle()); + sl::Mat& image_mat = camera.image(); sl::Mat& point_cloud_mat = camera.point_cloud(); int width = image_mat.getWidth(); int height = image_mat.getHeight(); + int64_t current_frame_id = frame_id++; + int64_t timestamp_ns = static_cast( + std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch() + ).count() + ); + + // --- image_bgr --- cv::Mat image_rgba(height, width, CV_8UC4, image_mat.getPtr()); cv::Mat image_bgr; cv::cvtColor(image_rgba, image_bgr, cv::COLOR_BGRA2BGR); @@ -93,12 +129,8 @@ int main() { 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", frame_id++); - image_metadata->set_int("timestamp_ns", static_cast( - std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch() - ).count() - )); + image_metadata->set_int("frame_id", current_frame_id); + image_metadata->set_int("timestamp_ns", timestamp_ns); auto image_result = send_output_with_metadata( dora_node.send_output, "image_bgr", image_slice, std::move(image_metadata)); @@ -106,6 +138,7 @@ int main() { std::cerr << "Error sending image_bgr: " << std::string(image_result.error) << std::endl; } + // --- point_cloud --- int pc_width = point_cloud_mat.getWidth(); int pc_height = point_cloud_mat.getHeight(); const size_t pc_size = static_cast(pc_width * pc_height * sizeof(sl::float4)); @@ -130,6 +163,7 @@ int main() { std::cerr << "Error sending point_cloud: " << std::string(pc_result.error) << std::endl; } + // --- camera_info --- float camera_data[4] = {calib.fx, calib.fy, calib.cx, calib.cy}; rust::Slice cam_slice{ reinterpret_cast(camera_data), sizeof(camera_data)}; @@ -145,6 +179,40 @@ int main() { if (!std::string(cam_result.error).empty()) { std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl; } + + // --- objects --- + const auto& objects = detector.objects(); + auto obj_buffer = serialize_objects(objects); + int num_objects = static_cast(objects.size()); + + const size_t obj_size = obj_buffer.size() * sizeof(float); + rust::Slice obj_slice{ + reinterpret_cast(obj_buffer.data()), obj_size}; + + auto obj_metadata = new_metadata(); + obj_metadata->set_string("dtype", "float32"); + obj_metadata->set_string("layout", "RC"); + obj_metadata->set_string("units", "mm"); + obj_metadata->set_list_int("shape", make_vector_int({num_objects, FIELDS_PER_OBJECT})); + obj_metadata->set_string("fields", + "label_id,confidence,x,y,z," + "bb2d_x0,bb2d_y0,bb2d_x1,bb2d_y1,bb2d_x2,bb2d_y2,bb2d_x3,bb2d_y3," + "bb3d_x0,bb3d_y0,bb3d_z0,bb3d_x1,bb3d_y1,bb3d_z1," + "bb3d_x2,bb3d_y2,bb3d_z2,bb3d_x3,bb3d_y3,bb3d_z3," + "bb3d_x4,bb3d_y4,bb3d_z4,bb3d_x5,bb3d_y5,bb3d_z5," + "bb3d_x6,bb3d_y6,bb3d_z6,bb3d_x7,bb3d_y7,bb3d_z7"); + obj_metadata->set_string("label_names", + "PERSON,VEHICLE,BAG,ANIMAL,ELECTRONICS,FRUIT_VEGETABLE,SPORT"); + 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; + } } return 0; diff --git a/src/util/env.h b/src/util/env.h new file mode 100644 index 0000000..efe182e --- /dev/null +++ b/src/util/env.h @@ -0,0 +1,45 @@ +#pragma once + +#include +#include +#include + +namespace util { + +inline int get_env_int(const char* name, int default_value) { + const char* value = std::getenv(name); + if (!value || std::string(value).empty()) { + return default_value; + } + return std::stoi(value); +} + +inline bool get_env_bool(const char* name, bool default_value) { + const char* value = std::getenv(name); + if (!value || std::string(value).empty()) { + return default_value; + } + std::string v(value); + for (auto& c : v) { + c = static_cast(std::tolower(c)); + } + return v == "1" || v == "true" || v == "yes" || v == "on"; +} + +inline std::string get_env_str(const char* name, const std::string& default_value) { + const char* value = std::getenv(name); + if (!value || std::string(value).empty()) { + return default_value; + } + return std::string(value); +} + +inline float get_env_float(const char* name, float default_value) { + const char* value = std::getenv(name); + if (!value || std::string(value).empty()) { + return default_value; + } + return std::stof(value); +} + +} // namespace util diff --git a/src/zed/camera.cc b/src/zed/camera.cc index 234cdf5..ecaaa9b 100644 --- a/src/zed/camera.cc +++ b/src/zed/camera.cc @@ -1,8 +1,7 @@ #include "zed/camera.h" +#include "util/env.h" -#include #include -#include #include #include #include @@ -10,34 +9,6 @@ namespace { -int get_env_int(const char* name, int default_value) { - const char* value = std::getenv(name); - if (!value || std::string(value).empty()) { - return default_value; - } - return std::stoi(value); -} - -bool get_env_bool(const char* name, bool default_value) { - const char* value = std::getenv(name); - if (!value || std::string(value).empty()) { - return default_value; - } - std::string v(value); - for (auto& c : v) { - c = static_cast(std::tolower(c)); - } - return v == "1" || v == "true" || v == "yes" || v == "on"; -} - -std::string get_env_str(const char* name, const std::string& default_value) { - const char* value = std::getenv(name); - if (!value || std::string(value).empty()) { - return default_value; - } - return std::string(value); -} - sl::RESOLUTION parse_resolution(int resolution) { switch (resolution) { case 1080: @@ -86,15 +57,15 @@ namespace zed { Config Config::from_env() { Config c; - c.resolution = get_env_int("ZED_RESOLUTION", c.resolution); - c.fps = get_env_int("ZED_FPS", c.fps); - c.depth_mode = get_env_str("ZED_DEPTH_MODE", c.depth_mode); - c.depth_min_mm = get_env_int("ZED_DEPTH_MIN_MM", c.depth_min_mm); - c.depth_max_mm = get_env_int("ZED_DEPTH_MAX_MM", c.depth_max_mm); - c.depth_fill = get_env_bool("ZED_DEPTH_FILL", c.depth_fill); - c.flip = get_env_str("ZED_FLIP", c.flip); - c.warmup_frames = get_env_int("ZED_WARMUP_FRAMES", c.warmup_frames); - int retry_seconds = get_env_int("ZED_RETRY_INTERVAL_SECONDS", 5); + c.resolution = util::get_env_int("ZED_RESOLUTION", c.resolution); + c.fps = util::get_env_int("ZED_FPS", c.fps); + c.depth_mode = util::get_env_str("ZED_DEPTH_MODE", c.depth_mode); + c.depth_min_mm = util::get_env_int("ZED_DEPTH_MIN_MM", c.depth_min_mm); + c.depth_max_mm = util::get_env_int("ZED_DEPTH_MAX_MM", c.depth_max_mm); + c.depth_fill = util::get_env_bool("ZED_DEPTH_FILL", c.depth_fill); + c.flip = util::get_env_str("ZED_FLIP", c.flip); + c.warmup_frames = util::get_env_int("ZED_WARMUP_FRAMES", c.warmup_frames); + int retry_seconds = util::get_env_int("ZED_RETRY_INTERVAL_SECONDS", 5); c.retry_interval_ms = retry_seconds * 1000; return c; } @@ -172,4 +143,8 @@ sl::Mat& Camera::point_cloud() { return point_cloud_mat_; } +sl::Camera& Camera::handle() { + return zed_; +} + } // namespace zed diff --git a/src/zed/camera.h b/src/zed/camera.h index 01785d4..2b2f2a4 100644 --- a/src/zed/camera.h +++ b/src/zed/camera.h @@ -37,6 +37,7 @@ public: Calibration calibration() const; sl::Mat& image(); sl::Mat& point_cloud(); + sl::Camera& handle(); private: Config config_;