From 903e669110e7aefdadc81c58979cd4ae8991b166 Mon Sep 17 00:00:00 2001 From: Guillaume Binet Date: Mon, 27 Jul 2026 13:34:28 -0500 Subject: [PATCH] adds integer and scale encoding --- components/libs/zed_sdk/src/camera.rs | 8 ++ components/sources/cu_zed/README.md | 14 ++- .../cu_zed/examples/zed_rerun_demo.ron | 6 +- .../sources/cu_zed/examples/zed_rerun_demo.rs | 95 ++++++--------- components/sources/cu_zed/src/lib.rs | 114 ++++++++++-------- components/sources/cu_zed/src/payloads.rs | 5 +- 6 files changed, 122 insertions(+), 120 deletions(-) diff --git a/components/libs/zed_sdk/src/camera.rs b/components/libs/zed_sdk/src/camera.rs index d2d024d..144c251 100644 --- a/components/libs/zed_sdk/src/camera.rs +++ b/components/libs/zed_sdk/src/camera.rs @@ -213,6 +213,14 @@ impl Camera { self.retrieve_measure_raw(sys::SL_MEASURE::SL_MEASURE_DEPTH, target) } + /// Retrieves a compact depth map in millimeters. + /// + /// Invalid values are zero and valid depths are clamped to 65000 millimeters + /// by the ZED SDK. + pub fn retrieve_depth_u16_mm(&mut self, target: &mut Mat) -> Result<()> { + self.retrieve_measure_raw(sys::SL_MEASURE::SL_MEASURE_DEPTH_U16_MM, target) + } + /// Retrieves the depth confidence map. pub fn retrieve_confidence(&mut self, target: &mut Mat) -> Result<()> { self.retrieve_measure_raw(sys::SL_MEASURE::SL_MEASURE_CONFIDENCE, target) diff --git a/components/sources/cu_zed/README.md b/components/sources/cu_zed/README.md index 8625690..1cc888f 100644 --- a/components/sources/cu_zed/README.md +++ b/components/sources/cu_zed/README.md @@ -17,7 +17,8 @@ The demo in this directory wires both together and streams the result to Rerun: `cu_zed::Zed` is a `CuSrcTask` with this output tuple: 1. `cu_zed::ZedStereoImages` -2. `cu_zed::ZedDepthMap>` +2. `cu_zed::ZedDepthMap`, a generic integer depth map specialized as + `CuDepthInteger` 3. `cu_zed::ZedConfidenceMap>` 4. `cu29::prelude::CuLatchedStateUpdate` 5. `cu29::prelude::CuLatchedStateUpdate` @@ -29,7 +30,8 @@ The demo in this directory wires both together and streams the result to Rerun: Semantics: - Stereo images are left/right RGBA frames in pooled host memory. -- Depth is a dense `f32` raster in pooled host memory. +- Depth is a dense `u16` millimeter raster in pooled host memory. Zero is invalid, + and the ZED SDK clamps valid values at 65000 millimeters. - Confidence is optional and only emitted when enabled in config. - Calibration and rig transforms are latched state updates: the source publishes `Set(...)` when the task starts, then `NoChange` after that. @@ -204,7 +206,7 @@ These are designed to be consumed as Copper latched state. A downstream task sho `ZedDepthToPointCloud` is a pure `CuTask` that consumes: -- `cu_zed::ZedDepthMap>` +- `cu_zed::ZedDepthMap` (`CuDepthInteger`) - `cu29::prelude::CuLatchedStateUpdate` and produces: @@ -215,8 +217,8 @@ The task: - caches the latest calibration bundle - rescales intrinsics if the depth raster size differs from the calibration image size -- converts the ZED depth unit into meters -- skips invalid depth samples (`NaN`, non-finite, or `<= 0`) +- converts millimeter samples into typed lengths/meters +- skips invalid zero-valued depth samples - writes a standard Copper point cloud with: - `x`, `y`, `z` in meters - `i = 0%` @@ -294,7 +296,7 @@ This is the standard graph shape: ( src: "zed", dst: "pointcloud", - msg: "cu_zed::ZedDepthMap>", + msg: "cu_sensor_payloads::CuDepthMap, cu_sensor_payloads::CuDepthInteger>", ), ( src: "zed", diff --git a/components/sources/cu_zed/examples/zed_rerun_demo.ron b/components/sources/cu_zed/examples/zed_rerun_demo.ron index 634c42c..f39a49d 100644 --- a/components/sources/cu_zed/examples/zed_rerun_demo.ron +++ b/components/sources/cu_zed/examples/zed_rerun_demo.ron @@ -39,7 +39,7 @@ ( src: "zed", dst: "rerun", - msg: "cu_zed::ZedDepthMap>", + msg: "cu_sensor_payloads::CuDepthMap, cu_sensor_payloads::CuDepthInteger>", ), ( src: "zed", @@ -59,7 +59,7 @@ ( src: "zed", dst: "pointcloud", - msg: "cu_zed::ZedDepthMap>", + msg: "cu_sensor_payloads::CuDepthMap, cu_sensor_payloads::CuDepthInteger>", ), ( src: "zed", @@ -92,4 +92,4 @@ msg: "cu_zed::ZedFrameMeta", ), ], -) \ No newline at end of file +) diff --git a/components/sources/cu_zed/examples/zed_rerun_demo.rs b/components/sources/cu_zed/examples/zed_rerun_demo.rs index 522e13b..010cf29 100644 --- a/components/sources/cu_zed/examples/zed_rerun_demo.rs +++ b/components/sources/cu_zed/examples/zed_rerun_demo.rs @@ -32,7 +32,7 @@ impl CuSinkTask for ZedRerunSink { type Input<'m> = input_msg!( 'm, ZedStereoImages, - ZedDepthMap>, + ZedDepthMap, ZedConfidenceMap>, CuLatchedStateUpdate, CuLatchedStateUpdate, @@ -83,12 +83,12 @@ impl CuSinkTask for ZedRerunSink { if let Some(depth) = depth_msg.payload() { apply_tov(&self.rec, &depth_msg.tov); - log_depth_map(&self.rec, "zed/left_camera/depth", depth)?; + log_u16_depth_map(&self.rec, "zed/left_camera/depth", depth)?; } if let Some(confidence) = confidence_msg.payload() { apply_tov(&self.rec, &confidence_msg.tov); - log_depth_map(&self.rec, "zed/left_camera/confidence", confidence)?; + log_float_raster(&self.rec, "zed/left_camera/confidence", confidence)?; } if let Some(pointcloud) = pointcloud_msg.payload() { @@ -353,69 +353,45 @@ fn log_axes(rec: &RecordingStream, path: &str, axis_len: f32) -> CuResult<()> { .map_err(|e| CuError::new_with_cause("Failed to log axes", e)) } -fn log_depth_map(rec: &RecordingStream, path: &str, payload: &T) -> CuResult<()> -where - T: ZedFloatRaster, -{ - let bytes = payload.packed_bytes(); +fn log_u16_depth_map(rec: &RecordingStream, path: &str, payload: &ZedDepthMap) -> CuResult<()> { + let bytes = payload.with_samples(|samples, format| { + pack_raster_bytes(samples, format.width, format.height, format.stride) + }); let image = DepthImage::from_data_type_and_bytes( bytes, - [payload.width(), payload.height()], - rerun::ChannelDatatype::F32, + [payload.format.width, payload.format.height], + rerun::ChannelDatatype::U16, ) - .with_meter(1.0) + .with_meter(ZedDepthMap::encoding_descriptor().units_per_meter) .with_colormap(rerun::components::Colormap::Turbo); rec.log(path, &image) .map_err(|e| CuError::new_with_cause("Failed to log depth image", e)) } -trait ZedFloatRaster { - fn width(&self) -> u32; - fn height(&self) -> u32; - fn packed_bytes(&self) -> Vec; -} - -impl ZedFloatRaster for ZedDepthMap> { - fn width(&self) -> u32 { - self.format.width - } - - fn height(&self) -> u32 { - self.format.height - } - - fn packed_bytes(&self) -> Vec { - self.buffer_handle.with_inner(|inner| { - pack_f32_raster_bytes( - &inner[..], - self.format.width, - self.format.height, - self.format.stride, - ) - }) - } -} - -impl ZedFloatRaster for ZedConfidenceMap> { - fn width(&self) -> u32 { - self.format.width - } - - fn height(&self) -> u32 { - self.format.height - } +fn log_float_raster( + rec: &RecordingStream, + path: &str, + payload: &ZedConfidenceMap>, +) -> CuResult<()> { + let bytes = payload.buffer_handle.with_inner(|inner| { + pack_raster_bytes( + &inner[..], + payload.format.width, + payload.format.height, + payload.format.stride, + ) + }); + let image = DepthImage::from_data_type_and_bytes( + bytes, + [payload.format.width, payload.format.height], + rerun::ChannelDatatype::F32, + ) + .with_meter(1.0) + .with_colormap(rerun::components::Colormap::Turbo); - fn packed_bytes(&self) -> Vec { - self.buffer_handle.with_inner(|inner| { - pack_f32_raster_bytes( - &inner[..], - self.format.width, - self.format.height, - self.format.stride, - ) - }) - } + rec.log(path, &image) + .map_err(|e| CuError::new_with_cause("Failed to log float raster", e)) } fn log_rgba_image( @@ -443,7 +419,12 @@ fn log_rgba_image( .map_err(|e| CuError::new_with_cause("Failed to log RGBA image", e)) } -fn pack_f32_raster_bytes(values: &[f32], width: u32, height: u32, stride: u32) -> Vec { +fn pack_raster_bytes( + values: &[T], + width: u32, + height: u32, + stride: u32, +) -> Vec { let width = width as usize; let height = height as usize; let stride = stride as usize; diff --git a/components/sources/cu_zed/src/lib.rs b/components/sources/cu_zed/src/lib.rs index 743ad4b..eb92c79 100644 --- a/components/sources/cu_zed/src/lib.rs +++ b/components/sources/cu_zed/src/lib.rs @@ -6,8 +6,8 @@ use bincode::de::Decoder; use bincode::error::DecodeError; use bincode::{Decode, Encode}; use cu_sensor_payloads::{ - BarometerPayload, CuImage, Distance, ImuPayload, MagnetometerPayload, PointCloudSoa, - Reflectivity, + BarometerPayload, CuDepthMapFormat, CuImage, Distance, ImuPayload, MagnetometerPayload, + PointCloudSoa, Reflectivity, }; use cu29::prelude::*; use cu29::units::si::length::meter; @@ -53,7 +53,7 @@ impl Decode<()> for ZedStereoImages { pub type ZedSourceOutputs = ( CuMsg, - CuMsg>>, + CuMsg, CuMsg>>, CuMsg>, CuMsg>, @@ -85,8 +85,7 @@ impl Freezable for ZedDepthToPointCloud {} impl CuTask for ZedDepthToPointCloud { type Resources<'r> = (); - type Input<'m> = - input_msg!('m, ZedDepthMap>, CuLatchedStateUpdate); + type Input<'m> = input_msg!('m, ZedDepthMap, CuLatchedStateUpdate); type Output<'m> = output_msg!(PointCloudSoa); fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult @@ -125,23 +124,28 @@ impl CuTask for ZedDepthToPointCloud { let pointcloud = output.payload_mut().get_or_insert_with(Default::default); pointcloud.len = 0; - depth.buffer_handle.with_inner(|samples| { - for row in 0..depth.format.height as usize { - let row_offset = row * depth.format.stride as usize; - for col in 0..depth.format.width as usize { - let depth_value = samples[row_offset + col]; - if !depth_value.is_finite() || depth_value <= 0.0 { + depth.with_samples(|samples, format| { + for row in 0..format.height as usize { + let row_offset = row * format.stride as usize; + for col in 0..format.width as usize { + let Some(depth_value) = + ZedDepthMap::decode_sample(samples[row_offset + col]) + else { continue; - } + }; if pointcloud.len == MAX_POINTS { return Err(CuError::from(format!( "ZED point cloud capacity {MAX_POINTS} exceeded while projecting {}x{} depth map", - depth.format.width, depth.format.height + format.width, format.height ))); } - let (x, y, z) = projection.project(col as f32, row as f32, depth_value)?; + let (x, y, z) = projection.project( + col as f32, + row as f32, + depth_value.get::(), + )?; let idx = pointcloud.len; pointcloud.tov[idx] = point_tov; pointcloud.x[idx] = Distance::new::(x); @@ -187,11 +191,10 @@ struct ProjectionIntrinsics { cx: f32, cy: f32, y_sign: f32, - depth_scale_m: f32, } impl ProjectionIntrinsics { - fn from_bundle(format: ZedRasterFormat, calibration: &ZedCalibrationBundle) -> CuResult { + fn from_bundle(format: CuDepthMapFormat, calibration: &ZedCalibrationBundle) -> CuResult { if calibration.left.width == 0 || calibration.left.height == 0 { return Err(CuError::from("ZED calibration reported a zero-sized image")); } @@ -217,12 +220,11 @@ impl ProjectionIntrinsics { cx: calibration.left.cx * scale_x, cy: calibration.left.cy * scale_y, y_sign, - depth_scale_m: depth_unit_scale_m(calibration.coordinate_unit), }) } - fn project(&self, pixel_x: f32, pixel_y: f32, raw_depth: f32) -> CuResult<(f32, f32, f32)> { - let z = raw_depth * self.depth_scale_m; + fn project(&self, pixel_x: f32, pixel_y: f32, depth_m: f32) -> CuResult<(f32, f32, f32)> { + let z = depth_m; let x = (pixel_x - self.cx) * z / self.fx; let y = self.y_sign * (pixel_y - self.cy) * z / self.fy; if !x.is_finite() || !y.is_finite() || !z.is_finite() { @@ -234,16 +236,6 @@ impl ProjectionIntrinsics { } } -fn depth_unit_scale_m(unit: ZedCoordinateUnit) -> f32 { - match unit { - ZedCoordinateUnit::Millimeter => 1.0e-3, - ZedCoordinateUnit::Centimeter => 1.0e-2, - ZedCoordinateUnit::Meter => 1.0, - ZedCoordinateUnit::Inch => 0.0254, - ZedCoordinateUnit::Foot => 0.3048, - } -} - fn representative_tov(tov: Tov) -> CuTime { match tov { Tov::Time(time) => time, @@ -311,6 +303,11 @@ mod linux_impl { mat: Mat, } + struct DepthSlot { + handle: CuHandle>, + mat: Mat, + } + struct RasterSlot { handle: CuHandle>, mat: Mat, @@ -319,7 +316,7 @@ mod linux_impl { struct OutputSlot { left: ImageSlot, right: ImageSlot, - depth: RasterSlot, + depth: DepthSlot, confidence: Option, } @@ -350,7 +347,7 @@ mod linux_impl { #[reflect(ignore)] right_format: CuImageBufferFormat, #[reflect(ignore)] - depth_format: ZedRasterFormat, + depth_format: CuDepthMapFormat, #[reflect(ignore)] confidence_format: Option, emit_confidence: bool, @@ -418,8 +415,8 @@ mod linux_impl { let left_format = rgba_format(resolution); let right_format = rgba_format(resolution); - let depth_format = raster_format(resolution); - let confidence_format = emit_confidence.then_some(depth_format); + let depth_format = depth_map_format(resolution); + let confidence_format = emit_confidence.then(|| raster_format(resolution)); let slots = build_output_slots( pool_slots, resolution, @@ -493,7 +490,7 @@ mod linux_impl { .retrieve_right(&mut slot.right.mat) .map_err(|e| CuError::new_with_cause("Could not retrieve ZED right image", e))?; self.camera - .retrieve_depth(&mut slot.depth.mat) + .retrieve_depth_u16_mm(&mut slot.depth.mat) .map_err(|e| CuError::new_with_cause("Could not retrieve ZED depth map", e))?; stereo.set_payload(ZedStereoImages { @@ -502,11 +499,9 @@ mod linux_impl { }); stereo.tov = Tov::Time(frame_tov); - depth.set_payload(raster_payload_from_handle( - seq, - &slot.depth.handle, + depth.set_payload(ZedDepthMap::from_integer( self.depth_format, - ZedDepthMap::new, + slot.depth.handle.clone(), )); depth.tov = Tov::Time(frame_tov); @@ -857,12 +852,20 @@ mod linux_impl { } } + fn depth_map_format(resolution: Resolution) -> CuDepthMapFormat { + CuDepthMapFormat { + width: resolution.width(), + height: resolution.height(), + stride: resolution.width(), + } + } + fn build_output_slots( slot_count: usize, resolution: Resolution, left_format: CuImageBufferFormat, right_format: CuImageBufferFormat, - depth_format: ZedRasterFormat, + depth_format: CuDepthMapFormat, confidence_format: Option, ) -> CuResult> { (0..slot_count) @@ -870,7 +873,7 @@ mod linux_impl { Ok(OutputSlot { left: build_image_slot(resolution, left_format)?, right: build_image_slot(resolution, right_format)?, - depth: build_raster_slot(resolution, depth_format)?, + depth: build_depth_slot(resolution, depth_format)?, confidence: confidence_format .map(|format| build_raster_slot(resolution, format)) .transpose()?, @@ -922,6 +925,17 @@ mod linux_impl { Ok(RasterSlot { handle, mat }) } + fn build_depth_slot(resolution: Resolution, format: CuDepthMapFormat) -> CuResult { + let handle = CuHandle::new_detached(vec![0u16; format.required_elements()]); + let (ptr, len_elements) = handle.with_inner_mut(|inner| (inner.as_mut_ptr(), inner.len())); + let mat = unsafe { + Mat::from_external_cpu_buffer(resolution, format.stride as usize, len_elements, ptr) + } + .map_err(|e| CuError::new_with_cause("Could not alias ZED depth buffer as sl::Mat", e))?; + + Ok(DepthSlot { handle, mat }) + } + fn handle_is_available(handle: &CuHandle) -> bool where T: ArrayLike, @@ -973,12 +987,6 @@ mod linux_impl { fn set_seq(&mut self, seq: u64); } - impl RasterSeq for ZedDepthMap> { - fn set_seq(&mut self, seq: u64) { - self.seq = seq; - } - } - impl RasterSeq for ZedConfidenceMap> { fn set_seq(&mut self, seq: u64) { self.seq = seq; @@ -1237,9 +1245,9 @@ mod tests { } } - fn depth_msg(width: u32, height: u32, values: Vec) -> CuMsg>> { - let mut msg = CuMsg::new(Some(ZedDepthMap::new( - ZedRasterFormat { + fn depth_msg(width: u32, height: u32, values: Vec) -> CuMsg { + let mut msg = CuMsg::new(Some(ZedDepthMap::from_integer( + CuDepthMapFormat { width, height, stride: width, @@ -1254,7 +1262,7 @@ mod tests { fn depth_to_pointcloud_projects_image_coordinates() { let ctx = CuContext::new_with_clock(); let mut task = ZedDepthToPointCloud::<4>::new(None, ()).expect("task"); - let depth = depth_msg(2, 2, vec![2.0, 2.0, 2.0, 2.0]); + let depth = depth_msg(2, 2, vec![2_000, 2_000, 2_000, 2_000]); let calibration = CuMsg::new(Some(CuLatchedStateUpdate::Set(calibration_bundle( ZedCoordinateSystem::Image, 2, @@ -1279,7 +1287,7 @@ mod tests { fn depth_to_pointcloud_flips_y_for_left_handed_y_up() { let ctx = CuContext::new_with_clock(); let mut task = ZedDepthToPointCloud::<4>::new(None, ()).expect("task"); - let depth = depth_msg(2, 2, vec![1.0, 1.0, 1.0, 1.0]); + let depth = depth_msg(2, 2, vec![1_000, 1_000, 1_000, 1_000]); let calibration = CuMsg::new(Some(CuLatchedStateUpdate::Set(calibration_bundle( ZedCoordinateSystem::LeftHandedYUp, 2, @@ -1300,7 +1308,7 @@ mod tests { fn depth_to_pointcloud_scales_intrinsics_to_raster_size() { let ctx = CuContext::new_with_clock(); let mut task = ZedDepthToPointCloud::<4>::new(None, ()).expect("task"); - let depth = depth_msg(2, 2, vec![4.0, 4.0, 4.0, 4.0]); + let depth = depth_msg(2, 2, vec![4_000, 4_000, 4_000, 4_000]); let calibration = CuMsg::new(Some(CuLatchedStateUpdate::Set(calibration_bundle( ZedCoordinateSystem::Image, 4, @@ -1321,7 +1329,7 @@ mod tests { fn depth_to_pointcloud_skips_invalid_depth_samples() { let ctx = CuContext::new_with_clock(); let mut task = ZedDepthToPointCloud::<4>::new(None, ()).expect("task"); - let depth = depth_msg(2, 2, vec![1.0, 0.0, f32::NAN, -1.0]); + let depth = depth_msg(2, 2, vec![1_000, 0, 0, 0]); let calibration = CuMsg::new(Some(CuLatchedStateUpdate::Set(calibration_bundle( ZedCoordinateSystem::Image, 2, diff --git a/components/sources/cu_zed/src/payloads.rs b/components/sources/cu_zed/src/payloads.rs index 7ff48f1..ff59c3a 100644 --- a/components/sources/cu_zed/src/payloads.rs +++ b/components/sources/cu_zed/src/payloads.rs @@ -2,6 +2,7 @@ use bincode::de::Decoder; use bincode::error::DecodeError; use bincode::{Decode, Encode}; use core::fmt::Debug; +use cu_sensor_payloads::{CuDepthInteger, CuDepthMap, CuDepthMillimeter}; use cu_transform::FrameTransform; use cu29::prelude::*; use serde::{Deserialize, Serialize, Serializer}; @@ -138,13 +139,15 @@ macro_rules! impl_f32_raster_payload { }; } -impl_f32_raster_payload!(ZedDepthMap, "cu_zed::ZedDepthMap", "ZedDepthMap"); impl_f32_raster_payload!( ZedConfidenceMap, "cu_zed::ZedConfidenceMap", "ZedConfidenceMap" ); +/// Compact ZED depth map using unsigned 16-bit samples at one millimeter per increment. +pub type ZedDepthMap = CuDepthMap, CuDepthInteger>; + #[derive( Default, Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize, Encode, Decode, Reflect, )]