spatialrust_registration/
transform.rs

1use spatialrust_core::{
2    FieldSemantic, HasNormals3, HasPositions3, PointBuffer, PointBufferSet, PointCloud,
3    SpatialResult,
4};
5use spatialrust_math::{Isometry3, TransformPoint, Vec3};
6
7/// Applies a rigid transform to point positions and normals in a point cloud copy.
8pub fn transform_point_cloud(
9    input: &PointCloud,
10    transform: Isometry3<f32>,
11) -> SpatialResult<PointCloud> {
12    if input.is_empty() {
13        return Ok(input.clone());
14    }
15
16    let (x, y, z) = input.positions3()?;
17    let mut tx = Vec::with_capacity(input.len());
18    let mut ty = Vec::with_capacity(input.len());
19    let mut tz = Vec::with_capacity(input.len());
20    for index in 0..input.len() {
21        let transformed = transform.transform_point(Vec3::new(x[index], y[index], z[index]));
22        tx.push(transformed.x);
23        ty.push(transformed.y);
24        tz.push(transformed.z);
25    }
26
27    let mut buffers = PointBufferSet::new();
28    for field in input.schema().fields() {
29        let source = input.field(&field.name)?;
30        let buffer = match field.semantic {
31            FieldSemantic::PositionX => PointBuffer::from_f32(tx.clone()),
32            FieldSemantic::PositionY => PointBuffer::from_f32(ty.clone()),
33            FieldSemantic::PositionZ => PointBuffer::from_f32(tz.clone()),
34            FieldSemantic::NormalX | FieldSemantic::NormalY | FieldSemantic::NormalZ => {
35                transform_normal_buffer(input, transform, field.semantic)?
36            }
37            _ => clone_buffer(source)?,
38        };
39        buffers.insert(field.name.clone(), buffer);
40    }
41
42    PointCloud::try_from_parts(input.schema().clone(), buffers, input.metadata().clone())
43}
44
45fn transform_normal_buffer(
46    input: &PointCloud,
47    transform: Isometry3<f32>,
48    semantic: FieldSemantic,
49) -> SpatialResult<PointBuffer> {
50    let (nx, ny, nz) = input.normals3()?;
51    let mut values = Vec::with_capacity(input.len());
52    for index in 0..input.len() {
53        let normal =
54            transform.transform_vector(Vec3::new(nx[index], ny[index], nz[index])).normalize();
55        values.push(match semantic {
56            FieldSemantic::NormalX => normal.x,
57            FieldSemantic::NormalY => normal.y,
58            FieldSemantic::NormalZ => normal.z,
59            _ => 0.0,
60        });
61    }
62    Ok(PointBuffer::from_f32(values))
63}
64
65fn clone_buffer(buffer: &PointBuffer) -> SpatialResult<PointBuffer> {
66    Ok(match buffer {
67        PointBuffer::F32(values) => PointBuffer::from_f32(values.clone()),
68        PointBuffer::F64(values) => PointBuffer::F64(values.clone()),
69        PointBuffer::U8(values) => PointBuffer::U8(values.clone()),
70        PointBuffer::U16(values) => PointBuffer::U16(values.clone()),
71        PointBuffer::U32(values) => PointBuffer::U32(values.clone()),
72        PointBuffer::I32(values) => PointBuffer::I32(values.clone()),
73    })
74}
75
76#[cfg(test)]
77mod tests {
78    use super::transform_point_cloud;
79    use spatialrust_core::{HasPositions3, PointCloudBuilder};
80    use spatialrust_math::{Isometry3, Vec3};
81
82    #[test]
83    fn transforms_positions() {
84        let mut builder = PointCloudBuilder::xyz();
85        builder.push_point([0.0, 0.0, 0.0]).unwrap();
86        builder.push_point([1.0, 0.0, 0.0]).unwrap();
87        let cloud = builder.build().unwrap();
88
89        let transform =
90            Isometry3::new(spatialrust_math::Quat::<f32>::identity(), Vec3::new(1.0, 2.0, 3.0));
91        let transformed = transform_point_cloud(&cloud, transform).unwrap();
92        let (x, y, z) = transformed.positions3().unwrap();
93        assert!((x[0] - 1.0).abs() < 1e-6);
94        assert!((y[1] - 2.0).abs() < 1e-6);
95        assert!((z[1] - 3.0).abs() < 1e-6);
96    }
97}
spatialrust_registration/transform.rs

spatialrust_registration/
transform.rs
