pasture_algorithms/
voxel_grid.rs

1use std::{collections::HashMap, u16};
2
3use pasture_core::{
4    containers::{
5        BorrowedBuffer, BorrowedBufferExt, OwningBuffer, UntypedPoint, UntypedPointBuffer,
6    },
7    layout::{
8        attributes::{self, POSITION_3D},
9        PointAttributeDataType, PointAttributeDefinition, PointLayout,
10    },
11    nalgebra::Vector3,
12};
13
14use crate::bounds::calculate_bounds;
15
16pub struct Voxel {
17    pos: (usize, usize, usize),
18    points: Vec<usize>,
19}
20
21/// finds leaf of point p by iterating over the marked axis
22fn find_leaf(
23    p: Vector3<f64>,
24    markers_x: &[f64],
25    markers_y: &[f64],
26    markers_z: &[f64],
27) -> (usize, usize, usize) {
28    let mut index_x = 0;
29    let mut index_y = 0;
30    let mut index_z = 0;
31    while !markers_x.is_empty() && markers_x[index_x] < p.x {
32        index_x += 1;
33    }
34    while !markers_y.is_empty() && markers_y[index_y] < p.y {
35        index_y += 1;
36    }
37    while !markers_z.is_empty() && markers_z[index_z] < p.z {
38        index_z += 1;
39    }
40    // clamp values to the better fitting marker: [i] or [i-1]
41    if index_x > 0 && p.x - markers_x[index_x - 1] < markers_x[index_x] - p.x {
42        index_x -= 1;
43    }
44    if index_y > 0 && p.y - markers_y[index_y - 1] < markers_y[index_y] - p.y {
45        index_y -= 1;
46    }
47    if index_z > 0 && p.z - markers_z[index_z - 1] < markers_z[index_z] - p.z {
48        index_z -= 1;
49    }
50    (index_x, index_y, index_z)
51}
52
53/// marks the axis with leafsize between markers
54fn create_markers_for_axis(
55    aabb: pasture_core::math::AABB<f64>,
56    leafsize_x: f64,
57    leafsize_y: f64,
58    leafsize_z: f64,
59) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
60    let mut markers_x = vec![];
61    let mut markers_y = vec![];
62    let mut markers_z = vec![];
63    let mut curr_x = aabb.min().x;
64    let mut curr_y = aabb.min().y;
65    let mut curr_z = aabb.min().z;
66    while curr_x < aabb.max().x {
67        curr_x += leafsize_x;
68        markers_x.push(curr_x);
69    }
70    while curr_y < aabb.max().y {
71        curr_y += leafsize_y;
72        markers_y.push(curr_y);
73    }
74    while curr_z < aabb.max().z {
75        curr_z += leafsize_z;
76        markers_z.push(curr_z);
77    }
78    (markers_x, markers_y, markers_z)
79}
80
81/// Downsamples `buffer` by applying a voxelgrid-filter.
82/// Currently only works for point_layouts which only contain the builtin-types.
83/// Writes results into `filtered_buffer`.
84///
85/// # Examples
86/// ```
87/// # use pasture_algorithms::voxel_grid::voxelgrid_filter;
88/// # use pasture_core::{containers::*, layout::{attributes, PointType}, nalgebra::{Scalar, Vector3}};
89/// # use pasture_derive::PointType;
90/// #[repr(C)]
91/// #[derive(PointType, Debug, Clone, Copy, bytemuck::AnyBitPattern, bytemuck::NoUninit)]
92/// # struct SimplePoint {
93/// #    #[pasture(BUILTIN_POSITION_3D)]
94/// #   pub position: Vector3<f64>,
95/// # }
96/// let mut points = vec![];
97/// // generate some points
98/// for i in 0..100{
99///     for j in 0..100{
100///         points.push(SimplePoint{position: Vector3::new(0.0, f64::from(i), f64::from(j))});
101///     }
102/// }
103/// let buffer = points.into_iter().collect::<HashMapBuffer>();
104/// let mut filtered = HashMapBuffer::new_from_layout(buffer.point_layout().clone());
105/// voxelgrid_filter(&buffer, 1.5, 1.5, 1.5, &mut filtered);
106/// // filtered now has fewer points than buffer
107/// assert!(filtered.len() < buffer.len() / 2);
108/// ```
109pub fn voxelgrid_filter<'a, 'b, PB: BorrowedBuffer<'a>, PBW: OwningBuffer<'b>>(
110    buffer: &'a PB,
111    leafsize_x: f64,
112    leafsize_y: f64,
113    leafsize_z: f64,
114    filtered_buffer: &'b mut PBW,
115) {
116    if !buffer
117        .point_layout()
118        .has_attribute(&attributes::POSITION_3D)
119    {
120        panic!("The PointBuffer does not have the attribute attributes::POSITION_3D which is needed for the creation of the voxel grid.");
121    }
122
123    // get the bounding box of the pointcloud
124    let aabb = calculate_bounds(buffer).unwrap();
125
126    // create arrays with all markers of one axis
127    let (markers_x, markers_y, markers_z) =
128        create_markers_for_axis(aabb, leafsize_x, leafsize_y, leafsize_z);
129
130    let mut voxels: Vec<Voxel> = Vec::with_capacity(buffer.len());
131
132    // create the VoxelGrid
133    for (i, p) in buffer
134        .view_attribute::<Vector3<f64>>(&POSITION_3D)
135        .into_iter()
136        .enumerate()
137    {
138        // get position of p's leaf
139        let pos = find_leaf(p, &markers_x, &markers_y, &markers_z);
140        // associate the right voxel with the point
141        match voxels.binary_search_by_key(&pos, |v| v.pos) {
142            // voxel already exists -> push p to points
143            Ok(index) => voxels[index].points.push(i),
144            // voxel not existing -> create new voxel
145            Err(index) => voxels.insert(
146                index,
147                Voxel {
148                    pos,
149                    points: vec![i],
150                },
151            ),
152        }
153    }
154    // approximate the centroid point in the voxel for all attributes in the given buffer
155    for v in &mut voxels {
156        let layout = filtered_buffer.point_layout().clone();
157        // using untyped point for now -> in future maybe different
158        let mut centroid = UntypedPointBuffer::new(&layout);
159        set_all_attributes(&layout, &mut centroid, v, buffer);
160        // This is safe because the `centroid` has the same `PointLayout` as `filtered_buffer`
161        unsafe {
162            filtered_buffer.push_points(centroid.get_cursor().into_inner());
163        }
164    }
165}
166
167/// calculates the attribute attribute_definition via max-pooling
168fn centroid_max_pool<'a, T: BorrowedBuffer<'a>>(
169    v: &Voxel,
170    buffer: &'a T,
171    attribute_definition: &PointAttributeDefinition,
172    point_type: PointAttributeDataType,
173) -> f64 {
174    let mut curr_max = 0.0;
175    let mut value;
176    for p in &v.points {
177        match point_type {
178            PointAttributeDataType::U8 => {
179                value = buffer.view_attribute::<u8>(attribute_definition).at(*p) as f64;
180            }
181            PointAttributeDataType::I8 => {
182                value = buffer.view_attribute::<i8>(attribute_definition).at(*p) as f64;
183            }
184            PointAttributeDataType::U16 => {
185                value = buffer.view_attribute::<u16>(attribute_definition).at(*p) as f64;
186            }
187            PointAttributeDataType::I16 => {
188                value = buffer.view_attribute::<i16>(attribute_definition).at(*p) as f64;
189            }
190            PointAttributeDataType::U32 => {
191                value = buffer.view_attribute::<u32>(attribute_definition).at(*p) as f64;
192            }
193            PointAttributeDataType::I32 => {
194                value = buffer.view_attribute::<i32>(attribute_definition).at(*p) as f64;
195            }
196            PointAttributeDataType::U64 => {
197                value = buffer.view_attribute::<u64>(attribute_definition).at(*p) as f64;
198            }
199            PointAttributeDataType::I64 => {
200                value = buffer.view_attribute::<i64>(attribute_definition).at(*p) as f64;
201            }
202            PointAttributeDataType::F32 => {
203                value = buffer.view_attribute::<f32>(attribute_definition).at(*p) as f64;
204            }
205            PointAttributeDataType::F64 => {
206                value = buffer.view_attribute::<f64>(attribute_definition).at(*p);
207            }
208            _ => unimplemented!(),
209        }
210        if value > curr_max {
211            curr_max = value;
212        }
213    }
214    curr_max
215}
216
217/// returns the most common value in the voxel for attribute_definition
218fn centroid_most_common<'a, T: BorrowedBuffer<'a>>(
219    v: &Voxel,
220    buffer: &'a T,
221    attribute_definition: &PointAttributeDefinition,
222    point_type: PointAttributeDataType,
223) -> isize {
224    let mut map = HashMap::new();
225    for p in &v.points {
226        match point_type {
227            PointAttributeDataType::U8 => {
228                *map.entry(
229                    buffer
230                        .view_attribute::<u8>(attribute_definition)
231                        .at(*p)
232                        .to_string(),
233                )
234                .or_insert(0) += 1
235            }
236            PointAttributeDataType::I8 => {
237                *map.entry(
238                    buffer
239                        .view_attribute::<i8>(attribute_definition)
240                        .at(*p)
241                        .to_string(),
242                )
243                .or_insert(0) += 1
244            }
245            PointAttributeDataType::U16 => {
246                *map.entry(
247                    buffer
248                        .view_attribute::<u16>(attribute_definition)
249                        .at(*p)
250                        .to_string(),
251                )
252                .or_insert(0) += 1
253            }
254            PointAttributeDataType::I16 => {
255                *map.entry(
256                    buffer
257                        .view_attribute::<i16>(attribute_definition)
258                        .at(*p)
259                        .to_string(),
260                )
261                .or_insert(0) += 1
262            }
263            PointAttributeDataType::U32 => {
264                *map.entry(
265                    buffer
266                        .view_attribute::<u32>(attribute_definition)
267                        .at(*p)
268                        .to_string(),
269                )
270                .or_insert(0) += 1
271            }
272            PointAttributeDataType::I32 => {
273                *map.entry(
274                    buffer
275                        .view_attribute::<i32>(attribute_definition)
276                        .at(*p)
277                        .to_string(),
278                )
279                .or_insert(0) += 1
280            }
281            PointAttributeDataType::U64 => {
282                *map.entry(
283                    buffer
284                        .view_attribute::<u64>(attribute_definition)
285                        .at(*p)
286                        .to_string(),
287                )
288                .or_insert(0) += 1
289            }
290            PointAttributeDataType::I64 => {
291                *map.entry(
292                    buffer
293                        .view_attribute::<i64>(attribute_definition)
294                        .at(*p)
295                        .to_string(),
296                )
297                .or_insert(0) += 1
298            }
299            PointAttributeDataType::F32 => {
300                *map.entry(
301                    buffer
302                        .view_attribute::<f32>(attribute_definition)
303                        .at(*p)
304                        .to_string(),
305                )
306                .or_insert(0) += 1
307            }
308            PointAttributeDataType::F64 => {
309                *map.entry(
310                    buffer
311                        .view_attribute::<f64>(attribute_definition)
312                        .at(*p)
313                        .to_string(),
314                )
315                .or_insert(0) += 1
316            }
317            _ => unimplemented!(),
318        }
319    }
320    let mut highest_count = 0;
321    let mut curr_key: String = "".to_string();
322    for (key, value) in map {
323        if value > highest_count {
324            highest_count = value;
325            curr_key = key;
326        }
327    }
328    curr_key.parse::<isize>().unwrap()
329}
330
331/// returns the average value in the voxel for attribute_definition
332/// vector types only
333fn centroid_average_vec<'a, T: BorrowedBuffer<'a>>(
334    v: &Voxel,
335    buffer: &'a T,
336    attribute_definition: &PointAttributeDefinition,
337    point_type: PointAttributeDataType,
338) -> Vector3<f64> {
339    let mut x_sum = 0.0;
340    let mut y_sum = 0.0;
341    let mut z_sum = 0.0;
342    for p in &v.points {
343        match point_type {
344            PointAttributeDataType::Vec3u8 => {
345                let vec = buffer
346                    .view_attribute::<Vector3<u8>>(attribute_definition)
347                    .at(*p);
348                x_sum += vec.x as f64;
349                y_sum += vec.y as f64;
350                z_sum += vec.z as f64;
351            }
352            PointAttributeDataType::Vec3u16 => {
353                let vec = buffer
354                    .view_attribute::<Vector3<u16>>(attribute_definition)
355                    .at(*p);
356                x_sum += vec.x as f64;
357                y_sum += vec.y as f64;
358                z_sum += vec.z as f64;
359            }
360            PointAttributeDataType::Vec3f32 => {
361                let vec = buffer
362                    .view_attribute::<Vector3<f32>>(attribute_definition)
363                    .at(*p);
364                x_sum += vec.x as f64;
365                y_sum += vec.y as f64;
366                z_sum += vec.z as f64;
367            }
368            PointAttributeDataType::Vec3f64 => {
369                let vec = buffer
370                    .view_attribute::<Vector3<f64>>(attribute_definition)
371                    .at(*p);
372                x_sum += vec.x;
373                y_sum += vec.y;
374                z_sum += vec.z;
375            }
376            PointAttributeDataType::Vec4u8 => unimplemented!(),
377            _ => panic!("Invalid data type for centroid_average_vec"),
378        }
379    }
380
381    // average over all values of one axis is the centroid's axis point
382    let num_of_points = v.points.len() as f64;
383    let centroid_x = x_sum / num_of_points;
384    let centroid_y = y_sum / num_of_points;
385    let centroid_z = z_sum / num_of_points;
386    Vector3::new(centroid_x, centroid_y, centroid_z)
387}
388
389/// returns the average value in the voxel for attribute_definition
390/// numeric types only
391fn centroid_average_num<'a, PB: BorrowedBuffer<'a>>(
392    v: &mut Voxel,
393    buffer: &'a PB,
394    attribute_definition: &PointAttributeDefinition,
395    point_type: PointAttributeDataType,
396) -> f64 {
397    let mut sum = 0.0;
398    for p in &v.points {
399        match point_type {
400            PointAttributeDataType::U8 => {
401                sum += buffer.view_attribute::<u8>(attribute_definition).at(*p) as f64
402            }
403            PointAttributeDataType::I8 => {
404                sum += buffer.view_attribute::<i8>(attribute_definition).at(*p) as f64
405            }
406            PointAttributeDataType::U16 => {
407                sum += buffer.view_attribute::<u16>(attribute_definition).at(*p) as f64
408            }
409            PointAttributeDataType::I16 => {
410                sum += buffer.view_attribute::<i16>(attribute_definition).at(*p) as f64
411            }
412            PointAttributeDataType::U32 => {
413                sum += buffer.view_attribute::<u32>(attribute_definition).at(*p) as f64
414            }
415            PointAttributeDataType::I32 => {
416                sum += buffer.view_attribute::<i32>(attribute_definition).at(*p) as f64
417            }
418            PointAttributeDataType::U64 => {
419                sum += buffer.view_attribute::<u64>(attribute_definition).at(*p) as f64
420            }
421            PointAttributeDataType::I64 => {
422                sum += buffer.view_attribute::<i64>(attribute_definition).at(*p) as f64
423            }
424            PointAttributeDataType::F32 => {
425                sum += buffer.view_attribute::<f32>(attribute_definition).at(*p) as f64
426            }
427            PointAttributeDataType::F64 => {
428                sum += buffer.view_attribute::<f64>(attribute_definition).at(*p)
429            }
430            PointAttributeDataType::Vec3u8 => panic!("For vector types use centroid_average_vec."),
431            PointAttributeDataType::Vec3u16 => panic!("For vector types use centroid_average_vec."),
432            PointAttributeDataType::Vec3f32 => panic!("For vector types use centroid_average_vec."),
433            PointAttributeDataType::Vec3f64 => panic!("For vector types use centroid_average_vec."),
434            PointAttributeDataType::Vec4u8 => panic!("For vector types use centroid_average_vec."),
435            _ => unimplemented!(),
436        }
437    }
438    sum / v.points.len() as f64
439}
440
441/// sets all attributes of the point-buffer for the centroid
442/// currently, only standard builtin types work.
443fn set_all_attributes<'a, PB: BorrowedBuffer<'a>>(
444    target_layout: &PointLayout,
445    centroid: &mut UntypedPointBuffer,
446    v: &mut Voxel,
447    buffer: &'a PB,
448) {
449    //TODO: for now we just check that the layout of the filtered_buffer does not contain any waveform values.
450    // A future version of this algorithm should take a separate object that contains a mapping between point attributes and the desired type of 'reduction function'.
451    // This way we could store defaults in this object but have the users overwrite the defaults with whatever they want.
452    if target_layout.has_attribute(&attributes::WAVEFORM_DATA_OFFSET)
453        || target_layout.has_attribute(&attributes::WAVEFORM_PACKET_SIZE)
454        || target_layout.has_attribute(&attributes::WAVEFORM_PARAMETERS)
455        || target_layout.has_attribute(&attributes::WAVE_PACKET_DESCRIPTOR_INDEX)
456        || target_layout.has_attribute(&attributes::RETURN_POINT_WAVEFORM_LOCATION)
457    {
458        panic!("Waveform data currently not supported!");
459    }
460
461    for a in target_layout.attributes() {
462        if a.name() == attributes::POSITION_3D.name()
463            && a.datatype() == attributes::POSITION_3D.datatype()
464        {
465            let position = centroid_average_vec(v, buffer, &attributes::POSITION_3D, a.datatype());
466            let pos_slice = bytemuck::bytes_of(&position);
467            centroid
468                .set_raw_attribute(&attributes::POSITION_3D, pos_slice)
469                .unwrap();
470        } else if a.name() == attributes::INTENSITY.name()
471            && a.datatype() == attributes::INTENSITY.datatype()
472        {
473            let average =
474                centroid_average_num::<PB>(v, buffer, &attributes::INTENSITY, a.datatype()) as u16;
475            let avg_slice = bytemuck::bytes_of(&average);
476            centroid
477                .set_raw_attribute(&attributes::INTENSITY, avg_slice)
478                .unwrap();
479        } else if a.name() == attributes::RETURN_NUMBER.name()
480            && a.datatype() == attributes::RETURN_NUMBER.datatype()
481        {
482            let most_common =
483                centroid_most_common::<PB>(v, buffer, &attributes::RETURN_NUMBER, a.datatype())
484                    as u8;
485            let mc_slice = bytemuck::bytes_of(&most_common);
486            centroid
487                .set_raw_attribute(&attributes::RETURN_NUMBER, mc_slice)
488                .unwrap();
489        } else if a.name() == attributes::NUMBER_OF_RETURNS.name()
490            && a.datatype() == attributes::NUMBER_OF_RETURNS.datatype()
491        {
492            let most_common =
493                centroid_most_common::<PB>(v, buffer, &attributes::NUMBER_OF_RETURNS, a.datatype())
494                    as u8;
495            let mc_slice = bytemuck::bytes_of(&most_common);
496            centroid
497                .set_raw_attribute(&attributes::NUMBER_OF_RETURNS, mc_slice)
498                .unwrap();
499        } else if a.name() == attributes::CLASSIFICATION_FLAGS.name()
500            && a.datatype() == attributes::CLASSIFICATION_FLAGS.datatype()
501        {
502            let max =
503                centroid_max_pool::<PB>(v, buffer, &attributes::CLASSIFICATION_FLAGS, a.datatype())
504                    as u8;
505            let m_slice = bytemuck::bytes_of(&max);
506            centroid
507                .set_raw_attribute(&attributes::CLASSIFICATION_FLAGS, m_slice)
508                .unwrap();
509        } else if a.name() == attributes::SCANNER_CHANNEL.name()
510            && a.datatype() == attributes::SCANNER_CHANNEL.datatype()
511        {
512            let most_common =
513                centroid_most_common::<PB>(v, buffer, &attributes::SCANNER_CHANNEL, a.datatype())
514                    as u8;
515            let mc_slice = bytemuck::bytes_of(&most_common);
516            centroid
517                .set_raw_attribute(&attributes::SCANNER_CHANNEL, mc_slice)
518                .unwrap();
519        } else if a.name() == attributes::SCAN_DIRECTION_FLAG.name()
520            && a.datatype() == attributes::SCAN_DIRECTION_FLAG.datatype()
521        {
522            let most_common = centroid_most_common::<PB>(
523                v,
524                buffer,
525                &attributes::SCAN_DIRECTION_FLAG,
526                a.datatype(),
527            ) != 0;
528            let mc_slice = bytemuck::bytes_of(&most_common);
529            centroid
530                .set_raw_attribute(&attributes::SCAN_DIRECTION_FLAG, mc_slice)
531                .unwrap();
532        } else if a.name() == attributes::EDGE_OF_FLIGHT_LINE.name()
533            && a.datatype() == attributes::EDGE_OF_FLIGHT_LINE.datatype()
534        {
535            let most_common = centroid_most_common::<PB>(
536                v,
537                buffer,
538                &attributes::EDGE_OF_FLIGHT_LINE,
539                a.datatype(),
540            ) != 0;
541            let mc_slice = bytemuck::bytes_of(&most_common);
542            centroid
543                .set_raw_attribute(&attributes::EDGE_OF_FLIGHT_LINE, mc_slice)
544                .unwrap();
545        } else if a.name() == attributes::CLASSIFICATION.name()
546            && a.datatype() == attributes::CLASSIFICATION.datatype()
547        {
548            let most_common =
549                centroid_most_common::<PB>(v, buffer, &attributes::CLASSIFICATION, a.datatype())
550                    as u8;
551            let mc_slice = bytemuck::bytes_of(&most_common);
552            centroid
553                .set_raw_attribute(&attributes::CLASSIFICATION, mc_slice)
554                .unwrap();
555        } else if a.name() == attributes::SCAN_ANGLE_RANK.name()
556            && a.datatype() == attributes::SCAN_ANGLE_RANK.datatype()
557        {
558            let most_common =
559                centroid_most_common::<PB>(v, buffer, &attributes::SCAN_ANGLE_RANK, a.datatype())
560                    as i8;
561            let mc_slice = bytemuck::bytes_of(&most_common);
562            centroid
563                .set_raw_attribute(&attributes::SCAN_ANGLE_RANK, mc_slice)
564                .unwrap();
565        } else if a.name() == attributes::SCAN_ANGLE.name()
566            && a.datatype() == attributes::SCAN_ANGLE.datatype()
567        {
568            let most_common =
569                centroid_most_common::<PB>(v, buffer, &attributes::SCAN_ANGLE, a.datatype()) as i16;
570            let mc_slice = bytemuck::bytes_of(&most_common);
571            centroid
572                .set_raw_attribute(&attributes::SCAN_ANGLE, mc_slice)
573                .unwrap();
574        } else if a.name() == attributes::USER_DATA.name()
575            && a.datatype() == attributes::USER_DATA.datatype()
576        {
577            let most_common =
578                centroid_most_common::<PB>(v, buffer, &attributes::USER_DATA, a.datatype()) as u8;
579            let mc_slice = bytemuck::bytes_of(&most_common);
580            centroid
581                .set_raw_attribute(&attributes::USER_DATA, mc_slice)
582                .unwrap();
583        } else if a.name() == attributes::POINT_SOURCE_ID.name()
584            && a.datatype() == attributes::POINT_SOURCE_ID.datatype()
585        {
586            let most_common =
587                centroid_most_common::<PB>(v, buffer, &attributes::POINT_SOURCE_ID, a.datatype())
588                    as u16;
589            let mc_slice = bytemuck::bytes_of(&most_common);
590            centroid
591                .set_raw_attribute(&attributes::POINT_SOURCE_ID, mc_slice)
592                .unwrap();
593        } else if a.name() == attributes::COLOR_RGB.name()
594            && a.datatype() == attributes::COLOR_RGB.datatype()
595        {
596            let color = centroid_average_vec(v, buffer, &attributes::COLOR_RGB, a.datatype());
597            let color_u16 = Vector3::new(color.x as u16, color.y as u16, color.z as u16);
598            let col_slice = bytemuck::bytes_of(&color_u16);
599            centroid
600                .set_raw_attribute(&attributes::COLOR_RGB, col_slice)
601                .unwrap();
602        } else if a.name() == attributes::GPS_TIME.name()
603            && a.datatype() == attributes::GPS_TIME.datatype()
604        {
605            let max = centroid_max_pool::<PB>(v, buffer, &attributes::GPS_TIME, a.datatype());
606            let m_slice = bytemuck::bytes_of(&max);
607            centroid
608                .set_raw_attribute(&attributes::GPS_TIME, m_slice)
609                .unwrap();
610        } else if a.name() == attributes::NIR.name() && a.datatype() == attributes::NIR.datatype() {
611            let average =
612                centroid_average_num::<PB>(v, buffer, &attributes::NIR, a.datatype()) as u16;
613            let avg_slice = bytemuck::bytes_of(&average);
614            centroid
615                .set_raw_attribute(&attributes::NIR, avg_slice)
616                .unwrap();
617
618        // The waveform-data attributes are implemented in the following lines, but there is no use-case for it currently.
619        /* } else if &a.name() == &attributes::WAVE_PACKET_DESCRIPTOR_INDEX.name() {
620            let most_common = centroid_most_common::<PB>(
621                v,
622                buffer,
623                &attributes::WAVE_PACKET_DESCRIPTOR_INDEX,
624                a.datatype(),
625            ) as u8;
626            let mc_slice = unsafe { view_raw_bytes(&most_common) };
627            &centroid.set_raw_attribute(&attributes::WAVE_PACKET_DESCRIPTOR_INDEX, mc_slice);
628        } else if &a.name() == &attributes::WAVEFORM_DATA_OFFSET.name() {
629            let most_common = centroid_most_common::<PB>(
630                v,
631                buffer,
632                &attributes::WAVEFORM_DATA_OFFSET,
633                a.datatype(),
634            ) as u64;
635            let mc_slice = unsafe { view_raw_bytes(&most_common) };
636            &centroid.set_raw_attribute(&attributes::WAVEFORM_DATA_OFFSET, mc_slice);
637        } else if &a.name() == &attributes::WAVEFORM_PACKET_SIZE.name() {
638            let most_common = centroid_most_common::<PB>(
639                v,
640                buffer,
641                &attributes::WAVEFORM_PACKET_SIZE,
642                a.datatype(),
643            ) as u32;
644            let mc_slice = unsafe { view_raw_bytes(&most_common) };
645            &centroid.set_raw_attribute(&attributes::WAVEFORM_PACKET_SIZE, mc_slice);
646        } else if &a.name() == &attributes::RETURN_POINT_WAVEFORM_LOCATION.name() {
647            let average = centroid_average_num::<PB>(
648                v,
649                buffer,
650                &attributes::RETURN_POINT_WAVEFORM_LOCATION,
651                a.datatype(),
652            ) as f32;
653            let avg_slice = unsafe { view_raw_bytes(&average) };
654            &centroid.set_raw_attribute(&attributes::RETURN_POINT_WAVEFORM_LOCATION, avg_slice);
655        } else if &a.name() == &attributes::WAVEFORM_PARAMETERS.name() {
656            let params =
657                centroid_average_vec(v, buffer, &attributes::WAVEFORM_PARAMETERS, a.datatype());
658            let params_f32 = Vector3::new(params.x as f32, params.y as f32, params.z as f32);
659            let par_slice = unsafe { view_raw_bytes(&params_f32) };
660            &centroid.set_raw_attribute(&attributes::WAVEFORM_PARAMETERS, par_slice); */
661        } else if a.name() == attributes::POINT_ID.name()
662            && a.datatype() == attributes::POINT_ID.datatype()
663        {
664            let max =
665                centroid_max_pool::<PB>(v, buffer, &attributes::POINT_ID, a.datatype()) as u64;
666            let m_slice = bytemuck::bytes_of(&max);
667            centroid
668                .set_raw_attribute(&attributes::POINT_ID, m_slice)
669                .unwrap();
670        } else if a.name() == attributes::NORMAL.name()
671            && a.datatype() == attributes::NORMAL.datatype()
672        {
673            let normal = centroid_average_vec(v, buffer, &attributes::NORMAL, a.datatype());
674            let normal_f32 = Vector3::new(normal.x as f32, normal.y as f32, normal.z as f32);
675            let nor_slice = bytemuck::bytes_of(&normal_f32);
676            centroid
677                .set_raw_attribute(&attributes::NORMAL, nor_slice)
678                .unwrap();
679        }
680        // we have a non-standard attribute -> use max-pooling for numbers and average for vec
681        // currently, only f64 and Vec3f64 is supported
682        else {
683            panic!(
684                "attribute is non-standard which is not supported currently: {:?}",
685                a
686            );
687        }
688    }
689}
690
691#[cfg(test)]
692mod tests {
693
694    use crate::voxel_grid::voxelgrid_filter;
695    use pasture_core::{
696        containers::{BorrowedBuffer, BorrowedBufferExt, HashMapBuffer, MakeBufferFromLayout},
697        layout::attributes,
698        nalgebra::Vector3,
699    };
700    use pasture_derive::PointType;
701    use rand::{prelude::ThreadRng, Rng};
702
703    #[repr(C, packed)]
704    #[derive(PointType, Debug, Copy, Clone, bytemuck::AnyBitPattern, bytemuck::NoUninit)]
705    pub struct CompletePoint {
706        #[pasture(BUILTIN_POSITION_3D)]
707        pub position: Vector3<f64>,
708        #[pasture(BUILTIN_INTENSITY)]
709        pub intensity: u16,
710        #[pasture(BUILTIN_RETURN_NUMBER)]
711        pub return_number: u8,
712        #[pasture(BUILTIN_NUMBER_OF_RETURNS)]
713        pub num_of_returns: u8,
714        #[pasture(BUILTIN_CLASSIFICATION_FLAGS)]
715        pub classification_flags: u8,
716        #[pasture(BUILTIN_SCANNER_CHANNEL)]
717        pub scanner_channel: u8,
718        #[pasture(BUILTIN_SCAN_DIRECTION_FLAG)]
719        pub scan_dir_flag: u8,
720        #[pasture(BUILTIN_EDGE_OF_FLIGHT_LINE)]
721        pub edge_of_flight_line: u8,
722        #[pasture(BUILTIN_CLASSIFICATION)]
723        pub classification: u8,
724        #[pasture(BUILTIN_SCAN_ANGLE_RANK)]
725        pub scan_angle_rank: i8,
726        #[pasture(BUILTIN_SCAN_ANGLE)]
727        pub scan_angle: i16,
728        #[pasture(BUILTIN_USER_DATA)]
729        pub user_data: u8,
730        #[pasture(BUILTIN_POINT_SOURCE_ID)]
731        pub point_source_id: u16,
732        #[pasture(BUILTIN_COLOR_RGB)]
733        pub color_rgb: Vector3<u16>,
734        #[pasture(BUILTIN_GPS_TIME)]
735        pub gps_time: f64,
736        #[pasture(BUILTIN_NIR)]
737        pub nir: u16,
738        // #[pasture(BUILTIN_WAVE_PACKET_DESCRIPTOR_INDEX)]
739        // pub wave_packet_descriptor_index: u8,
740        // #[pasture(BUILTIN_WAVEFORM_DATA_OFFSET)]
741        // pub waveform_data_offset: u64,
742        // #[pasture(BUILTIN_WAVEFORM_PACKET_SIZE)]
743        // pub waveform_packet_size: u32,
744        // #[pasture(BUILTIN_RETURN_POINT_WAVEFORM_LOCATION)]
745        // pub return_point_waveform_location: f32,
746        // #[pasture(BUILTIN_WAVEFORM_PARAMETERS)]
747        // pub waveform_parameters: Vector3<f32>,
748    }
749
750    fn _generate_vec3f32(rng: &mut ThreadRng) -> Vector3<f32> {
751        Vector3::new(rng.gen_range(-30.0..10.0), rng.gen_range(-11.1..10.0), 31.0)
752    }
753    fn generate_vec3u16(rng: &mut ThreadRng) -> Vector3<u16> {
754        Vector3::new(rng.gen_range(11..120), rng.gen_range(11..120), 42)
755    }
756
757    fn setup_point_cloud() -> HashMapBuffer {
758        let mut rng = rand::thread_rng();
759        let mut points = vec![];
760        // create vertices between 0.0 and 10.0
761        points.push(CompletePoint {
762            position: Vector3::new(0.0, 0.0, 0.0),
763            intensity: rng.gen_range(200..800),
764            return_number: rng.gen_range(20..80),
765            num_of_returns: rng.gen_range(20..80),
766            classification_flags: rng.gen_range(7..20),
767            scanner_channel: rng.gen_range(7..20),
768            scan_dir_flag: rng.gen_range(0..47),
769            edge_of_flight_line: rng.gen_range(0..81),
770            classification: rng.gen_range(121..200),
771            scan_angle_rank: rng.gen_range(-121..20),
772            scan_angle: rng.gen_range(-21..8),
773            user_data: rng.gen_range(1..8),
774            point_source_id: rng.gen_range(9..89),
775            color_rgb: generate_vec3u16(&mut rng),
776            gps_time: rng.gen_range(-22.4..81.3),
777            nir: rng.gen_range(4..82),
778            // wave_packet_descriptor_index: rng.gen_range(2..42),
779            // waveform_data_offset: rng.gen_range(1..31),
780            // waveform_packet_size: rng.gen_range(32..64),
781            // return_point_waveform_location: rng.gen_range(-32.2..64.1),
782            // waveform_parameters: generate_vec3f32(&mut rng),
783        });
784        points.push(CompletePoint {
785            position: Vector3::new(10.0, 10.0, 10.0),
786            intensity: rng.gen_range(200..800),
787            return_number: rng.gen_range(20..80),
788            num_of_returns: rng.gen_range(20..80),
789            classification_flags: rng.gen_range(7..20),
790            scanner_channel: rng.gen_range(7..20),
791            scan_dir_flag: rng.gen_range(0..47),
792            edge_of_flight_line: rng.gen_range(0..81),
793            classification: rng.gen_range(121..200),
794            scan_angle_rank: rng.gen_range(-121..20),
795            scan_angle: rng.gen_range(-21..8),
796            user_data: rng.gen_range(1..8),
797            point_source_id: rng.gen_range(9..89),
798            color_rgb: generate_vec3u16(&mut rng),
799            gps_time: rng.gen_range(-22.4..81.3),
800            nir: rng.gen_range(4..82),
801            // wave_packet_descriptor_index: rng.gen_range(2..42),
802            // waveform_data_offset: rng.gen_range(1..31),
803            // waveform_packet_size: rng.gen_range(32..64),
804            // return_point_waveform_location: rng.gen_range(-32.2..64.1),
805            // waveform_parameters: generate_vec3f32(&mut rng),
806        });
807        // generate 3000 points
808        for i in 0..10 {
809            for j in 0..10 {
810                for k in 0..10 {
811                    //1.5, 2.5, ...
812                    points.push(CompletePoint {
813                        // average_vec
814                        position: Vector3::new(
815                            f64::from(i) + 0.5,
816                            f64::from(j) + 0.5,
817                            f64::from(k) + 0.5,
818                        ),
819                        // average_num
820                        intensity: 2,
821                        // most_common num
822                        return_number: 32,
823                        num_of_returns: rng.gen_range(20..80),
824                        // max_pool
825                        classification_flags: 3,
826                        scanner_channel: rng.gen_range(7..20),
827                        // most_common bool
828                        scan_dir_flag: 0,
829                        edge_of_flight_line: rng.gen_range(0..81),
830                        classification: rng.gen_range(121..200),
831                        scan_angle_rank: rng.gen_range(-121..20),
832                        scan_angle: rng.gen_range(-21..8),
833                        user_data: rng.gen_range(1..8),
834                        point_source_id: rng.gen_range(9..89),
835                        color_rgb: generate_vec3u16(&mut rng),
836                        gps_time: rng.gen_range(-22.4..81.3),
837                        nir: rng.gen_range(4..82),
838                        // wave_packet_descriptor_index: rng.gen_range(2..42),
839                        // waveform_data_offset: rng.gen_range(1..31),
840                        // waveform_packet_size: rng.gen_range(32..64),
841                        // return_point_waveform_location: rng.gen_range(-32.2..64.1),
842                        // waveform_parameters: generate_vec3f32(&mut rng),
843                    });
844                    // 1.6, 2.6, ...
845                    points.push(CompletePoint {
846                        position: Vector3::new(
847                            f64::from(i) + 0.6,
848                            f64::from(j) + 0.6,
849                            f64::from(k) + 0.6,
850                        ),
851                        intensity: 4,
852                        return_number: 42,
853                        num_of_returns: rng.gen_range(20..80),
854                        classification_flags: 7,
855                        scanner_channel: rng.gen_range(7..20),
856                        scan_dir_flag: 0,
857                        edge_of_flight_line: rng.gen_range(0..81),
858                        classification: rng.gen_range(121..200),
859                        scan_angle_rank: rng.gen_range(-121..20),
860                        scan_angle: rng.gen_range(-21..8),
861                        user_data: rng.gen_range(1..8),
862                        point_source_id: rng.gen_range(9..89),
863                        color_rgb: generate_vec3u16(&mut rng),
864                        gps_time: rng.gen_range(-22.4..81.3),
865                        nir: rng.gen_range(4..82),
866                        // wave_packet_descriptor_index: rng.gen_range(2..42),
867                        // waveform_data_offset: rng.gen_range(1..31),
868                        // waveform_packet_size: rng.gen_range(32..64),
869                        // return_point_waveform_location: rng.gen_range(-32.2..64.1),
870                        // waveform_parameters: generate_vec3f32(&mut rng),
871                    });
872                    // 1.7, 2.7, ...
873                    points.push(CompletePoint {
874                        position: Vector3::new(
875                            f64::from(i) + 0.7,
876                            f64::from(j) + 0.7,
877                            f64::from(k) + 0.7,
878                        ),
879                        intensity: 6,
880                        return_number: 42,
881                        num_of_returns: rng.gen_range(20..80),
882                        classification_flags: 133,
883                        scanner_channel: rng.gen_range(7..20),
884                        scan_dir_flag: 1,
885                        edge_of_flight_line: rng.gen_range(0..81),
886                        classification: rng.gen_range(121..200),
887                        scan_angle_rank: rng.gen_range(-121..20),
888                        scan_angle: rng.gen_range(-21..8),
889                        user_data: rng.gen_range(1..8),
890                        point_source_id: rng.gen_range(9..89),
891                        color_rgb: generate_vec3u16(&mut rng),
892                        gps_time: rng.gen_range(-22.4..81.3),
893                        nir: rng.gen_range(4..82),
894                        // wave_packet_descriptor_index: rng.gen_range(2..42),
895                        // waveform_data_offset: rng.gen_range(1..31),
896                        // waveform_packet_size: rng.gen_range(32..64),
897                        // return_point_waveform_location: rng.gen_range(-32.2..64.1),
898                        // waveform_parameters: generate_vec3f32(&mut rng),
899                    });
900                }
901            }
902        }
903        points.into_iter().collect()
904    }
905
906    #[test]
907    fn test_voxel_grid_filter() {
908        let buffer = setup_point_cloud();
909        assert!(buffer.len() == 3002);
910        let mut filtered = HashMapBuffer::new_from_layout(buffer.point_layout().clone());
911        voxelgrid_filter(&buffer, 1.0, 1.0, 1.0, &mut filtered);
912        // filtered now has only 1000 points
913        assert!(filtered.len() == 1000);
914
915        // average_vec
916        let first_pos = filtered
917            .view_attribute::<Vector3<f64>>(&attributes::POSITION_3D)
918            .at(1);
919        assert!(first_pos.x > 0.59 && first_pos.x < 0.61);
920        assert!(first_pos.y > 0.59 && first_pos.y < 0.61);
921        assert!(first_pos.z > 1.59 && first_pos.z < 1.61);
922        // average_num
923        assert!(filtered.view_attribute::<u16>(&attributes::INTENSITY).at(1) == 4);
924        // most_common num
925        assert!(
926            filtered
927                .view_attribute::<u8>(&attributes::RETURN_NUMBER)
928                .at(1)
929                == 42
930        );
931        // max_pool
932        assert!(
933            filtered
934                .view_attribute::<u8>(&attributes::CLASSIFICATION_FLAGS)
935                .at(1)
936                == 133
937        );
938    }
939}
pasture_algorithms/voxel_grid.rs

pasture_algorithms/
voxel_grid.rs
