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#[cfg(feature = "streaming")]
13use crate::streaming::{
14    read_bounded_ascii_line, records_io, FormatStreamState, MAX_ASCII_RECORD_BYTES,
15};
16#[cfg(feature = "streaming")]
17use spatialrust_records::{
18    BoundedSpatialRecordSource, CancellationToken, MemoryTracker, RecordsResult, SchemaDescriptor,
19    SpatialRecordChunk, StreamOptions,
20};
21
22/// Reads point clouds from PCD files or streams.
23pub struct PcdReader<R: BufRead> {
24    reader: R,
25    header: PcdHeader,
26    metadata: SpatialMetadata,
27    schema: PointSchema,
28    loaded: bool,
29}
30
31impl<R: BufRead> PcdReader<R> {
32    /// Creates a reader and parses the PCD header eagerly.
33    pub fn new(mut reader: R) -> Result<Self, IoError> {
34        let (header, _) = PcdHeader::parse(&mut reader)?;
35        let schema = schema_from_pcd_fields(&header.fields)?;
36        let metadata = metadata_from_header(&header);
37        Ok(Self { reader, header, metadata, schema, loaded: false })
38    }
39
40    /// Returns the parsed PCD header.
41    #[must_use]
42    pub fn header(&self) -> &PcdHeader {
43        &self.header
44    }
45
46    /// Reads the point cloud payload.
47    pub fn read_cloud(&mut self) -> Result<PointCloud, IoError> {
48        if self.loaded {
49            return Err(pcd_format("PCD reader already consumed"));
50        }
51        self.loaded = true;
52        read_pcd_body(&self.header, &mut self.reader, self.schema.clone(), self.metadata.clone())
53    }
54}
55
56impl<R: BufRead> PointReader for PcdReader<R> {
57    fn schema(&self) -> spatialrust_core::SpatialResult<PointSchema> {
58        Ok(self.schema.clone())
59    }
60
61    fn metadata(&self) -> spatialrust_core::SpatialResult<SpatialMetadata> {
62        Ok(self.metadata.clone())
63    }
64
65    fn read(&mut self, _options: &ReadOptions) -> spatialrust_core::SpatialResult<PointCloud> {
66        self.read_cloud().map_err(|error| spatialrust_core::SpatialError::Io(error.to_string()))
67    }
68}
69
70/// Reads a complete PCD file from any buffered reader.
71pub fn read_pcd<R: BufRead>(reader: &mut R) -> Result<PointCloud, IoError> {
72    let (header, _) = PcdHeader::parse(reader)?;
73    let schema = schema_from_pcd_fields(&header.fields)?;
74    let metadata = metadata_from_header(&header);
75    read_pcd_body(&header, reader, schema, metadata)
76}
77
78fn read_pcd_body<R: BufRead>(
79    header: &PcdHeader,
80    reader: &mut R,
81    schema: PointSchema,
82    metadata: SpatialMetadata,
83) -> Result<PointCloud, IoError> {
84    let mut buffers = PointBufferSet::new();
85    for field in schema.fields() {
86        buffers.insert(field.name.clone(), PointBuffer::with_capacity(field.dtype, header.points));
87    }
88
89    match header.data {
90        PcdDataKind::Ascii => read_ascii_payload(reader, header, &schema, &mut buffers)?,
91        PcdDataKind::Binary => {
92            let payload = read_binary_payload(reader, header.point_step() * header.points)?;
93            decode_binary_payload(header, &schema, &payload, &mut buffers)?;
94        }
95        PcdDataKind::BinaryCompressed => {
96            let payload = read_binary_compressed_payload(reader)?;
97            decode_binary_compressed_payload(header, &schema, &payload, &mut buffers)?;
98        }
99    }
100
101    PointCloud::try_from_parts(schema, buffers, metadata).map_err(IoError::from)
102}
103
104fn metadata_from_header(_header: &PcdHeader) -> SpatialMetadata {
105    SpatialMetadata {
106        frame_id: spatialrust_core::FrameId::new("pcd"),
107        timestamp: spatialrust_core::Timestamp::from_nanos(0),
108        sensor_origin: None,
109        unit: "meter".to_owned(),
110    }
111}
112
113fn read_ascii_payload<R: BufRead>(
114    reader: &mut R,
115    header: &PcdHeader,
116    schema: &PointSchema,
117    buffers: &mut PointBufferSet,
118) -> Result<(), IoError> {
119    let mut loaded = 0usize;
120    while loaded < header.points {
121        let mut line = String::new();
122        let read = reader.read_line(&mut line)?;
123        if read == 0 {
124            return Err(pcd_parse(format!(
125                "unexpected EOF after {loaded} of {} ASCII points",
126                header.points
127            )));
128        }
129        let trimmed = line.trim();
130        if trimmed.is_empty() || trimmed.starts_with('#') {
131            continue;
132        }
133
134        let mut tokens = trimmed.split_whitespace();
135        for field in &header.fields {
136            if field.name.eq_ignore_ascii_case("rgb") {
137                let token =
138                    tokens.next().ok_or_else(|| pcd_parse("missing rgb token in ASCII PCD"))?;
139                let packed = parse_packed_rgb(token)?;
140                push_to_field(buffers, schema, "r", packed.0)?;
141                push_to_field(buffers, schema, "g", packed.1)?;
142                push_to_field(buffers, schema, "b", packed.2)?;
143                continue;
144            }
145
146            for _ in 0..field.count {
147                let token = tokens.next().ok_or_else(|| {
148                    pcd_parse(format!("missing token for field `{}`", field.name))
149                })?;
150                let value = token
151                    .parse::<f32>()
152                    .map_err(|_| pcd_parse(format!("invalid ASCII value `{token}`")))?;
153                push_to_field(buffers, schema, &field.name, value)?;
154            }
155        }
156        loaded += 1;
157    }
158    Ok(())
159}
160
161fn parse_packed_rgb(token: &str) -> Result<(f32, f32, f32), IoError> {
162    let float_value: f32 =
163        token.parse().map_err(|_| pcd_parse(format!("invalid rgb value `{token}`")))?;
164    let bits = float_value.to_bits();
165    Ok((((bits >> 16) & 0xFF) as f32, ((bits >> 8) & 0xFF) as f32, (bits & 0xFF) as f32))
166}
167
168fn read_binary_compressed_payload<R: BufRead>(reader: &mut R) -> Result<Vec<u8>, IoError> {
169    let mut size_buf = [0_u8; 4];
170    reader.read_exact(&mut size_buf)?;
171    let compressed_size = u32::from_le_bytes(size_buf) as usize;
172    reader.read_exact(&mut size_buf)?;
173    let uncompressed_size = u32::from_le_bytes(size_buf) as usize;
174
175    let compressed = read_binary_payload(reader, compressed_size)?;
176    lzf_decompress(&compressed, uncompressed_size)
177}
178
179fn lzf_decompress(input: &[u8], output_len: usize) -> Result<Vec<u8>, IoError> {
180    let mut output = vec![0_u8; output_len];
181    let mut ip = 0usize;
182    let mut op = 0usize;
183
184    while ip < input.len() {
185        let ctrl = input[ip];
186        ip += 1;
187
188        if ctrl < 32 {
189            let len = ctrl as usize + 1;
190            if ip + len > input.len() || op + len > output.len() {
191                return Err(pcd_format("truncated LZF literal run in binary_compressed PCD"));
192            }
193            output[op..op + len].copy_from_slice(&input[ip..ip + len]);
194            ip += len;
195            op += len;
196            continue;
197        }
198
199        let mut len = (ctrl >> 5) as usize;
200        let mut reference_offset = ((ctrl as usize & 0x1f) << 8) + 1;
201        if len == 7 {
202            if ip >= input.len() {
203                return Err(pcd_format("truncated LZF length in binary_compressed PCD"));
204            }
205            len += input[ip] as usize;
206            ip += 1;
207        }
208        if ip >= input.len() {
209            return Err(pcd_format("truncated LZF back-reference in binary_compressed PCD"));
210        }
211        reference_offset += input[ip] as usize;
212        ip += 1;
213
214        let copy_len = len + 2;
215        if reference_offset > op || op + copy_len > output.len() {
216            return Err(pcd_format("invalid LZF back-reference in binary_compressed PCD"));
217        }
218        let ref_start = op - reference_offset;
219        for offset in 0..copy_len {
220            output[op + offset] = output[ref_start + offset];
221        }
222        op += copy_len;
223    }
224
225    if op != output.len() {
226        return Err(pcd_format(format!(
227            "LZF payload size mismatch: expected {}, decoded {op}",
228            output.len()
229        )));
230    }
231    Ok(output)
232}
233
234fn decode_binary_payload(
235    header: &PcdHeader,
236    schema: &PointSchema,
237    payload: &[u8],
238    buffers: &mut PointBufferSet,
239) -> Result<(), IoError> {
240    let point_step = header.point_step();
241    if payload.len() != point_step * header.points {
242        return Err(pcd_format(format!(
243            "binary payload size mismatch: expected {}, found {}",
244            point_step * header.points,
245            payload.len()
246        )));
247    }
248
249    for point_index in 0..header.points {
250        let start = point_index * point_step;
251        let end = start + point_step;
252        decode_binary_point(&header.fields, &payload[start..end], schema, buffers)?;
253    }
254    Ok(())
255}
256
257fn decode_binary_compressed_payload(
258    header: &PcdHeader,
259    schema: &PointSchema,
260    payload: &[u8],
261    buffers: &mut PointBufferSet,
262) -> Result<(), IoError> {
263    let point_step = header.point_step();
264    if payload.len() != point_step * header.points {
265        return Err(pcd_format(format!(
266            "binary_compressed payload size mismatch: expected {}, found {}",
267            point_step * header.points,
268            payload.len()
269        )));
270    }
271
272    let mut field_base = 0usize;
273    for field in &header.fields {
274        let field_step = field.byte_size();
275        for point_index in 0..header.points {
276            let point_base = field_base + point_index * field_step;
277            if field.name.eq_ignore_ascii_case("rgb") && field.count == 1 && field.size == 4 {
278                let chunk = &payload[point_base..point_base + 4];
279                let bits = u32::from_le_bytes(chunk.try_into().expect("rgb chunk"));
280                push_to_field(buffers, schema, "r", ((bits >> 16) & 0xFF) as f32)?;
281                push_to_field(buffers, schema, "g", ((bits >> 8) & 0xFF) as f32)?;
282                push_to_field(buffers, schema, "b", (bits & 0xFF) as f32)?;
283                continue;
284            }
285
286            for component in 0..field.count {
287                let scalar_start = point_base + component * field.size;
288                let scalar_end = scalar_start + field.size;
289                let value = read_binary_scalar(field, &payload[scalar_start..scalar_end])?;
290                push_to_field(buffers, schema, &field.name, value)?;
291            }
292        }
293        field_base += field_step * header.points;
294    }
295    Ok(())
296}
297
298fn decode_binary_point(
299    fields: &[crate::pcd::schema::PcdFieldSpec],
300    bytes: &[u8],
301    schema: &PointSchema,
302    buffers: &mut PointBufferSet,
303) -> Result<(), IoError> {
304    let mut offset = 0usize;
305    for field in fields {
306        let size = field.byte_size();
307        if offset + size > bytes.len() {
308            return Err(pcd_parse("truncated binary PCD point"));
309        }
310        let field_start = offset;
311        offset += size;
312
313        if field.name.eq_ignore_ascii_case("rgb") && field.count == 1 && field.size == 4 {
314            let chunk = &bytes[field_start..field_start + 4];
315            let bits = u32::from_le_bytes(chunk.try_into().expect("rgb chunk"));
316            push_to_field(buffers, schema, "r", ((bits >> 16) & 0xFF) as f32)?;
317            push_to_field(buffers, schema, "g", ((bits >> 8) & 0xFF) as f32)?;
318            push_to_field(buffers, schema, "b", (bits & 0xFF) as f32)?;
319            continue;
320        }
321
322        for component in 0..field.count {
323            let scalar_start = field_start + component * field.size;
324            let scalar_end = scalar_start + field.size;
325            let value = read_binary_scalar(field, &bytes[scalar_start..scalar_end])?;
326            push_to_field(buffers, schema, &field.name, value)?;
327        }
328    }
329    Ok(())
330}
331
332fn read_binary_scalar(
333    field: &crate::pcd::schema::PcdFieldSpec,
334    chunk: &[u8],
335) -> Result<f32, IoError> {
336    let value = match (field.kind, field.size) {
337        (crate::pcd::schema::PcdType::F, 4) => f32::from_le_bytes(chunk.try_into().expect("f32")),
338        (crate::pcd::schema::PcdType::F, 8) => {
339            f64::from_le_bytes(chunk.try_into().expect("f64")) as f32
340        }
341        (crate::pcd::schema::PcdType::I, 4) => {
342            i32::from_le_bytes(chunk.try_into().expect("i32")) as f32
343        }
344        (crate::pcd::schema::PcdType::U, 1) => f32::from(chunk[0]),
345        (crate::pcd::schema::PcdType::U, 2) => {
346            f32::from(u16::from_le_bytes(chunk.try_into().expect("u16")))
347        }
348        (crate::pcd::schema::PcdType::U, 4) => {
349            u32::from_le_bytes(chunk.try_into().expect("u32")) as f32
350        }
351        _ => return Err(pcd_format(format!("unsupported binary field `{}`", field.name))),
352    };
353    Ok(value)
354}
355
356fn push_to_field(
357    buffers: &mut PointBufferSet,
358    schema: &PointSchema,
359    name: &str,
360    value: f32,
361) -> Result<(), IoError> {
362    let field = schema
363        .fields()
364        .iter()
365        .find(|field| field.name == name)
366        .ok_or_else(|| pcd_format(format!("schema missing mapped field `{name}`")))?;
367
368    let buffer = buffers
369        .get_mut(name)
370        .ok_or_else(|| pcd_format(format!("buffer missing for field `{name}`")))?;
371
372    match field.dtype {
373        DType::F32 | DType::F16 => buffer.push_f32(value).map_err(IoError::from),
374        DType::F64 => buffer.push_f64(f64::from(value)).map_err(IoError::from),
375        DType::U8 => buffer.push_u8(value.round() as u8).map_err(IoError::from),
376        DType::U16 => buffer.push_u16(value.round() as u16).map_err(IoError::from),
377        DType::I32 => buffer.push_i32(value.round() as i32).map_err(IoError::from),
378        DType::U32 => {
379            let PointBuffer::U32(values) = buffer else {
380                return Err(IoError::Core(spatialrust_core::SpatialError::UnsupportedDType(
381                    field.dtype,
382                )));
383            };
384            values.push(value.round() as u32);
385            Ok(())
386        }
387    }
388}
389
390/// Reads a PCD file from disk.
391pub fn read_pcd_file(path: impl AsRef<std::path::Path>) -> Result<PointCloud, IoError> {
392    let file = std::fs::File::open(path.as_ref())?;
393    let mut reader = std::io::BufReader::new(file);
394    read_pcd(&mut reader)
395}
396
397/// Bounded sequential PCD source for ASCII and interleaved binary payloads.
398///
399/// Field-major `binary_compressed` PCD requires a bounded spool adapter.
400#[cfg(feature = "streaming")]
401pub struct PcdChunkSource<R: BufRead> {
402    reader: R,
403    header: PcdHeader,
404    metadata: SpatialMetadata,
405    state: FormatStreamState,
406    loaded: usize,
407}
408
409#[cfg(feature = "streaming")]
410impl<R: BufRead> PcdChunkSource<R> {
411    /// Parses the header and creates a bounded PCD source.
412    pub fn new(
413        mut reader: R,
414        options: StreamOptions,
415        cancellation: CancellationToken,
416    ) -> Result<Self, IoError> {
417        let (header, _) = PcdHeader::parse(&mut reader)?;
418        if header.data == PcdDataKind::BinaryCompressed {
419            return Err(pcd_format("binary_compressed PCD requires the bounded spool adapter"));
420        }
421        let schema = schema_from_pcd_fields(&header.fields)?;
422        let metadata = metadata_from_header(&header);
423        let state = FormatStreamState::new("pcd", schema, options, cancellation)?;
424        Ok(Self { reader, header, metadata, state, loaded: 0 })
425    }
426
427    /// Returns the parsed header, including the declared total point count.
428    #[must_use]
429    pub fn header(&self) -> &PcdHeader {
430        &self.header
431    }
432}
433
434#[cfg(feature = "streaming")]
435impl PcdChunkSource<std::io::BufReader<std::fs::File>> {
436    /// Opens a local PCD file for bounded chunk reads.
437    pub fn open(
438        path: impl AsRef<std::path::Path>,
439        options: StreamOptions,
440        cancellation: CancellationToken,
441    ) -> Result<Self, IoError> {
442        Self::new(std::io::BufReader::new(std::fs::File::open(path)?), options, cancellation)
443    }
444}
445
446#[cfg(feature = "streaming")]
447impl<R: BufRead> BoundedSpatialRecordSource for PcdChunkSource<R> {
448    fn schema(&self) -> &SchemaDescriptor {
449        &self.state.schema
450    }
451
452    fn options(&self) -> &StreamOptions {
453        &self.state.options
454    }
455
456    fn memory_tracker(&self) -> &MemoryTracker {
457        &self.state.tracker
458    }
459
460    fn cancellation_token(&self) -> CancellationToken {
461        self.state.cancellation.clone()
462    }
463
464    fn max_chunk_bytes(&self) -> u64 {
465        self.state.max_chunk_bytes
466    }
467
468    fn next_chunk(&mut self) -> Option<RecordsResult<SpatialRecordChunk>> {
469        if self.loaded == self.header.points {
470            return None;
471        }
472        let count = (self.header.points - self.loaded).min(self.state.options.chunk_points());
473        let scratch = match self.header.data {
474            PcdDataKind::Ascii => 0,
475            PcdDataKind::Binary => match self
476                .header
477                .point_step()
478                .checked_mul(count)
479                .and_then(|bytes| u64::try_from(bytes).ok())
480            {
481                Some(bytes) => bytes,
482                None => {
483                    return Some(Err(spatialrust_records::RecordsError::InvalidChunk(
484                        "PCD binary chunk size overflow".into(),
485                    )));
486                }
487            },
488            PcdDataKind::BinaryCompressed => unreachable!("rejected by constructor"),
489        };
490        let reservation = match self.state.reserve_points_with_scratch(count, scratch) {
491            Ok(reservation) => reservation,
492            Err(error) => return Some(Err(error)),
493        };
494        let schema = self.state.schema.point_schema().clone();
495        let mut buffers = PointBufferSet::new();
496        for field in schema.fields() {
497            buffers.insert(field.name.clone(), PointBuffer::with_capacity(field.dtype, count));
498        }
499
500        let decoded = match self.header.data {
501            PcdDataKind::Ascii => read_ascii_chunk(
502                &mut self.reader,
503                &self.header,
504                &schema,
505                &mut buffers,
506                count,
507                self.loaded,
508            ),
509            PcdDataKind::Binary => {
510                let mut payload = vec![0_u8; self.header.point_step() * count];
511                std::io::Read::read_exact(&mut self.reader, &mut payload)
512                    .map_err(IoError::from)
513                    .and_then(|()| {
514                        payload.chunks_exact(self.header.point_step()).try_for_each(|point| {
515                            decode_binary_point(&self.header.fields, point, &schema, &mut buffers)
516                        })
517                    })
518            }
519            PcdDataKind::BinaryCompressed => unreachable!("rejected by constructor"),
520        };
521        if let Err(error) = decoded {
522            return Some(Err(records_io(error)));
523        }
524        let cloud = match PointCloud::try_from_parts(schema, buffers, self.metadata.clone()) {
525            Ok(cloud) => cloud,
526            Err(error) => return Some(Err(error.into())),
527        };
528        self.loaded += count;
529        Some(self.state.lease(cloud, reservation))
530    }
531}
532
533#[cfg(feature = "streaming")]
534fn read_ascii_chunk<R: BufRead>(
535    reader: &mut R,
536    header: &PcdHeader,
537    schema: &PointSchema,
538    buffers: &mut PointBufferSet,
539    count: usize,
540    point_offset: usize,
541) -> Result<(), IoError> {
542    let mut loaded = 0;
543    let mut line = [0_u8; MAX_ASCII_RECORD_BYTES];
544    while loaded < count {
545        let Some(line) = read_bounded_ascii_line(reader, &mut line, "PCD")? else {
546            return Err(pcd_parse(format!(
547                "unexpected EOF after {} of {} ASCII points",
548                point_offset + loaded,
549                header.points
550            )));
551        };
552        let trimmed = line.trim();
553        if trimmed.is_empty() || trimmed.starts_with('#') {
554            continue;
555        }
556        let mut tokens = trimmed.split_whitespace();
557        for field in &header.fields {
558            if field.name.eq_ignore_ascii_case("rgb") {
559                let packed = parse_packed_rgb(
560                    tokens.next().ok_or_else(|| pcd_parse("missing rgb token in ASCII PCD"))?,
561                )?;
562                push_to_field(buffers, schema, "r", packed.0)?;
563                push_to_field(buffers, schema, "g", packed.1)?;
564                push_to_field(buffers, schema, "b", packed.2)?;
565            } else {
566                for _ in 0..field.count {
567                    let token = tokens
568                        .next()
569                        .ok_or_else(|| pcd_parse(format!("missing field `{}`", field.name)))?;
570                    let value = token
571                        .parse::<f32>()
572                        .map_err(|_| pcd_parse(format!("invalid ASCII value `{token}`")))?;
573                    push_to_field(buffers, schema, &field.name, value)?;
574                }
575            }
576        }
577        loaded += 1;
578    }
579    Ok(())
580}
581
582#[cfg(test)]
583mod tests {
584    use super::read_pcd;
585    use crate::pcd::writer::{write_pcd, PcdWriteFormat};
586    use spatialrust_core::{HasIntensity, HasPositions3, PointCloudBuilder, StandardSchemas};
587    use std::io::Cursor;
588
589    const SAMPLE_XYZ_ASCII: &str = "\
590# .PCD v0.7 - Point Cloud Data file format
591VERSION 0.7
592FIELDS x y z
593SIZE 4 4 4
594TYPE F F F
595COUNT 1 1 1
596WIDTH 3
597HEIGHT 1
598VIEWPOINT 0 0 0 1 0 0 0
599POINTS 3
600DATA ascii
6010.0 0.0 0.0
6021.0 0.0 0.0
6030.0 1.0 0.0
604";
605
606    const SAMPLE_XYZI_ASCII: &str = "\
607VERSION 0.7
608FIELDS x y z intensity
609SIZE 4 4 4 4
610TYPE F F F F
611COUNT 1 1 1 1
612WIDTH 2
613HEIGHT 1
614VIEWPOINT 0 0 0 1 0 0 0
615POINTS 2
616DATA ascii
6170.0 0.0 0.0 0.5
6181.0 0.0 0.0 0.8
619";
620
621    fn binary_compressed_xyz_sample() -> Vec<u8> {
622        let header = b"\
623# .PCD v0.7 - Point Cloud Data file format
624VERSION 0.7
625FIELDS x y z
626SIZE 4 4 4
627TYPE F F F
628COUNT 1 1 1
629WIDTH 2
630HEIGHT 1
631VIEWPOINT 0 0 0 1 0 0 0
632POINTS 2
633DATA binary_compressed
634";
635        let mut uncompressed = Vec::new();
636        // PCL binary_compressed stores fields as structure-of-arrays:
637        // x0, x1, y0, y1, z0, z1 for this XYZ sample.
638        for value in [1.0_f32, 2.0, 3.0, 4.0, 5.0, 6.0] {
639            uncompressed.extend_from_slice(&value.to_le_bytes());
640        }
641
642        let mut compressed = Vec::with_capacity(uncompressed.len() + 1);
643        compressed.push((uncompressed.len() - 1) as u8);
644        compressed.extend_from_slice(&uncompressed);
645
646        let mut data = header.to_vec();
647        data.extend_from_slice(&(compressed.len() as u32).to_le_bytes());
648        data.extend_from_slice(&(uncompressed.len() as u32).to_le_bytes());
649        data.extend_from_slice(&compressed);
650        data
651    }
652
653    #[test]
654    fn reads_ascii_xyz() {
655        let mut reader = Cursor::new(SAMPLE_XYZ_ASCII.as_bytes());
656        let cloud = read_pcd(&mut reader).unwrap();
657        assert_eq!(cloud.len(), 3);
658        let (x, y, z) = cloud.positions3().unwrap();
659        assert_eq!(x, &[0.0, 1.0, 0.0]);
660        assert_eq!(y, &[0.0, 0.0, 1.0]);
661        assert_eq!(z, &[0.0, 0.0, 0.0]);
662    }
663
664    #[test]
665    fn reads_ascii_xyzi() {
666        let mut reader = Cursor::new(SAMPLE_XYZI_ASCII.as_bytes());
667        let cloud = read_pcd(&mut reader).unwrap();
668        assert_eq!(cloud.intensity().unwrap(), &[0.5, 0.8]);
669    }
670
671    #[test]
672    fn roundtrip_ascii_xyz() {
673        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
674        builder.push_point([0.0, 0.0, 0.0]).unwrap();
675        builder.push_point([1.0, 2.0, 3.0]).unwrap();
676        let cloud = builder.build().unwrap();
677
678        let mut buffer = Vec::new();
679        write_pcd(&mut buffer, &cloud, PcdWriteFormat::Ascii).unwrap();
680
681        let mut reader = Cursor::new(buffer);
682        let loaded = read_pcd(&mut reader).unwrap();
683        assert_eq!(loaded.len(), cloud.len());
684        let (x, y, z) = loaded.positions3().unwrap();
685        assert_eq!(x, cloud.field("x").unwrap().as_f32().unwrap());
686        assert_eq!(y, cloud.field("y").unwrap().as_f32().unwrap());
687        assert_eq!(z, cloud.field("z").unwrap().as_f32().unwrap());
688    }
689
690    #[test]
691    fn roundtrip_binary_xyz() {
692        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyz());
693        builder.push_point([0.5, 1.5, 2.5]).unwrap();
694        let cloud = builder.build().unwrap();
695
696        let mut buffer = Vec::new();
697        write_pcd(&mut buffer, &cloud, PcdWriteFormat::Binary).unwrap();
698
699        let mut reader = Cursor::new(buffer);
700        let loaded = read_pcd(&mut reader).unwrap();
701        let (x, _, _) = loaded.positions3().unwrap();
702        assert!((x[0] - 0.5).abs() < 1e-6);
703    }
704
705    #[test]
706    fn reads_binary_compressed_xyz() {
707        let data = binary_compressed_xyz_sample();
708        let mut reader = Cursor::new(data);
709        let loaded = read_pcd(&mut reader).unwrap();
710        let (x, y, z) = loaded.positions3().unwrap();
711        assert_eq!(x, &[1.0, 2.0]);
712        assert_eq!(y, &[3.0, 4.0]);
713        assert_eq!(z, &[5.0, 6.0]);
714    }
715}
spatialrust_io/pcd/reader.rs

spatialrust_io/pcd/
reader.rs
