add native 3D object detection using ZED SDK optimized models

Add engines::ZedNative class wrapping ZED SDK's TensorRT-optimized
object detection. On every frame grab, detected objects (label,
confidence, 3D position, 2D/3D bounding boxes) are serialized as a
flat float32 array and sent as a new "objects" dora output with
frame_id correlation to image_bgr.

Extract shared env-var helpers into src/util/env.h and expose
sl::Camera& via Camera::handle() for engine access.

Closes #3

src/engines/zed_native.cc

@@ -0,0 +1,95 @@
+#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_;
+}
+
+} // namespace engines

src/engines/zed_native.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <sl/Camera.hpp>
+#include <string>
+#include <vector>
+
+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<DetectedObject>& objects() const;
+
+private:
+    DetectionConfig config_;
+    sl::ObjectDetectionParameters det_params_;
+    sl::ObjectDetectionRuntimeParameters det_rt_params_;
+    sl::Objects raw_objects_;
+    std::vector<DetectedObject> detected_;
+};
+
+} // namespace engines

src/main.cc

@@ -1,15 +1,20 @@
+#include "engines/zed_native.h"
 #include "zed/camera.h"
 
 #include <dora-node-api.h>
 #include <opencv2/opencv.hpp>
 
 #include <chrono>
+#include <cstring>
 #include <initializer_list>
 #include <iostream>
 #include <string>
+#include <vector>
 
 namespace {
 
+constexpr int FIELDS_PER_OBJECT = 37;
+
 rust::Vec<int64_t> make_shape(int height, int width, int channels) {
     rust::Vec<int64_t> shape;
     shape.push_back(height);
@@ -34,6 +39,22 @@ rust::Vec<int64_t> make_vector_int(std::initializer_list<int64_t> values) {
     return out;
 }
 
+std::vector<float> serialize_objects(const std::vector<engines::DetectedObject>& objects) {
+    std::vector<float> 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<float>(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<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);
@@ -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<int64_t>(
-            std::chrono::duration_cast<std::chrono::nanoseconds>(
-                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<size_t>(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<const uint8_t> cam_slice{
             reinterpret_cast<const uint8_t*>(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<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",
+            "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;

src/util/env.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include <cctype>
+#include <cstdlib>
+#include <string>
+
+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<char>(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

src/zed/camera.cc

@@ -1,8 +1,7 @@
 #include "zed/camera.h"
+#include "util/env.h"
 
-#include <cctype>
 #include <chrono>
-#include <cstdlib>
 #include <iostream>
 #include <iterator>
 #include <string>
@@ -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<char>(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

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_;