mapping_algorithms/point_clouds/
downsample.rs

1// SPDX-License-Identifier: MIT
2/*
3 * Copyright (c) [2023 - Present] Emily Matheys <emilymatt96@gmail.com>
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a copy
6 * of this software and associated documentation files (the "Software"), to deal
7 * in the Software without restriction, including without limitation the rights
8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 * copies of the Software, and to permit persons to whom the Software is
10 * furnished to do so, subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be included in all
13 * copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 * SOFTWARE.
22 */
23
24use nalgebra::{ComplexField, Point, Scalar};
25use num_traits::{AsPrimitive, NumAssign};
26
27use crate::{array, HashMap, Vec};
28
29/// Downsample a points cloud, returning a new point cloud, with all points within each voxel combined into their mean.
30///
31/// # Arguments
32/// * `points`: a slice of [`Point`], representing the point cloud.
33/// * `voxel_size`: a floating point number, specifying the size for each voxel, all points inside that voxel will be downsampled to their centroid.
34///
35/// # Generics
36/// * `T`: Either an [`f32`] or [`f64`].
37/// * `N`: A const usize, representing the number of dimensions in the points.
38///
39/// # Returns
40/// A [`Vec`] of [`Point`] representing the downsampled point cloud.
41///
42/// # Warnings
43/// * Point cloud order is *never* guaranteed.
44/// * When compiling for no_std, a `BTreeMap` from the `alloc` crate is used in place of a [`HashMap`].
45#[cfg_attr(
46    feature = "tracing",
47    tracing::instrument("Downsample Point Cloud Using Voxels", skip_all)
48)]
49pub fn downsample_point_cloud_voxel<T, O, const N: usize>(
50    points: &[Point<T, N>],
51    voxel_size: O,
52) -> Vec<Point<T, N>>
53where
54    O: AsPrimitive<isize> + ComplexField + Copy,
55    T: AsPrimitive<O> + Scalar + NumAssign,
56    usize: AsPrimitive<T>,
57{
58    let mut voxel_map: HashMap<[isize; N], Vec<Point<T, N>>> = HashMap::new();
59
60    // Assign points to voxels
61    for point in points {
62        let voxel_coords: [isize; N] = array::from_fn(|idx| {
63            (AsPrimitive::<O>::as_(point[idx]) / voxel_size)
64                .floor()
65                .as_()
66        });
67        voxel_map.entry(voxel_coords).or_default().push(*point);
68    }
69
70    // Compute centroid for each voxel and collect them as the downsampled points
71    voxel_map
72        .into_values()
73        .map(|points_in_voxel| {
74            let num_points = points_in_voxel.len().as_();
75            let sum = points_in_voxel
76                .into_iter()
77                .fold(Point::default(), |acc, p| acc + p.coords);
78            sum / num_points
79        })
80        .collect()
81}
82
83#[cfg(test)]
84mod tests {
85    use super::*;
86    use nalgebra::Point3;
87
88    #[test]
89    fn test_downsample_point_cloud() {
90        let point_cloud = [
91            Point3::new(-5.9, -5.0, -3.9), // These two are very close now
92            Point3::new(-6.0, -5.0, -4.0), // Will end up in the same voxel
93            Point3::new(-1.0, -2.0, -3.0),
94            Point3::new(0.0, 0.0, 0.0),    // These two are also very close
95            Point3::new(0.05, 0.08, 0.01), // Will end up in the same voxel
96            Point3::new(1.0, 2.0, 3.0),
97            Point3::new(6.0, 5.0, 4.0),
98        ];
99
100        // We should be left with 5 voxels
101        let res = downsample_point_cloud_voxel(point_cloud.as_slice(), 0.5);
102        assert_eq!(res.len(), 5);
103
104        // Moreover, the most negative voxel had two points, (-5.9, -5.0, -4.0) and (-6.0, -5.0, -4.0)
105        // Meaning there should be a voxel resulting in the elements' centroid
106        assert!(res
107            .iter()
108            .any(|element| *element == Point3::new(-5.95, -5.0, -3.95)));
109    }
110}
mapping_algorithms/point_clouds/downsample.rs

mapping_algorithms/point_clouds/
downsample.rs
