diff --git a/applications/autoware/Cargo.toml b/applications/autoware/Cargo.toml index 31b7485c2..76e747578 100644 --- a/applications/autoware/Cargo.toml +++ b/applications/autoware/Cargo.toml @@ -16,5 +16,5 @@ awkernel_lib = { path = "../../awkernel_lib", default-features = false } imu_driver = { path = "./imu_driver", default-features = false } imu_corrector = { path = "./imu_corrector", default-features = false } vehicle_velocity_converter = { path = "./vehicle_velocity_converter", default-features = false } -gyro_odometer = { path = "./gyro_odometer", default-features = false} - +voxel_grid_downsample_filter = { path = "./voxel_grid_downsample_filter", default-features = false } +gyro_odometer = { path = "./gyro_odometer", default-features = false } diff --git a/applications/autoware/common_types/src/centroid.rs b/applications/autoware/common_types/src/centroid.rs new file mode 100644 index 000000000..db4ee05dc --- /dev/null +++ b/applications/autoware/common_types/src/centroid.rs @@ -0,0 +1,278 @@ +use nalgebra::{RealField, SVector}; + +/// Errors that can occur when working with centroids. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[non_exhaustive] +pub enum CentroidError { + NonFinite, + CountOverflow, + CountConv, +} + +impl core::fmt::Display for CentroidError { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + Self::NonFinite => write!(f, "Centroid contains a non-finite component"), + Self::CountOverflow => write!(f, "Centroid point count overflows u32"), + Self::CountConv => write!(f, "Centroid point count conversion failed"), + } + } +} + +impl core::error::Error for CentroidError {} + +pub type Result = core::result::Result; + +/// An allocation-free accumulator for N-dimensional points. +/// +/// [`Self::try_new`] starts with one point; [`Self::default`] starts empty. +/// Add at least one point before calculating the mean of a default accumulator. +/// Components are accumulated sequentially in the selected scalar type. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub struct Centroid { + sum: SVector, + count: u32, +} + +/// A centroid accumulated in single precision. +pub type CentroidF32 = Centroid; + +/// A centroid accumulated in double precision. +pub type CentroidF64 = Centroid; + +impl Centroid { + /// Starts an accumulator with one point, rejecting non-finite components. + #[inline] + pub fn try_new(point: SVector) -> Result { + if point.iter().any(|value| !value.is_finite()) { + return Err(CentroidError::NonFinite); + } + Ok(Self { + sum: point, + count: 1, + }) + } + + /// Adds a point. Any error leaves the accumulator unchanged. + /// + /// # Errors + /// + /// - `CentroidError::NonFinite` if any component of the accumulated point would not be finite. + /// - `CentroidError::CountOverflow` if the point count would overflow. + pub fn try_add_point(&mut self, point: SVector) -> Result<()> { + let new_count = self + .count + .checked_add(1) + .ok_or(CentroidError::CountOverflow)?; + let new_sum = self.sum + point; + if new_sum.iter().any(|value| !value.is_finite()) { + return Err(CentroidError::NonFinite); + } + + self.sum = new_sum; + self.count = new_count; + Ok(()) + } + + /// Returns the component-wise arithmetic mean. + /// + /// # Errors + /// + /// - `CentroidError::CountConv` if the point count cannot be converted to the scalar type. + #[inline] + pub fn try_calc_centroid(&self) -> Result> { + let count = T::from_u32(self.count).ok_or(CentroidError::CountConv)?; + Ok(self.sum / count) + } +} + +impl Default for Centroid { + #[inline] + fn default() -> Self { + Self { + sum: SVector::::zeros(), + count: 0, + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn assert_close(actual: SVector, expected: SVector) { + for (actual, expected) in actual.iter().zip(expected.iter()) { + assert!((actual - expected).abs() <= f32::EPSILON); + } + } + + #[test] + fn default_accumulator_does_not_count_an_implicit_zero_point() { + let mut centroid = CentroidF32::<3>::default(); + assert_eq!(centroid.count, 0); + assert_eq!(centroid.sum, SVector::::zeros()); + + let first = SVector::::new(-4.0, 2.0, 8.0); + centroid.try_add_point(first).unwrap(); + assert_close(centroid.try_calc_centroid().unwrap(), first); + + centroid + .try_add_point(SVector::::new(2.0, 4.0, -2.0)) + .unwrap(); + assert_close( + centroid.try_calc_centroid().unwrap(), + SVector::::new(-1.0, 3.0, 3.0), + ); + } + + #[test] + fn single_point_is_its_own_centroid() { + let point = SVector::::new(-3.0, 0.0, 7.5); + let centroid = CentroidF32::<3>::try_new(point).unwrap(); + assert_close(centroid.try_calc_centroid().unwrap(), point); + } + + #[test] + fn averages_two_dimensional_points() { + let mut centroid = Centroid::try_new(SVector::::new(-4.0, 2.0)).unwrap(); + centroid + .try_add_point(SVector::::new(2.0, 4.0)) + .unwrap(); + centroid + .try_add_point(SVector::::new(-1.0, 9.0)) + .unwrap(); + assert_close( + centroid.try_calc_centroid().unwrap(), + SVector::::new(-1.0, 5.0), + ); + } + + #[test] + fn averages_four_components_including_fractional_intensity() { + let mut centroid = Centroid::try_new(SVector::::new(-2.0, 4.0, 8.0, 10.0)).unwrap(); + centroid + .try_add_point(SVector::::new(4.0, -2.0, 2.0, 11.0)) + .unwrap(); + assert_close( + centroid.try_calc_centroid().unwrap(), + SVector::::new(1.0, 1.0, 5.0, 10.5), + ); + } + + #[test] + fn rejects_non_finite_first_point() { + for value in [f32::NAN, f32::INFINITY, f32::NEG_INFINITY] { + assert!(matches!( + Centroid::try_new(SVector::::new(1.0, value)), + Err(CentroidError::NonFinite) + )); + } + } + + #[test] + fn invalid_addition_leaves_state_unchanged() { + let mut centroid = Centroid::try_new(SVector::::new(1.0, 2.0)).unwrap(); + let before = centroid; + for value in [f32::NAN, f32::INFINITY, f32::NEG_INFINITY] { + assert_eq!( + centroid.try_add_point(SVector::::new(3.0, value)), + Err(CentroidError::NonFinite) + ); + assert_eq!(centroid.sum, before.sum); + assert_eq!(centroid.count, before.count); + } + centroid + .try_add_point(SVector::::new(3.0, 4.0)) + .unwrap(); + assert_close( + centroid.try_calc_centroid().unwrap(), + SVector::::new(2.0, 3.0), + ); + } + + #[test] + fn sum_overflow_leaves_state_unchanged() { + let mut centroid = Centroid::try_new(SVector::::new(1.0, f32::MAX)).unwrap(); + let before = centroid; + assert_eq!( + centroid.try_add_point(SVector::::new(2.0, f32::MAX)), + Err(CentroidError::NonFinite) + ); + assert_eq!(centroid.sum, before.sum); + assert_eq!(centroid.count, before.count); + } + + #[test] + fn count_overflow_leaves_state_unchanged() { + let mut centroid = Centroid { + sum: SVector::::new(1.0, 2.0), + count: u32::MAX, + }; + let before = centroid; + assert_eq!( + centroid.try_add_point(SVector::::new(3.0, 4.0)), + Err(CentroidError::CountOverflow) + ); + assert_eq!(centroid.sum, before.sum); + assert_eq!(centroid.count, before.count); + } + + #[test] + fn double_precision_mean_preserves_values_not_representable_in_single_precision() { + let mut centroid = + CentroidF64::<2>::try_new(SVector::::new(16_777_217.0, -4.0)).unwrap(); + centroid + .try_add_point(SVector::::new(16_777_219.0, 1.0)) + .unwrap(); + let expected = SVector::::new(16_777_218.0, -1.5); + for (actual, expected) in centroid + .try_calc_centroid() + .unwrap() + .iter() + .zip(expected.iter()) + { + assert!((actual - expected).abs() <= f64::EPSILON); + } + } + + #[test] + fn double_precision_rejects_non_finite_input_without_mutation() { + let mut centroid = CentroidF64::<2>::try_new(SVector::::new(1.0, 2.0)).unwrap(); + let before = centroid; + for value in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] { + assert!(matches!( + CentroidF64::<2>::try_new(SVector::::new(1.0, value)), + Err(CentroidError::NonFinite) + )); + assert_eq!( + centroid.try_add_point(SVector::::new(3.0, value)), + Err(CentroidError::NonFinite) + ); + assert_eq!(centroid.sum, before.sum); + assert_eq!(centroid.count, before.count); + } + } + + #[test] + fn double_precision_sum_overflow_leaves_state_unchanged() { + let mut centroid = + CentroidF64::<2>::try_new(SVector::::new(1.0, f64::MAX)).unwrap(); + let before = centroid; + assert_eq!( + centroid.try_add_point(SVector::::new(2.0, f64::MAX)), + Err(CentroidError::NonFinite) + ); + assert_eq!(centroid.sum, before.sum); + assert_eq!(centroid.count, before.count); + } + + #[test] + fn double_precision_count_conversion_preserves_all_u32_bits() { + let centroid = Centroid:: { + sum: SVector::::new(f64::from(u32::MAX)), + count: u32::MAX, + }; + let mean = centroid.try_calc_centroid().unwrap(); + assert!((mean.x - 1.0).abs() <= f64::EPSILON); + } +} diff --git a/applications/autoware/common_types/src/lib.rs b/applications/autoware/common_types/src/lib.rs index a9d2b7660..28b8c77fd 100644 --- a/applications/autoware/common_types/src/lib.rs +++ b/applications/autoware/common_types/src/lib.rs @@ -16,6 +16,7 @@ extern crate alloc; +pub mod centroid; pub mod geometry; pub mod point_cloud; pub mod point_field; @@ -27,4 +28,4 @@ pub struct Header { pub timestamp: u64, } -pub use nalgebra::{Isometry3, Matrix4, Quaternion, UnitQuaternion, Vector3, Vector4}; +pub use nalgebra::{Isometry3, Matrix4, Quaternion, SVector, UnitQuaternion, Vector3, Vector4}; diff --git a/applications/autoware/common_types/src/point_cloud.rs b/applications/autoware/common_types/src/point_cloud.rs index 5919884c0..3cf11d2f2 100644 --- a/applications/autoware/common_types/src/point_cloud.rs +++ b/applications/autoware/common_types/src/point_cloud.rs @@ -6,7 +6,7 @@ use alloc::vec::Vec; use core::ops::Range; /// Errors that can occur when constructing point clouds or accessing their points. -#[derive(Debug, Clone, PartialEq, Eq)] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] #[non_exhaustive] pub enum PointCloudError { UnsupportedEndianness, diff --git a/applications/autoware/voxel_grid_downsample_filter/Cargo.toml b/applications/autoware/voxel_grid_downsample_filter/Cargo.toml new file mode 100644 index 000000000..218b0f1b5 --- /dev/null +++ b/applications/autoware/voxel_grid_downsample_filter/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "voxel_grid_downsample_filter" +version = "0.1.0" +edition = "2021" + +[dependencies] +common_types = { path = "../common_types", default-features = false } +libm = "0.2" diff --git a/applications/autoware/voxel_grid_downsample_filter/src/lib.rs b/applications/autoware/voxel_grid_downsample_filter/src/lib.rs new file mode 100644 index 000000000..1ce25a5b0 --- /dev/null +++ b/applications/autoware/voxel_grid_downsample_filter/src/lib.rs @@ -0,0 +1,397 @@ +#![no_std] + +extern crate alloc; + +use alloc::{collections::BTreeMap, vec}; +use common_types::{ + centroid::{CentroidError, CentroidF32}, + point_cloud::{PointCloud, PointCloudError}, + point_field::{PointFieldError, TypedPointField}, + Matrix4, Vector3, Vector4, +}; + +#[derive(Debug, Clone, Copy, PartialEq)] +#[non_exhaustive] +pub enum VoxelGridDownsampleFilterError { + InvalidVoxelSize(f32, f32, f32), + MissingOffsets, + VoxelSizeTooSmall, + PointField(PointFieldError), + Centroid(CentroidError), + PointCloud(PointCloudError), +} + +impl core::fmt::Display for VoxelGridDownsampleFilterError { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + Self::InvalidVoxelSize(size_x, size_y, size_z) => { + write!(f, "Invalid voxel size: ({size_x}, {size_y}, {size_z})") + } + Self::MissingOffsets => write!(f, "Missing offsets"), + Self::VoxelSizeTooSmall => write!(f, "Voxel size too small"), + Self::PointField(e) => write!(f, "Point field error: {e}"), + Self::Centroid(e) => write!(f, "Centroid error: {e}"), + Self::PointCloud(e) => write!(f, "Point cloud error: {e}"), + } + } +} + +impl core::error::Error for VoxelGridDownsampleFilterError { + fn source(&self) -> Option<&(dyn core::error::Error + 'static)> { + match self { + Self::PointField(e) => Some(e), + Self::Centroid(e) => Some(e), + Self::PointCloud(e) => Some(e), + _ => None, + } + } +} + +impl From for VoxelGridDownsampleFilterError { + fn from(e: PointFieldError) -> Self { + Self::PointField(e) + } +} + +impl From for VoxelGridDownsampleFilterError { + fn from(e: PointCloudError) -> Self { + Self::PointCloud(e) + } +} + +impl From for VoxelGridDownsampleFilterError { + fn from(e: CentroidError) -> Self { + Self::Centroid(e) + } +} + +pub type Result = core::result::Result; + +/// Offsets for the point fields in the point cloud. +#[derive(Debug, Clone)] +struct Offsets { + /// Offset for the x field. + x: TypedPointField, + /// Offset for the y field. + y: TypedPointField, + /// Offset for the z field. + z: TypedPointField, + /// Offset for the intensity field, if present. + intensity: Option>, +} + +#[derive(Debug, Clone)] +pub struct VoxelGridDownsampleFilter { + inverse_voxel_size: Vector3, + offsets: Option, +} + +#[inline] +const fn valid_size(value: f32) -> bool { + value > 0.0 && value.is_finite() +} + +impl VoxelGridDownsampleFilter { + pub const fn new() -> Self { + Self { + inverse_voxel_size: Vector3::new(0.0, 0.0, 0.0), + offsets: None, + } + } + + pub fn set_voxel_size(&mut self, size_x: f32, size_y: f32, size_z: f32) -> Result<()> { + if !valid_size(size_x) || !valid_size(size_y) || !valid_size(size_z) { + return Err(VoxelGridDownsampleFilterError::InvalidVoxelSize( + size_x, size_y, size_z, + )); + } + let div = Vector3::new(1.0, 1.0, 1.0); + let denom = Vector3::new(size_x, size_y, size_z); + self.inverse_voxel_size = div.component_div(&denom); + Ok(()) + } + + pub fn set_field_offsets(&mut self, input: &PointCloud) -> Result<()> { + self.offsets = Some(Offsets { + x: input.get_field::("x")?, + y: input.get_field::("y")?, + z: input.get_field::("z")?, + intensity: input.get_field::("intensity").ok(), + }); + Ok(()) + } + + pub fn filter( + &mut self, + input: &PointCloud, + transform: Option<&Matrix4>, + ) -> Result { + // Check if the field offset has been set + if self.offsets.is_none() { + self.set_field_offsets(input)?; + } + + // Compute the voxel coordinates for the input point cloud + let (min_voxel, max_voxel) = self.get_min_max_voxel(input)?; + let voxel_centroid_map = self.calc_centroids_each_voxel(input, &max_voxel, &min_voxel)?; + + // Initialize the output point cloud + let n_points = voxel_centroid_map.len(); + let row_step = n_points * input.point_step(); + + let mut output = PointCloud::try_new( + input.header().clone(), + 1, // height is 1 for unorganized point clouds + n_points as u32, + input.fields().to_vec(), + input.is_bigendian(), + input.point_step() as u32, + row_step as u32, + vec![0; row_step], + true, // we skip invalid points + )?; + + // Copy the centroids to the output + self.copy_centroids_to_output(&voxel_centroid_map, &mut output, transform)?; + Ok(output) + } + + fn get_point_values(&self, point: &[u8]) -> Result> { + match self.offsets { + None => return Err(VoxelGridDownsampleFilterError::MissingOffsets), + Some(ref offsets) => { + let x = offsets.x.try_read(point, 0)?; + let y = offsets.y.try_read(point, 0)?; + let z = offsets.z.try_read(point, 0)?; + let intensity = match offsets.intensity { + Some(ref field) => field.try_read(point, 0).map(|v| v as f32).ok(), + None => None, + }; + Ok(Vector4::new(x, y, z, intensity.unwrap_or(0.))) + } + } + } + + fn get_min_max_voxel(&self, input: &PointCloud) -> Result<(Vector3, Vector3)> { + // Compute the minimum and maximum point coordinates + let mut min_point = Vector3::new(f32::MAX, f32::MAX, f32::MAX); + let mut max_point = Vector3::new(f32::MIN, f32::MIN, f32::MIN); + for point in input.iter_points() { + let coords = self.get_point_values(point)?.xyz(); + if coords.x.is_finite() && coords.y.is_finite() && coords.z.is_finite() { + min_point = min_point.inf(&coords); + max_point = max_point.sup(&coords); + } + } + + // Check that the voxel size is not too small, given the size of the data + let voxel_dims = (max_point - min_point) + .component_mul(&self.inverse_voxel_size) + .map(|v| libm::roundf(v) as i64 + 1); + if voxel_dims.product() > i32::MAX as i64 { + return Err(VoxelGridDownsampleFilterError::VoxelSizeTooSmall); + } + + // Compute the minimum and maximum voxel coordinates + let min_voxel = min_point + .component_mul(&self.inverse_voxel_size) + .map(|v| libm::floorf(v) as i32); + + let max_voxel = max_point + .component_mul(&self.inverse_voxel_size) + .map(|v| libm::floorf(v) as i32); + + Ok((min_voxel, max_voxel)) + } + + fn calc_centroids_each_voxel( + &self, + input: &PointCloud, + max_voxel: &Vector3, + min_voxel: &Vector3, + ) -> Result>> { + let mut voxel_centroid_map: BTreeMap> = BTreeMap::new(); + + // Compute the number of divisions needed along all axes + let div_b = max_voxel - min_voxel + Vector3::repeat(1); + // Set up the division multiplier + let div_b_mul = Vector3::new(1, div_b.x, div_b.x * div_b.y); + + for point in input.iter_points() { + let coords = self.get_point_values(point)?; + let xyz = coords.xyz(); + + if xyz.iter().all(|c| c.is_finite()) { + // Calculate the voxel index to which the point belongs + let ijk = xyz + .component_mul(&self.inverse_voxel_size) + .map(|v| libm::floorf(v) as i32) + - min_voxel; + + let voxel_id = ijk.dot(&div_b_mul) as u32; + + // Add the point to the corresponding centroid + if let Some(centroid) = voxel_centroid_map.get_mut(&voxel_id) { + centroid.try_add_point(coords)?; + } else { + voxel_centroid_map.insert(voxel_id, CentroidF32::try_new(coords)?); + } + } + } + + Ok(voxel_centroid_map) + } + + fn copy_centroids_to_output( + &self, + voxel_centroid_map: &BTreeMap>, + output: &mut PointCloud, + transform: Option<&Matrix4>, + ) -> Result<()> { + let Offsets { x, y, z, intensity } = self + .offsets + .as_ref() + .ok_or(VoxelGridDownsampleFilterError::MissingOffsets)?; + + for (i, centroid_data) in voxel_centroid_map.values().enumerate() { + let mut centroid = centroid_data.try_calc_centroid()?; + if let Some(tf) = transform { + centroid = tf * centroid; + } + + let point = output.point_mut(i)?; + + x.try_write(point, 0, centroid.x)?; + y.try_write(point, 0, centroid.y)?; + z.try_write(point, 0, centroid.z)?; + + if let Some(intensity) = intensity { + intensity.try_write(point, 0, centroid.w as u8)?; + } + } + Ok(()) + } +} + +impl Default for VoxelGridDownsampleFilter { + fn default() -> Self { + Self::new() + } +} + +#[cfg(test)] +mod tests { + use super::*; + use alloc::vec::Vec; + use common_types::{ + point_field::{FieldDatatype, PointField}, + Header, + }; + + fn make_cloud(points: &[(f32, f32, f32, u8, u16)]) -> PointCloud { + let fields = vec![ + PointField::try_new("x", 0, FieldDatatype::Float32, 1).unwrap(), + PointField::try_new("y", 4, FieldDatatype::Float32, 1).unwrap(), + PointField::try_new("z", 8, FieldDatatype::Float32, 1).unwrap(), + PointField::try_new("intensity", 12, FieldDatatype::Uint8, 1).unwrap(), + // Padding byte! + PointField::try_new("ring", 14, FieldDatatype::Uint16, 1).unwrap(), + ]; + let point_step = 16; + let mut data = Vec::with_capacity(points.len() * point_step); + + for &(x, y, z, intensity, ring) in points { + data.extend_from_slice(&x.to_ne_bytes()); + data.extend_from_slice(&y.to_ne_bytes()); + data.extend_from_slice(&z.to_ne_bytes()); + data.push(intensity); + data.push(0); + data.extend_from_slice(&ring.to_ne_bytes()); + } + + PointCloud::try_new( + Header { + frame_id: "map", + timestamp: 0, + }, + 1, + points.len() as u32, + fields, + cfg!(target_endian = "big"), + point_step as u32, + (points.len() * point_step) as u32, + data, + true, + ) + .unwrap() + } + + #[test] + fn filter_averages_points_per_voxel_and_leaves_other_fields_zeroed() { + let input = make_cloud(&[ + (0.1, 0.1, 0.1, 10, 9), + (0.4, 0.2, 0.3, 20, 11), + (1.2, 0.1, 0.1, 40, 13), + ]); + let mut filter = VoxelGridDownsampleFilter::new(); + filter.set_voxel_size(1.0, 1.0, 1.0).unwrap(); + + let output = filter.filter(&input, None).unwrap(); + + assert_eq!(output.width(), 2); + assert_eq!(output.height(), 1); + let x = output.get_field::("x").unwrap(); + let y = output.get_field::("y").unwrap(); + let z = output.get_field::("z").unwrap(); + let intensity = output.get_field::("intensity").unwrap(); + let ring = output.get_field::("ring").unwrap(); + + let first = output.point(0).unwrap(); + assert!((x.try_read(first, 0).unwrap() - 0.25).abs() <= f32::EPSILON); + assert!((y.try_read(first, 0).unwrap() - 0.15).abs() <= f32::EPSILON); + assert!((z.try_read(first, 0).unwrap() - 0.2).abs() <= f32::EPSILON); + assert_eq!(intensity.try_read(first, 0).unwrap(), 15); + assert_eq!(ring.try_read(first, 0).unwrap(), 0); + + let second = output.point(1).unwrap(); + assert!((x.try_read(second, 0).unwrap() - 1.2).abs() <= f32::EPSILON); + assert_eq!(intensity.try_read(second, 0).unwrap(), 40); + assert_eq!(ring.try_read(second, 0).unwrap(), 0); + } + + #[test] + fn filter_skips_points_with_non_finite_coordinates() { + let input = make_cloud(&[ + (0.2, 0.3, 0.4, 17, 1), + (f32::NAN, 0.3, 0.4, 99, 2), + (0.2, f32::INFINITY, 0.4, 88, 3), + ]); + let mut filter = VoxelGridDownsampleFilter::new(); + filter.set_voxel_size(1.0, 1.0, 1.0).unwrap(); + + let output = filter.filter(&input, None).unwrap(); + + assert_eq!(output.width(), 1); + let x = output.get_field::("x").unwrap(); + let intensity = output.get_field::("intensity").unwrap(); + let point = output.point(0).unwrap(); + assert!((x.try_read(point, 0).unwrap() - 0.2).abs() <= f32::EPSILON); + assert_eq!(intensity.try_read(point, 0).unwrap(), 17); + } + + #[test] + fn set_voxel_size_rejects_non_positive_and_non_finite_dimensions() { + for (size_x, size_y, size_z) in [ + (0.0, 1.0, 1.0), + (1.0, -1.0, 1.0), + (1.0, 1.0, f32::NAN), + (f32::INFINITY, 1.0, 1.0), + ] { + let mut filter = VoxelGridDownsampleFilter::new(); + assert!(matches!( + filter.set_voxel_size(size_x, size_y, size_z), + Err(VoxelGridDownsampleFilterError::InvalidVoxelSize(..)) + )); + } + } +}