add open-vocab YOLO-World engine with CLIP text encoder

- Add OpenVocab engine: YOLO-World ONNX detection with runtime
  vocabulary via CLIP text embeddings (two-input model support)
- Add header-only C++ CLIP BPE tokenizer (no external deps)
- Add detect(cv::Mat) overload to OpenVocab for camera-free testing
- Add standalone test binary (test_open_vocab) using test/assets images
- Add export scripts for YOLO-World and CLIP text encoder (uv/PEP 723)
- Add installation scripts for Linux x64, ARM (Jetson), and Windows
- Update CMakeLists to use find_package(onnxruntime) instead of FetchContent
- Update README and AGENTS with env vars, setup workflow, and test usage
This commit is contained in:
cristhian aguilera
2026-03-28 10:41:11 -03:00
parent 6e6dc8ef27
commit fd6993e294
18 changed files with 1611 additions and 37 deletions
+27
View File
@@ -0,0 +1,27 @@
#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
+587
View File
@@ -0,0 +1,587 @@
#include "engines/open_vocab.h"
#include "util/clip_tokenizer.h"
#include "util/env.h"
#include <onnxruntime_cxx_api.h>
#include <opencv2/opencv.hpp>
#include <sl/Camera.hpp>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <numeric>
#include <sstream>
#include <string>
#include <vector>
namespace {
// --- NMS helpers ---
struct RawDetection {
float x1, y1, x2, y2;
float score;
int class_id;
};
float iou(const RawDetection& a, const RawDetection& b) {
float inter_x1 = std::max(a.x1, b.x1);
float inter_y1 = std::max(a.y1, b.y1);
float inter_x2 = std::min(a.x2, b.x2);
float inter_y2 = std::min(a.y2, b.y2);
float inter_w = std::max(0.0f, inter_x2 - inter_x1);
float inter_h = std::max(0.0f, inter_y2 - inter_y1);
float inter_area = inter_w * inter_h;
float area_a = (a.x2 - a.x1) * (a.y2 - a.y1);
float area_b = (b.x2 - b.x1) * (b.y2 - b.y1);
float union_area = area_a + area_b - inter_area;
return union_area > 0.0f ? inter_area / union_area : 0.0f;
}
std::vector<RawDetection> nms(std::vector<RawDetection> dets, float threshold) {
std::sort(dets.begin(), dets.end(),
[](const RawDetection& a, const RawDetection& b) {
return a.score > b.score;
});
std::vector<bool> suppressed(dets.size(), false);
std::vector<RawDetection> result;
for (size_t i = 0; i < dets.size(); i++) {
if (suppressed[i]) continue;
result.push_back(dets[i]);
for (size_t j = i + 1; j < dets.size(); j++) {
if (!suppressed[j] && iou(dets[i], dets[j]) > threshold) {
suppressed[j] = true;
}
}
}
return result;
}
// --- Letterbox resize ---
cv::Mat letterbox(const cv::Mat& src, int size, float& scale, float& pad_x, float& pad_y) {
float scale_w = static_cast<float>(size) / src.cols;
float scale_h = static_cast<float>(size) / src.rows;
scale = std::min(scale_w, scale_h);
int new_w = static_cast<int>(std::round(src.cols * scale));
int new_h = static_cast<int>(std::round(src.rows * scale));
pad_x = (size - new_w) / 2.0f;
pad_y = (size - new_h) / 2.0f;
cv::Mat resized;
cv::resize(src, resized, {new_w, new_h});
cv::Mat canvas(size, size, CV_8UC3, cv::Scalar(114, 114, 114));
resized.copyTo(canvas(cv::Rect(
static_cast<int>(pad_x), static_cast<int>(pad_y), new_w, new_h)));
return canvas;
}
// --- Point cloud fusion ---
struct FusionResult {
float cx, cy, cz;
float depth_near, depth_far;
bool valid = false;
};
FusionResult fuse_bbox(const sl::Mat& pc, int x1, int y1, int x2, int y2) {
FusionResult result{};
int w = static_cast<int>(pc.getWidth());
int h = static_cast<int>(pc.getHeight());
x1 = std::max(x1, 0);
y1 = std::max(y1, 0);
x2 = std::min(x2, w - 1);
y2 = std::min(y2, h - 1);
std::vector<float> xs, ys, zs;
xs.reserve((x2 - x1) * (y2 - y1));
ys.reserve(xs.capacity());
zs.reserve(xs.capacity());
for (int y = y1; y <= y2; y++) {
for (int x = x1; x <= x2; x++) {
sl::float4 pt;
pc.getValue(x, y, &pt);
if (!std::isnan(pt.x) && !std::isnan(pt.y) && !std::isnan(pt.z) &&
!std::isinf(pt.x) && !std::isinf(pt.y) && !std::isinf(pt.z)) {
xs.push_back(pt.x);
ys.push_back(pt.y);
zs.push_back(pt.z);
}
}
}
if (xs.empty()) return result;
std::sort(xs.begin(), xs.end());
std::sort(ys.begin(), ys.end());
std::sort(zs.begin(), zs.end());
size_t n = xs.size();
size_t mid = n / 2;
size_t p25 = n / 4;
size_t p75 = 3 * n / 4;
result.cx = xs[mid];
result.cy = ys[mid];
result.cz = zs[mid];
result.depth_near = zs[p25];
result.depth_far = zs[p75];
result.valid = true;
return result;
}
void backproject(float u, float v, float z, float fx, float fy, float cx, float cy,
float& X, float& Y, float& Z) {
Z = z;
X = (u - cx) * z / fx;
Y = (v - cy) * z / fy;
}
// Split a comma-separated string into a vector of trimmed tokens
std::vector<std::string> split_labels(const std::string& s) {
std::vector<std::string> out;
std::stringstream ss(s);
std::string tok;
while (std::getline(ss, tok, ',')) {
size_t l = tok.find_first_not_of(" \t");
size_t r = tok.find_last_not_of(" \t");
if (l != std::string::npos)
out.push_back(tok.substr(l, r - l + 1));
}
return out;
}
} // namespace
namespace engines {
// --- Impl ---
struct OpenVocab::Impl {
// Visual model
Ort::Env env{ORT_LOGGING_LEVEL_WARNING, "open_vocab"};
Ort::SessionOptions session_options;
std::unique_ptr<Ort::Session> session;
Ort::AllocatorWithDefaultOptions allocator;
std::string input_name; // visual model: image input
std::string output_name; // visual model: detection output
std::string text_input_name; // visual model: text_feats input (empty if single-input model)
int num_classes = 0;
bool has_text_input = false;
// Text encoder (optional; used when has_text_input == true)
Ort::SessionOptions text_session_options;
std::unique_ptr<Ort::Session> text_session;
std::string text_enc_input_name; // "input_ids"
std::string text_enc_output_name; // "text_embeds"
// Tokenizer + embedding cache
util::ClipTokenizer tokenizer;
std::vector<float> cached_text_feats; // (num_classes * 512) flat, row-major
std::string cached_labels;
// Preprocessing + ONNX inference + NMS.
// Takes a BGR image (cv::imread format); returns 2D detections in original image coords.
std::vector<RawDetection> run_inference(const OpenVocabConfig& config,
const cv::Mat& image_bgr);
};
// --- Impl::run_inference ---
std::vector<RawDetection> OpenVocab::Impl::run_inference(
const OpenVocabConfig& config, const cv::Mat& image_bgr)
{
if (!session) return {};
if (has_text_input && cached_text_feats.empty()) return {};
int orig_w = image_bgr.cols;
int orig_h = image_bgr.rows;
// BGR → RGB (YOLO expects RGB)
cv::Mat rgb;
cv::cvtColor(image_bgr, rgb, cv::COLOR_BGR2RGB);
float scale, pad_x, pad_y;
cv::Mat lb = letterbox(rgb, config.input_size, scale, pad_x, pad_y);
cv::Mat lb_float;
lb.convertTo(lb_float, CV_32F, 1.0f / 255.0f);
int sz = config.input_size;
std::vector<float> input_data(3 * sz * sz);
for (int c = 0; c < 3; c++) {
for (int h = 0; h < sz; h++) {
for (int w = 0; w < sz; w++) {
input_data[c * sz * sz + h * sz + w] =
lb_float.at<cv::Vec3f>(h, w)[c];
}
}
}
auto mem_info = Ort::MemoryInfo::CreateCpu(
OrtAllocatorType::OrtArenaAllocator, OrtMemTypeDefault);
std::vector<int64_t> image_shape{1, 3, sz, sz};
Ort::Value image_tensor = Ort::Value::CreateTensor<float>(
mem_info, input_data.data(), input_data.size(),
image_shape.data(), image_shape.size());
const char* output_name_cstr = output_name.c_str();
std::vector<Ort::Value> outputs;
try {
if (has_text_input) {
std::vector<int64_t> text_shape{1, num_classes, 512};
Ort::Value text_tensor = Ort::Value::CreateTensor<float>(
mem_info,
cached_text_feats.data(),
cached_text_feats.size(),
text_shape.data(), text_shape.size());
const char* input_names[2] = {
input_name.c_str(),
text_input_name.c_str()
};
Ort::Value input_tensors[2] = {
std::move(image_tensor),
std::move(text_tensor)
};
outputs = session->Run(
Ort::RunOptions{nullptr},
input_names, input_tensors, 2,
&output_name_cstr, 1);
} else {
const char* input_name_cstr = input_name.c_str();
outputs = session->Run(
Ort::RunOptions{nullptr},
&input_name_cstr, &image_tensor, 1,
&output_name_cstr, 1);
}
} catch (const Ort::Exception& e) {
std::cerr << "OpenVocab: inference failed: " << e.what() << std::endl;
return {};
}
// Post-process: output shape (1, 4+num_classes, num_anchors)
auto out_shape = outputs[0].GetTensorTypeAndShapeInfo().GetShape();
if (out_shape.size() < 3) return {};
int num_anchors = static_cast<int>(out_shape[2]);
int nc = num_classes;
if (nc <= 0) return {};
const float* out = outputs[0].GetTensorData<float>();
std::vector<RawDetection> candidates;
for (int a = 0; a < num_anchors; a++) {
float cx_lb = out[0 * num_anchors + a];
float cy_lb = out[1 * num_anchors + a];
float w_lb = out[2 * num_anchors + a];
float h_lb = out[3 * num_anchors + a];
float best_score = 0.0f;
int best_class = 0;
for (int c = 0; c < nc; c++) {
float s = out[(4 + c) * num_anchors + a];
if (s > best_score) { best_score = s; best_class = c; }
}
if (best_score < config.confidence_threshold) continue;
float x1_lb = cx_lb - w_lb / 2.0f;
float y1_lb = cy_lb - h_lb / 2.0f;
float x2_lb = cx_lb + w_lb / 2.0f;
float y2_lb = cy_lb + h_lb / 2.0f;
float x1 = std::max(0.0f, std::min((x1_lb - pad_x) / scale, static_cast<float>(orig_w - 1)));
float y1 = std::max(0.0f, std::min((y1_lb - pad_y) / scale, static_cast<float>(orig_h - 1)));
float x2 = std::max(0.0f, std::min((x2_lb - pad_x) / scale, static_cast<float>(orig_w - 1)));
float y2 = std::max(0.0f, std::min((y2_lb - pad_y) / scale, static_cast<float>(orig_h - 1)));
candidates.push_back({x1, y1, x2, y2, best_score, best_class});
}
return nms(std::move(candidates), config.nms_threshold);
}
// --- Config ---
OpenVocabConfig OpenVocabConfig::from_env() {
OpenVocabConfig c;
c.model_path = util::get_env_str("ZED_ONNX_MODEL_PATH", c.model_path);
c.confidence_threshold = util::get_env_float("ZED_ONNX_CONFIDENCE", c.confidence_threshold);
c.nms_threshold = util::get_env_float("ZED_ONNX_NMS_THRESHOLD", c.nms_threshold);
c.input_size = util::get_env_int("ZED_ONNX_INPUT_SIZE", c.input_size);
c.labels = util::get_env_str("ZED_ONNX_LABELS", c.labels);
c.text_encoder_path = util::get_env_str("ZED_ONNX_TEXT_ENCODER_PATH", c.text_encoder_path);
c.clip_vocab_path = util::get_env_str("ZED_ONNX_CLIP_VOCAB", c.clip_vocab_path);
c.clip_merges_path = util::get_env_str("ZED_ONNX_CLIP_MERGES", c.clip_merges_path);
return c;
}
// --- OpenVocab ---
OpenVocab::OpenVocab(const OpenVocabConfig& config)
: config_(config), impl_(std::make_unique<Impl>()) {}
OpenVocab::~OpenVocab() = default;
void OpenVocab::enable(sl::Camera& /* camera */) {
if (config_.model_path.empty()) {
std::cerr << "OpenVocab: ZED_ONNX_MODEL_PATH is not set." << std::endl;
return;
}
// --- Load visual model ---
OrtCUDAProviderOptions cuda_opts{};
cuda_opts.device_id = 0;
try {
impl_->session_options.AppendExecutionProvider_CUDA(cuda_opts);
} catch (const Ort::Exception& e) {
std::cerr << "OpenVocab: CUDA unavailable (" << e.what() << "), using CPU." << std::endl;
}
impl_->session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL);
std::cout << "OpenVocab: loading visual model (" << config_.model_path << ")..." << std::endl;
try {
impl_->session = std::make_unique<Ort::Session>(
impl_->env, config_.model_path.c_str(), impl_->session_options);
} catch (const Ort::Exception& e) {
std::cerr << "OpenVocab: failed to load visual model: " << e.what() << std::endl;
return;
}
impl_->input_name = impl_->session->GetInputNameAllocated(0, impl_->allocator).get();
impl_->output_name = impl_->session->GetOutputNameAllocated(0, impl_->allocator).get();
size_t num_inputs = impl_->session->GetInputCount();
impl_->has_text_input = (num_inputs >= 2);
if (impl_->has_text_input) {
impl_->text_input_name = impl_->session->GetInputNameAllocated(1, impl_->allocator).get();
}
auto out_info = impl_->session->GetOutputTypeInfo(0).GetTensorTypeAndShapeInfo();
auto shape = out_info.GetShape();
if (shape.size() == 3 && shape[1] > 4) {
impl_->num_classes = static_cast<int>(shape[1]) - 4;
}
std::cout << "OpenVocab: visual model loaded."
<< " Inputs=" << num_inputs
<< " Classes=" << impl_->num_classes
<< (impl_->has_text_input ? " (open-vocab)" : " (fixed-vocab)")
<< std::endl;
// --- Load text encoder + tokenizer (only needed for open-vocab model) ---
if (impl_->has_text_input) {
if (config_.text_encoder_path.empty()) {
std::cerr << "OpenVocab: ZED_ONNX_TEXT_ENCODER_PATH is not set "
"(required for open-vocab model)." << std::endl;
return;
}
impl_->text_session_options.SetGraphOptimizationLevel(
GraphOptimizationLevel::ORT_ENABLE_ALL);
std::cout << "OpenVocab: loading text encoder (" << config_.text_encoder_path
<< ")..." << std::endl;
try {
impl_->text_session = std::make_unique<Ort::Session>(
impl_->env, config_.text_encoder_path.c_str(), impl_->text_session_options);
} catch (const Ort::Exception& e) {
std::cerr << "OpenVocab: failed to load text encoder: " << e.what() << std::endl;
return;
}
impl_->text_enc_input_name =
impl_->text_session->GetInputNameAllocated(0, impl_->allocator).get();
impl_->text_enc_output_name =
impl_->text_session->GetOutputNameAllocated(0, impl_->allocator).get();
if (!config_.clip_vocab_path.empty() && !config_.clip_merges_path.empty()) {
try {
impl_->tokenizer.load(config_.clip_vocab_path, config_.clip_merges_path);
std::cout << "OpenVocab: tokenizer loaded." << std::endl;
} catch (const std::exception& e) {
std::cerr << "OpenVocab: failed to load tokenizer: " << e.what() << std::endl;
return;
}
} else {
std::cerr << "OpenVocab: ZED_ONNX_CLIP_VOCAB / ZED_ONNX_CLIP_MERGES not set." << std::endl;
return;
}
if (!config_.labels.empty()) {
encode_labels(config_.labels);
}
}
}
// Encode a comma-separated label string into cached_text_feats.
// Always fills exactly num_classes * 512 floats (pads with zeros if fewer labels).
void OpenVocab::encode_labels(const std::string& labels_str) {
if (!impl_->text_session || !impl_->tokenizer.is_loaded()) return;
auto label_vec = split_labels(labels_str);
int N = impl_->num_classes;
impl_->cached_text_feats.assign(static_cast<size_t>(N) * 512, 0.0f);
auto mem_info = Ort::MemoryInfo::CreateCpu(
OrtAllocatorType::OrtArenaAllocator, OrtMemTypeDefault);
int encode_n = std::min(static_cast<int>(label_vec.size()), N);
for (int i = 0; i < encode_n; i++) {
auto tokens = impl_->tokenizer.tokenize(label_vec[i]);
std::vector<int64_t> input_shape{1, util::ClipTokenizer::SEQ_LEN};
Ort::Value token_tensor = Ort::Value::CreateTensor<int64_t>(
mem_info, tokens.data(), tokens.size(),
input_shape.data(), input_shape.size());
const char* in = impl_->text_enc_input_name.c_str();
const char* out = impl_->text_enc_output_name.c_str();
std::vector<Ort::Value> outputs;
try {
outputs = impl_->text_session->Run(
Ort::RunOptions{nullptr}, &in, &token_tensor, 1, &out, 1);
} catch (const Ort::Exception& e) {
std::cerr << "OpenVocab: text encoder failed for \"" << label_vec[i]
<< "\": " << e.what() << std::endl;
continue;
}
const float* emb = outputs[0].GetTensorData<float>();
// L2 normalize
float norm = 0.0f;
for (int j = 0; j < 512; j++) norm += emb[j] * emb[j];
norm = std::sqrt(norm);
float inv_norm = (norm > 1e-8f) ? 1.0f / norm : 0.0f;
for (int j = 0; j < 512; j++)
impl_->cached_text_feats[i * 512 + j] = emb[j] * inv_norm;
}
impl_->cached_labels = labels_str;
std::cout << "OpenVocab: encoded " << encode_n << " label(s): " << labels_str << std::endl;
}
// --- detect(sl::Camera&) — production path with 3D fusion ---
void OpenVocab::detect(sl::Camera& camera) {
detected_.clear();
if (!impl_->session) return;
// Re-encode if labels changed (on-the-fly vocabulary switching)
if (impl_->has_text_input) {
const char* env_labels = std::getenv("ZED_ONNX_LABELS");
std::string current_labels = env_labels ? env_labels : config_.labels;
if (current_labels != impl_->cached_labels) {
encode_labels(current_labels);
}
if (impl_->cached_text_feats.empty()) return;
}
// Retrieve image and point cloud from camera
sl::Mat img_mat;
camera.retrieveImage(img_mat, sl::VIEW::LEFT);
sl::Mat pc_mat;
camera.retrieveMeasure(pc_mat, sl::MEASURE::XYZRGBA);
sl::CameraInformation info = camera.getCameraInformation();
auto calib = info.camera_configuration.calibration_parameters.left_cam;
float fx = calib.fx, fy = calib.fy, cx = calib.cx, cy = calib.cy;
int orig_w = static_cast<int>(img_mat.getWidth());
int orig_h = static_cast<int>(img_mat.getHeight());
// RGBA → BGR for run_inference
cv::Mat rgba(orig_h, orig_w, CV_8UC4, img_mat.getPtr<sl::uchar1>());
cv::Mat bgr;
cv::cvtColor(rgba, bgr, cv::COLOR_RGBA2BGR);
auto kept = impl_->run_inference(config_, bgr);
// 3D fusion
for (const auto& det : kept) {
FusionResult fusion = fuse_bbox(pc_mat,
static_cast<int>(det.x1), static_cast<int>(det.y1),
static_cast<int>(det.x2), static_cast<int>(det.y2));
DetectedObject obj{};
obj.label_id = det.class_id;
obj.confidence = det.score * 100.0f;
if (fusion.valid) {
obj.position[0] = fusion.cx;
obj.position[1] = fusion.cy;
obj.position[2] = fusion.cz;
}
obj.bounding_box_2d[0] = det.x1; obj.bounding_box_2d[1] = det.y1;
obj.bounding_box_2d[2] = det.x2; obj.bounding_box_2d[3] = det.y1;
obj.bounding_box_2d[4] = det.x2; obj.bounding_box_2d[5] = det.y2;
obj.bounding_box_2d[6] = det.x1; obj.bounding_box_2d[7] = det.y2;
if (fusion.valid) {
float corners_u[4] = {det.x1, det.x2, det.x2, det.x1};
float corners_v[4] = {det.y1, det.y1, det.y2, det.y2};
float depths[2] = {fusion.depth_near, fusion.depth_far};
for (int face = 0; face < 2; face++) {
for (int c = 0; c < 4; c++) {
float X, Y, Z;
backproject(corners_u[c], corners_v[c], depths[face],
fx, fy, cx, cy, X, Y, Z);
int idx = (face * 4 + c) * 3;
obj.bounding_box_3d[idx + 0] = X;
obj.bounding_box_3d[idx + 1] = Y;
obj.bounding_box_3d[idx + 2] = Z;
}
}
}
detected_.push_back(obj);
}
}
// --- detect(const cv::Mat&) — image-only path, no 3D fusion ---
void OpenVocab::detect(const cv::Mat& image_bgr) {
detected_.clear();
if (!impl_->session) return;
// Re-encode if labels changed
if (impl_->has_text_input) {
const char* env_labels = std::getenv("ZED_ONNX_LABELS");
std::string current_labels = env_labels ? env_labels : config_.labels;
if (current_labels != impl_->cached_labels) {
encode_labels(current_labels);
}
if (impl_->cached_text_feats.empty()) return;
}
auto kept = impl_->run_inference(config_, image_bgr);
for (const auto& det : kept) {
DetectedObject obj{};
obj.label_id = det.class_id;
obj.confidence = det.score * 100.0f;
// position stays zero (no point cloud available)
obj.bounding_box_2d[0] = det.x1; obj.bounding_box_2d[1] = det.y1;
obj.bounding_box_2d[2] = det.x2; obj.bounding_box_2d[3] = det.y1;
obj.bounding_box_2d[4] = det.x2; obj.bounding_box_2d[5] = det.y2;
obj.bounding_box_2d[6] = det.x1; obj.bounding_box_2d[7] = det.y2;
// bounding_box_3d stays zero
detected_.push_back(obj);
}
}
const std::vector<DetectedObject>& OpenVocab::objects() const {
return detected_;
}
std::string OpenVocab::label_names() const {
return impl_->cached_labels.empty() ? config_.labels : impl_->cached_labels;
}
} // namespace engines
+46
View File
@@ -0,0 +1,46 @@
#pragma once
#include "engines/engine.h"
#include <opencv2/core.hpp>
#include <memory>
#include <string>
namespace engines {
struct OpenVocabConfig {
std::string model_path; // ZED_ONNX_MODEL_PATH (required)
float confidence_threshold = 0.25f; // ZED_ONNX_CONFIDENCE
float nms_threshold = 0.45f; // ZED_ONNX_NMS_THRESHOLD
int input_size = 640; // ZED_ONNX_INPUT_SIZE
std::string labels; // ZED_ONNX_LABELS (comma-separated)
// Open-vocabulary text encoder (required when model has two inputs)
std::string text_encoder_path; // ZED_ONNX_TEXT_ENCODER_PATH
std::string clip_vocab_path; // ZED_ONNX_CLIP_VOCAB
std::string clip_merges_path; // ZED_ONNX_CLIP_MERGES
static OpenVocabConfig from_env();
};
class OpenVocab : public Engine {
public:
explicit OpenVocab(const OpenVocabConfig& config);
~OpenVocab() override;
void enable(sl::Camera& camera) override;
void detect(sl::Camera& camera) override;
void detect(const cv::Mat& image_bgr); // image-only; no 3D fusion
const std::vector<DetectedObject>& objects() const override;
std::string label_names() const override;
private:
OpenVocabConfig config_;
struct Impl;
std::unique_ptr<Impl> impl_;
std::vector<DetectedObject> detected_;
void encode_labels(const std::string& labels_str);
};
} // namespace engines
+4
View File
@@ -92,4 +92,8 @@ 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
+7 -14
View File
@@ -1,8 +1,8 @@
#pragma once
#include <sl/Camera.hpp>
#include "engines/engine.h"
#include <string>
#include <vector>
namespace engines {
@@ -14,21 +14,14 @@ struct DetectionConfig {
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 {
class ZedNative : public Engine {
public:
explicit ZedNative(const DetectionConfig& config);
void enable(sl::Camera& camera);
void detect(sl::Camera& camera);
const std::vector<DetectedObject>& objects() const;
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_;
+19 -8
View File
@@ -1,4 +1,7 @@
#include "engines/engine.h"
#include "engines/open_vocab.h"
#include "engines/zed_native.h"
#include "util/env.h"
#include "zed/camera.h"
#include <dora-node-api.h>
@@ -8,6 +11,7 @@
#include <cstring>
#include <initializer_list>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
@@ -66,9 +70,16 @@ int main() {
camera.warmup();
auto calib = camera.calibration();
auto det_config = engines::DetectionConfig::from_env();
engines::ZedNative detector(det_config);
detector.enable(camera.handle());
std::string engine_name = util::get_env_str("ZED_ENGINE", "native");
std::unique_ptr<engines::Engine> detector;
if (engine_name == "open_vocab") {
auto cfg = engines::OpenVocabConfig::from_env();
detector = std::make_unique<engines::OpenVocab>(cfg);
} else {
auto cfg = engines::DetectionConfig::from_env();
detector = std::make_unique<engines::ZedNative>(cfg);
}
detector->enable(camera.handle());
int64_t frame_id = 0;
while (true) {
@@ -91,11 +102,11 @@ int main() {
camera.close();
camera.open();
calib = camera.calibration();
detector.enable(camera.handle());
detector->enable(camera.handle());
continue;
}
detector.detect(camera.handle());
detector->detect(camera.handle());
sl::Mat& image_mat = camera.image();
sl::Mat& point_cloud_mat = camera.point_cloud();
@@ -181,7 +192,7 @@ int main() {
}
// --- objects ---
const auto& objects = detector.objects();
const auto& objects = detector->objects();
auto obj_buffer = serialize_objects(objects);
int num_objects = static_cast<int>(objects.size());
@@ -201,8 +212,8 @@ int main() {
"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_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);
+142
View File
@@ -0,0 +1,142 @@
// Standalone test for the OpenVocab detection engine.
// No dora framework, no live ZED camera — loads images from disk.
//
// Usage:
// bin/test_open_vocab [yolo_image] [yoloworld_image]
// yolo_image default: test/assets/yolo.png
// yoloworld_image default: test/assets/yoloworld.png
//
// Uses the same env vars as the main binary:
// ZED_ONNX_MODEL_PATH path to YOLO ONNX model (required)
// ZED_ONNX_LABELS comma-separated labels (required)
// ZED_ONNX_CONFIDENCE detection threshold (default 0.25)
// ZED_ONNX_NMS_THRESHOLD NMS IoU threshold (default 0.45)
// ZED_ONNX_INPUT_SIZE model input size (default 640)
// ZED_ONNX_TEXT_ENCODER_PATH clip text encoder ONNX (open-vocab only)
// ZED_ONNX_CLIP_VOCAB clip_vocab.txt path (open-vocab only)
// ZED_ONNX_CLIP_MERGES clip_merges.txt path (open-vocab only)
// ZED_ONNX_WORLD_MODEL_PATH alternative ONNX model for the second image;
// falls back to ZED_ONNX_MODEL_PATH if not set
#include "engines/open_vocab.h"
#include "util/env.h"
#include <opencv2/opencv.hpp>
#include <sl/Camera.hpp>
#include <filesystem>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
namespace fs = std::filesystem;
namespace {
std::vector<std::string> parse_csv(const std::string& s) {
std::vector<std::string> out;
std::istringstream ss(s);
std::string tok;
while (std::getline(ss, tok, ',')) {
size_t l = tok.find_first_not_of(" \t");
size_t r = tok.find_last_not_of(" \t");
if (l != std::string::npos)
out.push_back(tok.substr(l, r - l + 1));
}
return out;
}
void draw_detections(cv::Mat& img,
const std::vector<engines::DetectedObject>& objects,
const std::vector<std::string>& labels) {
for (const auto& obj : objects) {
int x1 = static_cast<int>(obj.bounding_box_2d[0]);
int y1 = static_cast<int>(obj.bounding_box_2d[1]);
int x2 = static_cast<int>(obj.bounding_box_2d[4]);
int y2 = static_cast<int>(obj.bounding_box_2d[5]);
std::string name = (obj.label_id >= 0 && obj.label_id < static_cast<int>(labels.size()))
? labels[obj.label_id] : std::to_string(obj.label_id);
char buf[64];
std::snprintf(buf, sizeof(buf), " %.2f", obj.confidence / 100.0f);
std::string text = name + buf;
cv::rectangle(img, {x1, y1}, {x2, y2}, {0, 200, 0}, 2);
cv::putText(img, text, {x1, std::max(y1 - 6, 14)},
cv::FONT_HERSHEY_SIMPLEX, 0.55, {0, 200, 0}, 2, cv::LINE_AA);
}
}
bool run_test(engines::OpenVocab& engine, const std::string& image_path) {
cv::Mat img = cv::imread(image_path);
if (img.empty()) {
std::cerr << "Error: cannot read image: " << image_path << std::endl;
return false;
}
engine.detect(img);
const auto& objects = engine.objects();
auto labels = parse_csv(engine.label_names());
std::cout << "[" << image_path << "] " << objects.size() << " detection(s):" << std::endl;
for (const auto& obj : objects) {
std::string name = (obj.label_id >= 0 && obj.label_id < static_cast<int>(labels.size()))
? labels[obj.label_id] : std::to_string(obj.label_id);
std::cout << " " << name
<< " conf=" << obj.confidence / 100.0f
<< " bbox=["
<< obj.bounding_box_2d[0] << "," << obj.bounding_box_2d[1]
<< " -> "
<< obj.bounding_box_2d[4] << "," << obj.bounding_box_2d[5]
<< "]" << std::endl;
}
cv::Mat annotated = img.clone();
draw_detections(annotated, objects, labels);
fs::path stem = fs::path(image_path).stem();
fs::path parent = fs::path(image_path).parent_path();
std::string result_path = (parent / (stem.string() + "_result.png")).string();
cv::imwrite(result_path, annotated);
std::cout << "Saved: " << result_path << std::endl;
return true;
}
} // namespace
int main(int argc, char* argv[]) {
std::string yolo_path = (argc > 1) ? argv[1] : "test/assets/yolo.png";
std::string yoloworld_path = (argc > 2) ? argv[2] : "test/assets/yoloworld.png";
auto config = engines::OpenVocabConfig::from_env();
// enable() ignores this parameter for OpenVocab
sl::Camera dummy_cam;
int failures = 0;
// --- Test 1: yolo image ---
{
std::cout << "\n=== Test 1: " << yolo_path << " ===" << std::endl;
engines::OpenVocab engine(config);
engine.enable(dummy_cam);
if (!run_test(engine, yolo_path)) ++failures;
}
// --- Test 2: yoloworld image (optionally with a different model) ---
{
std::cout << "\n=== Test 2: " << yoloworld_path << " ===" << std::endl;
engines::OpenVocabConfig config2 = config;
const char* world_model = std::getenv("ZED_ONNX_WORLD_MODEL_PATH");
if (world_model && *world_model)
config2.model_path = world_model;
engines::OpenVocab engine(config2);
engine.enable(dummy_cam);
if (!run_test(engine, yoloworld_path)) ++failures;
}
return failures > 0 ? 1 : 0;
}
+226
View File
@@ -0,0 +1,226 @@
#pragma once
// Minimal CLIP ViT-B/32 BPE tokenizer — header-only, C++17, no external deps.
//
// Loads clip_vocab.txt (token\tid, tab-separated) and clip_merges.txt
// (a b, space-separated BPE pairs) produced by export_clip_text_encoder.py.
// Returns 77 int64 token IDs compatible with clip_text_encoder.onnx.
#include <algorithm>
#include <climits>
#include <fstream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <unordered_map>
#include <vector>
namespace util {
class ClipTokenizer {
public:
static constexpr int SEQ_LEN = 77;
static constexpr int64_t BOS = 49406;
static constexpr int64_t EOS = 49407;
static constexpr int64_t PAD = 0;
void load(const std::string& vocab_path, const std::string& merges_path) {
load_vocab(vocab_path);
load_merges(merges_path);
init_byte_encoder();
}
bool is_loaded() const { return !vocab_.empty(); }
// Tokenize a label string. Returns exactly SEQ_LEN int64 token IDs.
std::vector<int64_t> tokenize(const std::string& text) const {
std::vector<int64_t> ids;
ids.reserve(SEQ_LEN);
ids.push_back(BOS);
std::string lower = text;
std::transform(lower.begin(), lower.end(), lower.begin(),
[](unsigned char c) { return static_cast<char>(std::tolower(c)); });
// Split on whitespace — sufficient for typical detection label names
std::istringstream ss(lower);
std::string word;
while (ss >> word) {
if (static_cast<int>(ids.size()) >= SEQ_LEN - 1) break;
// Byte-encode the word via the GPT-2 byte encoder
std::string encoded;
encoded.reserve(word.size() * 3);
for (unsigned char c : word) {
auto it = byte_encoder_.find(c);
if (it != byte_encoder_.end())
encoded += utf8_of(it->second);
}
if (encoded.empty()) continue;
// Apply BPE
auto tokens = bpe_encode(encoded);
for (const auto& tok : tokens) {
if (static_cast<int>(ids.size()) >= SEQ_LEN - 1) break;
auto vit = vocab_.find(tok);
if (vit != vocab_.end())
ids.push_back(static_cast<int64_t>(vit->second));
}
}
ids.push_back(EOS);
ids.resize(SEQ_LEN, PAD);
return ids;
}
private:
std::unordered_map<std::string, int> vocab_; // token string → id
std::unordered_map<std::string, int> merge_ranks_; // "a b" → rank
std::unordered_map<unsigned char, uint32_t> byte_encoder_; // byte → unicode codepoint
// -----------------------------------------------------------------
// Byte encoder — matches Python's bytes_to_unicode() in OpenAI CLIP
// -----------------------------------------------------------------
void init_byte_encoder() {
// Printable ASCII + Latin-1 supplement ranges that map to themselves
std::vector<int> bs;
for (int i = '!'; i <= '~'; i++) bs.push_back(i);
for (int i = 0xA1; i <= 0xAC; i++) bs.push_back(i);
for (int i = 0xAE; i <= 0xFF; i++) bs.push_back(i);
std::vector<uint32_t> cs;
cs.reserve(bs.size());
for (int b : bs) cs.push_back(static_cast<uint32_t>(b));
// Remaining bytes map to codepoints starting at U+0100
int n = 0;
for (int b = 0; b < 256; b++) {
if (std::find(bs.begin(), bs.end(), b) == bs.end()) {
bs.push_back(b);
cs.push_back(256u + static_cast<uint32_t>(n));
++n;
}
}
byte_encoder_.clear();
for (size_t i = 0; i < bs.size(); i++)
byte_encoder_[static_cast<unsigned char>(bs[i])] = cs[i];
}
// Encode a unicode codepoint as a UTF-8 std::string
static std::string utf8_of(uint32_t cp) {
std::string s;
if (cp < 0x80u) {
s += static_cast<char>(cp);
} else if (cp < 0x800u) {
s += static_cast<char>(0xC0u | (cp >> 6));
s += static_cast<char>(0x80u | (cp & 0x3Fu));
} else if (cp < 0x10000u) {
s += static_cast<char>(0xE0u | (cp >> 12));
s += static_cast<char>(0x80u | ((cp >> 6) & 0x3Fu));
s += static_cast<char>(0x80u | (cp & 0x3Fu));
} else {
s += static_cast<char>(0xF0u | (cp >> 18));
s += static_cast<char>(0x80u | ((cp >> 12) & 0x3Fu));
s += static_cast<char>(0x80u | ((cp >> 6) & 0x3Fu));
s += static_cast<char>(0x80u | (cp & 0x3Fu));
}
return s;
}
// -----------------------------------------------------------------
// BPE
// -----------------------------------------------------------------
// Split a UTF-8 string into individual Unicode character substrings
static std::vector<std::string> split_utf8_chars(const std::string& s) {
std::vector<std::string> chars;
for (size_t i = 0; i < s.size(); ) {
unsigned char c = static_cast<unsigned char>(s[i]);
int len = (c < 0x80) ? 1 : (c < 0xE0) ? 2 : (c < 0xF0) ? 3 : 4;
chars.push_back(s.substr(i, len));
i += len;
}
return chars;
}
// BPE-encode a single byte-encoded word, returning the token strings
std::vector<std::string> bpe_encode(const std::string& word_str) const {
std::vector<std::string> word = split_utf8_chars(word_str);
if (word.size() <= 1) return word;
while (true) {
// Find the pair with the lowest merge rank
int best_rank = INT_MAX;
int best_i = -1;
for (int i = 0; i + 1 < static_cast<int>(word.size()); i++) {
auto it = merge_ranks_.find(word[i] + " " + word[i + 1]);
if (it != merge_ranks_.end() && it->second < best_rank) {
best_rank = it->second;
best_i = i;
}
}
if (best_i < 0) break;
// Apply merge at best_i
std::string merged = word[best_i] + word[best_i + 1];
word.erase(word.begin() + best_i, word.begin() + best_i + 2);
word.insert(word.begin() + best_i, std::move(merged));
}
return word;
}
// -----------------------------------------------------------------
// File loading
// -----------------------------------------------------------------
void load_vocab(const std::string& path) {
std::ifstream f(path);
if (!f.is_open())
throw std::runtime_error("ClipTokenizer: cannot open vocab file: " + path);
std::string line;
while (std::getline(f, line)) {
auto tab = line.rfind('\t');
if (tab == std::string::npos) continue;
std::string token = unescape(line.substr(0, tab));
int id = std::stoi(line.substr(tab + 1));
vocab_[token] = id;
}
}
void load_merges(const std::string& path) {
std::ifstream f(path);
if (!f.is_open())
throw std::runtime_error("ClipTokenizer: cannot open merges file: " + path);
std::string line;
int rank = 0;
while (std::getline(f, line)) {
if (line.empty() || line[0] == '#') continue;
auto sp = line.find(' ');
if (sp == std::string::npos) continue;
merge_ranks_[line] = rank++; // key is "a b" as written in the file
}
}
static std::string unescape(const std::string& s) {
std::string r;
r.reserve(s.size());
for (size_t i = 0; i < s.size(); i++) {
if (s[i] == '\\' && i + 1 < s.size()) {
switch (s[++i]) {
case '\\': r += '\\'; break;
case 'n': r += '\n'; break;
case 't': r += '\t'; break;
default: r += '\\'; r += s[i]; break;
}
} else {
r += s[i];
}
}
return r;
}
};
} // namespace util