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

make point_cloud optional and publish camera_info once per session

Browse Source 
ZED_POINT_CLOUD, off by default, gates both retrieveMeasure(XYZRGBA) and the
point_cloud output; when off the measure is not retrieved at all. At 1080p a
full cloud is ~14.7 MB per frame and costs capture time whether or not anything
consumes it, and consumers that need 3D can back-project depth instead.

camera_info now goes out once per camera session rather than on every frame:
fx, fy, cx, cy and the distortion coefficients are fixed for a given camera and
resolution, so repeating them per frame adds no information. It rides the first
frame after the camera opens, and again after a reconnection, so it still
carries that frame's timestamps.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Y7g2cBJ5e3r15ZbJ9WZn8c
This commit is contained in:
cesar
2026-09-08 09:21:11 -03:00
co-authored by Claude Opus 5
parent 617b8ceb50
commit 42c2b791fd
5 changed files with 84 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main.cc
+63 -51
View File
@@ -104,6 +104,12 @@ int main() {
std::string pc_mode_str = util::get_env_str("ZED_PC_MODE", "FULL");
PcMode pc_mode = (pc_mode_str == "HALF") ? PcMode::HALF : PcMode::FULL;
// fx, fy, cx, cy and the distortion coefficients are fixed for a given camera
// and resolution, so they are published once per camera session rather than on
// every frame. It goes out with the first frame of the session so that it
// carries that frame's timestamps like every other output.
bool camera_info_pending = true;
int64_t frame_id = 0;
while (true) {
auto event = dora_node.events->next();
@@ -122,12 +128,12 @@ int main() {
camera.close();
camera.open();
calib = camera.calibration();
camera_info_pending = true;
continue;
}
sl::Mat& image_mat = camera.image();
sl::Mat& depth_mat = camera.depth();
sl::Mat& point_cloud_mat = camera.point_cloud();
int width = image_mat.getWidth();
int height = image_mat.getHeight();
@@ -181,66 +187,72 @@ int main() {
std::cerr << "Error sending depth: " << std::string(depth_result.error) << std::endl;
}
// --- point_cloud ---
int pc_width = point_cloud_mat.getWidth();
int pc_height = point_cloud_mat.getHeight();
if (config.point_cloud) {
// --- point_cloud ---
sl::Mat& point_cloud_mat = camera.point_cloud();
int pc_width = point_cloud_mat.getWidth();
int pc_height = point_cloud_mat.getHeight();
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("channels", "XYZRGBA");
pc_metadata->set_string("units", "mm");
set_intrinsics(*pc_metadata, calib);
set_frame_stamps(*pc_metadata, current_frame_id, device_ns, host_ns);
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("channels", "XYZRGBA");
pc_metadata->set_string("units", "mm");
set_intrinsics(*pc_metadata, calib);
set_frame_stamps(*pc_metadata, current_frame_id, device_ns, host_ns);
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;
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 {
// 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));
}
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 {
// 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()) {
std::cerr << "Error sending point_cloud: " << std::string(pc_result.error) << std::endl;
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()) {
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)};
if (camera_info_pending) {
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)};
auto cam_metadata = new_metadata();
cam_metadata->set_string("dtype", "float32");
cam_metadata->set_string("layout", "C");
cam_metadata->set_list_int("shape", make_vector_int({4}));
cam_metadata->set_list_float("distortion", make_distortion(calib.distortion));
set_frame_stamps(*cam_metadata, current_frame_id, device_ns, host_ns);
auto cam_metadata = new_metadata();
cam_metadata->set_string("dtype", "float32");
cam_metadata->set_string("layout", "C");
cam_metadata->set_list_int("shape", make_vector_int({4}));
cam_metadata->set_list_float("distortion", make_distortion(calib.distortion));
set_frame_stamps(*cam_metadata, current_frame_id, device_ns, host_ns);
auto cam_result = send_output_with_metadata(
dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata));
if (!std::string(cam_result.error).empty()) {
std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl;
auto cam_result = send_output_with_metadata(
dora_node.send_output, "camera_info", cam_slice, std::move(cam_metadata));
if (!std::string(cam_result.error).empty()) {
std::cerr << "Error sending camera_info: " << std::string(cam_result.error) << std::endl;
}
camera_info_pending = false;
}
}
src/zed/camera.cc
+6 -1
View File
@@ -78,6 +78,7 @@ Config Config::from_env() {
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.point_cloud = util::get_env_bool("ZED_POINT_CLOUD", c.point_cloud);
c.confidence_threshold = util::get_env_int("ZED_CONFIDENCE_THRESHOLD", c.confidence_threshold);
c.texture_confidence_threshold =
util::get_env_int("ZED_TEXTURE_CONFIDENCE_THRESHOLD", c.texture_confidence_threshold);
@@ -149,7 +150,11 @@ bool Camera::grab() {
}
zed_.retrieveImage(image_mat_, sl::VIEW::LEFT);
zed_.retrieveMeasure(depth_mat_, sl::MEASURE::DEPTH);
zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA);
// At 1080p a full XYZRGBA cloud is ~14.7 MB per frame, and retrieving it
// costs capture time whether or not anything consumes it.
if (config_.point_cloud) {
zed_.retrieveMeasure(point_cloud_mat_, sl::MEASURE::XYZRGBA);
}
return true;
}
src/zed/camera.h
+1
View File
@@ -14,6 +14,7 @@ struct Config {
int depth_min_mm = 200;
int depth_max_mm = 600;
bool depth_fill = false;
bool point_cloud = false;
int confidence_threshold = 60;
int texture_confidence_threshold = 100;
std::string flip = "ON";