liria/dora-zed
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity

add native 3D object detection using ZED SDK optimized models

Browse Source 
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
This commit is contained in:
cristhian aguilera
2026-03-26 10:53:38 -03:00
parent 55a87d3a14
commit 6e6dc8ef27
10 changed files with 303 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main.cc
+74 -6
View File
@@ -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;