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

cu_sensor_payloads/
pointcloud.rs
