mod payloads;
pub use payloads::*;
use cu_sensor_payloads::{
BarometerPayload, CuDepthMapFormat, CuImage, Distance, ImuPayload, MagnetometerPayload,
PointCloudSoa, Reflectivity,
};
use cu29::bincode::de::Decoder;
use cu29::bincode::error::DecodeError;
use cu29::bincode::{Decode, Encode};
use cu29::prelude::*;
use cu29::units::si::length::meter;
use cu29::units::si::ratio::percent;
use serde::{Deserialize, Serialize};
#[derive(Default, Debug, Clone, Encode, Serialize, Deserialize, Reflect)]
#[bincode(crate = "cu29::bincode")]
#[reflect(from_reflect = false, no_field_bounds, type_path = false)]
pub struct ZedStereoImages {
pub left: CuImage<Vec<u8>>,
pub right: CuImage<Vec<u8>>,
}
impl TypePath for ZedStereoImages {
fn type_path() -> &'static str {
"cu_zed::ZedStereoImages"
}
fn short_type_path() -> &'static str {
"ZedStereoImages"
}
fn type_ident() -> Option<&'static str> {
Some("ZedStereoImages")
}
fn crate_name() -> Option<&'static str> {
Some("cu_zed")
}
fn module_path() -> Option<&'static str> {
Some("cu_zed")
}
}
impl Decode<()> for ZedStereoImages {
fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
let left: CuImage<Vec<u8>> = Decode::decode(decoder)?;
let right: CuImage<Vec<u8>> = Decode::decode(decoder)?;
Ok(Self { left, right })
}
}
pub type ZedSourceOutputs = (
CuMsg<ZedStereoImages>,
CuMsg<ZedDepthMap>,
CuMsg<ZedConfidenceMap<Vec<f32>>>,
CuMsg<CuLatchedStateUpdate<ZedCalibrationBundle>>,
CuMsg<CuLatchedStateUpdate<ZedRigTransforms>>,
CuMsg<ImuPayload>,
CuMsg<MagnetometerPayload>,
CuMsg<BarometerPayload>,
CuMsg<ZedFrameMeta>,
);
pub type ZedPointCloudHd720 = PointCloudSoa<{ 1280 * 720 }>;
pub type ZedDepthToPointCloudHd720 = ZedDepthToPointCloud<{ 1280 * 720 }>;
#[derive(Default, Reflect)]
#[reflect(from_reflect = false)]
pub struct ZedDepthToPointCloud<const MAX_POINTS: usize> {
calibration: CuLatchedState<ZedCalibrationBundle>,
}
impl<const MAX_POINTS: usize> Freezable for ZedDepthToPointCloud<MAX_POINTS> {}
impl<const MAX_POINTS: usize> CuTask for ZedDepthToPointCloud<MAX_POINTS> {
type Resources<'r> = ();
type Input<'m> = input_msg!('m, ZedDepthMap, CuLatchedStateUpdate<ZedCalibrationBundle>);
type Output<'m> = output_msg!(PointCloudSoa<MAX_POINTS>);
fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
where
Self: Sized,
{
Ok(Self::default())
}
fn process(
&mut self,
_ctx: &CuContext,
input: &Self::Input<'_>,
output: &mut Self::Output<'_>,
) -> CuResult<()> {
let (depth_msg, calibration_msg) = *input;
self.apply_calibration_update(calibration_msg.payload());
let Some(depth) = depth_msg.payload() else {
output.clear_payload();
output.tov = depth_msg.tov;
output.metadata.set_status("no depth");
return Ok(());
};
let Some(calibration) = self.calibration.as_ref() else {
output.clear_payload();
output.tov = depth_msg.tov;
output.metadata.set_status("no calib");
return Ok(());
};
let projection = ProjectionIntrinsics::from_bundle(depth.format, calibration)?;
let point_tov = representative_tov(depth_msg.tov);
let projected_points = {
let pointcloud = output.payload_mut().get_or_insert_with(Default::default);
pointcloud.len = 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",
format.width, format.height
)));
}
let (x, y, z) = projection.project(
col as f32,
row as f32,
depth_value.get::<meter>(),
)?;
let idx = pointcloud.len;
pointcloud.tov[idx] = point_tov;
pointcloud.x[idx] = Distance::new::<meter>(x);
pointcloud.y[idx] = Distance::new::<meter>(y);
pointcloud.z[idx] = Distance::new::<meter>(z);
pointcloud.i[idx] = Reflectivity::new::<percent>(0.0);
pointcloud.return_order[idx] = 0;
pointcloud.len += 1;
}
}
Ok(pointcloud.len)
})?
};
output.tov = depth_msg.tov;
output.metadata.set_status(projected_points);
Ok(())
}
}
impl<const MAX_POINTS: usize> ZedDepthToPointCloud<MAX_POINTS> {
fn apply_calibration_update(
&mut self,
update: Option<&CuLatchedStateUpdate<ZedCalibrationBundle>>,
) {
match update {
Some(CuLatchedStateUpdate::Set(bundle)) => {
self.calibration = CuLatchedState::Set(bundle.clone());
}
Some(CuLatchedStateUpdate::Clear) => {
self.calibration = CuLatchedState::Unset;
}
Some(CuLatchedStateUpdate::NoChange) | None => {}
}
}
}
#[derive(Clone, Copy, Debug)]
struct ProjectionIntrinsics {
fx: f32,
fy: f32,
cx: f32,
cy: f32,
y_sign: f32,
}
impl ProjectionIntrinsics {
fn from_bundle(format: CuDepthMapFormat, calibration: &ZedCalibrationBundle) -> CuResult<Self> {
if calibration.left.width == 0 || calibration.left.height == 0 {
return Err(CuError::from("ZED calibration reported a zero-sized image"));
}
if calibration.left.fx.abs() <= f32::EPSILON || calibration.left.fy.abs() <= f32::EPSILON {
return Err(CuError::from("ZED calibration reported zero focal length"));
}
let scale_x = format.width as f32 / calibration.left.width as f32;
let scale_y = format.height as f32 / calibration.left.height as f32;
let y_sign = match calibration.coordinate_system {
ZedCoordinateSystem::Image => 1.0,
ZedCoordinateSystem::LeftHandedYUp => -1.0,
other => {
return Err(CuError::from(format!(
"ZedDepthToPointCloud only supports IMAGE and LEFT_HANDED_Y_UP depth projections, got {other:?}"
)));
}
};
Ok(Self {
fx: calibration.left.fx * scale_x,
fy: calibration.left.fy * scale_y,
cx: calibration.left.cx * scale_x,
cy: calibration.left.cy * scale_y,
y_sign,
})
}
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() {
return Err(CuError::from(
"ZED depth projection produced a non-finite point",
));
}
Ok((x, y, z))
}
}
fn representative_tov(tov: Tov) -> CuTime {
match tov {
Tov::Time(time) => time,
Tov::Range(range) => range.start,
Tov::None => CuTime::default(),
}
}
#[cfg(not(target_os = "linux"))]
mod empty_impl {
use super::*;
#[derive(Reflect)]
#[reflect(from_reflect = false)]
pub struct Zed;
impl Freezable for Zed {}
impl CuSrcTask for Zed {
type Resources<'r> = ();
type Output<'m> = ZedSourceOutputs;
fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
where
Self: Sized,
{
Ok(Self)
}
fn process(&mut self, _ctx: &CuContext, output: &mut Self::Output<'_>) -> CuResult<()> {
let (stereo, depth, confidence, calib, transforms, imu, mag, baro, meta) = output;
stereo.clear_payload();
depth.clear_payload();
confidence.clear_payload();
calib.set_payload(CuLatchedStateUpdate::NoChange);
transforms.set_payload(CuLatchedStateUpdate::NoChange);
imu.clear_payload();
mag.clear_payload();
baro.clear_payload();
meta.clear_payload();
Ok(())
}
}
}
#[cfg(not(target_os = "linux"))]
pub use empty_impl::Zed;
#[cfg(target_os = "linux")]
mod linux_impl {
use super::*;
use core::mem::size_of;
use cu_sensor_payloads::CuImageBufferFormat;
use cu_transform::FrameTransform;
use std::path::PathBuf;
use zed_sdk::{
CalibrationParameters, Camera, CameraImuTransform, CameraInformation, CameraParameters,
CoordinateSystem, DepthMode, ErrorCode, InputSource, OpenOptions, ReferenceFrame,
Resolution, ResolutionPreset, RuntimeParameters, SensorsConfiguration, SensorsData, Unit,
};
use zed_sdk::{Mat, Rgba8};
struct ImageSlot {
handle: CuHandle<Vec<u8>>,
mat: Mat<Rgba8>,
}
struct DepthSlot {
handle: CuHandle<Vec<u16>>,
mat: Mat<u16>,
}
struct RasterSlot {
handle: CuHandle<Vec<f32>>,
mat: Mat<f32>,
}
struct OutputSlot {
left: ImageSlot,
right: ImageSlot,
depth: DepthSlot,
confidence: Option<RasterSlot>,
}
impl OutputSlot {
fn is_available(&self) -> bool {
handle_is_available(&self.left.handle)
&& handle_is_available(&self.right.handle)
&& handle_is_available(&self.depth.handle)
&& self
.confidence
.as_ref()
.is_none_or(|slot| handle_is_available(&slot.handle))
}
}
#[derive(Reflect)]
#[reflect(from_reflect = false)]
pub struct Zed {
#[reflect(ignore)]
camera: Camera,
#[reflect(ignore)]
runtime: RuntimeParameters,
#[reflect(ignore)]
slots: Vec<OutputSlot>,
next_slot: usize,
#[reflect(ignore)]
left_format: CuImageBufferFormat,
#[reflect(ignore)]
right_format: CuImageBufferFormat,
#[reflect(ignore)]
depth_format: CuDepthMapFormat,
#[reflect(ignore)]
confidence_format: Option<ZedRasterFormat>,
emit_confidence: bool,
emit_imu: bool,
emit_mag: bool,
emit_baro: bool,
seq: u64,
#[reflect(ignore)]
pending_calibration: Option<ZedCalibrationBundle>,
#[reflect(ignore)]
pending_transforms: Option<ZedRigTransforms>,
}
impl Freezable for Zed {}
unsafe impl Send for Zed {}
unsafe impl Sync for Zed {}
impl CuSrcTask for Zed {
type Resources<'r> = ();
type Output<'m> = ZedSourceOutputs;
fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
where
Self: Sized,
{
let coordinate_system = config_zed_coordinate_system(config)?;
let coordinate_unit = config_zed_coordinate_unit(config)?;
let open = build_open_options(config, coordinate_system, coordinate_unit)?;
let runtime = build_runtime_parameters(config)?;
let emit_confidence = config_bool(config, "emit_confidence", false)?;
let emit_imu = config_bool(config, "emit_imu", true)?;
let emit_mag = config_bool(config, "emit_mag", true)?;
let emit_baro = config_bool(config, "emit_baro", true)?;
let pool_slots = config_u32(config, "pool_slots", 4)?.max(1) as usize;
let frame_prefix =
config_string_opt(config, "frame_id_prefix")?.unwrap_or_else(|| "zed".to_string());
let camera = Camera::open(open)
.map_err(|e| CuError::new_with_cause("Could not open ZED camera", e))?;
let info = camera.info().map_err(|e| {
CuError::new_with_cause("Could not fetch ZED camera information", e)
})?;
let calibration = camera.calibration_parameters(false).map_err(|e| {
CuError::new_with_cause("Could not fetch ZED calibration parameters", e)
})?;
let sensors_configuration = camera.sensors_configuration().ok();
let camera_imu = sensors_configuration
.as_ref()
.filter(|config| has_motion_sensors(config))
.map(|_| camera.camera_imu_transform())
.transpose()
.map_err(|e| {
CuError::new_with_cause("Could not fetch ZED camera->IMU transform", e)
})?;
let resolution = camera
.resolution()
.map_err(|e| CuError::new_with_cause("Could not read ZED camera resolution", e))?;
let left_format = rgba_format(resolution);
let right_format = rgba_format(resolution);
let depth_format = depth_map_format(resolution);
let confidence_format = emit_confidence.then(|| raster_format(resolution));
let slots = build_output_slots(
pool_slots,
resolution,
left_format,
right_format,
depth_format,
confidence_format,
)?;
let pending_calibration = Some(build_calibration_bundle(
&info,
&calibration,
camera_imu.as_ref(),
coordinate_system,
coordinate_unit,
));
let pending_left_to_right = build_left_to_right_transform(&frame_prefix, &calibration);
let pending_camera_to_imu = camera_imu
.as_ref()
.map(|transform| build_camera_to_imu_transform(&frame_prefix, transform));
let pending_transforms = Some(ZedRigTransforms {
left_to_right: ZedNamedTransform::from_frame_transform(&pending_left_to_right),
camera_to_imu: pending_camera_to_imu
.as_ref()
.map(ZedNamedTransform::from_frame_transform)
.unwrap_or_default(),
has_camera_to_imu: pending_camera_to_imu.is_some(),
});
Ok(Self {
camera,
runtime,
slots,
next_slot: 0,
left_format,
right_format,
depth_format,
confidence_format,
emit_confidence,
emit_imu,
emit_mag,
emit_baro,
seq: 0,
pending_calibration,
pending_transforms,
})
}
fn process(&mut self, _ctx: &CuContext, output: &mut Self::Output<'_>) -> CuResult<()> {
let (stereo, depth, confidence, calibration, transforms, imu, mag, baro, meta) = output;
self.camera
.grab(&self.runtime)
.map_err(|e| CuError::new_with_cause("ZED grab failed", e))?;
let frame_tov: CuTime = self.camera.image_timestamp().into();
let seq = self.seq;
self.seq = self.seq.wrapping_add(1);
let slot_index = acquire_output_slot_index(&self.slots, &mut self.next_slot)
.ok_or_else(|| {
CuError::from(
"No reusable ZED output slot available; increase pool_slots or release downstream handles sooner",
)
})?;
let slot = &mut self.slots[slot_index];
self.camera
.retrieve_left(&mut slot.left.mat)
.map_err(|e| CuError::new_with_cause("Could not retrieve ZED left image", e))?;
self.camera
.retrieve_right(&mut slot.right.mat)
.map_err(|e| CuError::new_with_cause("Could not retrieve ZED right image", e))?;
self.camera
.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 {
left: image_payload_from_handle(seq, &slot.left.handle, self.left_format),
right: image_payload_from_handle(seq, &slot.right.handle, self.right_format),
});
stereo.tov = Tov::Time(frame_tov);
depth.set_payload(ZedDepthMap::from_integer(
self.depth_format,
slot.depth.handle.clone(),
));
depth.tov = Tov::Time(frame_tov);
if self.emit_confidence {
let confidence_slot = slot.confidence.as_mut().ok_or_else(|| {
CuError::from("confidence output requested without a backing mat")
})?;
self.camera
.retrieve_confidence(&mut confidence_slot.mat)
.map_err(|e| {
CuError::new_with_cause("Could not retrieve ZED confidence map", e)
})?;
let confidence_format = self
.confidence_format
.ok_or_else(|| CuError::from("confidence output requested without a format"))?;
confidence.set_payload(raster_payload_from_handle(
seq,
&confidence_slot.handle,
confidence_format,
ZedConfidenceMap::new,
));
confidence.tov = Tov::Time(frame_tov);
} else {
confidence.clear_payload();
}
emit_latched(calibration, frame_tov, self.pending_calibration.take());
emit_latched(transforms, frame_tov, self.pending_transforms.take());
let sensor_result = self.camera.sensors_data();
let sensors = match sensor_result {
Ok(sensors) => Some(sensors),
Err(err) if is_optional_sensor_error(&err) => None,
Err(err) => {
return Err(CuError::new_with_cause(
"Could not retrieve ZED sensors data",
err,
));
}
};
if self.emit_imu {
if let Some(imu_data) = sensors.as_ref().and_then(|data| data.imu) {
let temperature_c = sensors
.as_ref()
.map(|data| data.temperature.imu_temp_c)
.unwrap_or_default();
imu.set_payload(ImuPayload::from_raw(
[
imu_data.linear_acceleration.x,
imu_data.linear_acceleration.y,
imu_data.linear_acceleration.z,
],
[
imu_data.angular_velocity.x,
imu_data.angular_velocity.y,
imu_data.angular_velocity.z,
],
temperature_c,
));
imu.tov = Tov::Time(imu_data.timestamp_ns.into());
} else {
imu.clear_payload();
}
} else {
imu.clear_payload();
}
if self.emit_mag {
if let Some(mag_data) = sensors.as_ref().and_then(|data| data.magnetometer) {
mag.set_payload(MagnetometerPayload::from_raw([
mag_data.magnetic_field_ut.x,
mag_data.magnetic_field_ut.y,
mag_data.magnetic_field_ut.z,
]));
mag.tov = Tov::Time(mag_data.timestamp_ns.into());
} else {
mag.clear_payload();
}
} else {
mag.clear_payload();
}
if self.emit_baro {
if let Some(baro_data) = sensors.as_ref().and_then(|data| data.barometer) {
let temperature_c = sensors
.as_ref()
.map(|data| data.temperature.barometer_temp_c)
.unwrap_or_default();
baro.set_payload(BarometerPayload::from_raw(
baro_data.pressure_pa,
temperature_c,
));
baro.tov = Tov::Time(baro_data.timestamp_ns.into());
} else {
baro.clear_payload();
}
} else {
baro.clear_payload();
}
meta.set_payload(build_frame_meta(seq, &self.camera, sensors.as_ref()));
meta.tov = Tov::Time(frame_tov);
Ok(())
}
}
fn config_bool(config: Option<&ComponentConfig>, key: &str, default: bool) -> CuResult<bool> {
Ok(match config {
Some(config) => config.get::<bool>(key)?.unwrap_or(default),
None => default,
})
}
fn config_u32(config: Option<&ComponentConfig>, key: &str, default: u32) -> CuResult<u32> {
Ok(match config {
Some(config) => config.get::<u32>(key)?.unwrap_or(default),
None => default,
})
}
fn config_string_opt(config: Option<&ComponentConfig>, key: &str) -> CuResult<Option<String>> {
match config {
Some(config) => config.get::<String>(key).map_err(Into::into),
None => Ok(None),
}
}
fn build_open_options(
config: Option<&ComponentConfig>,
coordinate_system: ZedCoordinateSystem,
coordinate_unit: ZedCoordinateUnit,
) -> CuResult<OpenOptions> {
let mut options = OpenOptions::default();
if let (Some(serial_number), Some(port)) = (
config_u32_opt(config, "gmsl_serial_number")?,
config_i32_opt(config, "gmsl_port")?,
) {
options = options.source(InputSource::Gmsl {
serial_number,
port,
});
} else if let Some(serial_number) = config_u32_opt(config, "serial_number")? {
options = options.source(InputSource::SerialNumber(serial_number));
} else if let Some(svo_file) = config_string_opt(config, "svo_file")? {
options = options.source(InputSource::SvoFile(PathBuf::from(svo_file)));
} else if let Some(stream_ip) = config_string_opt(config, "stream_ip")? {
let stream_port = config_i32_opt(config, "stream_port")?.unwrap_or(30000);
options = options.source(InputSource::Stream {
ip: stream_ip,
port: stream_port,
});
} else {
let device_id = config_i32_opt(config, "device_id")?.unwrap_or(0);
options = options.camera_device_id(device_id);
}
if let Some(resolution) = config_string_opt(config, "resolution")? {
let parsed = resolution
.to_ascii_uppercase()
.parse::<ResolutionPreset>()
.map_err(|err| CuError::from(format!("Invalid ZED resolution preset: {err}")))?;
options = options.resolution(parsed);
}
if let Some(fps) = config_i32_opt(config, "fps")? {
options = options.fps(fps);
}
if let Some(depth_mode) = config_string_opt(config, "depth_mode")? {
let parsed = depth_mode
.to_ascii_uppercase()
.parse::<DepthMode>()
.map_err(|err| CuError::from(format!("Invalid ZED depth mode: {err}")))?;
options = options.depth_mode(parsed);
}
options = options.coordinate_system(to_sdk_coordinate_system(coordinate_system));
options = options.coordinate_unit(to_sdk_coordinate_unit(coordinate_unit));
let depth_min = config_f32_opt(config, "depth_minimum_distance_m")?;
let depth_max = config_f32_opt(config, "depth_maximum_distance_m")?.unwrap_or(40.0);
options = options.depth_range_m(depth_min, depth_max);
if let Some(timeout_ms) = config_u32_opt(config, "open_timeout_ms")? {
options = options.open_timeout(core::time::Duration::from_millis(timeout_ms as u64));
}
if let Some(sensors_required) = config_bool_opt(config, "sensors_required")? {
options = options.sensors_required(sensors_required);
}
if let Some(verbose) = config_i32_opt(config, "sdk_verbose")? {
options = options.sdk_verbose(verbose);
}
if let Some(settings_path) = config_string_opt(config, "settings_path")? {
options = options.settings_path(settings_path);
}
if let Some(calibration_path) = config_string_opt(config, "opencv_calibration_path")? {
options = options.opencv_calibration_path(calibration_path);
}
Ok(options)
}
fn config_zed_coordinate_system(
config: Option<&ComponentConfig>,
) -> CuResult<ZedCoordinateSystem> {
match config_string_opt(config, "coordinate_system")? {
Some(value) => parse_zed_coordinate_system(&value),
None => Ok(ZedCoordinateSystem::default()),
}
}
fn config_zed_coordinate_unit(config: Option<&ComponentConfig>) -> CuResult<ZedCoordinateUnit> {
match config_string_opt(config, "coordinate_unit")? {
Some(value) => parse_zed_coordinate_unit(&value),
None => Ok(ZedCoordinateUnit::default()),
}
}
fn build_runtime_parameters(config: Option<&ComponentConfig>) -> CuResult<RuntimeParameters> {
let mut runtime = RuntimeParameters::default();
if let Some(reference_frame) = config_string_opt(config, "reference_frame")? {
let parsed = parse_reference_frame(&reference_frame)?;
runtime = runtime.reference_frame(parsed);
}
if let Some(enable_fill_mode) = config_bool_opt(config, "enable_fill_mode")? {
runtime = runtime.enable_fill_mode(enable_fill_mode);
}
if let Some(confidence_threshold) = config_i32_opt(config, "confidence_threshold")? {
runtime = runtime.confidence_threshold(confidence_threshold);
}
if let Some(texture_confidence_threshold) =
config_i32_opt(config, "texture_confidence_threshold")?
{
runtime = runtime.texture_confidence_threshold(texture_confidence_threshold);
}
Ok(runtime)
}
fn config_bool_opt(config: Option<&ComponentConfig>, key: &str) -> CuResult<Option<bool>> {
match config {
Some(config) => config.get::<bool>(key).map_err(Into::into),
None => Ok(None),
}
}
fn config_u32_opt(config: Option<&ComponentConfig>, key: &str) -> CuResult<Option<u32>> {
match config {
Some(config) => config.get::<u32>(key).map_err(Into::into),
None => Ok(None),
}
}
fn config_i32_opt(config: Option<&ComponentConfig>, key: &str) -> CuResult<Option<i32>> {
match config {
Some(config) => config.get::<i32>(key).map_err(Into::into),
None => Ok(None),
}
}
fn config_f32_opt(config: Option<&ComponentConfig>, key: &str) -> CuResult<Option<f32>> {
match config {
Some(config) => config
.get::<f64>(key)
.map(|value| value.map(|value| value as f32))
.map_err(Into::into),
None => Ok(None),
}
}
fn parse_zed_coordinate_system(value: &str) -> CuResult<ZedCoordinateSystem> {
match value.to_ascii_uppercase().as_str() {
"IMAGE" => Ok(ZedCoordinateSystem::Image),
"LEFT_HANDED_Y_UP" => Ok(ZedCoordinateSystem::LeftHandedYUp),
"RIGHT_HANDED_Y_UP" => Ok(ZedCoordinateSystem::RightHandedYUp),
"RIGHT_HANDED_Z_UP" => Ok(ZedCoordinateSystem::RightHandedZUp),
"LEFT_HANDED_Z_UP" => Ok(ZedCoordinateSystem::LeftHandedZUp),
"RIGHT_HANDED_Z_UP_X_FWD" | "RIGHT_HANDED_Z_UP_X_FORWARD" => {
Ok(ZedCoordinateSystem::RightHandedZUpXForward)
}
_ => Err(CuError::from(format!(
"Invalid ZED coordinate_system: {value}"
))),
}
}
fn parse_zed_coordinate_unit(value: &str) -> CuResult<ZedCoordinateUnit> {
match value.to_ascii_uppercase().as_str() {
"MILLIMETER" | "MILLIMETERS" => Ok(ZedCoordinateUnit::Millimeter),
"CENTIMETER" | "CENTIMETERS" => Ok(ZedCoordinateUnit::Centimeter),
"METER" | "METERS" => Ok(ZedCoordinateUnit::Meter),
"INCH" | "INCHES" => Ok(ZedCoordinateUnit::Inch),
"FOOT" | "FEET" => Ok(ZedCoordinateUnit::Foot),
_ => Err(CuError::from(format!(
"Invalid ZED coordinate_unit: {value}"
))),
}
}
fn to_sdk_coordinate_system(value: ZedCoordinateSystem) -> CoordinateSystem {
match value {
ZedCoordinateSystem::Image => CoordinateSystem::Image,
ZedCoordinateSystem::LeftHandedYUp => CoordinateSystem::LeftHandedYUp,
ZedCoordinateSystem::RightHandedYUp => CoordinateSystem::RightHandedYUp,
ZedCoordinateSystem::RightHandedZUp => CoordinateSystem::RightHandedZUp,
ZedCoordinateSystem::LeftHandedZUp => CoordinateSystem::LeftHandedZUp,
ZedCoordinateSystem::RightHandedZUpXForward => CoordinateSystem::RightHandedZUpXForward,
}
}
fn to_sdk_coordinate_unit(value: ZedCoordinateUnit) -> Unit {
match value {
ZedCoordinateUnit::Millimeter => Unit::Millimeter,
ZedCoordinateUnit::Centimeter => Unit::Centimeter,
ZedCoordinateUnit::Meter => Unit::Meter,
ZedCoordinateUnit::Inch => Unit::Inch,
ZedCoordinateUnit::Foot => Unit::Foot,
}
}
fn parse_reference_frame(value: &str) -> CuResult<ReferenceFrame> {
match value.to_ascii_uppercase().as_str() {
"WORLD" => Ok(ReferenceFrame::World),
"CAMERA" => Ok(ReferenceFrame::Camera),
_ => Err(CuError::from(format!(
"Invalid ZED reference_frame: {value}"
))),
}
}
fn has_motion_sensors(config: &SensorsConfiguration) -> bool {
config.accelerometer.is_available || config.gyroscope.is_available
}
fn rgba_format(resolution: Resolution) -> CuImageBufferFormat {
CuImageBufferFormat {
width: resolution.width(),
height: resolution.height(),
stride: (resolution.width() as usize * size_of::<Rgba8>()) as u32,
pixel_format: *b"RGBA",
}
}
fn raster_format(resolution: Resolution) -> ZedRasterFormat {
ZedRasterFormat {
width: resolution.width(),
height: resolution.height(),
stride: resolution.width(),
}
}
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: CuDepthMapFormat,
confidence_format: Option<ZedRasterFormat>,
) -> CuResult<Vec<OutputSlot>> {
(0..slot_count)
.map(|_| {
Ok(OutputSlot {
left: build_image_slot(resolution, left_format)?,
right: build_image_slot(resolution, right_format)?,
depth: build_depth_slot(resolution, depth_format)?,
confidence: confidence_format
.map(|format| build_raster_slot(resolution, format))
.transpose()?,
})
})
.collect()
}
fn build_image_slot(
resolution: Resolution,
format: CuImageBufferFormat,
) -> CuResult<ImageSlot> {
let handle = CuHandle::new_detached(vec![0u8; format.byte_size()]);
let stride_bytes = format.stride as usize;
let stride_elems = stride_bytes / size_of::<Rgba8>();
if stride_elems * size_of::<Rgba8>() != stride_bytes {
return Err(CuError::from(
"ZED image stride is not aligned to the RGBA pixel size",
));
}
let (ptr, len_bytes) = handle.with_inner_mut(|inner| (inner.as_mut_ptr(), inner.len()));
if len_bytes % size_of::<Rgba8>() != 0 {
return Err(CuError::from(
"ZED image buffer length is not aligned to the RGBA pixel size",
));
}
let mat = unsafe {
Mat::from_external_cpu_buffer(
resolution,
stride_elems,
len_bytes / size_of::<Rgba8>(),
ptr.cast::<Rgba8>(),
)
}
.map_err(|e| CuError::new_with_cause("Could not alias ZED image buffer as sl::Mat", e))?;
Ok(ImageSlot { handle, mat })
}
fn build_raster_slot(resolution: Resolution, format: ZedRasterFormat) -> CuResult<RasterSlot> {
let handle = CuHandle::new_detached(vec![0f32; format.len_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 raster buffer as sl::Mat", e))?;
Ok(RasterSlot { handle, mat })
}
fn build_depth_slot(resolution: Resolution, format: CuDepthMapFormat) -> CuResult<DepthSlot> {
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<T>(handle: &CuHandle<T>) -> bool
where
T: ArrayLike,
{
handle.is_unique()
}
fn acquire_output_slot_index(slots: &[OutputSlot], next_slot: &mut usize) -> Option<usize> {
if slots.is_empty() {
return None;
}
for offset in 0..slots.len() {
let index = (*next_slot + offset) % slots.len();
if slots[index].is_available() {
*next_slot = (index + 1) % slots.len();
return Some(index);
}
}
None
}
fn image_payload_from_handle(
seq: u64,
handle: &CuHandle<Vec<u8>>,
format: CuImageBufferFormat,
) -> CuImage<Vec<u8>> {
let mut image = CuImage::new(format, handle.clone());
image.seq = seq;
image
}
fn raster_payload_from_handle<P>(
seq: u64,
handle: &CuHandle<Vec<f32>>,
format: ZedRasterFormat,
ctor: impl FnOnce(ZedRasterFormat, CuHandle<Vec<f32>>) -> P,
) -> P
where
P: RasterSeq,
{
let mut payload = ctor(format, handle.clone());
payload.set_seq(seq);
payload
}
trait RasterSeq {
fn set_seq(&mut self, seq: u64);
}
impl RasterSeq for ZedConfidenceMap<Vec<f32>> {
fn set_seq(&mut self, seq: u64) {
self.seq = seq;
}
}
fn emit_latched<T: CuMsgPayload>(
msg: &mut CuMsg<CuLatchedStateUpdate<T>>,
tov: CuTime,
pending: Option<T>,
) {
msg.tov = Tov::Time(tov);
match pending {
Some(value) => msg.set_payload(CuLatchedStateUpdate::Set(value)),
None => msg.set_payload(CuLatchedStateUpdate::NoChange),
}
}
fn build_calibration_bundle(
info: &CameraInformation,
calibration: &CalibrationParameters,
camera_imu: Option<&CameraImuTransform>,
coordinate_system: ZedCoordinateSystem,
coordinate_unit: ZedCoordinateUnit,
) -> ZedCalibrationBundle {
ZedCalibrationBundle {
serial_number: info.serial_number,
width: info.resolution.width(),
height: info.resolution.height(),
fps: info.fps,
coordinate_system,
coordinate_unit,
left: intrinsics_from_sdk(&calibration.left_cam),
right: intrinsics_from_sdk(&calibration.right_cam),
stereo_rotation_rodrigues: calibration.rotation_rodrigues,
stereo_translation_m: [
calibration.translation.x,
calibration.translation.y,
calibration.translation.z,
],
camera_to_imu_translation_m: camera_imu.map(|transform| {
[
transform.translation.x,
transform.translation.y,
transform.translation.z,
]
}),
camera_to_imu_quaternion_xyzw: camera_imu.map(|transform| transform.rotation_xyzw),
}
}
fn intrinsics_from_sdk(parameters: &CameraParameters) -> ZedCameraIntrinsics {
ZedCameraIntrinsics {
fx: parameters.fx,
fy: parameters.fy,
cx: parameters.cx,
cy: parameters.cy,
disto: parameters.disto,
v_fov: parameters.v_fov,
h_fov: parameters.h_fov,
d_fov: parameters.d_fov,
width: parameters.image_size.width(),
height: parameters.image_size.height(),
focal_length_metric: parameters.focal_length_metric,
}
}
fn build_left_to_right_transform(
frame_prefix: &str,
calibration: &CalibrationParameters,
) -> FrameTransform<f32> {
let transform = transform_from_rodrigues_translation(
calibration.rotation_rodrigues,
[
calibration.translation.x,
calibration.translation.y,
calibration.translation.z,
],
);
FrameTransform::new(
transform,
left_frame_id(frame_prefix),
right_frame_id(frame_prefix),
)
}
fn build_camera_to_imu_transform(
frame_prefix: &str,
camera_imu: &CameraImuTransform,
) -> FrameTransform<f32> {
let transform = transform_from_quaternion_translation(
camera_imu.rotation_xyzw,
[
camera_imu.translation.x,
camera_imu.translation.y,
camera_imu.translation.z,
],
);
FrameTransform::new(
transform,
left_frame_id(frame_prefix),
imu_frame_id(frame_prefix),
)
}
fn left_frame_id(frame_prefix: &str) -> String {
format!("{frame_prefix}_left")
}
fn right_frame_id(frame_prefix: &str) -> String {
format!("{frame_prefix}_right")
}
fn imu_frame_id(frame_prefix: &str) -> String {
format!("{frame_prefix}_imu")
}
fn build_frame_meta(seq: u64, camera: &Camera, sensors: Option<&SensorsData>) -> ZedFrameMeta {
let temps = sensors.map(|data| data.temperature);
ZedFrameMeta {
seq,
image_timestamp_ns: camera.image_timestamp(),
current_timestamp_ns: camera.current_timestamp(),
current_fps: camera.current_fps(),
camera_moving_state: sensors.map(|data| data.camera_moving_state),
image_sync_trigger: sensors.map(|data| data.image_sync_trigger),
imu_temp_c: temps.map(|data| data.imu_temp_c),
barometer_temp_c: temps.map(|data| data.barometer_temp_c),
onboard_left_temp_c: temps.map(|data| data.onboard_left_temp_c),
onboard_right_temp_c: temps.map(|data| data.onboard_right_temp_c),
}
}
fn is_optional_sensor_error(err: &zed_sdk::Error) -> bool {
matches!(
err.sdk_code(),
Some(
ErrorCode::SensorsDataRequired
| ErrorCode::SensorsNotAvailable
| ErrorCode::SensorsNotInitialized
| ErrorCode::MotionSensorsRequired
)
)
}
fn transform_from_rodrigues_translation(
rotation_rodrigues: [f32; 3],
translation: [f32; 3],
) -> cu_transform::Transform3D<f32> {
let theta = (rotation_rodrigues[0] * rotation_rodrigues[0]
+ rotation_rodrigues[1] * rotation_rodrigues[1]
+ rotation_rodrigues[2] * rotation_rodrigues[2])
.sqrt();
let mut matrix = [
[1.0f32, 0.0, 0.0, translation[0]],
[0.0, 1.0, 0.0, translation[1]],
[0.0, 0.0, 1.0, translation[2]],
[0.0, 0.0, 0.0, 1.0],
];
if theta > 1.0e-6 {
let x = rotation_rodrigues[0] / theta;
let y = rotation_rodrigues[1] / theta;
let z = rotation_rodrigues[2] / theta;
let cos_theta = theta.cos();
let sin_theta = theta.sin();
let one_minus_cos = 1.0 - cos_theta;
matrix[0][0] = cos_theta + x * x * one_minus_cos;
matrix[0][1] = x * y * one_minus_cos - z * sin_theta;
matrix[0][2] = x * z * one_minus_cos + y * sin_theta;
matrix[1][0] = y * x * one_minus_cos + z * sin_theta;
matrix[1][1] = cos_theta + y * y * one_minus_cos;
matrix[1][2] = y * z * one_minus_cos - x * sin_theta;
matrix[2][0] = z * x * one_minus_cos - y * sin_theta;
matrix[2][1] = z * y * one_minus_cos + x * sin_theta;
matrix[2][2] = cos_theta + z * z * one_minus_cos;
}
cu_transform::Transform3D::from_matrix(matrix)
}
fn transform_from_quaternion_translation(
rotation_xyzw: [f32; 4],
translation: [f32; 3],
) -> cu_transform::Transform3D<f32> {
let [x, y, z, w] = rotation_xyzw;
let norm = (x * x + y * y + z * z + w * w).sqrt();
let (x, y, z, w) = if norm > 1.0e-6 {
(x / norm, y / norm, z / norm, w / norm)
} else {
(0.0, 0.0, 0.0, 1.0)
};
cu_transform::Transform3D::from_matrix([
[
1.0 - 2.0 * (y * y + z * z),
2.0 * (x * y - z * w),
2.0 * (x * z + y * w),
translation[0],
],
[
2.0 * (x * y + z * w),
1.0 - 2.0 * (x * x + z * z),
2.0 * (y * z - x * w),
translation[1],
],
[
2.0 * (x * z - y * w),
2.0 * (y * z + x * w),
1.0 - 2.0 * (x * x + y * y),
translation[2],
],
[0.0, 0.0, 0.0, 1.0],
])
}
}
#[cfg(target_os = "linux")]
pub use linux_impl::Zed;
#[cfg(test)]
mod tests {
use super::*;
fn calibration_bundle(
coordinate_system: ZedCoordinateSystem,
width: u32,
height: u32,
) -> ZedCalibrationBundle {
ZedCalibrationBundle {
serial_number: 1,
width,
height,
fps: 30.0,
coordinate_system,
coordinate_unit: ZedCoordinateUnit::Meter,
left: ZedCameraIntrinsics {
fx: width as f32,
fy: height as f32,
cx: width as f32 / 2.0,
cy: height as f32 / 2.0,
width,
height,
..Default::default()
},
right: ZedCameraIntrinsics {
width,
height,
..Default::default()
},
..Default::default()
}
}
fn depth_msg(width: u32, height: u32, values: Vec<u16>) -> CuMsg<ZedDepthMap> {
let mut msg = CuMsg::new(Some(ZedDepthMap::from_integer(
CuDepthMapFormat {
width,
height,
stride: width,
},
CuHandle::new_detached(values),
)));
msg.tov = Tov::from(CuDuration(42));
msg
}
#[test]
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_000, 2_000, 2_000, 2_000]);
let calibration = CuMsg::new(Some(CuLatchedStateUpdate::Set(calibration_bundle(
ZedCoordinateSystem::Image,
2,
2,
))));
let input = (&depth, &calibration);
let mut output: <ZedDepthToPointCloud<4> as CuTask>::Output<'_> = Default::default();
task.process(&ctx, &input, &mut output).expect("process");
let payload = output.payload().expect("payload");
assert_eq!(payload.len, 4);
assert_eq!(payload.x[0].value, -1.0);
assert_eq!(payload.y[0].value, -1.0);
assert_eq!(payload.z[0].value, 2.0);
assert_eq!(payload.x[3].value, 0.0);
assert_eq!(payload.y[3].value, 0.0);
assert_eq!(payload.z[3].value, 2.0);
}
#[test]
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_000, 1_000, 1_000, 1_000]);
let calibration = CuMsg::new(Some(CuLatchedStateUpdate::Set(calibration_bundle(
ZedCoordinateSystem::LeftHandedYUp,
2,
2,
))));
let input = (&depth, &calibration);
let mut output: <ZedDepthToPointCloud<4> as CuTask>::Output<'_> = Default::default();
task.process(&ctx, &input, &mut output).expect("process");
let payload = output.payload().expect("payload");
assert_eq!(payload.len, 4);
assert_eq!(payload.y[0].value, 0.5);
assert_eq!(payload.y[2].value, -0.0);
}
#[test]
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_000, 4_000, 4_000, 4_000]);
let calibration = CuMsg::new(Some(CuLatchedStateUpdate::Set(calibration_bundle(
ZedCoordinateSystem::Image,
4,
4,
))));
let input = (&depth, &calibration);
let mut output: <ZedDepthToPointCloud<4> as CuTask>::Output<'_> = Default::default();
task.process(&ctx, &input, &mut output).expect("process");
let payload = output.payload().expect("payload");
assert_eq!(payload.len, 4);
assert_eq!(payload.x[3].value, 0.0);
assert_eq!(payload.y[3].value, 0.0);
}
#[test]
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_000, 0, 0, 0]);
let calibration = CuMsg::new(Some(CuLatchedStateUpdate::Set(calibration_bundle(
ZedCoordinateSystem::Image,
2,
2,
))));
let input = (&depth, &calibration);
let mut output: <ZedDepthToPointCloud<4> as CuTask>::Output<'_> = Default::default();
task.process(&ctx, &input, &mut output).expect("process");
let payload = output.payload().expect("payload");
assert_eq!(payload.len, 1);
assert_eq!(payload.z[0].value, 1.0);
}
}