cu_sensor_payloads/
pointcloud.rs

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

cu_sensor_payloads/
pointcloud.rs
