rigidity_io/
ply.rs

1//! Reading and writing PLY.
2//!
3//! `ascii` and `binary_little_endian` are supported. `binary_big_endian`
4//! is rejected explicitly: it is practically never seen, and silently
5//! misinterpreting bytes is worse than refusing.
6//!
7//! Only the first element is read, and it must be called `vertex`. Faces
8//! are of no use here, and partial support for complex files creates an
9//! illusion of compatibility.
10
11use std::fs::File;
12use std::io::{BufRead, BufReader, BufWriter, Read, Write};
13use std::path::Path;
14
15use nalgebra::Vector3;
16use rigidity_core::{Attribute, AttributeData, PointCloud};
17
18use crate::IoError;
19
20/// The comment that carries the cloud's origin.
21///
22/// The PLY standard has no place for an offset. Other programs ignore
23/// comments, so the file stays readable while our own round trip stays
24/// exact. For clouds with a zero origin — that is, for all synthetic data
25/// — the comment is not written at all.
26const ORIGIN_COMMENT: &str = "rigidity_origin";
27
28#[derive(Debug, Clone, Copy, PartialEq, Eq)]
29enum Format {
30    Ascii,
31    BinaryLittleEndian,
32}
33
34#[derive(Debug, Clone, Copy, PartialEq, Eq)]
35enum ScalarType {
36    I8,
37    U8,
38    I16,
39    U16,
40    I32,
41    U32,
42    F32,
43    F64,
44}
45
46impl ScalarType {
47    fn parse(name: &str) -> Result<Self, IoError> {
48        Ok(match name {
49            "char" | "int8" => Self::I8,
50            "uchar" | "uint8" => Self::U8,
51            "short" | "int16" => Self::I16,
52            "ushort" | "uint16" => Self::U16,
53            "int" | "int32" => Self::I32,
54            "uint" | "uint32" => Self::U32,
55            "float" | "float32" => Self::F32,
56            "double" | "float64" => Self::F64,
57            other => return Err(IoError::UnknownPropertyType(other.to_string())),
58        })
59    }
60
61    fn size(self) -> usize {
62        match self {
63            Self::I8 | Self::U8 => 1,
64            Self::I16 | Self::U16 => 2,
65            Self::I32 | Self::U32 | Self::F32 => 4,
66            Self::F64 => 8,
67        }
68    }
69
70    /// The type name used when writing: the canonical one, not a synonym.
71    fn ply_name(self) -> &'static str {
72        match self {
73            Self::I8 => "char",
74            Self::U8 => "uchar",
75            Self::I16 => "short",
76            Self::U16 => "ushort",
77            Self::I32 => "int",
78            Self::U32 => "uint",
79            Self::F32 => "float",
80            Self::F64 => "double",
81        }
82    }
83
84    /// An empty column of the matching type. `i8` and `i16` widen to
85    /// `i32`: separate storage variants are not worth it for rare types.
86    fn empty_column(self, capacity: usize) -> AttributeData {
87        match self {
88            Self::U8 => AttributeData::U8(Vec::with_capacity(capacity)),
89            Self::U16 => AttributeData::U16(Vec::with_capacity(capacity)),
90            Self::U32 => AttributeData::U32(Vec::with_capacity(capacity)),
91            Self::I8 | Self::I16 | Self::I32 => AttributeData::I32(Vec::with_capacity(capacity)),
92            Self::F32 => AttributeData::F32(Vec::with_capacity(capacity)),
93            Self::F64 => AttributeData::F64(Vec::with_capacity(capacity)),
94        }
95    }
96}
97
98/// A value as read, before conversion to the column type.
99#[derive(Debug, Clone, Copy)]
100enum Scalar {
101    Signed(i64),
102    Unsigned(u64),
103    Float(f64),
104}
105
106impl Scalar {
107    fn as_f64(self) -> f64 {
108        match self {
109            Self::Signed(v) => v as f64,
110            Self::Unsigned(v) => v as f64,
111            Self::Float(v) => v,
112        }
113    }
114
115    fn as_i64(self) -> i64 {
116        match self {
117            Self::Signed(v) => v,
118            Self::Unsigned(v) => v as i64,
119            Self::Float(v) => v as i64,
120        }
121    }
122}
123
124fn read_binary(ty: ScalarType, buffer: &[u8], offset: usize) -> Scalar {
125    match ty {
126        ScalarType::I8 => Scalar::Signed(i64::from(buffer[offset] as i8)),
127        ScalarType::U8 => Scalar::Unsigned(u64::from(buffer[offset])),
128        ScalarType::I16 => Scalar::Signed(i64::from(i16::from_le_bytes(
129            buffer[offset..offset + 2].try_into().unwrap(),
130        ))),
131        ScalarType::U16 => Scalar::Unsigned(u64::from(u16::from_le_bytes(
132            buffer[offset..offset + 2].try_into().unwrap(),
133        ))),
134        ScalarType::I32 => Scalar::Signed(i64::from(i32::from_le_bytes(
135            buffer[offset..offset + 4].try_into().unwrap(),
136        ))),
137        ScalarType::U32 => Scalar::Unsigned(u64::from(u32::from_le_bytes(
138            buffer[offset..offset + 4].try_into().unwrap(),
139        ))),
140        ScalarType::F32 => Scalar::Float(f64::from(f32::from_le_bytes(
141            buffer[offset..offset + 4].try_into().unwrap(),
142        ))),
143        ScalarType::F64 => Scalar::Float(f64::from_le_bytes(
144            buffer[offset..offset + 8].try_into().unwrap(),
145        )),
146    }
147}
148
149fn parse_ascii(ty: ScalarType, token: &str) -> Result<Scalar, IoError> {
150    let bad = || IoError::BadNumber(token.to_string());
151    Ok(match ty {
152        ScalarType::F32 | ScalarType::F64 => Scalar::Float(token.parse().map_err(|_| bad())?),
153        ScalarType::U8 | ScalarType::U16 | ScalarType::U32 => {
154            Scalar::Unsigned(token.parse().map_err(|_| bad())?)
155        }
156        ScalarType::I8 | ScalarType::I16 | ScalarType::I32 => {
157            Scalar::Signed(token.parse().map_err(|_| bad())?)
158        }
159    })
160}
161
162fn push_scalar(column: &mut AttributeData, value: Scalar) {
163    match column {
164        AttributeData::U8(v) => v.push(value.as_i64() as u8),
165        AttributeData::U16(v) => v.push(value.as_i64() as u16),
166        AttributeData::U32(v) => v.push(value.as_i64() as u32),
167        AttributeData::I32(v) => v.push(value.as_i64() as i32),
168        AttributeData::F32(v) => v.push(value.as_f64() as f32),
169        AttributeData::F64(v) => v.push(value.as_f64()),
170    }
171}
172
173/// Where a property's value goes.
174enum Target {
175    X,
176    Y,
177    Z,
178    Column(usize),
179}
180
181struct Property {
182    ty: ScalarType,
183    target: Target,
184}
185
186struct Header {
187    format: Format,
188    count: usize,
189    properties: Vec<Property>,
190    column_names: Vec<String>,
191    column_types: Vec<ScalarType>,
192    origin: Vector3<f64>,
193}
194
195fn parse_header(reader: &mut impl BufRead) -> Result<Header, IoError> {
196    let mut line = String::new();
197    reader.read_line(&mut line)?;
198    if line.trim_end() != "ply" {
199        return Err(IoError::NotPly);
200    }
201
202    let mut format = None;
203    let mut count = None;
204    let mut properties = Vec::new();
205    let mut column_names = Vec::new();
206    let mut column_types = Vec::new();
207    let mut origin = Vector3::zeros();
208    let mut inside_vertex = false;
209
210    loop {
211        line.clear();
212        if reader.read_line(&mut line)? == 0 {
213            return Err(IoError::BadHeader("no end_header line".into()));
214        }
215        let trimmed = line.trim_end_matches(['\r', '\n']);
216        let mut tokens = trimmed.split_whitespace();
217        let Some(keyword) = tokens.next() else {
218            continue;
219        };
220
221        match keyword {
222            "end_header" => break,
223            "comment" => {
224                let rest: Vec<&str> = tokens.collect();
225                if rest.len() == 4 && rest[0] == ORIGIN_COMMENT {
226                    for (axis, text) in rest[1..].iter().enumerate() {
227                        origin[axis] = text
228                            .parse()
229                            .map_err(|_| IoError::BadNumber((*text).to_string()))?;
230                    }
231                }
232            }
233            "format" => {
234                let name = tokens.next().unwrap_or("");
235                format = Some(match name {
236                    "ascii" => Format::Ascii,
237                    "binary_little_endian" => Format::BinaryLittleEndian,
238                    other => return Err(IoError::UnsupportedFormat(other.to_string())),
239                });
240            }
241            "element" => {
242                let name = tokens.next().unwrap_or("");
243                if count.is_some() {
244                    // A second element: vertices are already described,
245                    // so stop here.
246                    inside_vertex = false;
247                    continue;
248                }
249                if name != "vertex" {
250                    return Err(IoError::VertexNotFirst(name.to_string()));
251                }
252                let text = tokens.next().unwrap_or("");
253                count = Some(
254                    text.parse()
255                        .map_err(|_| IoError::BadNumber(text.to_string()))?,
256                );
257                inside_vertex = true;
258            }
259            "property" if inside_vertex => {
260                let first = tokens.next().unwrap_or("");
261                if first == "list" {
262                    return Err(IoError::ListInVertex);
263                }
264                let ty = ScalarType::parse(first)?;
265                let name = tokens.next().unwrap_or("").to_string();
266                let target = match name.as_str() {
267                    "x" => Target::X,
268                    "y" => Target::Y,
269                    "z" => Target::Z,
270                    _ => {
271                        column_names.push(name);
272                        column_types.push(ty);
273                        Target::Column(column_names.len() - 1)
274                    }
275                };
276                properties.push(Property { ty, target });
277            }
278            _ => {}
279        }
280    }
281
282    let format = format.ok_or_else(|| IoError::BadHeader("no format line".into()))?;
283    let count = count.ok_or_else(|| IoError::BadHeader("no vertex element".into()))?;
284
285    let has_all_coordinates = properties
286        .iter()
287        .filter(|p| matches!(p.target, Target::X | Target::Y | Target::Z))
288        .count()
289        == 3;
290    if !has_all_coordinates {
291        return Err(IoError::MissingCoordinates);
292    }
293
294    Ok(Header {
295        format,
296        count,
297        properties,
298        column_names,
299        column_types,
300        origin,
301    })
302}
303
304/// Reads a cloud from a PLY file.
305pub fn read_ply(path: &Path) -> Result<PointCloud, IoError> {
306    let mut reader = BufReader::new(File::open(path)?);
307    let header = parse_header(&mut reader)?;
308
309    let mut x = Vec::with_capacity(header.count);
310    let mut y = Vec::with_capacity(header.count);
311    let mut z = Vec::with_capacity(header.count);
312    let mut columns: Vec<AttributeData> = header
313        .column_types
314        .iter()
315        .map(|ty| ty.empty_column(header.count))
316        .collect();
317
318    match header.format {
319        Format::BinaryLittleEndian => {
320            let record_size: usize = header.properties.iter().map(|p| p.ty.size()).sum();
321            let expected = record_size * header.count;
322            let mut body = Vec::with_capacity(expected);
323            reader.read_to_end(&mut body)?;
324            if body.len() < expected {
325                return Err(IoError::Truncated {
326                    expected,
327                    actual: body.len(),
328                });
329            }
330            for record in 0..header.count {
331                let mut offset = record * record_size;
332                for property in &header.properties {
333                    let value = read_binary(property.ty, &body, offset);
334                    match property.target {
335                        Target::X => x.push(value.as_f64() as f32),
336                        Target::Y => y.push(value.as_f64() as f32),
337                        Target::Z => z.push(value.as_f64() as f32),
338                        Target::Column(index) => push_scalar(&mut columns[index], value),
339                    }
340                    offset += property.ty.size();
341                }
342            }
343        }
344        Format::Ascii => {
345            let mut line = String::new();
346            for _ in 0..header.count {
347                line.clear();
348                if reader.read_line(&mut line)? == 0 {
349                    return Err(IoError::Truncated {
350                        expected: header.count,
351                        actual: x.len(),
352                    });
353                }
354                let mut tokens = line.split_whitespace();
355                for property in &header.properties {
356                    let token = tokens
357                        .next()
358                        .ok_or_else(|| IoError::BadHeader("row shorter than the header".into()))?;
359                    let value = parse_ascii(property.ty, token)?;
360                    match property.target {
361                        Target::X => x.push(value.as_f64() as f32),
362                        Target::Y => y.push(value.as_f64() as f32),
363                        Target::Z => z.push(value.as_f64() as f32),
364                        Target::Column(index) => push_scalar(&mut columns[index], value),
365                    }
366                }
367            }
368        }
369    }
370
371    let mut cloud = PointCloud::from_columns(header.origin, x, y, z)?;
372    for (name, data) in header.column_names.into_iter().zip(columns) {
373        cloud.push_attribute(Attribute { name, data })?;
374    }
375    Ok(cloud)
376}
377
378fn attribute_scalar_type(data: &AttributeData) -> ScalarType {
379    match data {
380        AttributeData::F32(_) => ScalarType::F32,
381        AttributeData::F64(_) => ScalarType::F64,
382        AttributeData::U8(_) => ScalarType::U8,
383        AttributeData::U16(_) => ScalarType::U16,
384        AttributeData::U32(_) => ScalarType::U32,
385        AttributeData::I32(_) => ScalarType::I32,
386    }
387}
388
389fn write_attribute_value(
390    writer: &mut impl Write,
391    data: &AttributeData,
392    index: usize,
393) -> Result<(), IoError> {
394    match data {
395        AttributeData::F32(v) => writer.write_all(&v[index].to_le_bytes())?,
396        AttributeData::F64(v) => writer.write_all(&v[index].to_le_bytes())?,
397        AttributeData::U8(v) => writer.write_all(&v[index].to_le_bytes())?,
398        AttributeData::U16(v) => writer.write_all(&v[index].to_le_bytes())?,
399        AttributeData::U32(v) => writer.write_all(&v[index].to_le_bytes())?,
400        AttributeData::I32(v) => writer.write_all(&v[index].to_le_bytes())?,
401    }
402    Ok(())
403}
404
405/// Writes a cloud to PLY (`binary_little_endian`).
406///
407/// Coordinates are written as stored, that is, relative to the origin;
408/// the origin itself goes into a comment. A round trip through
409/// [`read_ply`] returns bit-for-bit the same values.
410pub fn write_ply(cloud: &PointCloud, path: &Path) -> Result<(), IoError> {
411    let mut writer = BufWriter::new(File::create(path)?);
412
413    writeln!(writer, "ply")?;
414    writeln!(writer, "format binary_little_endian 1.0")?;
415    writeln!(writer, "comment written by rigidity")?;
416    let origin = cloud.origin();
417    if origin != Vector3::zeros() {
418        writeln!(
419            writer,
420            "comment {ORIGIN_COMMENT} {:.17} {:.17} {:.17}",
421            origin.x, origin.y, origin.z
422        )?;
423    }
424    writeln!(writer, "element vertex {}", cloud.len())?;
425    for axis in ["x", "y", "z"] {
426        writeln!(writer, "property float {axis}")?;
427    }
428    for attribute in cloud.attributes() {
429        writeln!(
430            writer,
431            "property {} {}",
432            attribute_scalar_type(&attribute.data).ply_name(),
433            attribute.name
434        )?;
435    }
436    writeln!(writer, "end_header")?;
437
438    let (xs, ys, zs) = cloud.columns();
439    for i in 0..cloud.len() {
440        writer.write_all(&xs[i].to_le_bytes())?;
441        writer.write_all(&ys[i].to_le_bytes())?;
442        writer.write_all(&zs[i].to_le_bytes())?;
443        for attribute in cloud.attributes() {
444            write_attribute_value(&mut writer, &attribute.data, i)?;
445        }
446    }
447    writer.flush()?;
448    Ok(())
449}
rigidity_io/ply.rs

rigidity_io/
ply.rs
