spatialrust_io/e57/
reader.rs

1use std::path::Path;
2
3use e57::{
4    CartesianCoordinate, E57Reader as ExternalE57Reader, Point as E57Point,
5    PointCloud as E57PointCloud,
6};
7use spatialrust_core::{
8    DType, FieldSemantic, PointBuffer, PointBufferSet, PointCloud, PointField, PointSchema,
9    SpatialMetadata,
10};
11
12use crate::e57::schema::schema_for_e57_pointcloud;
13use crate::error::{e57_format, e57_parse, IoError};
14use crate::{PointReader, ReadOptions};
15
16/// Reads point clouds from E57 files.
17pub struct E57Reader {
18    metadata: SpatialMetadata,
19}
20
21impl E57Reader {
22    /// Opens an E57 file and parses its header eagerly.
23    pub fn open(_path: impl AsRef<Path>) -> Result<Self, IoError> {
24        Ok(Self { metadata: metadata_from_file() })
25    }
26}
27
28impl PointReader for E57Reader {
29    fn schema(&self) -> spatialrust_core::SpatialResult<PointSchema> {
30        Err(spatialrust_core::SpatialError::InvalidArgument(
31            "E57 schema is scan-specific; use read_e57_file instead".to_owned(),
32        ))
33    }
34
35    fn metadata(&self) -> spatialrust_core::SpatialResult<SpatialMetadata> {
36        Ok(self.metadata.clone())
37    }
38
39    fn read(&mut self, _options: &ReadOptions) -> spatialrust_core::SpatialResult<PointCloud> {
40        Err(spatialrust_core::SpatialError::InvalidArgument(
41            "E57Reader requires read_e57_file for scan-aware loading".to_owned(),
42        ))
43    }
44}
45
46/// Reads all scans from an E57 file and merges them into one point cloud.
47pub fn read_e57(path: impl AsRef<Path>) -> Result<PointCloud, IoError> {
48    read_e57_file(path)
49}
50
51/// Reads all scans from an E57 file and merges them into one point cloud.
52pub fn read_e57_file(path: impl AsRef<Path>) -> Result<PointCloud, IoError> {
53    let mut reader =
54        ExternalE57Reader::from_file(path).map_err(|error| e57_parse(error.to_string()))?;
55    read_from_reader(&mut reader)
56}
57
58fn read_from_reader(
59    reader: &mut ExternalE57Reader<impl std::io::Read + std::io::Seek>,
60) -> Result<PointCloud, IoError> {
61    let scans = reader.pointclouds();
62    if scans.is_empty() {
63        return Err(e57_format("E57 file contains no point clouds".to_owned()));
64    }
65
66    let schema = merged_schema(&scans);
67    let mut buffers = PointBufferSet::new();
68    for field in schema.fields() {
69        buffers.insert(field.name.clone(), PointBuffer::with_capacity(field.dtype, 0));
70    }
71
72    for scan in scans {
73        let mut iter =
74            reader.pointcloud_simple(&scan).map_err(|error| e57_parse(error.to_string()))?;
75        iter.normalize_intensity(false);
76        iter.normalize_color(false);
77
78        for point in iter {
79            let point = point.map_err(|error| e57_parse(error.to_string()))?;
80            append_e57_point(&schema, &mut buffers, &point)?;
81        }
82    }
83
84    PointCloud::try_from_parts(schema, buffers, metadata_from_file()).map_err(IoError::from)
85}
86
87fn merged_schema(scans: &[E57PointCloud]) -> PointSchema {
88    let mut has_intensity = false;
89    let mut has_color = false;
90    for scan in scans {
91        if scan.has_intensity() {
92            has_intensity = true;
93        }
94        if scan.has_color() {
95            has_color = true;
96        }
97    }
98
99    let mut schema = schema_for_e57_pointcloud(&scans[0]);
100    if has_intensity && schema.find_semantic(FieldSemantic::Intensity).is_none() {
101        schema = schema.with_field(PointField::scalar(
102            "intensity",
103            FieldSemantic::Intensity,
104            DType::F32,
105        ));
106    }
107    if has_color && schema.find_semantic(FieldSemantic::ColorR).is_none() {
108        schema = schema
109            .with_field(PointField::scalar("r", FieldSemantic::ColorR, DType::U8))
110            .with_field(PointField::scalar("g", FieldSemantic::ColorG, DType::U8))
111            .with_field(PointField::scalar("b", FieldSemantic::ColorB, DType::U8));
112    }
113    schema
114}
115
116fn metadata_from_file() -> SpatialMetadata {
117    SpatialMetadata {
118        frame_id: spatialrust_core::FrameId::new("e57"),
119        timestamp: spatialrust_core::Timestamp::from_nanos(0),
120        sensor_origin: None,
121        unit: "meter".to_owned(),
122    }
123}
124
125fn append_e57_point(
126    schema: &PointSchema,
127    buffers: &mut PointBufferSet,
128    point: &E57Point,
129) -> Result<(), IoError> {
130    let (x, y, z) = cartesian_xyz(point)?;
131    for field in schema.fields() {
132        let value = match field.semantic {
133            FieldSemantic::PositionX => x,
134            FieldSemantic::PositionY => y,
135            FieldSemantic::PositionZ => z,
136            FieldSemantic::Intensity => point.intensity.ok_or_else(|| {
137                e57_parse(
138                    "missing intensity for E57 point with intensity-enabled schema".to_owned(),
139                )
140            })?,
141            FieldSemantic::ColorR => color_channel(point, |color| color.red)?,
142            FieldSemantic::ColorG => color_channel(point, |color| color.green)?,
143            FieldSemantic::ColorB => color_channel(point, |color| color.blue)?,
144            _ => {
145                return Err(e57_parse(format!("unsupported E57 export field `{}`", field.name)));
146            }
147        };
148        push_field(buffers, field, value)?;
149    }
150    Ok(())
151}
152
153fn cartesian_xyz(point: &E57Point) -> Result<(f32, f32, f32), IoError> {
154    match point.cartesian {
155        CartesianCoordinate::Valid { x, y, z } => Ok((x as f32, y as f32, z as f32)),
156        CartesianCoordinate::Direction { .. } => {
157            Err(e57_parse("direction-only Cartesian E57 points are not supported".to_owned()))
158        }
159        CartesianCoordinate::Invalid => Err(e57_parse("invalid Cartesian E57 point".to_owned())),
160    }
161}
162
163fn color_channel(point: &E57Point, channel: impl Fn(&e57::Color) -> f32) -> Result<f32, IoError> {
164    point.color.as_ref().map(channel).ok_or_else(|| {
165        e57_parse("missing color for E57 point with color-enabled schema".to_owned())
166    })
167}
168
169fn push_field(buffers: &mut PointBufferSet, field: &PointField, value: f32) -> Result<(), IoError> {
170    let buffer = buffers
171        .get_mut(&field.name)
172        .ok_or_else(|| spatialrust_core::SpatialError::MissingField(field.name.clone()))?;
173    match field.dtype {
174        DType::F32 | DType::F16 => {
175            buffer.push_f32(value).map_err(IoError::from)?;
176        }
177        DType::U8 => {
178            buffer.push_u8(value.round() as u8).map_err(IoError::from)?;
179        }
180        DType::U16 => {
181            buffer.push_u16(value.round() as u16).map_err(IoError::from)?;
182        }
183        _ => return Err(spatialrust_core::SpatialError::UnsupportedDType(field.dtype).into()),
184    }
185    Ok(())
186}
187
188#[cfg(test)]
189mod tests {
190    use super::read_e57_file;
191    use crate::e57::writer::write_e57_file;
192    use spatialrust_core::{HasIntensity, HasPositions3, PointCloudBuilder, StandardSchemas};
193
194    #[test]
195    fn roundtrip_xyz_intensity() {
196        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyzi());
197        builder.push_point([1.0, 2.0, 3.0, 100.0]).unwrap();
198        builder.push_point([4.0, 5.0, 6.0, 200.0]).unwrap();
199        let cloud = builder.build().unwrap();
200
201        let path = std::env::temp_dir().join(format!("spatialrust_e57_{}.e57", std::process::id()));
202        write_e57_file(&path, &cloud).unwrap();
203        let loaded = read_e57_file(&path).unwrap();
204        let _ = std::fs::remove_file(path);
205
206        assert_eq!(loaded.len(), 2);
207        let (x, _, _) = loaded.positions3().unwrap();
208        assert!((x[0] - 1.0).abs() < 1e-4);
209        assert!((loaded.intensity().unwrap()[1] - 200.0).abs() < 1e-3);
210    }
211}
spatialrust_io/e57/reader.rs

spatialrust_io/e57/
reader.rs
