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

add ZED_PC_MODE for point cloud bandwidth control

Browse Source 
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
2026-03-28 11:15:38 -03:00
parent 2d10a68527
commit 5e30b65416
3 changed files with 70 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main.cc
+59 -6
View File
@@ -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();
@@ -161,24 +168,70 @@ int main() {
}
// --- point_cloud ---
int pc_width = point_cloud_mat.getWidth();
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()) {