rigidity_io/
e57_format.rs

1//! Reading and writing E57, the surveying interchange format.
2//!
3//! Through the `e57` crate, which does both directions — which is why this
4//! module could be written and tested at all. `rigidity`'s own plan
5//! deferred E57 "for want of test data", and a format that can be written
6//! supplies its own: a cloud goes out and comes back, and the round trip
7//! is the test.
8//!
9//! An E57 file holds *several* clouds, each with its own pose — a survey
10//! is a set of scans, not one cloud. Reading concatenates them, applying
11//! each scan's transform, because everything upstream of here works on one
12//! cloud at a time. Stage three of the viewer's plan is where the scans
13//! stay apart.
14
15use std::path::Path;
16
17use nalgebra::{Quaternion, UnitQuaternion, Vector3};
18use rigidity_core::PointCloud;
19
20use crate::IoError;
21
22/// Reads a cloud from E57, concatenating every scan it holds.
23pub fn read_e57(path: &Path) -> Result<PointCloud, IoError> {
24    let mut file = e57::E57Reader::from_file(path).map_err(|e| IoError::E57(e.to_string()))?;
25    let clouds = file.pointclouds();
26
27    let mut points: Vec<Vector3<f64>> = Vec::new();
28    for cloud in &clouds {
29        // Each scan states where it was taken from; its points are in its
30        // own frame until that is applied.
31        let (rotation, translation) = cloud.transform.as_ref().map_or_else(
32            || (UnitQuaternion::identity(), Vector3::zeros()),
33            |transform| {
34                let quaternion = Quaternion::new(
35                    transform.rotation.w,
36                    transform.rotation.x,
37                    transform.rotation.y,
38                    transform.rotation.z,
39                );
40                (
41                    UnitQuaternion::from_quaternion(quaternion),
42                    Vector3::new(
43                        transform.translation.x,
44                        transform.translation.y,
45                        transform.translation.z,
46                    ),
47                )
48            },
49        );
50
51        let reader = file
52            .pointcloud_simple(cloud)
53            .map_err(|e| IoError::E57(e.to_string()))?;
54        for point in reader {
55            let point = point.map_err(|e| IoError::E57(e.to_string()))?;
56            if let e57::CartesianCoordinate::Valid { x, y, z } = point.cartesian {
57                points.push(rotation * Vector3::new(x, y, z) + translation);
58            }
59        }
60    }
61
62    if points.is_empty() {
63        return Ok(PointCloud::new());
64    }
65    let mut min = Vector3::repeat(f64::INFINITY);
66    let mut max = Vector3::repeat(f64::NEG_INFINITY);
67    for point in &points {
68        min = min.inf(point);
69        max = max.sup(point);
70    }
71    let mut cloud = PointCloud::with_origin((min + max) * 0.5);
72    for point in points {
73        cloud.push(point);
74    }
75    Ok(cloud)
76}
77
78/// Writes a cloud as a single-scan E57 file.
79///
80/// The coordinates go out as `f64` in the file's own frame, with no scan
81/// transform: what came in as absolute coordinates goes out as absolute
82/// coordinates, and a reader that ignores transforms still gets the right
83/// answer.
84pub fn write_e57(cloud: &PointCloud, path: &Path) -> Result<(), IoError> {
85    let mut file =
86        e57::E57Writer::from_file(path, "rigidity").map_err(|e| IoError::E57(e.to_string()))?;
87    let prototype = vec![
88        e57::Record::CARTESIAN_X_F64,
89        e57::Record::CARTESIAN_Y_F64,
90        e57::Record::CARTESIAN_Z_F64,
91    ];
92    let mut writer = file
93        .add_pointcloud("rigidity-scan", prototype)
94        .map_err(|e| IoError::E57(e.to_string()))?;
95    for index in 0..cloud.len() {
96        let point = cloud.point(index);
97        writer
98            .add_point(vec![
99                e57::RecordValue::Double(point.x),
100                e57::RecordValue::Double(point.y),
101                e57::RecordValue::Double(point.z),
102            ])
103            .map_err(|e| IoError::E57(e.to_string()))?;
104    }
105    writer.finalize().map_err(|e| IoError::E57(e.to_string()))?;
106    file.finalize().map_err(|e| IoError::E57(e.to_string()))?;
107    Ok(())
108}
rigidity_io/e57_format.rs

rigidity_io/
e57_format.rs
