epoint_transform/
transform.rs

1use crate::Error;
2use crate::Error::InvalidNumber;
3use ecoord::FrameId;
4use epoint_core::PointCloud;
5use epoint_core::PointDataColumnType;
6use nalgebra::{Isometry3, Point3, UnitQuaternion, Vector3};
7use rand::{RngExt, SeedableRng};
8use rand_chacha::ChaCha8Rng;
9use rayon::prelude::*;
10use std::collections::HashSet;
11
12pub fn translate(
13    point_cloud: &PointCloud,
14    translation: Vector3<f64>,
15    frame_id: &FrameId,
16) -> Result<PointCloud, Error> {
17    if point_cloud.point_data.contains_frame_id_column() {
18        todo!("Mixed frame ID column not supported for translation operation yet.");
19    }
20
21    let mut translated_data = point_cloud.point_data.data_frame.clone();
22    if point_cloud.info.frame_id.as_ref().expect("must be set") == frame_id {
23        translated_data.apply(PointDataColumnType::X.as_str(), |x| x + translation.x)?;
24        translated_data.apply(PointDataColumnType::Y.as_str(), |y| y + translation.y)?;
25        translated_data.apply(PointDataColumnType::Z.as_str(), |z| z + translation.z)?;
26
27        if point_cloud.contains_sensor_translation() {
28            translated_data.apply(PointDataColumnType::SensorTranslationX.as_str(), |x| {
29                x + translation.x
30            })?;
31            translated_data.apply(PointDataColumnType::SensorTranslationY.as_str(), |y| {
32                y + translation.y
33            })?;
34            translated_data.apply(PointDataColumnType::SensorTranslationZ.as_str(), |z| {
35                z + translation.z
36            })?;
37        }
38    }
39
40    let info = point_cloud.info().clone();
41    let mut transform_tree = point_cloud.transform_tree().clone();
42
43    for (current_id, current_transform) in transform_tree.edges_mut().iter_mut() {
44        if current_id.parent_frame_id != *frame_id {
45            continue;
46        }
47
48        current_transform.prepend_isometry(&Isometry3::from_parts(
49            translation.into(),
50            UnitQuaternion::identity(),
51        ));
52    }
53
54    let point_cloud = PointCloud::from_data_frame(translated_data, info, transform_tree)?;
55    Ok(point_cloud)
56}
57
58pub fn apply_isometry(
59    point_cloud: &PointCloud,
60    isometry: Isometry3<f64>,
61) -> Result<PointCloud, Error> {
62    let transformed_points: Vec<Point3<f64>> = point_cloud
63        .point_data
64        .get_all_points()
65        .par_iter()
66        .map(|p| isometry * p)
67        .collect();
68    let mut transformed_point_cloud = point_cloud.clone();
69    transformed_point_cloud
70        .point_data
71        .update_points_in_place(transformed_points)?;
72
73    if let Ok(all_sensor_translations) = point_cloud.point_data.get_all_sensor_translations() {
74        let transformed_sensor_translations: Vec<Point3<f64>> = all_sensor_translations
75            .par_iter()
76            .map(|p| isometry * p)
77            .collect();
78
79        transformed_point_cloud
80            .point_data
81            .update_sensor_translations_in_place(transformed_sensor_translations)?;
82    }
83
84    Ok(transformed_point_cloud)
85}
86
87pub fn deterministic_downsample(
88    point_cloud: &PointCloud,
89    target_size: usize,
90    seed_number: Option<u64>,
91) -> Result<PointCloud, Error> {
92    if point_cloud.size() < target_size {
93        return Ok(point_cloud.clone());
94    }
95
96    let rng = ChaCha8Rng::seed_from_u64(seed_number.unwrap_or_default());
97    let row_indices = generate_random_numbers(rng, point_cloud.size(), target_size)?;
98
99    let downsampled_point_cloud = point_cloud.filter_by_row_indices(row_indices)?;
100    Ok(downsampled_point_cloud)
101}
102
103fn generate_random_numbers(
104    mut rng: ChaCha8Rng,
105    number_max: usize,
106    len: usize,
107) -> Result<HashSet<usize>, Error> {
108    if number_max < len {
109        return Err(InvalidNumber);
110    }
111
112    let mut numbers: HashSet<usize> = HashSet::with_capacity(len);
113    while numbers.len() < len {
114        let n: usize = rng.random_range(0..number_max);
115        numbers.insert(n);
116    }
117    Ok(numbers)
118}
epoint_transform/transform.rs

epoint_transform/
transform.rs
