epoint_transform/
transform.rs

1use epoint_core::PointCloud;
2use std::collections::HashSet;
3
4use crate::Error;
5use crate::Error::InvalidNumber;
6use epoint_core::PointDataColumnType;
7use nalgebra::{Isometry3, Point3, Vector3};
8use rand::{Rng, SeedableRng};
9use rand_chacha::ChaCha8Rng;
10use rayon::prelude::*;
11
12pub fn translate(point_cloud: &PointCloud, translation: Vector3<f64>) -> Result<PointCloud, Error> {
13    let mut translated_data = point_cloud.point_data.data_frame.clone();
14    translated_data.apply(PointDataColumnType::X.as_str(), |x| x + translation.x)?;
15    translated_data.apply(PointDataColumnType::Y.as_str(), |y| y + translation.y)?;
16    translated_data.apply(PointDataColumnType::Z.as_str(), |z| z + translation.z)?;
17
18    if point_cloud.contains_sensor_translation() {
19        translated_data.apply(PointDataColumnType::SensorTranslationX.as_str(), |x| {
20            x + translation.x
21        })?;
22        translated_data.apply(PointDataColumnType::SensorTranslationY.as_str(), |y| {
23            y + translation.y
24        })?;
25        translated_data.apply(PointDataColumnType::SensorTranslationZ.as_str(), |z| {
26            z + translation.z
27        })?;
28    }
29
30    let info = point_cloud.info().clone();
31    let frames = point_cloud.reference_frames().clone();
32    let point_cloud = PointCloud::from_data_frame(translated_data, info, frames)?;
33    Ok(point_cloud)
34}
35
36pub fn apply_isometry(
37    point_cloud: &PointCloud,
38    isometry: Isometry3<f64>,
39) -> Result<PointCloud, Error> {
40    let transformed_points: Vec<Point3<f64>> = point_cloud
41        .point_data
42        .get_all_points()
43        .par_iter()
44        .map(|p| isometry * p)
45        .collect();
46    let mut transformed_point_cloud = point_cloud.clone();
47    transformed_point_cloud
48        .point_data
49        .update_points_in_place(transformed_points)?;
50
51    if let Ok(all_sensor_translations) = point_cloud.point_data.get_all_sensor_translations() {
52        let transformed_sensor_translations: Vec<Point3<f64>> = all_sensor_translations
53            .par_iter()
54            .map(|p| isometry * p)
55            .collect();
56
57        transformed_point_cloud
58            .point_data
59            .update_sensor_translations_in_place(transformed_sensor_translations)?;
60    }
61
62    Ok(transformed_point_cloud)
63}
64
65pub fn deterministic_downsample(
66    point_cloud: &PointCloud,
67    target_size: usize,
68    seed_number: Option<u64>,
69) -> Result<PointCloud, Error> {
70    if point_cloud.size() < target_size {
71        return Ok(point_cloud.clone());
72    }
73
74    let rng = ChaCha8Rng::seed_from_u64(seed_number.unwrap_or_default());
75    let row_indices = generate_random_numbers(rng, point_cloud.size(), target_size)?;
76
77    let downsampled_point_cloud = point_cloud.filter_by_row_indices(row_indices)?;
78    Ok(downsampled_point_cloud)
79}
80
81fn generate_random_numbers(
82    mut rng: ChaCha8Rng,
83    number_max: usize,
84    len: usize,
85) -> Result<HashSet<usize>, Error> {
86    if number_max < len {
87        return Err(InvalidNumber);
88    }
89
90    let mut numbers: HashSet<usize> = HashSet::with_capacity(len);
91    while numbers.len() < len {
92        let n: usize = rng.random_range(0..number_max);
93        numbers.insert(n);
94    }
95    Ok(numbers)
96}
epoint_transform/transform.rs

epoint_transform/
transform.rs
