spatialrust_io/pcd/
header.rs

1use std::io::{BufRead, Read};
2
3use crate::error::{pcd_format, pcd_parse, IoError};
4use crate::pcd::schema::PcdFieldSpec;
5
6/// PCD payload encoding.
7#[derive(Clone, Copy, Debug, PartialEq, Eq)]
8pub enum PcdDataKind {
9    /// ASCII payload.
10    Ascii,
11    /// Binary little-endian payload.
12    Binary,
13    /// LZF-compressed binary payload.
14    BinaryCompressed,
15}
16
17/// Parsed PCD header.
18#[derive(Clone, Debug, PartialEq)]
19pub struct PcdHeader {
20    /// PCD version string.
21    pub version: String,
22    /// Field specifications.
23    pub fields: Vec<PcdFieldSpec>,
24    /// Cloud width.
25    pub width: usize,
26    /// Cloud height.
27    pub height: usize,
28    /// Viewpoint as seven float values.
29    pub viewpoint: [f32; 7],
30    /// Number of points.
31    pub points: usize,
32    /// Payload encoding.
33    pub data: PcdDataKind,
34}
35
36impl PcdHeader {
37    /// Returns the byte size of one point in binary PCD.
38    #[must_use]
39    pub fn point_step(&self) -> usize {
40        self.fields.iter().map(PcdFieldSpec::byte_size).sum()
41    }
42
43    /// Parses a PCD header from a buffered reader.
44    #[allow(unused_assignments)]
45    pub fn parse<R: BufRead>(reader: &mut R) -> Result<(Self, usize), IoError> {
46        let mut version = String::new();
47        let mut field_names = Vec::new();
48        let mut sizes = Vec::new();
49        let mut kinds = Vec::new();
50        let mut counts = Vec::new();
51        let mut width = 0usize;
52        let mut height = 0usize;
53        let mut viewpoint = [0.0_f32; 7];
54        let mut points = 0usize;
55        let mut data: Option<PcdDataKind> = None;
56        let mut header_bytes = 0usize;
57
58        loop {
59            let mut line = String::new();
60            let read = reader.read_line(&mut line)?;
61            if read == 0 {
62                return Err(pcd_parse("unexpected EOF while reading PCD header"));
63            }
64            header_bytes += read;
65
66            let trimmed = line.trim();
67            if trimmed.is_empty() || trimmed.starts_with('#') {
68                continue;
69            }
70
71            if let Some(rest) = trimmed.strip_prefix("VERSION ") {
72                version = rest.trim().to_owned();
73                continue;
74            }
75            if let Some(rest) = trimmed.strip_prefix("FIELDS ") {
76                field_names = rest.split_whitespace().map(str::to_owned).collect();
77                continue;
78            }
79            if let Some(rest) = trimmed.strip_prefix("SIZE ") {
80                sizes = parse_usize_list(rest)?;
81                continue;
82            }
83            if let Some(rest) = trimmed.strip_prefix("TYPE ") {
84                kinds = parse_type_list(rest)?;
85                continue;
86            }
87            if let Some(rest) = trimmed.strip_prefix("COUNT ") {
88                counts = parse_usize_list(rest)?;
89                continue;
90            }
91            if let Some(rest) = trimmed.strip_prefix("WIDTH ") {
92                width = rest
93                    .trim()
94                    .parse()
95                    .map_err(|_| pcd_parse(format!("invalid WIDTH `{rest}`")))?;
96                continue;
97            }
98            if let Some(rest) = trimmed.strip_prefix("HEIGHT ") {
99                height = rest
100                    .trim()
101                    .parse()
102                    .map_err(|_| pcd_parse(format!("invalid HEIGHT `{rest}`")))?;
103                continue;
104            }
105            if let Some(rest) = trimmed.strip_prefix("VIEWPOINT ") {
106                let values = parse_f32_list(rest)?;
107                if values.len() != 7 {
108                    return Err(pcd_parse("VIEWPOINT must contain 7 values"));
109                }
110                viewpoint.copy_from_slice(&values);
111                continue;
112            }
113            if let Some(rest) = trimmed.strip_prefix("POINTS ") {
114                points = rest
115                    .trim()
116                    .parse()
117                    .map_err(|_| pcd_parse(format!("invalid POINTS `{rest}`")))?;
118                continue;
119            }
120            if let Some(rest) = trimmed.strip_prefix("DATA ") {
121                data = Some(match rest.trim().to_ascii_lowercase().as_str() {
122                    "ascii" => PcdDataKind::Ascii,
123                    "binary" => PcdDataKind::Binary,
124                    "binary_compressed" => PcdDataKind::BinaryCompressed,
125                    other => return Err(pcd_format(format!("unsupported DATA mode `{other}`"))),
126                });
127                break;
128            }
129
130            return Err(pcd_parse(format!("unexpected PCD header line `{trimmed}`")));
131        }
132
133        if version.is_empty() {
134            version = "0.7".to_owned();
135        }
136        if field_names.is_empty() {
137            return Err(pcd_parse("PCD header missing FIELDS"));
138        }
139        if sizes.len() != field_names.len()
140            || kinds.len() != field_names.len()
141            || counts.len() != field_names.len()
142        {
143            return Err(pcd_parse("FIELDS/SIZE/TYPE/COUNT length mismatch in PCD header"));
144        }
145
146        let fields = field_names
147            .into_iter()
148            .zip(sizes)
149            .zip(kinds)
150            .zip(counts)
151            .map(|(((name, size), kind), count)| PcdFieldSpec { name, size, kind, count })
152            .collect();
153
154        let data_kind = data.ok_or_else(|| pcd_parse("PCD header missing DATA"))?;
155        if points == 0 && width > 0 && height > 0 {
156            points = width * height;
157        }
158
159        Ok((
160            Self { version, fields, width, height, viewpoint, points, data: data_kind },
161            header_bytes,
162        ))
163    }
164}
165
166fn parse_usize_list(input: &str) -> Result<Vec<usize>, IoError> {
167    input
168        .split_whitespace()
169        .map(|value| {
170            value.parse().map_err(|_| pcd_parse(format!("invalid integer `{value}` in PCD header")))
171        })
172        .collect()
173}
174
175fn parse_f32_list(input: &str) -> Result<Vec<f32>, IoError> {
176    input
177        .split_whitespace()
178        .map(|value| {
179            value.parse().map_err(|_| pcd_parse(format!("invalid float `{value}` in PCD header")))
180        })
181        .collect()
182}
183
184fn parse_type_list(input: &str) -> Result<Vec<crate::pcd::schema::PcdType>, IoError> {
185    input
186        .split_whitespace()
187        .map(|value| match value {
188            "I" => Ok(crate::pcd::schema::PcdType::I),
189            "U" => Ok(crate::pcd::schema::PcdType::U),
190            "F" => Ok(crate::pcd::schema::PcdType::F),
191            _ => Err(pcd_parse(format!("invalid TYPE token `{value}`"))),
192        })
193        .collect()
194}
195
196/// Reads the remaining binary payload bytes after the header.
197pub fn read_binary_payload<R: Read>(reader: &mut R, byte_len: usize) -> Result<Vec<u8>, IoError> {
198    let mut data = vec![0_u8; byte_len];
199    reader.read_exact(&mut data)?;
200    Ok(data)
201}
202
203#[cfg(test)]
204mod tests {
205    use super::{PcdDataKind, PcdHeader};
206    use std::io::Cursor;
207
208    const SAMPLE_ASCII_HEADER: &str = "\
209# .PCD v0.7 - Point Cloud Data file format
210VERSION 0.7
211FIELDS x y z
212SIZE 4 4 4
213TYPE F F F
214COUNT 1 1 1
215WIDTH 2
216HEIGHT 1
217VIEWPOINT 0 0 0 1 0 0 0
218POINTS 2
219DATA ascii
220";
221
222    #[test]
223    fn parses_ascii_header() {
224        let mut reader = Cursor::new(SAMPLE_ASCII_HEADER);
225        let (header, _) = PcdHeader::parse(&mut reader).unwrap();
226        assert_eq!(header.points, 2);
227        assert_eq!(header.data, PcdDataKind::Ascii);
228        assert_eq!(header.fields.len(), 3);
229    }
230}
spatialrust_io/pcd/header.rs

spatialrust_io/pcd/
header.rs
