add ZED_PC_MODE for point cloud bandwidth control
Three modes: FULL (full HxW grid, zero-copy), HALF (stride-2 downsample, ~4x reduction), ROI (valid points within detected bounding boxes, flat [N,4] array). Parsed once before the event loop.
This commit is contained in:
cristhian aguilera
@@ -36,7 +36,7 @@ Camera and detection behavior is configured entirely through environment variabl
|
||||
- **`src/engines/zed_native.h/cc`** — `engines::ZedNative`: wraps ZED SDK's TensorRT-optimized object detection module.
|
||||
- **`src/engines/open_vocab.h/cc`** — `engines::OpenVocab`: YOLO-World open-vocabulary detection via ONNX Runtime. Supports two modes: single-input (fixed vocab, legacy) and two-input (open-vocab, `images` + `text_feats`). In two-input mode, reads `ZED_ONNX_LABELS` on every tick; when the labels change, re-tokenizes and re-encodes via the CLIP text encoder ONNX session, then passes the resulting `(1, N, 512)` L2-normalized embeddings to the visual model. Preprocesses images (letterbox, normalize, CHW), runs inference, applies NMS, then fuses 2D detections with the ZED point cloud to compute 3D positions and bounding boxes.
|
||||
- **`src/util/clip_tokenizer.h`** — Header-only C++ CLIP BPE tokenizer (no external deps). Implements the GPT-2 `bytes_to_unicode()` byte encoder + iterative BPE merge algorithm. Loads `clip_vocab.txt` and `clip_merges.txt` generated by `scripts/export_clip_text_encoder.py`. Returns 77 `int64` token IDs (BOS=49406, EOS=49407, padded with 0).
|
||||
- **`src/main.cc`** — Dora event loop. Selects engine via `ZED_ENGINE` env var, holds a `std::unique_ptr<engines::Engine>`. On `tick` input: calls `camera.grab()` and `detector->detect()`, then sends `image_bgr`, `point_cloud`, `camera_info`, and `objects` outputs. On `update_vocabulary` input: decodes payload as a UTF-8 label string and calls `detector->set_labels()` for immediate re-encoding. Handles reconnection on grab failure.
|
||||
- **`src/main.cc`** — Dora event loop. Selects engine via `ZED_ENGINE` env var, holds a `std::unique_ptr<engines::Engine>`. Parses `ZED_PC_MODE` (`FULL`/`HALF`/`ROI`) once before the loop. On `tick` input: 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. On `update_vocabulary` input: decodes payload as a UTF-8 label string and calls `detector->set_labels()` for immediate re-encoding. 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.
|
||||
- **`scripts/export_yoloworld.py`** — PEP 723 uv script. Exports a YOLOv8-worldv2 model to ONNX with N class slots (labels arbitrary at runtime). Run: `uv run scripts/export_yoloworld.py --model s --num-classes 10`.
|
||||
|
||||
@@ -102,6 +102,7 @@ Set-ExecutionPolicy -Scope Process -ExecutionPolicy Bypass
|
||||
| `ZED_ONNX_TEXT_ENCODER_PATH` | _(required for open-vocab)_ | (`open_vocab`) Path to `clip_text_encoder.onnx`. Required when the visual model has two inputs (`images` + `text_feats`) |
|
||||
| `ZED_ONNX_CLIP_VOCAB` | _(required for open-vocab)_ | (`open_vocab`) Path to `clip_vocab.txt` BPE vocabulary file |
|
||||
| `ZED_ONNX_CLIP_MERGES` | _(required for open-vocab)_ | (`open_vocab`) Path to `clip_merges.txt` BPE merge rules file |
|
||||
| `ZED_PC_MODE` | `FULL` | Point cloud output mode: `FULL` (full HxW grid), `HALF` (stride-2 downsample, ~4× smaller), or `ROI` (valid points within detected bounding boxes only, flat list) |
|
||||
|
||||
## Inputs
|
||||
|
||||
@@ -118,7 +119,15 @@ BGR image as raw bytes. Metadata includes `shape` (HxWx3), `encoding` (`bgr8`),
|
||||
|
||||
### `point_cloud`
|
||||
|
||||
XYZRGBA point cloud as float32 bytes. Metadata includes `shape` (HxWx4), `units` (`mm`), and camera intrinsics.
|
||||
XYZRGBA point cloud as float32 bytes. Controlled by `ZED_PC_MODE`:
|
||||
|
||||
| Mode | Shape | Layout | Description |
|
||||
|---|---|---|---|
|
||||
| `FULL` | `[H, W, 4]` | `HWC` | Full resolution grid (default). ~14.7 MB/frame at 720p |
|
||||
| `HALF` | `[H/2, W/2, 4]` | `HWC` | Stride-2 downsample (~4× smaller) |
|
||||
| `ROI` | `[N, 4]` | `NC` | Valid points within detected object bounding boxes only. `N` varies by scene. Metadata includes `mode=roi` |
|
||||
|
||||
Metadata includes `units` (`mm`) and camera intrinsics.
|
||||
|
||||
### `camera_info`
|
||||
|
||||
|
||||
+58
-5
@@ -19,6 +19,8 @@ namespace {
|
||||
|
||||
constexpr int FIELDS_PER_OBJECT = 37;
|
||||
|
||||
enum class PcMode { FULL, HALF, ROI };
|
||||
|
||||
rust::Vec<int64_t> make_shape(int height, int width, int channels) {
|
||||
rust::Vec<int64_t> shape;
|
||||
shape.push_back(height);
|
||||
@@ -81,6 +83,11 @@ int main() {
|
||||
}
|
||||
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) {
|
||||
auto event = dora_node.events->next();
|
||||
@@ -163,22 +170,68 @@ int main() {
|
||||
// --- 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));
|
||||
const auto* pc_ptr = reinterpret_cast<const uint8_t*>(point_cloud_mat.getPtr<sl::float4>());
|
||||
rust::Slice<const uint8_t> pc_slice{pc_ptr, pc_size};
|
||||
|
||||
std::vector<float> pc_buf;
|
||||
const uint8_t* pc_data_ptr = nullptr;
|
||||
size_t pc_data_size = 0;
|
||||
auto pc_metadata = new_metadata();
|
||||
pc_metadata->set_string("dtype", "float32");
|
||||
pc_metadata->set_string("layout", "HWC");
|
||||
pc_metadata->set_string("channels", "XYZRGBA");
|
||||
pc_metadata->set_string("units", "mm");
|
||||
pc_metadata->set_list_int("shape", make_shape(pc_height, pc_width, 4));
|
||||
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_list_int("shape", make_shape(hh, hw, 4));
|
||||
pc_data_ptr = reinterpret_cast<const uint8_t*>(pc_buf.data());
|
||||
pc_data_size = pc_buf.size() * sizeof(float);
|
||||
} else if (pc_mode == PcMode::ROI) {
|
||||
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};
|
||||
auto pc_result = send_output_with_metadata(
|
||||
dora_node.send_output, "point_cloud", pc_slice, std::move(pc_metadata));
|
||||
if (!std::string(pc_result.error).empty()) {
|
||||
|
||||
Reference in New Issue
Block a user