rigidity_core/
cloud.rs

1//! Structure-of-arrays storage for point clouds.
2
3use nalgebra::Vector3;
4
5/// Errors from building or modifying a cloud.
6#[derive(Debug, thiserror::Error)]
7pub enum CloudError {
8    /// The coordinate arrays have different lengths.
9    #[error("coordinate arrays differ in length: x={x}, y={y}, z={z}")]
10    MismatchedCoordinates {
11        /// Length of the x array.
12        x: usize,
13        /// Length of the y array.
14        y: usize,
15        /// Length of the z array.
16        z: usize,
17    },
18    /// An attribute column does not match the point count.
19    #[error("attribute \"{name}\" has length {actual}, expected {expected}")]
20    MismatchedAttribute {
21        /// Attribute name.
22        name: String,
23        /// Actual length.
24        actual: usize,
25        /// Expected length.
26        expected: usize,
27    },
28    /// An attribute of that name already exists.
29    #[error("attribute \"{0}\" already exists")]
30    DuplicateAttribute(String),
31    /// Invalid voxel size.
32    #[error("voxel size must be positive and finite, got {0}")]
33    InvalidVoxelSize(f64),
34    /// A coordinate is not a finite number.
35    #[error("point {index} has a non-finite coordinate")]
36    NonFinite {
37        /// Index of the point.
38        index: usize,
39    },
40}
41
42/// A column of attribute values.
43///
44/// The column type is preserved as read: turning a `u8` colour into `f32`
45/// is a decision for application code, not for storage.
46#[derive(Debug, Clone, PartialEq)]
47pub enum AttributeData {
48    /// 32-bit floating point.
49    F32(Vec<f32>),
50    /// 64-bit floating point.
51    F64(Vec<f64>),
52    /// Unsigned 8-bit.
53    U8(Vec<u8>),
54    /// Unsigned 16-bit.
55    U16(Vec<u16>),
56    /// Unsigned 32-bit.
57    U32(Vec<u32>),
58    /// Signed 32-bit.
59    I32(Vec<i32>),
60}
61
62impl AttributeData {
63    /// Number of elements in the column.
64    pub fn len(&self) -> usize {
65        match self {
66            Self::F32(v) => v.len(),
67            Self::F64(v) => v.len(),
68            Self::U8(v) => v.len(),
69            Self::U16(v) => v.len(),
70            Self::U32(v) => v.len(),
71            Self::I32(v) => v.len(),
72        }
73    }
74
75    /// Whether the column is empty.
76    pub fn is_empty(&self) -> bool {
77        self.len() == 0
78    }
79}
80
81/// A named attribute column.
82#[derive(Debug, Clone, PartialEq)]
83pub struct Attribute {
84    /// The name as written in the file.
85    pub name: String,
86    /// The values.
87    pub data: AttributeData,
88}
89
90/// A point cloud.
91///
92/// # Layout
93///
94/// Coordinates live in three separate arrays rather than in an array of
95/// structs. At hundreds of millions of points that matters: filtering by a
96/// mask does not drag unused attributes along, and the coordinate arrays
97/// map directly onto SIMD registers and GPU buffers.
98///
99/// # Precision and origin
100///
101/// Coordinates are stored as `f32` **relative to** [`origin`](Self::origin),
102/// which is kept in `f64`. The reason is georeferenced data: at a
103/// coordinate of 500 000 m the `f32` step is about 3 cm, so storing
104/// absolute coordinates directly would destroy millimetre accuracy before
105/// the first computation. Offset storage keeps both the halved memory and
106/// the absolute accuracy.
107///
108/// [`point`](Self::point) returns the absolute coordinate in `f64`;
109/// [`local`](Self::local) returns the offset one.
110#[derive(Debug, Clone, Default, PartialEq)]
111pub struct PointCloud {
112    origin: Vector3<f64>,
113    x: Vec<f32>,
114    y: Vec<f32>,
115    z: Vec<f32>,
116    attributes: Vec<Attribute>,
117}
118
119impl PointCloud {
120    /// An empty cloud whose origin is at zero.
121    pub fn new() -> Self {
122        Self::default()
123    }
124
125    /// An empty cloud with the given origin.
126    pub fn with_origin(origin: Vector3<f64>) -> Self {
127        Self {
128            origin,
129            ..Self::default()
130        }
131    }
132
133    /// Moves the origin while preserving absolute point coordinates.
134    ///
135    /// The operation loses precision, since coordinates are recomputed
136    /// through `f32`. It is worth calling once, right after reading a file.
137    pub fn rebase(&mut self, new_origin: Vector3<f64>) {
138        let shift = self.origin - new_origin;
139        let (dx, dy, dz) = (shift.x as f32, shift.y as f32, shift.z as f32);
140        for i in 0..self.len() {
141            self.x[i] += dx;
142            self.y[i] += dy;
143            self.z[i] += dz;
144        }
145        self.origin = new_origin;
146    }
147
148    /// An empty cloud with capacity reserved.
149    pub fn with_capacity(capacity: usize) -> Self {
150        Self {
151            origin: Vector3::zeros(),
152            x: Vec::with_capacity(capacity),
153            y: Vec::with_capacity(capacity),
154            z: Vec::with_capacity(capacity),
155            attributes: Vec::new(),
156        }
157    }
158
159    /// Builds a cloud from three coordinate arrays.
160    ///
161    /// Coordinates are interpreted as offsets from `origin`.
162    pub fn from_columns(
163        origin: Vector3<f64>,
164        x: Vec<f32>,
165        y: Vec<f32>,
166        z: Vec<f32>,
167    ) -> Result<Self, CloudError> {
168        if x.len() != y.len() || y.len() != z.len() {
169            return Err(CloudError::MismatchedCoordinates {
170                x: x.len(),
171                y: y.len(),
172                z: z.len(),
173            });
174        }
175        Ok(Self {
176            origin,
177            x,
178            y,
179            z,
180            attributes: Vec::new(),
181        })
182    }
183
184    /// Number of points.
185    pub fn len(&self) -> usize {
186        self.x.len()
187    }
188
189    /// Whether the cloud is empty.
190    pub fn is_empty(&self) -> bool {
191        self.x.is_empty()
192    }
193
194    /// The origin the stored coordinates are relative to.
195    pub fn origin(&self) -> Vector3<f64> {
196        self.origin
197    }
198
199    /// Absolute coordinate of a point.
200    pub fn point(&self, index: usize) -> Vector3<f64> {
201        self.origin + self.local(index)
202    }
203
204    /// Coordinate of a point relative to [`origin`](Self::origin).
205    pub fn local(&self, index: usize) -> Vector3<f64> {
206        Vector3::new(
207            f64::from(self.x[index]),
208            f64::from(self.y[index]),
209            f64::from(self.z[index]),
210        )
211    }
212
213    /// The coordinate columns.
214    pub fn columns(&self) -> (&[f32], &[f32], &[f32]) {
215        (&self.x, &self.y, &self.z)
216    }
217
218    /// Iterator over absolute coordinates.
219    pub fn iter(&self) -> impl Iterator<Item = Vector3<f64>> + '_ {
220        (0..self.len()).map(|i| self.point(i))
221    }
222
223    /// Appends a point given in absolute coordinates.
224    ///
225    /// # Panics
226    ///
227    /// If the cloud already carries attributes: appending a point without
228    /// attribute values would break column consistency.
229    pub fn push(&mut self, point: Vector3<f64>) {
230        assert!(
231            self.attributes.is_empty(),
232            "pushing into a cloud with attributes would break column lengths"
233        );
234        let local = point - self.origin;
235        self.x.push(local.x as f32);
236        self.y.push(local.y as f32);
237        self.z.push(local.z as f32);
238    }
239
240    /// All attributes.
241    pub fn attributes(&self) -> &[Attribute] {
242        &self.attributes
243    }
244
245    /// An attribute by name.
246    pub fn attribute(&self, name: &str) -> Option<&Attribute> {
247        self.attributes.iter().find(|a| a.name == name)
248    }
249
250    /// Appends an attribute column.
251    pub fn push_attribute(&mut self, attribute: Attribute) -> Result<(), CloudError> {
252        if attribute.data.len() != self.len() {
253            return Err(CloudError::MismatchedAttribute {
254                name: attribute.name,
255                actual: attribute.data.len(),
256                expected: self.len(),
257            });
258        }
259        if self.attribute(&attribute.name).is_some() {
260            return Err(CloudError::DuplicateAttribute(attribute.name));
261        }
262        self.attributes.push(attribute);
263        Ok(())
264    }
265
266    /// Checks that every coordinate is finite.
267    ///
268    /// Called by algorithms for which a NaN means not "a bad point" but a
269    /// silently corrupted result — voxelisation, for instance, where
270    /// `NaN as i64` yields zero and the point lands in an arbitrary cell.
271    pub fn check_finite(&self) -> Result<(), CloudError> {
272        for i in 0..self.len() {
273            if !(self.x[i].is_finite() && self.y[i].is_finite() && self.z[i].is_finite()) {
274                return Err(CloudError::NonFinite { index: i });
275            }
276        }
277        Ok(())
278    }
279
280    /// Axis-aligned bounding box in absolute coordinates.
281    pub fn bounds(&self) -> Option<(Vector3<f64>, Vector3<f64>)> {
282        if self.is_empty() {
283            return None;
284        }
285        let mut min = self.point(0);
286        let mut max = min;
287        for i in 1..self.len() {
288            let p = self.point(i);
289            min = min.inf(&p);
290            max = max.sup(&p);
291        }
292        Some((min, max))
293    }
294}
295
296#[cfg(test)]
297mod tests {
298    use super::*;
299
300    /// Offset storage keeps millimetres at georeferenced coordinates while
301    /// plain `f32` does not. That is the entire reason `origin` exists.
302    #[test]
303    fn origin_preserves_precision_at_utm_scale() {
304        let absolute = Vector3::new(499_123.456_7, 5_432_198.765_4, 231.0);
305        let mut cloud = PointCloud::with_origin(Vector3::new(499_000.0, 5_432_000.0, 0.0));
306        cloud.push(absolute);
307
308        let error = (cloud.point(0) - absolute).norm();
309        assert!(error < 1e-4, "offset storage: error {error:.3e} m");
310
311        // For comparison, the same without the offset.
312        let naive = f64::from(absolute.x as f32) - absolute.x;
313        assert!(
314            naive.abs() > 1e-2,
315            "plain f32 must lose centimetres here, lost {naive:.3e}"
316        );
317    }
318
319    #[test]
320    fn mismatched_columns_are_rejected() {
321        let err =
322            PointCloud::from_columns(Vector3::zeros(), vec![1.0, 2.0], vec![1.0], vec![1.0, 2.0]);
323        assert!(matches!(err, Err(CloudError::MismatchedCoordinates { .. })));
324    }
325
326    #[test]
327    fn attribute_length_is_checked() {
328        let mut cloud =
329            PointCloud::from_columns(Vector3::zeros(), vec![0.0; 3], vec![0.0; 3], vec![0.0; 3])
330                .unwrap();
331        let bad = Attribute {
332            name: "intensity".into(),
333            data: AttributeData::U16(vec![1, 2]),
334        };
335        assert!(cloud.push_attribute(bad).is_err());
336    }
337}
rigidity_core/cloud.rs

rigidity_core/
cloud.rs
