pointrain_io/pcd/
point.rs

1use pointrain_core::{
2    pc::point::{
3        PointXYZ, PointXYZI, PointXYZINormal, PointXYZNormal, PointXYZRgb, PointXYZRgbNormal,
4    },
5    types::{Normal, Position},
6};
7
8use crate::{
9    error::{MissingField, PointRainIOError},
10    field::{PointField, PointFieldDatum},
11};
12
13fn find_field(fields: &[PointField], name: &'static str) -> Result<usize, MissingField> {
14    fields
15        .iter()
16        .enumerate()
17        .find(|(_, f)| f.name == name)
18        .map(|(i, _)| i)
19        .ok_or(MissingField(name))
20}
21
22fn find_xyz(fields: &[PointField]) -> Result<(usize, usize, usize), MissingField> {
23    Ok((
24        find_field(fields, "x")?,
25        find_field(fields, "y")?,
26        find_field(fields, "z")?,
27    ))
28}
29
30fn find_normal(fields: &[PointField]) -> Result<(usize, usize, usize, usize), MissingField> {
31    Ok((
32        find_field(fields, "normal_x")?,
33        find_field(fields, "normal_y")?,
34        find_field(fields, "normal_z")?,
35        find_field(fields, "curvature")?,
36    ))
37}
38
39fn find_intensity(fields: &[PointField]) -> Result<usize, MissingField> {
40    find_field(fields, "intensity")
41}
42
43fn find_rgb(fields: &[PointField]) -> Result<usize, MissingField> {
44    find_field(fields, "rgb")
45}
46
47pub type PointMapper<T> = Box<dyn Fn(&[PointFieldDatum]) -> Result<T, PointRainIOError>>;
48
49pub trait PointReadable: Sized {
50    fn read_data_func(fields: &[PointField]) -> Result<PointMapper<Self>, PointRainIOError>;
51}
52
53impl PointReadable for PointXYZ {
54    fn read_data_func(fields: &[PointField]) -> Result<PointMapper<Self>, PointRainIOError> {
55        let (x, y, z) = find_xyz(fields)?;
56
57        let closure = move |data: &[PointFieldDatum]| {
58            Ok(Self {
59                pos: Position::new(data[x].to_float(), data[y].to_float(), data[z].to_float()),
60            })
61        };
62
63        Ok(Box::new(closure))
64    }
65}
66
67impl PointReadable for PointXYZNormal {
68    fn read_data_func(fields: &[PointField]) -> Result<PointMapper<Self>, PointRainIOError> {
69        let (x, y, z) = find_xyz(fields)?;
70        let (nx, ny, nz, curvature) = find_normal(fields)?;
71
72        let closure = move |data: &[PointFieldDatum]| {
73            Ok(Self {
74                pos: Position::new(data[x].to_float(), data[y].to_float(), data[z].to_float()),
75                normal: Normal::new(
76                    data[nx].to_float(),
77                    data[ny].to_float(),
78                    data[nz].to_float(),
79                ),
80                curvature: data[curvature].to_float(),
81            })
82        };
83
84        Ok(Box::new(closure))
85    }
86}
87
88impl PointReadable for PointXYZI {
89    fn read_data_func(fields: &[PointField]) -> Result<PointMapper<Self>, PointRainIOError> {
90        let (x, y, z) = find_xyz(fields)?;
91        let intensity = find_intensity(fields)?;
92
93        let closure = move |data: &[PointFieldDatum]| {
94            Ok(Self {
95                pos: Position::new(data[x].to_float(), data[y].to_float(), data[z].to_float()),
96                intensity: data[intensity].to_float(),
97            })
98        };
99
100        Ok(Box::new(closure))
101    }
102}
103
104impl PointReadable for PointXYZINormal {
105    fn read_data_func(fields: &[PointField]) -> Result<PointMapper<Self>, PointRainIOError> {
106        let (x, y, z) = find_xyz(fields)?;
107        let intensity = find_intensity(fields)?;
108        let (nx, ny, nz, curvature) = find_normal(fields)?;
109
110        let closure = move |data: &[PointFieldDatum]| {
111            Ok(Self {
112                pos: Position::new(data[x].to_float(), data[y].to_float(), data[z].to_float()),
113                intensity: data[intensity].to_float(),
114                normal: Normal::new(
115                    data[nx].to_float(),
116                    data[ny].to_float(),
117                    data[nz].to_float(),
118                ),
119                curvature: data[curvature].to_float(),
120            })
121        };
122
123        Ok(Box::new(closure))
124    }
125}
126
127impl PointReadable for PointXYZRgb {
128    fn read_data_func(fields: &[PointField]) -> Result<PointMapper<Self>, PointRainIOError> {
129        let (x, y, z) = find_xyz(fields)?;
130        let rgb = find_rgb(fields)?;
131
132        let closure = move |data: &[PointFieldDatum]| {
133            Ok(Self {
134                pos: Position::new(data[x].to_float(), data[y].to_float(), data[z].to_float()),
135                color: data[rgb].to_color()?,
136            })
137        };
138
139        Ok(Box::new(closure))
140    }
141}
142
143impl PointReadable for PointXYZRgbNormal {
144    fn read_data_func(fields: &[PointField]) -> Result<PointMapper<Self>, PointRainIOError> {
145        let (x, y, z) = find_xyz(fields)?;
146        let rgb = find_rgb(fields)?;
147        let (nx, ny, nz, curvature) = find_normal(fields)?;
148
149        let closure = move |data: &[PointFieldDatum]| {
150            Ok(Self {
151                pos: Position::new(data[x].to_float(), data[y].to_float(), data[z].to_float()),
152                color: data[rgb].to_color()?,
153                normal: Normal::new(
154                    data[nx].to_float(),
155                    data[ny].to_float(),
156                    data[nz].to_float(),
157                ),
158                curvature: data[curvature].to_float(),
159            })
160        };
161
162        Ok(Box::new(closure))
163    }
164}
pointrain_io/pcd/point.rs

pointrain_io/pcd/
point.rs
