cu_sensor_payloads/
pointcloud.rs

1use bincode::{Decode, Encode};
2use cu29::prelude::*;
3use cu29::units::si::f32::{Length, Ratio};
4use cu29::units::si::length::meter;
5use cu29::units::si::ratio::percent;
6use cu29_clock::CuTime;
7use cu29_soa_derive::Soa;
8use serde::{Deserialize, Serialize};
9
10pub type Distance = Length;
11pub type Reflectivity = Ratio;
12
13/// Standardized PointCloud.
14/// note: the derive(Soa) will generate a PointCloudSoa struct that will store the data in a SoA format.
15/// The Soa format is appropriate for early pipeline operations like changing their frame of reference.
16/// important: The ToV of the points are not assumed to be sorted.
17#[derive(
18    Default, Clone, PartialEq, Debug, Soa, Serialize, Deserialize, Encode, Decode, Reflect,
19)]
20#[reflect(from_reflect = false)]
21pub struct PointCloud {
22    pub tov: CuTime, // Time of Validity, not sorted.
23    pub x: Distance,
24    pub y: Distance,
25    pub z: Distance,
26    pub i: Reflectivity,
27    pub return_order: u8, // 0 for first return, 1 for second return, etc.
28}
29
30impl PointCloud {
31    pub fn new(tov: CuTime, x: f32, y: f32, z: f32, i: f32, return_order: Option<u8>) -> Self {
32        Self {
33            tov,
34            x: Distance::new::<meter>(x),
35            y: Distance::new::<meter>(y),
36            z: Distance::new::<meter>(z),
37            i: Reflectivity::new::<percent>(i),
38            return_order: return_order.unwrap_or(0),
39        }
40    }
41
42    pub fn new_units(
43        tov: CuTime,
44        x: Length,
45        y: Length,
46        z: Length,
47        i: Ratio,
48        return_order: Option<u8>,
49    ) -> Self {
50        Self {
51            tov,
52            x,
53            y,
54            z,
55            i,
56            return_order: return_order.unwrap_or(0),
57        }
58    }
59}
60
61impl<const N: usize> PointCloudSoa<N> {
62    /// Sort in place the point cloud so it can be ready for merge sorts for example
63    pub fn sort(&mut self) {
64        self.quicksort(0, N - 1);
65    }
66
67    /// Implementation of the sort
68    fn quicksort(&mut self, low: usize, high: usize) {
69        if low < high {
70            let pivot_index = self.partition(low, high);
71            if pivot_index > 0 {
72                self.quicksort(low, pivot_index - 1);
73            }
74            self.quicksort(pivot_index + 1, high);
75        }
76    }
77
78    /// Used by quicksort.
79    fn partition(&mut self, low: usize, high: usize) -> usize {
80        let pivot = self.tov[high];
81        let mut i = low;
82        for j in low..high {
83            if self.tov[j] <= pivot {
84                self.swap(i, j);
85                i += 1;
86            }
87        }
88        self.swap(i, high);
89        i
90    }
91
92    /// swap the elements at index a and b
93    pub fn swap(&mut self, a: usize, b: usize) {
94        self.tov.swap(a, b);
95        self.x.swap(a, b);
96        self.y.swap(a, b);
97        self.z.swap(a, b);
98        self.i.swap(a, b);
99    }
100}
101
102#[cfg(test)]
103mod tests {
104    use super::*;
105    use cu29_clock::CuDuration;
106
107    #[test]
108    fn test_point_payload() {
109        let payload = PointCloud::new(CuDuration(1), 1.0, 2.0, 3.0, 0.0, None);
110        assert_eq!(payload.x.value, 1.0);
111        assert_eq!(payload.y.value, 2.0);
112        assert_eq!(payload.z.value, 3.0);
113    }
114
115    #[test]
116    fn test_length_add_sub() {
117        let a = Distance::new::<meter>(1.0);
118        let b = Distance::new::<meter>(2.0);
119        let c = a + b;
120        assert_eq!(c.value, 3.0);
121        let d = c - a;
122        assert_eq!(d.value, 2.0);
123    }
124
125    #[test]
126    fn test_encoding_length() {
127        let a = Distance::new::<meter>(1.0);
128        let mut encoded = vec![0u8; 1024]; // Reserve a buffer with sufficient capacity
129
130        let length =
131            bincode::encode_into_slice(a, &mut encoded, bincode::config::standard()).unwrap();
132        assert_eq!(length, 4);
133    }
134}
cu_sensor_payloads/pointcloud.rs

cu_sensor_payloads/
pointcloud.rs
