spatialrust_io/pcd/
reader.rs

1use std::io::BufRead;
2
3use spatialrust_core::{
4    DType, PointBuffer, PointBufferSet, PointCloud, PointSchema, SpatialMetadata,
5};
6
7use crate::error::{pcd_format, pcd_parse, IoError};
8use crate::pcd::header::{read_binary_payload, PcdDataKind, PcdHeader};
9use crate::pcd::schema::schema_from_pcd_fields;
10use crate::{PointReader, ReadOptions};
11
12/// Reads point clouds from PCD files or streams.
13pub struct PcdReader<R: BufRead> {
14    reader: R,
15    header: PcdHeader,
16    metadata: SpatialMetadata,
17    schema: PointSchema,
18    loaded: bool,
19}
20
21impl<R: BufRead> PcdReader<R> {
22    /// Creates a reader and parses the PCD header eagerly.
23    pub fn new(mut reader: R) -> Result<Self, IoError> {
24        let (header, _) = PcdHeader::parse(&mut reader)?;
25        let schema = schema_from_pcd_fields(&header.fields)?;
26        let metadata = metadata_from_header(&header);
27        Ok(Self { reader, header, metadata, schema, loaded: false })
28    }
29
30    /// Returns the parsed PCD header.
31    #[must_use]
32    pub fn header(&self) -> &PcdHeader {
33        &self.header
34    }
35
36    /// Reads the point cloud payload.
37    pub fn read_cloud(&mut self) -> Result<PointCloud, IoError> {
38        if self.loaded {
39            return Err(pcd_format("PCD reader already consumed"));
40        }
41        self.loaded = true;
42        read_pcd_body(&self.header, &mut self.reader, self.schema.clone(), self.metadata.clone())
43    }
44}
45
46impl<R: BufRead> PointReader for PcdReader<R> {
47    fn schema(&self) -> spatialrust_core::SpatialResult<PointSchema> {
48        Ok(self.schema.clone())
49    }
50
51    fn metadata(&self) -> spatialrust_core::SpatialResult<SpatialMetadata> {
52        Ok(self.metadata.clone())
53    }
54
55    fn read(&mut self, _options: &ReadOptions) -> spatialrust_core::SpatialResult<PointCloud> {
56        self.read_cloud().map_err(|error| spatialrust_core::SpatialError::Io(error.to_string()))
57    }
58}
59
60/// Reads a complete PCD file from any buffered reader.
61pub fn read_pcd<R: BufRead>(reader: &mut R) -> Result<PointCloud, IoError> {
62    let (header, _) = PcdHeader::parse(reader)?;
63    let schema = schema_from_pcd_fields(&header.fields)?;
64    let metadata = metadata_from_header(&header);
65    read_pcd_body(&header, reader, schema, metadata)
66}
67
68fn read_pcd_body<R: BufRead>(
69    header: &PcdHeader,
70    reader: &mut R,
71    schema: PointSchema,
72    metadata: SpatialMetadata,
73) -> Result<PointCloud, IoError> {
74    let mut buffers = PointBufferSet::new();
75    for field in schema.fields() {
76        buffers.insert(field.name.clone(), PointBuffer::with_capacity(field.dtype, header.points));
77    }
78
79    match header.data {
80        PcdDataKind::Ascii => read_ascii_payload(reader, header, &schema, &mut buffers)?,
81        PcdDataKind::Binary => {
82            let payload = read_binary_payload(reader, header.point_step() * header.points)?;
83            decode_binary_payload(header, &schema, &payload, &mut buffers)?;
84        }
85        PcdDataKind::BinaryCompressed => {
86            let payload = read_binary_compressed_payload(reader)?;
87            decode_binary_compressed_payload(header, &schema, &payload, &mut buffers)?;
88        }
89    }
90
91    PointCloud::try_from_parts(schema, buffers, metadata).map_err(IoError::from)
92}
93
94fn metadata_from_header(_header: &PcdHeader) -> SpatialMetadata {
95    SpatialMetadata {
96        frame_id: spatialrust_core::FrameId::new("pcd"),
97        timestamp: spatialrust_core::Timestamp::from_nanos(0),
98        sensor_origin: None,
99        unit: "meter".to_owned(),
100    }
101}
102
103fn read_ascii_payload<R: BufRead>(
104    reader: &mut R,
105    header: &PcdHeader,
106    schema: &PointSchema,
107    buffers: &mut PointBufferSet,
108) -> Result<(), IoError> {
109    let mut loaded = 0usize;
110    while loaded < header.points {
111        let mut line = String::new();
112        let read = reader.read_line(&mut line)?;
113        if read == 0 {
114            return Err(pcd_parse(format!(
115                "unexpected EOF after {loaded} of {} ASCII points",
116                header.points
117            )));
118        }
119        let trimmed = line.trim();
120        if trimmed.is_empty() || trimmed.starts_with('#') {
121            continue;
122        }
123
124        let mut tokens = trimmed.split_whitespace();
125        for field in &header.fields {
126            if field.name.eq_ignore_ascii_case("rgb") {
127                let token =
128                    tokens.next().ok_or_else(|| pcd_parse("missing rgb token in ASCII PCD"))?;
129                let packed = parse_packed_rgb(token)?;
130                push_to_field(buffers, schema, "r", packed.0)?;
131                push_to_field(buffers, schema, "g", packed.1)?;
132                push_to_field(buffers, schema, "b", packed.2)?;
133                continue;
134            }
135
136            for _ in 0..field.count {
137                let token = tokens.next().ok_or_else(|| {
138                    pcd_parse(format!("missing token for field `{}`", field.name))
139                })?;
140                let value = token
141                    .parse::<f32>()
142                    .map_err(|_| pcd_parse(format!("invalid ASCII value `{token}`")))?;
143                push_to_field(buffers, schema, &field.name, value)?;
144            }
145        }
146        loaded += 1;
147    }
148    Ok(())
149}
150
151fn parse_packed_rgb(token: &str) -> Result<(f32, f32, f32), IoError> {
152    let float_value: f32 =
153        token.parse().map_err(|_| pcd_parse(format!("invalid rgb value `{token}`")))?;
154    let bits = float_value.to_bits();
155    Ok((((bits >> 16) & 0xFF) as f32, ((bits >> 8) & 0xFF) as f32, (bits & 0xFF) as f32))
156}
157
158fn read_binary_compressed_payload<R: BufRead>(reader: &mut R) -> Result<Vec<u8>, IoError> {
159    let mut size_buf = [0_u8; 4];
160    reader.read_exact(&mut size_buf)?;
161    let compressed_size = u32::from_le_bytes(size_buf) as usize;
162    reader.read_exact(&mut size_buf)?;
163    let uncompressed_size = u32::from_le_bytes(size_buf) as usize;
164
165    let compressed = read_binary_payload(reader, compressed_size)?;
166    lzf_decompress(&compressed, uncompressed_size)
167}
168
169fn lzf_decompress(input: &[u8], output_len: usize) -> Result<Vec<u8>, IoError> {
170    let mut output = vec![0_u8; output_len];
171    let mut ip = 0usize;
172    let mut op = 0usize;
173
174    while ip < input.len() {
175        let ctrl = input[ip];
176        ip += 1;
177
178        if ctrl < 32 {
179            let len = ctrl as usize + 1;
180            if ip + len > input.len() || op + len > output.len() {
181                return Err(pcd_format("truncated LZF literal run in binary_compressed PCD"));
182            }
183            output[op..op + len].copy_from_slice(&input[ip..ip + len]);
184            ip += len;
185            op += len;
186            continue;
187        }
188
189        let mut len = (ctrl >> 5) as usize;
190        let mut reference_offset = ((ctrl as usize & 0x1f) << 8) + 1;
191        if len == 7 {
192            if ip >= input.len() {
193                return Err(pcd_format("truncated LZF length in binary_compressed PCD"));
194            }
195            len += input[ip] as usize;
196            ip += 1;
197        }
198        if ip >= input.len() {
199            return Err(pcd_format("truncated LZF back-reference in binary_compressed PCD"));
200        }
201        reference_offset += input[ip] as usize;
202        ip += 1;
203
204        let copy_len = len + 2;
205        if reference_offset > op || op + copy_len > output.len() {
206            return Err(pcd_format("invalid LZF back-reference in binary_compressed PCD"));
207        }
208        let ref_start = op - reference_offset;
209        for offset in 0..copy_len {
210            output[op + offset] = output[ref_start + offset];
211        }
212        op += copy_len;
213    }
214
215    if op != output.len() {
216        return Err(pcd_format(format!(
217            "LZF payload size mismatch: expected {}, decoded {op}",
218            output.len()
219        )));
220    }
221    Ok(output)
222}
223
224fn decode_binary_payload(
225    header: &PcdHeader,
226    schema: &PointSchema,
227    payload: &[u8],
228    buffers: &mut PointBufferSet,
229) -> Result<(), IoError> {
230    let point_step = header.point_step();
231    if payload.len() != point_step * header.points {
232        return Err(pcd_format(format!(
233            "binary payload size mismatch: expected {}, found {}",
234            point_step * header.points,
235            payload.len()
236        )));
237    }
238
239    for point_index in 0..header.points {
240        let start = point_index * point_step;
241        let end = start + point_step;
242        decode_binary_point(&header.fields, &payload[start..end], schema, buffers)?;
243    }
244    Ok(())
245}
246
247fn decode_binary_compressed_payload(
248    header: &PcdHeader,
249    schema: &PointSchema,
250    payload: &[u8],
251    buffers: &mut PointBufferSet,
252) -> Result<(), IoError> {
253    let point_step = header.point_step();
254    if payload.len() != point_step * header.points {
255        return Err(pcd_format(format!(
256            "binary_compressed payload size mismatch: expected {}, found {}",
257            point_step * header.points,
258            payload.len()
259        )));
260    }
261
262    let mut field_base = 0usize;
263    for field in &header.fields {
264        let field_step = field.byte_size();
265        for point_index in 0..header.points {
266            let point_base = field_base + point_index * field_step;
267            if field.name.eq_ignore_ascii_case("rgb") && field.count == 1 && field.size == 4 {
268                let chunk = &payload[point_base..point_base + 4];
269                let bits = u32::from_le_bytes(chunk.try_into().expect("rgb chunk"));
270                push_to_field(buffers, schema, "r", ((bits >> 16) & 0xFF) as f32)?;
271                push_to_field(buffers, schema, "g", ((bits >> 8) & 0xFF) as f32)?;
272                push_to_field(buffers, schema, "b", (bits & 0xFF) as f32)?;
273                continue;
274            }
275
276            for component in 0..field.count {
277                let scalar_start = point_base + component * field.size;
278                let scalar_end = scalar_start + field.size;
279                let value = read_binary_scalar(field, &payload[scalar_start..scalar_end])?;
280                push_to_field(buffers, schema, &field.name, value)?;
281            }
282        }
283        field_base += field_step * header.points;
284    }
285    Ok(())
286}
287
288fn decode_binary_point(
289    fields: &[crate::pcd::schema::PcdFieldSpec],
290    bytes: &[u8],
291    schema: &PointSchema,
292    buffers: &mut PointBufferSet,
293) -> Result<(), IoError> {
294    let mut offset = 0usize;
295    for field in fields {
296        let size = field.byte_size();
297        if offset + size > bytes.len() {
298            return Err(pcd_parse("truncated binary PCD point"));
299        }
300        let field_start = offset;
301        offset += size;
302
303        if field.name.eq_ignore_ascii_case("rgb") && field.count == 1 && field.size == 4 {
304            let chunk = &bytes[field_start..field_start + 4];
305            let bits = u32::from_le_bytes(chunk.try_into().expect("rgb chunk"));
306            push_to_field(buffers, schema, "r", ((bits >> 16) & 0xFF) as f32)?;
307            push_to_field(buffers, schema, "g", ((bits >> 8) & 0xFF) as f32)?;
308            push_to_field(buffers, schema, "b", (bits & 0xFF) as f32)?;
309            continue;
310        }
311
312        for component in 0..field.count {
313            let scalar_start = field_start + component * field.size;
314            let scalar_end = scalar_start + field.size;
315            let value = read_binary_scalar(field, &bytes[scalar_start..scalar_end])?;
316            push_to_field(buffers, schema, &field.name, value)?;
317        }
318    }
319    Ok(())
320}
321
322fn read_binary_scalar(
323    field: &crate::pcd::schema::PcdFieldSpec,
324    chunk: &[u8],
325) -> Result<f32, IoError> {
326    let value = match (field.kind, field.size) {
327        (crate::pcd::schema::PcdType::F, 4) => f32::from_le_bytes(chunk.try_into().expect("f32")),
328        (crate::pcd::schema::PcdType::F, 8) => {
329            f64::from_le_bytes(chunk.try_into().expect("f64")) as f32
330        }
331        (crate::pcd::schema::PcdType::I, 4) => {
332            i32::from_le_bytes(chunk.try_into().expect("i32")) as f32
333        }
334        (crate::pcd::schema::PcdType::U, 1) => f32::from(chunk[0]),
335        (crate::pcd::schema::PcdType::U, 2) => {
336            f32::from(u16::from_le_bytes(chunk.try_into().expect("u16")))
337        }
338        (crate::pcd::schema::PcdType::U, 4) => {
339            u32::from_le_bytes(chunk.try_into().expect("u32")) as f32
340        }
341        _ => return Err(pcd_format(format!("unsupported binary field `{}`", field.name))),
342    };
343    Ok(value)
344}
345
346fn push_to_field(
347    buffers: &mut PointBufferSet,
348    schema: &PointSchema,
349    name: &str,
350    value: f32,
351) -> Result<(), IoError> {
352    let field = schema
353        .fields()
354        .iter()
355        .find(|field| field.name == name)
356        .ok_or_else(|| pcd_format(format!("schema missing mapped field `{name}`")))?;
357
358    let buffer = buffers
359        .get_mut(name)
360        .ok_or_else(|| pcd_format(format!("buffer missing for field `{name}`")))?;
361
362    match field.dtype {
363        DType::F32 | DType::F16 => buffer.push_f32(value).map_err(IoError::from),
364        DType::F64 => buffer.push_f64(f64::from(value)).map_err(IoError::from),
365        DType::U8 => buffer.push_u8(value.round() as u8).map_err(IoError::from),
366        DType::U16 => buffer.push_u16(value.round() as u16).map_err(IoError::from),
367        DType::I32 => buffer.push_i32(value.round() as i32).map_err(IoError::from),
368        DType::U32 => {
369            let PointBuffer::U32(values) = buffer else {
370                return Err(IoError::Core(spatialrust_core::SpatialError::UnsupportedDType(
371                    field.dtype,
372                )));
373            };
374            values.push(value.round() as u32);
375            Ok(())
376        }
377    }
378}
379
380/// Reads a PCD file from disk.
381pub fn read_pcd_file(path: impl AsRef<std::path::Path>) -> Result<PointCloud, IoError> {
382    let file = std::fs::File::open(path.as_ref())?;
383    let mut reader = std::io::BufReader::new(file);
384    read_pcd(&mut reader)
385}
386
387#[cfg(test)]
388mod tests {
389    use super::read_pcd;
390    use crate::pcd::writer::{write_pcd, PcdWriteFormat};
391    use spatialrust_core::{HasIntensity, HasPositions3, PointCloudBuilder, StandardSchemas};
392    use std::io::Cursor;
393
394    const SAMPLE_XYZ_ASCII: &str = "\
395# .PCD v0.7 - Point Cloud Data file format
396VERSION 0.7
397FIELDS x y z
398SIZE 4 4 4
399TYPE F F F
400COUNT 1 1 1
401WIDTH 3
402HEIGHT 1
403VIEWPOINT 0 0 0 1 0 0 0
404POINTS 3
405DATA ascii
4060.0 0.0 0.0
4071.0 0.0 0.0
4080.0 1.0 0.0
409";
410
411    const SAMPLE_XYZI_ASCII: &str = "\
412VERSION 0.7
413FIELDS x y z intensity
414SIZE 4 4 4 4
415TYPE F F F F
416COUNT 1 1 1 1
417WIDTH 2
418HEIGHT 1
419VIEWPOINT 0 0 0 1 0 0 0
420POINTS 2
421DATA ascii
4220.0 0.0 0.0 0.5
4231.0 0.0 0.0 0.8
424";
425
426    fn binary_compressed_xyz_sample() -> Vec<u8> {
427        let header = b"\
428# .PCD v0.7 - Point Cloud Data file format
429VERSION 0.7
430FIELDS x y z
431SIZE 4 4 4
432TYPE F F F
433COUNT 1 1 1
434WIDTH 2
435HEIGHT 1
436VIEWPOINT 0 0 0 1 0 0 0
437POINTS 2
438DATA binary_compressed
439";
440        let mut uncompressed = Vec::new();
441        // PCL binary_compressed stores fields as structure-of-arrays:
442        // x0, x1, y0, y1, z0, z1 for this XYZ sample.
443        for value in [1.0_f32, 2.0, 3.0, 4.0, 5.0, 6.0] {
444            uncompressed.extend_from_slice(&value.to_le_bytes());
445        }
446
447        let mut compressed = Vec::with_capacity(uncompressed.len() + 1);
448        compressed.push((uncompressed.len() - 1) as u8);
449        compressed.extend_from_slice(&uncompressed);
450
451        let mut data = header.to_vec();
452        data.extend_from_slice(&(compressed.len() as u32).to_le_bytes());
453        data.extend_from_slice(&(uncompressed.len() as u32).to_le_bytes());
454        data.extend_from_slice(&compressed);
455        data
456    }
457
458    #[test]
459    fn reads_ascii_xyz() {
460        let mut reader = Cursor::new(SAMPLE_XYZ_ASCII.as_bytes());
461        let cloud = read_pcd(&mut reader).unwrap();
462        assert_eq!(cloud.len(), 3);
463        let (x, y, z) = cloud.positions3().unwrap();
464        assert_eq!(x, &[0.0, 1.0, 0.0]);
465        assert_eq!(y, &[0.0, 0.0, 1.0]);
466        assert_eq!(z, &[0.0, 0.0, 0.0]);
467    }
468
469    #[test]
470    fn reads_ascii_xyzi() {
471        let mut reader = Cursor::new(SAMPLE_XYZI_ASCII.as_bytes());
472        let cloud = read_pcd(&mut reader).unwrap();
473        assert_eq!(cloud.intensity().unwrap(), &[0.5, 0.8]);
474    }
475
476    #[test]
477    fn roundtrip_ascii_xyz() {
478        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
479        builder.push_point([0.0, 0.0, 0.0]).unwrap();
480        builder.push_point([1.0, 2.0, 3.0]).unwrap();
481        let cloud = builder.build().unwrap();
482
483        let mut buffer = Vec::new();
484        write_pcd(&mut buffer, &cloud, PcdWriteFormat::Ascii).unwrap();
485
486        let mut reader = Cursor::new(buffer);
487        let loaded = read_pcd(&mut reader).unwrap();
488        assert_eq!(loaded.len(), cloud.len());
489        let (x, y, z) = loaded.positions3().unwrap();
490        assert_eq!(x, cloud.field("x").unwrap().as_f32().unwrap());
491        assert_eq!(y, cloud.field("y").unwrap().as_f32().unwrap());
492        assert_eq!(z, cloud.field("z").unwrap().as_f32().unwrap());
493    }
494
495    #[test]
496    fn roundtrip_binary_xyz() {
497        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
498        builder.push_point([0.5, 1.5, 2.5]).unwrap();
499        let cloud = builder.build().unwrap();
500
501        let mut buffer = Vec::new();
502        write_pcd(&mut buffer, &cloud, PcdWriteFormat::Binary).unwrap();
503
504        let mut reader = Cursor::new(buffer);
505        let loaded = read_pcd(&mut reader).unwrap();
506        let (x, _, _) = loaded.positions3().unwrap();
507        assert!((x[0] - 0.5).abs() < 1e-6);
508    }
509
510    #[test]
511    fn reads_binary_compressed_xyz() {
512        let data = binary_compressed_xyz_sample();
513        let mut reader = Cursor::new(data);
514        let loaded = read_pcd(&mut reader).unwrap();
515        let (x, y, z) = loaded.positions3().unwrap();
516        assert_eq!(x, &[1.0, 2.0]);
517        assert_eq!(y, &[3.0, 4.0]);
518        assert_eq!(z, &[5.0, 6.0]);
519    }
520}
spatialrust_io/pcd/reader.rs

spatialrust_io/pcd/
reader.rs
