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

cu_sensor_payloads/
pointcloud.rs
