cu_sensor_payloads/
pointcloud.rs

1use bincode::de::{BorrowDecode, BorrowDecoder, Decode, Decoder};
2use bincode::enc::{Encode, Encoder};
3use bincode::error::{DecodeError, EncodeError};
4use cu29_clock::CuTime;
5use cu29_soa_derive::Soa;
6use derive_more::{Add, Deref, Div, From, Mul, Sub};
7use serde::Serialize;
8use uom::si::f32::{Length, Ratio};
9use uom::si::length::meter;
10use uom::si::ratio::percent;
11
12#[derive(Default, PartialEq, Debug, Copy, Clone, Add, Deref, Sub, From, Mul, Div, Serialize)]
13pub struct Reflectivity(Ratio);
14
15impl From<f32> for Reflectivity {
16    fn from(value: f32) -> Self {
17        Self(Ratio::new::<percent>(value))
18    }
19}
20
21/// Encode as f32
22impl Encode for Reflectivity {
23    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
24        let Reflectivity(ratio) = self;
25        Encode::encode(&ratio.value, encoder)
26    }
27}
28
29/// Decode as f32
30impl Decode<()> for Reflectivity {
31    fn decode<D: Decoder>(decoder: &mut D) -> Result<Self, DecodeError> {
32        let value: f32 = Decode::decode(decoder)?;
33        Ok(Reflectivity(Ratio::new::<percent>(value)))
34    }
35}
36
37/// Decode as f32
38impl<'de> BorrowDecode<'de, ()> for Reflectivity {
39    fn borrow_decode<D: BorrowDecoder<'de>>(decoder: &mut D) -> Result<Self, DecodeError> {
40        let value: f32 = Decode::decode(decoder)?;
41        Ok(Reflectivity(Ratio::new::<percent>(value)))
42    }
43}
44
45#[derive(Default, PartialEq, Debug, Copy, Clone, Add, Deref, Sub, From, Mul, Div, Serialize)]
46pub struct Distance(pub Length);
47
48/// Encode it as a f32 in m
49impl Encode for Distance {
50    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
51        let Distance(length) = self;
52        Encode::encode(&length.value, encoder)
53    }
54}
55
56impl From<f32> for Distance {
57    fn from(value: f32) -> Self {
58        Self(Length::new::<meter>(value))
59    }
60}
61
62/// Decode it as a f32 in m
63impl Decode<()> for Distance {
64    fn decode<D: Decoder>(decoder: &mut D) -> Result<Self, DecodeError> {
65        let value: f32 = Decode::decode(decoder)?;
66        Ok(Distance(Length::new::<meter>(value)))
67    }
68}
69
70/// Decode it as a f32 in m
71impl<'de> BorrowDecode<'de, ()> for Distance {
72    fn borrow_decode<D: BorrowDecoder<'de>>(decoder: &mut D) -> Result<Self, DecodeError> {
73        let value: f32 = Decode::decode(decoder)?;
74        Ok(Distance(Length::new::<meter>(value)))
75    }
76}
77
78/// Standardized PointCloud.
79/// note: the derive(Soa) will generate a PointCloudSoa struct that will store the data in a SoA format.
80/// The Soa format is appropriate for early pipeline operations like changing their frame of reference.
81/// important: The ToV of the points are not assumed to be sorted.
82#[derive(Default, Clone, PartialEq, Debug, Soa, Serialize)]
83pub struct PointCloud {
84    pub tov: CuTime, // Time of Validity, not sorted.
85    pub x: Distance,
86    pub y: Distance,
87    pub z: Distance,
88    pub i: Reflectivity,
89    pub return_order: u8, // 0 for first return, 1 for second return, etc.
90}
91
92impl Encode for PointCloud {
93    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
94        self.tov.encode(encoder)?;
95        self.x.encode(encoder)?;
96        self.y.encode(encoder)?;
97        self.z.encode(encoder)?;
98        self.i.encode(encoder)?;
99        self.return_order.encode(encoder)
100    }
101}
102
103impl Decode<()> for PointCloud {
104    fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
105        let tov = CuTime::decode(decoder)?;
106        let x = Distance::decode(decoder)?;
107        let y = Distance::decode(decoder)?;
108        let z = Distance::decode(decoder)?;
109        let i = Reflectivity::decode(decoder)?;
110        let return_order = u8::decode(decoder)?;
111        Ok(PointCloud {
112            tov,
113            x,
114            y,
115            z,
116            i,
117            return_order,
118        })
119    }
120}
121
122impl PointCloud {
123    pub fn new(tov: CuTime, x: f32, y: f32, z: f32, i: f32, return_order: Option<u8>) -> Self {
124        Self {
125            tov,
126            x: Distance(Length::new::<meter>(x)),
127            y: Distance(Length::new::<meter>(y)),
128            z: Distance(Length::new::<meter>(z)),
129            i: Reflectivity(Ratio::new::<percent>(i)),
130            return_order: return_order.unwrap_or(0),
131        }
132    }
133
134    pub fn new_uom(
135        tov: CuTime,
136        x: Length,
137        y: Length,
138        z: Length,
139        i: Ratio,
140        return_order: Option<u8>,
141    ) -> Self {
142        Self {
143            tov,
144            x: Distance(x),
145            y: Distance(y),
146            z: Distance(z),
147            i: Reflectivity(i),
148            return_order: return_order.unwrap_or(0),
149        }
150    }
151}
152
153impl<const N: usize> PointCloudSoa<N> {
154    /// Sort in place the point cloud so it can be ready for merge sorts for example
155    pub fn sort(&mut self) {
156        self.quicksort(0, N - 1);
157    }
158
159    /// Implementation of the sort
160    fn quicksort(&mut self, low: usize, high: usize) {
161        if low < high {
162            let pivot_index = self.partition(low, high);
163            if pivot_index > 0 {
164                self.quicksort(low, pivot_index - 1);
165            }
166            self.quicksort(pivot_index + 1, high);
167        }
168    }
169
170    /// Used by quicksort.
171    fn partition(&mut self, low: usize, high: usize) -> usize {
172        let pivot = self.tov[high];
173        let mut i = low;
174        for j in low..high {
175            if self.tov[j] <= pivot {
176                self.swap(i, j);
177                i += 1;
178            }
179        }
180        self.swap(i, high);
181        i
182    }
183
184    /// swap the elements at index a and b
185    pub fn swap(&mut self, a: usize, b: usize) {
186        self.tov.swap(a, b);
187        self.x.swap(a, b);
188        self.y.swap(a, b);
189        self.z.swap(a, b);
190        self.i.swap(a, b);
191    }
192}
193
194#[cfg(test)]
195mod tests {
196    use super::*;
197    use cu29_clock::CuDuration;
198
199    #[test]
200    fn test_point_payload() {
201        let payload = PointCloud::new(CuDuration(1), 1.0, 2.0, 3.0, 0.0, None);
202        assert_eq!(payload.x.0.value, 1.0);
203        assert_eq!(payload.y.0.value, 2.0);
204        assert_eq!(payload.z.0.value, 3.0);
205    }
206
207    #[test]
208    fn test_length_add_sub() {
209        let a = Distance(Length::new::<meter>(1.0));
210        let b = Distance(Length::new::<meter>(2.0));
211        let c = a + b;
212        assert_eq!(c.value, 3.0);
213        let d = c - a;
214        assert_eq!(d.value, 2.0);
215    }
216
217    #[test]
218    fn test_encoding_length() {
219        let a = Distance(Length::new::<meter>(1.0));
220        let mut encoded = vec![0u8; 1024]; // Reserve a buffer with sufficient capacity
221
222        let length =
223            bincode::encode_into_slice(a, &mut encoded, bincode::config::standard()).unwrap();
224        assert_eq!(length, 4);
225    }
226}
cu_sensor_payloads/pointcloud.rs

cu_sensor_payloads/
pointcloud.rs
