From 772171196eecd330d4510e421182453f8abac486 Mon Sep 17 00:00:00 2001 From: Daniil Mordanov Date: Tue, 4 Aug 2026 19:52:57 +0700 Subject: [PATCH 1/2] feat(payloads): add latched camera calibration model --- .../payloads/cu_sensor_payloads/Cargo.toml | 1 + .../payloads/cu_sensor_payloads/README.md | 16 +- .../payloads/cu_sensor_payloads/src/camera.rs | 350 ++++++++++++++++++ .../payloads/cu_sensor_payloads/src/lib.rs | 2 + 4 files changed, 367 insertions(+), 2 deletions(-) create mode 100644 components/payloads/cu_sensor_payloads/src/camera.rs diff --git a/components/payloads/cu_sensor_payloads/Cargo.toml b/components/payloads/cu_sensor_payloads/Cargo.toml index 978cb1e5666..d2394e6ebb7 100644 --- a/components/payloads/cu_sensor_payloads/Cargo.toml +++ b/components/payloads/cu_sensor_payloads/Cargo.toml @@ -25,6 +25,7 @@ cu29-clock = { path = "../../../core/cu29_clock", version = "1.1.0-dev", default cu29 = { path = "../../../core/cu29", version = "1.1.0-dev", default-features = false, features = [ "units", ] } +libm = { version = "0.2", default-features = false } rerun = { workspace = true, optional = true } image = { workspace = true, default-features = false, optional = true } kornia-image = { version = "0.1.10", optional = true } diff --git a/components/payloads/cu_sensor_payloads/README.md b/components/payloads/cu_sensor_payloads/README.md index ae996ae1734..9b31fef1f75 100644 --- a/components/payloads/cu_sensor_payloads/README.md +++ b/components/payloads/cu_sensor_payloads/README.md @@ -3,7 +3,20 @@ Standardized sensor payload definitions for Copper. The crate contains common payload types used by Copper sources, tasks, and -sinks, including image and point-cloud related data structures. +sinks, including image, depth-map, camera-calibration, and point-cloud data +structures. + +## Camera calibration propagation + +`CuCameraModel` provides a fixed-size, allocation-free camera model containing +pinhole intrinsics and a standard distortion model. It is intentionally separate +from `CuImage` and `CuDepthMap`: a source can expose the model on a dedicated +output as `CuCameraModelUpdate` and send `Set(model)` only when calibration first +becomes available or changes. Later cycles carry `NoChange`, while each consumer +keeps a `CuCameraModelState` cache. + +This keeps the per-frame payload small without hiding calibration changes from +Copper's unified log and deterministic replay. ## Features @@ -12,4 +25,3 @@ sinks, including image and point-cloud related data structures. - `image` - `kornia` - `rerun` - diff --git a/components/payloads/cu_sensor_payloads/src/camera.rs b/components/payloads/cu_sensor_payloads/src/camera.rs new file mode 100644 index 00000000000..05b2741e420 --- /dev/null +++ b/components/payloads/cu_sensor_payloads/src/camera.rs @@ -0,0 +1,350 @@ +use bincode::{Decode, Encode}; +use core::fmt; +use cu29::prelude::*; +use serde::{Deserialize, Serialize}; + +/// Maximum number of distortion coefficients carried by [`CuCameraDistortion`]. +/// +/// Four coefficients cover equidistant fisheye models, five cover the common +/// Brown-Conrady model, and eight cover OpenCV's rational polynomial model. +pub const CAMERA_DISTORTION_COEFFICIENT_CAPACITY: usize = 8; + +/// Errors found while validating a camera model. +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +pub enum CuCameraModelError { + ZeroImageDimension, + NonFiniteIntrinsic, + NonPositiveFocalLength, + InvalidDistortionCoefficientCount { + model: CuCameraDistortionModel, + expected: usize, + found: usize, + }, + NonFiniteDistortionCoefficient { + index: usize, + }, +} + +impl fmt::Display for CuCameraModelError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + Self::ZeroImageDimension => write!(f, "camera image dimensions must be non-zero"), + Self::NonFiniteIntrinsic => write!(f, "camera intrinsics must be finite"), + Self::NonPositiveFocalLength => { + write!(f, "camera focal lengths must be positive") + } + Self::InvalidDistortionCoefficientCount { + model, + expected, + found, + } => write!( + f, + "{model:?} distortion expects {expected} coefficients, found {found}" + ), + Self::NonFiniteDistortionCoefficient { index } => { + write!(f, "camera distortion coefficient {index} is not finite") + } + } + } +} + +impl core::error::Error for CuCameraModelError {} + +/// Pinhole camera intrinsics in pixel units. +/// +/// Pixel centers use the conventional integer coordinate system. Image bounds +/// therefore run from `-0.5` to `width - 0.5` horizontally and from `-0.5` to +/// `height - 0.5` vertically. Keeping this convention explicit avoids the +/// half-pixel field-of-view errors that otherwise appear when consumers use a +/// different interpretation of `cx` and `cy`. A principal point outside those +/// bounds is allowed because cropped sensors can legitimately place the optical +/// axis outside the delivered image. +#[derive( + Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect, +)] +pub struct CuCameraIntrinsics { + pub width: u32, + pub height: u32, + pub fx: f32, + pub fy: f32, + pub cx: f32, + pub cy: f32, + pub skew: f32, +} + +impl CuCameraIntrinsics { + pub fn new( + width: u32, + height: u32, + fx: f32, + fy: f32, + cx: f32, + cy: f32, + skew: f32, + ) -> Result { + let intrinsics = Self { + width, + height, + fx, + fy, + cx, + cy, + skew, + }; + intrinsics.validate()?; + Ok(intrinsics) + } + + pub fn validate(&self) -> Result<(), CuCameraModelError> { + if self.width == 0 || self.height == 0 { + return Err(CuCameraModelError::ZeroImageDimension); + } + if ![self.fx, self.fy, self.cx, self.cy, self.skew] + .iter() + .all(|value| value.is_finite()) + { + return Err(CuCameraModelError::NonFiniteIntrinsic); + } + if self.fx <= 0.0 || self.fy <= 0.0 { + return Err(CuCameraModelError::NonPositiveFocalLength); + } + + Ok(()) + } + + /// Horizontal field of view in radians, measured between the outer pixel edges. + pub fn horizontal_fov_rad(&self) -> Result { + self.validate()?; + let left_extent = self.cx + 0.5; + let right_extent = self.width as f32 - 0.5 - self.cx; + Ok(libm::atanf(left_extent / self.fx) + libm::atanf(right_extent / self.fx)) + } + + /// Vertical field of view in radians, measured between the outer pixel edges. + pub fn vertical_fov_rad(&self) -> Result { + self.validate()?; + let top_extent = self.cy + 0.5; + let bottom_extent = self.height as f32 - 0.5 - self.cy; + Ok(libm::atanf(top_extent / self.fy) + libm::atanf(bottom_extent / self.fy)) + } + + /// Convert a rectified pixel coordinate into an unnormalized camera-frame ray. + /// + /// The result is `[x, y, 1]`. Lens distortion is intentionally not applied; + /// callers should rectify the pixel according to [`CuCameraDistortion`] first. + pub fn rectified_pixel_ray(&self, pixel: [f32; 2]) -> Result<[f32; 3], CuCameraModelError> { + self.validate()?; + let y = (pixel[1] - self.cy) / self.fy; + let x = (pixel[0] - self.cx - self.skew * y) / self.fx; + Ok([x, y, 1.0]) + } +} + +/// Standard distortion models used by common camera drivers and ROS CameraInfo. +#[derive( + Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize, Encode, Decode, Reflect, +)] +pub enum CuCameraDistortionModel { + #[default] + None, + /// Brown-Conrady / ROS `plumb_bob`: `[k1, k2, t1, t2, k3]`. + PlumbBob, + /// OpenCV rational polynomial: `[k1, k2, t1, t2, k3, k4, k5, k6]`. + RationalPolynomial, + /// OpenCV fisheye / ROS `equidistant`: `[k1, k2, k3, k4]`. + Equidistant, +} + +impl CuCameraDistortionModel { + pub const fn coefficient_count(self) -> usize { + match self { + Self::None => 0, + Self::PlumbBob => 5, + Self::RationalPolynomial => 8, + Self::Equidistant => 4, + } + } +} + +/// Fixed-capacity, allocation-free lens distortion parameters. +#[derive( + Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect, +)] +pub struct CuCameraDistortion { + pub model: CuCameraDistortionModel, + coefficients: [f32; CAMERA_DISTORTION_COEFFICIENT_CAPACITY], +} + +impl CuCameraDistortion { + pub fn new( + model: CuCameraDistortionModel, + coefficients: &[f32], + ) -> Result { + let expected = model.coefficient_count(); + if coefficients.len() != expected { + return Err(CuCameraModelError::InvalidDistortionCoefficientCount { + model, + expected, + found: coefficients.len(), + }); + } + + let mut storage = [0.0; CAMERA_DISTORTION_COEFFICIENT_CAPACITY]; + for (index, coefficient) in coefficients.iter().copied().enumerate() { + if !coefficient.is_finite() { + return Err(CuCameraModelError::NonFiniteDistortionCoefficient { index }); + } + storage[index] = coefficient; + } + Ok(Self { + model, + coefficients: storage, + }) + } + + pub fn coefficients(&self) -> &[f32] { + &self.coefficients[..self.model.coefficient_count()] + } + + pub fn validate(&self) -> Result<(), CuCameraModelError> { + for (index, coefficient) in self.coefficients.iter().enumerate() { + if !coefficient.is_finite() { + return Err(CuCameraModelError::NonFiniteDistortionCoefficient { index }); + } + } + Ok(()) + } +} + +/// Standard camera geometry shared by image and depth-map producers. +/// +/// A source should publish this as [`CuCameraModelUpdate::Set`] when the model +/// first becomes available or changes, then publish `NoChange` on later cycles. +/// Consumers keep a [`CuCameraModelState`] cache. This transmits the full model +/// only on state transitions while keeping calibration changes in Copper's +/// deterministic log and replay stream. +#[derive( + Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect, +)] +pub struct CuCameraModel { + pub intrinsics: CuCameraIntrinsics, + pub distortion: CuCameraDistortion, +} + +impl CuCameraModel { + pub fn new( + intrinsics: CuCameraIntrinsics, + distortion: CuCameraDistortion, + ) -> Result { + let model = Self { + intrinsics, + distortion, + }; + model.validate()?; + Ok(model) + } + + pub fn validate(&self) -> Result<(), CuCameraModelError> { + self.intrinsics.validate()?; + self.distortion.validate() + } +} + +/// Producer-side camera model update carried on a dedicated Copper output. +pub type CuCameraModelUpdate = CuLatchedStateUpdate; + +/// Consumer-side cache for the latest camera model. +pub type CuCameraModelState = CuLatchedState; + +#[cfg(test)] +mod tests { + use super::*; + use bincode::config; + use core::f32::consts::FRAC_PI_2; + + fn centered_intrinsics() -> CuCameraIntrinsics { + CuCameraIntrinsics::new(640, 480, 320.0, 240.0, 319.5, 239.5, 0.0).unwrap() + } + + #[test] + fn centered_intrinsics_compute_ninety_degree_fov() { + let intrinsics = centered_intrinsics(); + assert!((intrinsics.horizontal_fov_rad().unwrap() - FRAC_PI_2).abs() < 1.0e-6); + assert!((intrinsics.vertical_fov_rad().unwrap() - FRAC_PI_2).abs() < 1.0e-6); + } + + #[test] + fn rectified_pixel_ray_accounts_for_skew() { + let intrinsics = + CuCameraIntrinsics::new(640, 480, 400.0, 200.0, 300.0, 200.0, 10.0).unwrap(); + let ray = intrinsics.rectified_pixel_ray([412.0, 240.0]).unwrap(); + assert_eq!(ray, [0.275, 0.2, 1.0]); + } + + #[test] + fn invalid_intrinsics_are_rejected() { + assert_eq!( + CuCameraIntrinsics::new(640, 480, 0.0, 240.0, 319.5, 239.5, 0.0), + Err(CuCameraModelError::NonPositiveFocalLength) + ); + assert_eq!( + CuCameraIntrinsics::new(640, 480, 320.0, 240.0, f32::NAN, 239.5, 0.0), + Err(CuCameraModelError::NonFiniteIntrinsic) + ); + } + + #[test] + fn cropped_camera_may_have_principal_point_outside_image() { + let intrinsics = CuCameraIntrinsics::new(640, 480, 320.0, 240.0, -0.5, 239.5, 0.0).unwrap(); + let expected = libm::atanf(2.0); + assert!((intrinsics.horizontal_fov_rad().unwrap() - expected).abs() < 1.0e-6); + } + + #[test] + fn distortion_uses_model_specific_coefficient_counts() { + let distortion = CuCameraDistortion::new( + CuCameraDistortionModel::PlumbBob, + &[0.1, -0.02, 0.001, -0.001, 0.0], + ) + .unwrap(); + assert_eq!(distortion.coefficients().len(), 5); + assert_eq!( + CuCameraDistortion::new(CuCameraDistortionModel::Equidistant, &[0.1; 3]), + Err(CuCameraModelError::InvalidDistortionCoefficientCount { + model: CuCameraDistortionModel::Equidistant, + expected: 4, + found: 3, + }) + ); + } + + #[test] + fn camera_model_round_trips_through_bincode() { + let model = + CuCameraModel::new(centered_intrinsics(), CuCameraDistortion::default()).unwrap(); + let cfg = config::standard(); + let mut buffer = [0_u8; 256]; + let len = bincode::encode_into_slice(model, &mut buffer, cfg).unwrap(); + let (decoded, used) = + bincode::decode_from_slice::(&buffer[..len], cfg).unwrap(); + assert_eq!(used, len); + assert_eq!(decoded, model); + } + + #[test] + fn latched_updates_send_full_model_only_on_change() { + let model = + CuCameraModel::new(centered_intrinsics(), CuCameraDistortion::default()).unwrap(); + let cfg = config::standard(); + let set = bincode::encode_to_vec(CuCameraModelUpdate::Set(model), cfg).unwrap(); + let no_change = bincode::encode_to_vec(CuCameraModelUpdate::NoChange, cfg).unwrap(); + assert!(no_change.len() < set.len()); + + let mut state = CuCameraModelState::default(); + state.update_owned(CuCameraModelUpdate::Set(model)); + state.update_owned(CuCameraModelUpdate::NoChange); + assert_eq!(state.get(), Some(&model)); + state.update_owned(CuCameraModelUpdate::Clear); + assert!(state.is_unset()); + } +} diff --git a/components/payloads/cu_sensor_payloads/src/lib.rs b/components/payloads/cu_sensor_payloads/src/lib.rs index fdc646ea79f..53bb0fbf5a9 100644 --- a/components/payloads/cu_sensor_payloads/src/lib.rs +++ b/components/payloads/cu_sensor_payloads/src/lib.rs @@ -3,6 +3,7 @@ extern crate alloc; mod barometer; +mod camera; #[cfg(feature = "std")] mod depth; #[cfg(feature = "std")] @@ -17,6 +18,7 @@ mod ranging; mod rerun_components; pub use barometer::*; +pub use camera::*; #[cfg(feature = "std")] pub use depth::*; #[cfg(feature = "std")] From 8a3f3c9582dca8579aacf17f05a7025007ea8005 Mon Sep 17 00:00:00 2001 From: Daniil Mordanov Date: Tue, 4 Aug 2026 22:07:33 +0700 Subject: [PATCH 2/2] refactor(payloads): type camera distortion at compile time --- .../payloads/cu_sensor_payloads/README.md | 22 +- .../payloads/cu_sensor_payloads/src/camera.rs | 228 +++++++++--------- 2 files changed, 125 insertions(+), 125 deletions(-) diff --git a/components/payloads/cu_sensor_payloads/README.md b/components/payloads/cu_sensor_payloads/README.md index 9b31fef1f75..beb0589956f 100644 --- a/components/payloads/cu_sensor_payloads/README.md +++ b/components/payloads/cu_sensor_payloads/README.md @@ -6,14 +6,20 @@ The crate contains common payload types used by Copper sources, tasks, and sinks, including image, depth-map, camera-calibration, and point-cloud data structures. -## Camera calibration propagation - -`CuCameraModel` provides a fixed-size, allocation-free camera model containing -pinhole intrinsics and a standard distortion model. It is intentionally separate -from `CuImage` and `CuDepthMap`: a source can expose the model on a dedicated -output as `CuCameraModelUpdate` and send `Set(model)` only when calibration first -becomes available or changes. Later cycles carry `NoChange`, while each consumer -keeps a `CuCameraModelState` cache. +## Dynamic camera calibration + +`CuCameraModel` provides a fixed-size, allocation-free camera model containing +pinhole intrinsics and a compile-time distortion type. A robot chooses `D` when +it is built—for example, `CuPlumbBobDistortion` or +`CuEquidistantDistortion`—so a running camera cannot silently change its +mathematical model. The intrinsics and coefficients may still be updated by a +dynamic-calibration task. + +The model is intentionally separate from `CuImage` and `CuDepthMap`: a source +can expose it on a dedicated output as `CuCameraModelUpdate` and send +`Set(model)` only when calibration first becomes available or changes. Later +cycles carry `NoChange`, while each consumer keeps a `CuCameraModelState` +cache. This keeps the per-frame payload small without hiding calibration changes from Copper's unified log and deterministic replay. diff --git a/components/payloads/cu_sensor_payloads/src/camera.rs b/components/payloads/cu_sensor_payloads/src/camera.rs index 05b2741e420..a797b713e04 100644 --- a/components/payloads/cu_sensor_payloads/src/camera.rs +++ b/components/payloads/cu_sensor_payloads/src/camera.rs @@ -3,26 +3,13 @@ use core::fmt; use cu29::prelude::*; use serde::{Deserialize, Serialize}; -/// Maximum number of distortion coefficients carried by [`CuCameraDistortion`]. -/// -/// Four coefficients cover equidistant fisheye models, five cover the common -/// Brown-Conrady model, and eight cover OpenCV's rational polynomial model. -pub const CAMERA_DISTORTION_COEFFICIENT_CAPACITY: usize = 8; - /// Errors found while validating a camera model. #[derive(Clone, Copy, Debug, PartialEq, Eq)] pub enum CuCameraModelError { ZeroImageDimension, NonFiniteIntrinsic, NonPositiveFocalLength, - InvalidDistortionCoefficientCount { - model: CuCameraDistortionModel, - expected: usize, - found: usize, - }, - NonFiniteDistortionCoefficient { - index: usize, - }, + NonFiniteDistortionCoefficient { index: usize }, } impl fmt::Display for CuCameraModelError { @@ -33,14 +20,6 @@ impl fmt::Display for CuCameraModelError { Self::NonPositiveFocalLength => { write!(f, "camera focal lengths must be positive") } - Self::InvalidDistortionCoefficientCount { - model, - expected, - found, - } => write!( - f, - "{model:?} distortion expects {expected} coefficients, found {found}" - ), Self::NonFiniteDistortionCoefficient { index } => { write!(f, "camera distortion coefficient {index} is not finite") } @@ -131,7 +110,7 @@ impl CuCameraIntrinsics { /// Convert a rectified pixel coordinate into an unnormalized camera-frame ray. /// /// The result is `[x, y, 1]`. Lens distortion is intentionally not applied; - /// callers should rectify the pixel according to [`CuCameraDistortion`] first. + /// callers should rectify the pixel according to the model's distortion type first. pub fn rectified_pixel_ray(&self, pixel: [f32; 2]) -> Result<[f32; 3], CuCameraModelError> { self.validate()?; let y = (pixel[1] - self.cy) / self.fy; @@ -140,102 +119,115 @@ impl CuCameraIntrinsics { } } -/// Standard distortion models used by common camera drivers and ROS CameraInfo. -#[derive( - Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize, Encode, Decode, Reflect, -)] -pub enum CuCameraDistortionModel { - #[default] - None, - /// Brown-Conrady / ROS `plumb_bob`: `[k1, k2, t1, t2, k3]`. - PlumbBob, - /// OpenCV rational polynomial: `[k1, k2, t1, t2, k3, k4, k5, k6]`. - RationalPolynomial, - /// OpenCV fisheye / ROS `equidistant`: `[k1, k2, k3, k4]`. - Equidistant, -} - -impl CuCameraDistortionModel { - pub const fn coefficient_count(self) -> usize { - match self { - Self::None => 0, - Self::PlumbBob => 5, - Self::RationalPolynomial => 8, - Self::Equidistant => 4, +/// Compile-time lens-distortion model for a camera. +/// +/// The concrete distortion type is part of [`CuCameraModel`]'s Rust type. A +/// running robot may update the coefficients of its chosen calibration model, +/// but it cannot silently switch from (for example) `plumb_bob` to `equidistant`. +pub trait CuCameraDistortion { + fn coefficients(&self) -> &[f32]; + + fn validate(&self) -> Result<(), CuCameraModelError> { + for (index, coefficient) in self.coefficients().iter().enumerate() { + if !coefficient.is_finite() { + return Err(CuCameraModelError::NonFiniteDistortionCoefficient { index }); + } } + Ok(()) } } -/// Fixed-capacity, allocation-free lens distortion parameters. +/// Camera without a lens-distortion correction model. #[derive( - Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect, + Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize, Encode, Decode, Reflect, )] -pub struct CuCameraDistortion { - pub model: CuCameraDistortionModel, - coefficients: [f32; CAMERA_DISTORTION_COEFFICIENT_CAPACITY], +pub struct CuNoDistortion; + +impl CuCameraDistortion for CuNoDistortion { + fn coefficients(&self) -> &[f32] { + &[] + } } -impl CuCameraDistortion { - pub fn new( - model: CuCameraDistortionModel, - coefficients: &[f32], - ) -> Result { - let expected = model.coefficient_count(); - if coefficients.len() != expected { - return Err(CuCameraModelError::InvalidDistortionCoefficientCount { - model, - expected, - found: coefficients.len(), - }); +macro_rules! define_camera_distortion { + ($(#[$meta:meta])* $name:ident, $coefficient_count:literal) => { + $(#[$meta])* + /// + /// The coefficient count is encoded in the constructor's array type, so + /// an invalid count is rejected by the Rust compiler rather than at runtime. + #[derive( + Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect, + )] + pub struct $name { + coefficients: [f32; $coefficient_count], } - let mut storage = [0.0; CAMERA_DISTORTION_COEFFICIENT_CAPACITY]; - for (index, coefficient) in coefficients.iter().copied().enumerate() { - if !coefficient.is_finite() { - return Err(CuCameraModelError::NonFiniteDistortionCoefficient { index }); + impl $name { + pub fn new( + coefficients: [f32; $coefficient_count], + ) -> Result { + let distortion = Self { coefficients }; + distortion.validate()?; + Ok(distortion) } - storage[index] = coefficient; - } - Ok(Self { - model, - coefficients: storage, - }) - } - pub fn coefficients(&self) -> &[f32] { - &self.coefficients[..self.model.coefficient_count()] - } + pub const fn coefficients(&self) -> &[f32; $coefficient_count] { + &self.coefficients + } + } - pub fn validate(&self) -> Result<(), CuCameraModelError> { - for (index, coefficient) in self.coefficients.iter().enumerate() { - if !coefficient.is_finite() { - return Err(CuCameraModelError::NonFiniteDistortionCoefficient { index }); + impl CuCameraDistortion for $name { + fn coefficients(&self) -> &[f32] { + &self.coefficients } } - Ok(()) - } + }; } +define_camera_distortion!( + /// Brown-Conrady / ROS `plumb_bob`: `[k1, k2, t1, t2, k3]`. + /// + /// ```compile_fail + /// use cu_sensor_payloads::CuPlumbBobDistortion; + /// let _ = CuPlumbBobDistortion::new([0.0; 4]); + /// ``` + CuPlumbBobDistortion, + 5 +); + +define_camera_distortion!( + /// OpenCV rational polynomial: `[k1, k2, t1, t2, k3, k4, k5, k6]`. + CuRationalPolynomialDistortion, + 8 +); + +define_camera_distortion!( + /// OpenCV fisheye / ROS `equidistant`: `[k1, k2, k3, k4]`. + CuEquidistantDistortion, + 4 +); + /// Standard camera geometry shared by image and depth-map producers. /// -/// A source should publish this as [`CuCameraModelUpdate::Set`] when the model -/// first becomes available or changes, then publish `NoChange` on later cycles. -/// Consumers keep a [`CuCameraModelState`] cache. This transmits the full model -/// only on state transitions while keeping calibration changes in Copper's -/// deterministic log and replay stream. +/// `D` fixes the distortion model at compile time. A source performing dynamic +/// calibration should publish this as [`CuCameraModelUpdate::Set`] when its +/// intrinsics or coefficients first become available or change, then publish +/// `NoChange` on later cycles. Consumers keep a [`CuCameraModelState`] cache. +/// This transmits the full calibration only on state transitions while keeping +/// every change in Copper's deterministic log and replay stream. #[derive( Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize, Encode, Decode, Reflect, )] -pub struct CuCameraModel { +pub struct CuCameraModel { pub intrinsics: CuCameraIntrinsics, - pub distortion: CuCameraDistortion, + pub distortion: D, } -impl CuCameraModel { - pub fn new( - intrinsics: CuCameraIntrinsics, - distortion: CuCameraDistortion, - ) -> Result { +impl CuCameraModel +where + D: CuCameraDistortion, +{ + pub fn new(intrinsics: CuCameraIntrinsics, distortion: D) -> Result { let model = Self { intrinsics, distortion, @@ -250,11 +242,11 @@ impl CuCameraModel { } } -/// Producer-side camera model update carried on a dedicated Copper output. -pub type CuCameraModelUpdate = CuLatchedStateUpdate; +/// Producer-side dynamic calibration update carried on a dedicated Copper output. +pub type CuCameraModelUpdate = CuLatchedStateUpdate>; -/// Consumer-side cache for the latest camera model. -pub type CuCameraModelState = CuLatchedState; +/// Consumer-side cache for the latest dynamic calibration. +pub type CuCameraModelState = CuLatchedState>; #[cfg(test)] mod tests { @@ -301,43 +293,45 @@ mod tests { } #[test] - fn distortion_uses_model_specific_coefficient_counts() { - let distortion = CuCameraDistortion::new( - CuCameraDistortionModel::PlumbBob, - &[0.1, -0.02, 0.001, -0.001, 0.0], - ) - .unwrap(); - assert_eq!(distortion.coefficients().len(), 5); + fn distortion_models_have_compile_time_coefficient_counts() { + let plumb_bob = CuPlumbBobDistortion::new([0.1, -0.02, 0.001, -0.001, 0.0]).unwrap(); + let rational = CuRationalPolynomialDistortion::new([0.0; 8]).unwrap(); + let equidistant = CuEquidistantDistortion::new([0.0; 4]).unwrap(); + + assert_eq!(plumb_bob.coefficients().len(), 5); + assert_eq!(rational.coefficients().len(), 8); + assert_eq!(equidistant.coefficients().len(), 4); assert_eq!( - CuCameraDistortion::new(CuCameraDistortionModel::Equidistant, &[0.1; 3]), - Err(CuCameraModelError::InvalidDistortionCoefficientCount { - model: CuCameraDistortionModel::Equidistant, - expected: 4, - found: 3, - }) + CuEquidistantDistortion::new([0.0, f32::NAN, 0.0, 0.0]), + Err(CuCameraModelError::NonFiniteDistortionCoefficient { index: 1 }) ); } #[test] fn camera_model_round_trips_through_bincode() { - let model = - CuCameraModel::new(centered_intrinsics(), CuCameraDistortion::default()).unwrap(); + let model = CuCameraModel::new(centered_intrinsics(), CuNoDistortion).unwrap(); let cfg = config::standard(); let mut buffer = [0_u8; 256]; let len = bincode::encode_into_slice(model, &mut buffer, cfg).unwrap(); let (decoded, used) = - bincode::decode_from_slice::(&buffer[..len], cfg).unwrap(); + bincode::decode_from_slice::, _>(&buffer[..len], cfg) + .unwrap(); assert_eq!(used, len); assert_eq!(decoded, model); } #[test] fn latched_updates_send_full_model_only_on_change() { - let model = - CuCameraModel::new(centered_intrinsics(), CuCameraDistortion::default()).unwrap(); + let model = CuCameraModel::new( + centered_intrinsics(), + CuPlumbBobDistortion::new([0.1, -0.02, 0.001, -0.001, 0.0]).unwrap(), + ) + .unwrap(); let cfg = config::standard(); let set = bincode::encode_to_vec(CuCameraModelUpdate::Set(model), cfg).unwrap(); - let no_change = bincode::encode_to_vec(CuCameraModelUpdate::NoChange, cfg).unwrap(); + let no_change = + bincode::encode_to_vec(CuCameraModelUpdate::::NoChange, cfg) + .unwrap(); assert!(no_change.len() < set.len()); let mut state = CuCameraModelState::default();