threecrate_io/
pcd.rs

1//! PCD (Point Cloud Data) format support
2//!
3//! This module provides comprehensive PCD format reading and writing capabilities
4//! including ASCII and binary formats, with support for various field types.
5
6#[cfg(feature = "io-mmap")]
7use crate::mmap::MmapReader;
8use crate::registry::{
9    PointCloudReader as RegistryPointCloudReader, PointCloudWriter as RegistryPointCloudWriter,
10};
11use crate::{PointCloudReader, PointCloudWriter};
12use std::collections::HashMap;
13use std::fs::File;
14use std::io::{BufRead, BufReader, Read, Write};
15use std::path::Path;
16use threecrate_core::{Error, Point3f, PointCloud, Result};
17
18/// PCD data format variants
19#[derive(Debug, Clone, Copy, PartialEq, Eq)]
20pub enum PcdDataFormat {
21    Ascii,
22    Binary,
23    BinaryCompressed,
24}
25
26/// PCD field data types
27#[derive(Debug, Clone, Copy, PartialEq, Eq)]
28pub enum PcdFieldType {
29    I8,
30    U8,
31    I16,
32    U16,
33    I32,
34    U32,
35    F32,
36    F64,
37}
38
39/// PCD field definition
40#[derive(Debug, Clone)]
41pub struct PcdField {
42    pub name: String,
43    pub field_type: PcdFieldType,
44    pub count: usize,
45}
46
47/// PCD header information
48#[derive(Debug, Clone)]
49pub struct PcdHeader {
50    pub version: String,
51    pub fields: Vec<PcdField>,
52    pub width: usize,
53    pub height: usize,
54    pub viewpoint: [f64; 7], // tx, ty, tz, qw, qx, qy, qz
55    pub data_format: PcdDataFormat,
56}
57
58/// PCD field value
59#[derive(Debug, Clone)]
60pub enum PcdValue {
61    I8(i8),
62    U8(u8),
63    I16(i16),
64    U16(u16),
65    I32(i32),
66    U32(u32),
67    F32(f32),
68    F64(f64),
69}
70
71/// PCD point data (all fields for a single point)
72pub type PcdPoint = HashMap<String, Vec<PcdValue>>;
73
74/// PCD write options
75#[derive(Debug, Clone)]
76pub struct PcdWriteOptions {
77    pub data_format: PcdDataFormat,
78    pub version: String,
79    pub viewpoint: Option<[f64; 7]>,
80    pub additional_fields: Vec<PcdField>,
81}
82
83impl Default for PcdWriteOptions {
84    fn default() -> Self {
85        Self {
86            data_format: PcdDataFormat::Binary,
87            version: "0.7".to_string(),
88            viewpoint: None,
89            additional_fields: Vec::new(),
90        }
91    }
92}
93
94/// Enhanced PCD reader with comprehensive format support
95pub struct RobustPcdReader;
96
97impl RobustPcdReader {
98    /// Read PCD file and return header and point data
99    pub fn read_pcd_file<P: AsRef<Path>>(path: P) -> Result<(PcdHeader, Vec<PcdPoint>)> {
100        let path = path.as_ref();
101
102        // Try memory-mapped reading for binary files if feature is enabled
103        #[cfg(feature = "io-mmap")]
104        {
105            if let Some((header, points)) = Self::try_read_pcd_mmap(path)? {
106                return Ok((header, points));
107            }
108        }
109
110        // Fall back to standard buffered reading
111        let file = File::open(path)?;
112        let mut reader = BufReader::new(file);
113        Self::read_pcd_data(&mut reader)
114    }
115
116    /// Try to read PCD file using memory mapping (binary files only)
117    #[cfg(feature = "io-mmap")]
118    fn try_read_pcd_mmap<P: AsRef<Path>>(path: P) -> Result<Option<(PcdHeader, Vec<PcdPoint>)>> {
119        let path = path.as_ref();
120
121        // Check if we should use memory mapping
122        if !crate::mmap::should_use_mmap(path) {
123            return Ok(None);
124        }
125
126        // First, read the header using standard I/O to determine format
127        let file = File::open(path)?;
128        let mut reader = BufReader::new(file);
129        let header = Self::read_header(&mut reader)?;
130
131        // Only use mmap for binary formats
132        match header.data_format {
133            PcdDataFormat::Binary => {
134                // Calculate header size by reading until "DATA binary"
135                let file = File::open(path)?;
136                let mut reader = BufReader::new(file);
137                let mut header_size = 0;
138                let mut line = String::new();
139
140                loop {
141                    line.clear();
142                    let bytes_read = reader.read_line(&mut line)?;
143                    if bytes_read == 0 {
144                        return Err(Error::InvalidData(
145                            "Unexpected end of file in PCD header".to_string(),
146                        ));
147                    }
148                    header_size += bytes_read;
149
150                    let line = line.trim();
151                    if line == "DATA binary" {
152                        break;
153                    }
154                }
155
156                // Now use memory mapping for the data section
157                if let Some(mut mmap_reader) = MmapReader::new(path)? {
158                    // Skip to the data section
159                    mmap_reader.seek(header_size)?;
160
161                    // Read points using memory mapping
162                    let points = Self::read_binary_points_mmap(&mut mmap_reader, &header)?;
163
164                    return Ok(Some((header, points)));
165                }
166            }
167            PcdDataFormat::Ascii | PcdDataFormat::BinaryCompressed => {
168                // ASCII and compressed formats - use standard buffered I/O
169                return Ok(None);
170            }
171        }
172
173        Ok(None)
174    }
175
176    /// Read binary format points using memory mapping
177    #[cfg(feature = "io-mmap")]
178    fn read_binary_points_mmap(
179        reader: &mut MmapReader,
180        header: &PcdHeader,
181    ) -> Result<Vec<PcdPoint>> {
182        let mut points = Vec::with_capacity(crate::capacity_hint(header.width * header.height));
183
184        for _ in 0..(header.width * header.height) {
185            let mut point = PcdPoint::new();
186
187            for field in &header.fields {
188                let field_values = Self::read_binary_field_values_mmap(reader, field)?;
189                point.insert(field.name.clone(), field_values);
190            }
191
192            points.push(point);
193        }
194
195        Ok(points)
196    }
197
198    /// Read binary field values using memory mapping
199    #[cfg(feature = "io-mmap")]
200    fn read_binary_field_values_mmap(
201        reader: &mut MmapReader,
202        field: &PcdField,
203    ) -> Result<Vec<PcdValue>> {
204        let mut field_values = Vec::with_capacity(crate::capacity_hint(field.count));
205
206        for _ in 0..field.count {
207            let value = match field.field_type {
208                PcdFieldType::I8 => PcdValue::I8(reader.read_u8()? as i8),
209                PcdFieldType::U8 => PcdValue::U8(reader.read_u8()?),
210                PcdFieldType::I16 => PcdValue::I16(reader.read_u16_le()? as i16),
211                PcdFieldType::U16 => PcdValue::U16(reader.read_u16_le()?),
212                PcdFieldType::I32 => PcdValue::I32(reader.read_u32_le()? as i32),
213                PcdFieldType::U32 => PcdValue::U32(reader.read_u32_le()?),
214                PcdFieldType::F32 => PcdValue::F32(reader.read_f32_le()?),
215                PcdFieldType::F64 => PcdValue::F64(reader.read_f64_le()?),
216            };
217
218            field_values.push(value);
219        }
220
221        Ok(field_values)
222    }
223
224    /// Read PCD data from a reader
225    pub fn read_pcd_data<R: BufRead>(reader: &mut R) -> Result<(PcdHeader, Vec<PcdPoint>)> {
226        let header = Self::read_header(reader)?;
227        let points = Self::read_points(reader, &header)?;
228        Ok((header, points))
229    }
230
231    /// Read PCD header
232    fn read_header<R: BufRead>(reader: &mut R) -> Result<PcdHeader> {
233        let mut version = None;
234        let mut fields = Vec::new();
235        let mut size = Vec::new();
236        let mut field_types = Vec::new();
237        let mut count = Vec::new();
238        let mut width = None;
239        let mut height = None;
240        let mut viewpoint = [0.0; 7];
241        let mut points = None;
242        let mut _data_format = None;
243
244        let mut line = String::new();
245
246        loop {
247            line.clear();
248            let bytes_read = reader.read_line(&mut line)?;
249            if bytes_read == 0 {
250                return Err(Error::InvalidData(
251                    "Unexpected end of file in PCD header".to_string(),
252                ));
253            }
254
255            let line = line.trim();
256            if line.is_empty() {
257                continue;
258            }
259
260            if line.starts_with('#') {
261                continue; // Skip comments
262            }
263
264            if line == "DATA ascii" {
265                _data_format = Some(PcdDataFormat::Ascii);
266                break;
267            } else if line == "DATA binary" {
268                _data_format = Some(PcdDataFormat::Binary);
269                break;
270            } else if line == "DATA binary_compressed" {
271                _data_format = Some(PcdDataFormat::BinaryCompressed);
272                break;
273            }
274
275            let parts: Vec<&str> = line.split_whitespace().collect();
276            if parts.is_empty() {
277                continue;
278            }
279
280            match parts[0] {
281                "VERSION" => {
282                    if parts.len() >= 2 {
283                        version = Some(parts[1].to_string());
284                    }
285                }
286                "FIELDS" => {
287                    if parts.len() >= 2 {
288                        for &field_name in &parts[1..] {
289                            fields.push(PcdField {
290                                name: field_name.to_string(),
291                                field_type: PcdFieldType::F32, // Will be updated by TYPE
292                                count: 1,                      // Will be updated by COUNT
293                            });
294                        }
295                    }
296                }
297                "SIZE" => {
298                    if parts.len() >= 2 {
299                        for &size_str in &parts[1..] {
300                            size.push(size_str.parse::<usize>().map_err(|_| {
301                                Error::InvalidData(format!("Invalid SIZE value: {}", size_str))
302                            })?);
303                        }
304                    }
305                }
306                "TYPE" => {
307                    if parts.len() >= 2 {
308                        for (i, &type_str) in parts[1..].iter().enumerate() {
309                            let size = if i < size.len() { size[i] } else { 4 }; // Default to 4 if SIZE not specified
310                            let field_type = match (type_str, size) {
311                                ("I", 1) => PcdFieldType::I8,
312                                ("I", 2) => PcdFieldType::I16,
313                                ("I", 4) | ("I", _) => PcdFieldType::I32, // Default to I32 for unknown sizes
314                                ("U", 1) => PcdFieldType::U8,
315                                ("U", 2) => PcdFieldType::U16,
316                                ("U", 4) | ("U", _) => PcdFieldType::U32,
317                                ("F", 4) => PcdFieldType::F32,
318                                ("F", 8) | ("F", _) => PcdFieldType::F64,
319                                _ => {
320                                    return Err(Error::InvalidData(format!(
321                                        "Unknown field type/size combination: {}/{}",
322                                        type_str, size
323                                    )))
324                                }
325                            };
326                            field_types.push(field_type);
327                        }
328                    }
329                }
330                "COUNT" => {
331                    if parts.len() >= 2 {
332                        for &count_str in &parts[1..] {
333                            count.push(count_str.parse::<usize>().map_err(|_| {
334                                Error::InvalidData(format!("Invalid COUNT value: {}", count_str))
335                            })?);
336                        }
337                    }
338                }
339                "WIDTH" => {
340                    if parts.len() >= 2 {
341                        width = Some(parts[1].parse::<usize>().map_err(|_| {
342                            Error::InvalidData(format!("Invalid WIDTH value: {}", parts[1]))
343                        })?);
344                    }
345                }
346                "HEIGHT" => {
347                    if parts.len() >= 2 {
348                        height = Some(parts[1].parse::<usize>().map_err(|_| {
349                            Error::InvalidData(format!("Invalid HEIGHT value: {}", parts[1]))
350                        })?);
351                    }
352                }
353                "VIEWPOINT" => {
354                    if parts.len() >= 8 {
355                        for i in 0..7 {
356                            viewpoint[i] = parts[i + 1].parse::<f64>().map_err(|_| {
357                                Error::InvalidData(format!(
358                                    "Invalid VIEWPOINT value: {}",
359                                    parts[i + 1]
360                                ))
361                            })?;
362                        }
363                    }
364                }
365                "POINTS" => {
366                    if parts.len() >= 2 {
367                        points = Some(parts[1].parse::<usize>().map_err(|_| {
368                            Error::InvalidData(format!("Invalid POINTS value: {}", parts[1]))
369                        })?);
370                    }
371                }
372                _ => {
373                    // Ignore unknown header fields
374                }
375            }
376        }
377
378        let version = version
379            .ok_or_else(|| Error::InvalidData("Missing VERSION in PCD header".to_string()))?;
380        let width =
381            width.ok_or_else(|| Error::InvalidData("Missing WIDTH in PCD header".to_string()))?;
382        let height =
383            height.ok_or_else(|| Error::InvalidData("Missing HEIGHT in PCD header".to_string()))?;
384        let data_format = _data_format
385            .ok_or_else(|| Error::InvalidData("Missing DATA format in PCD header".to_string()))?;
386
387        // Update field definitions with type and count information
388        if fields.len() == field_types.len() && fields.len() == count.len() {
389            for (i, field) in fields.iter_mut().enumerate() {
390                field.field_type = field_types[i];
391                field.count = count[i];
392            }
393        } else {
394            return Err(Error::InvalidData(
395                "Mismatch between FIELDS, TYPE, and COUNT declarations".to_string(),
396            ));
397        }
398
399        // Everything after this multiplies width * height, so it must fit.
400        let total = width.checked_mul(height).ok_or_else(|| {
401            Error::InvalidData(format!("PCD WIDTH {width} x HEIGHT {height} is too large"))
402        })?;
403        // If POINTS is specified and different from WIDTH * HEIGHT, validate it
404        if let Some(points) = points {
405            if points != total {
406                return Err(Error::InvalidData(format!(
407                    "POINTS ({points}) doesn't match WIDTH * HEIGHT ({total})"
408                )));
409            }
410        }
411
412        Ok(PcdHeader {
413            version,
414            fields,
415            width,
416            height,
417            viewpoint,
418            data_format,
419        })
420    }
421
422    /// Read point data based on header format
423    fn read_points<R: BufRead>(reader: &mut R, header: &PcdHeader) -> Result<Vec<PcdPoint>> {
424        match header.data_format {
425            PcdDataFormat::Ascii => Self::read_ascii_points(reader, header),
426            PcdDataFormat::Binary => Self::read_binary_points(reader, header),
427            PcdDataFormat::BinaryCompressed => Err(Error::Unsupported(
428                "Binary compressed PCD format not yet supported".to_string(),
429            )),
430        }
431    }
432
433    /// Read ASCII format points
434    fn read_ascii_points<R: BufRead>(reader: &mut R, header: &PcdHeader) -> Result<Vec<PcdPoint>> {
435        let total = header.width * header.height;
436        let mut points = Vec::with_capacity(crate::capacity_hint(total));
437
438        // Blank lines don't count as points, and the data must not run out early.
439        while points.len() < total {
440            let mut line = String::new();
441            if reader.read_line(&mut line)? == 0 {
442                return Err(Error::InvalidData(format!(
443                    "PCD data ended after {} of {total} points",
444                    points.len()
445                )));
446            }
447            let line = line.trim();
448
449            if line.is_empty() {
450                continue;
451            }
452
453            let values: Vec<&str> = line.split_whitespace().collect();
454            let mut value_idx = 0;
455            let mut point = PcdPoint::new();
456
457            for field in &header.fields {
458                let field_values = Self::read_ascii_field_values(&values, &mut value_idx, field)?;
459                point.insert(field.name.clone(), field_values);
460            }
461
462            points.push(point);
463        }
464
465        Ok(points)
466    }
467
468    /// Read binary format points
469    fn read_binary_points<R: Read>(reader: &mut R, header: &PcdHeader) -> Result<Vec<PcdPoint>> {
470        let mut points = Vec::with_capacity(crate::capacity_hint(header.width * header.height));
471
472        for _ in 0..(header.width * header.height) {
473            let mut point = PcdPoint::new();
474
475            for field in &header.fields {
476                let field_values = Self::read_binary_field_values(reader, field)?;
477                point.insert(field.name.clone(), field_values);
478            }
479
480            points.push(point);
481        }
482
483        Ok(points)
484    }
485
486    /// Read ASCII field values
487    fn read_ascii_field_values(
488        values: &[&str],
489        value_idx: &mut usize,
490        field: &PcdField,
491    ) -> Result<Vec<PcdValue>> {
492        let mut field_values = Vec::with_capacity(crate::capacity_hint(field.count));
493
494        for _ in 0..field.count {
495            if *value_idx >= values.len() {
496                return Err(Error::InvalidData(
497                    "Not enough values in ASCII PCD line".to_string(),
498                ));
499            }
500
501            let value = match field.field_type {
502                PcdFieldType::I8 => {
503                    PcdValue::I8(values[*value_idx].parse::<i8>().map_err(|_| {
504                        Error::InvalidData(format!("Invalid I8 value: {}", values[*value_idx]))
505                    })?)
506                }
507                PcdFieldType::U8 => {
508                    PcdValue::U8(values[*value_idx].parse::<u8>().map_err(|_| {
509                        Error::InvalidData(format!("Invalid U8 value: {}", values[*value_idx]))
510                    })?)
511                }
512                PcdFieldType::I16 => {
513                    PcdValue::I16(values[*value_idx].parse::<i16>().map_err(|_| {
514                        Error::InvalidData(format!("Invalid I16 value: {}", values[*value_idx]))
515                    })?)
516                }
517                PcdFieldType::U16 => {
518                    PcdValue::U16(values[*value_idx].parse::<u16>().map_err(|_| {
519                        Error::InvalidData(format!("Invalid U16 value: {}", values[*value_idx]))
520                    })?)
521                }
522                PcdFieldType::I32 => {
523                    PcdValue::I32(values[*value_idx].parse::<i32>().map_err(|_| {
524                        Error::InvalidData(format!("Invalid I32 value: {}", values[*value_idx]))
525                    })?)
526                }
527                PcdFieldType::U32 => {
528                    PcdValue::U32(values[*value_idx].parse::<u32>().map_err(|_| {
529                        Error::InvalidData(format!("Invalid U32 value: {}", values[*value_idx]))
530                    })?)
531                }
532                PcdFieldType::F32 => {
533                    PcdValue::F32(values[*value_idx].parse::<f32>().map_err(|_| {
534                        Error::InvalidData(format!("Invalid F32 value: {}", values[*value_idx]))
535                    })?)
536                }
537                PcdFieldType::F64 => {
538                    PcdValue::F64(values[*value_idx].parse::<f64>().map_err(|_| {
539                        Error::InvalidData(format!("Invalid F64 value: {}", values[*value_idx]))
540                    })?)
541                }
542            };
543
544            field_values.push(value);
545            *value_idx += 1;
546        }
547
548        Ok(field_values)
549    }
550
551    /// Read binary field values
552    fn read_binary_field_values<R: Read>(
553        reader: &mut R,
554        field: &PcdField,
555    ) -> Result<Vec<PcdValue>> {
556        let mut field_values = Vec::with_capacity(crate::capacity_hint(field.count));
557
558        for _ in 0..field.count {
559            let value = match field.field_type {
560                PcdFieldType::I8 => {
561                    let mut buf = [0u8; 1];
562                    reader.read_exact(&mut buf)?;
563                    PcdValue::I8(buf[0] as i8)
564                }
565                PcdFieldType::U8 => {
566                    let mut buf = [0u8; 1];
567                    reader.read_exact(&mut buf)?;
568                    PcdValue::U8(buf[0])
569                }
570                PcdFieldType::I16 => {
571                    let mut buf = [0u8; 2];
572                    reader.read_exact(&mut buf)?;
573                    PcdValue::I16(i16::from_le_bytes(buf))
574                }
575                PcdFieldType::U16 => {
576                    let mut buf = [0u8; 2];
577                    reader.read_exact(&mut buf)?;
578                    PcdValue::U16(u16::from_le_bytes(buf))
579                }
580                PcdFieldType::I32 => {
581                    let mut buf = [0u8; 4];
582                    reader.read_exact(&mut buf)?;
583                    PcdValue::I32(i32::from_le_bytes(buf))
584                }
585                PcdFieldType::U32 => {
586                    let mut buf = [0u8; 4];
587                    reader.read_exact(&mut buf)?;
588                    PcdValue::U32(u32::from_le_bytes(buf))
589                }
590                PcdFieldType::F32 => {
591                    let mut buf = [0u8; 4];
592                    reader.read_exact(&mut buf)?;
593                    PcdValue::F32(f32::from_le_bytes(buf))
594                }
595                PcdFieldType::F64 => {
596                    let mut buf = [0u8; 8];
597                    reader.read_exact(&mut buf)?;
598                    PcdValue::F64(f64::from_le_bytes(buf))
599                }
600            };
601
602            field_values.push(value);
603        }
604
605        Ok(field_values)
606    }
607
608    /// Convert PCD data to PointCloud
609    pub fn pcd_to_point_cloud(
610        _header: &PcdHeader,
611        points: &[PcdPoint],
612    ) -> Result<PointCloud<Point3f>> {
613        let mut cloud_points = Vec::with_capacity(points.len());
614
615        for point in points {
616            // Extract x, y, z coordinates
617            let x_values = point.get("x").ok_or_else(|| {
618                Error::InvalidData("Missing x coordinate in PCD point".to_string())
619            })?;
620            let y_values = point.get("y").ok_or_else(|| {
621                Error::InvalidData("Missing y coordinate in PCD point".to_string())
622            })?;
623            let z_values = point.get("z").ok_or_else(|| {
624                Error::InvalidData("Missing z coordinate in PCD point".to_string())
625            })?;
626
627            if x_values.is_empty() || y_values.is_empty() || z_values.is_empty() {
628                return Err(Error::InvalidData(
629                    "Empty coordinate values in PCD point".to_string(),
630                ));
631            }
632
633            let x = Self::pcd_value_to_f64(&x_values[0])?;
634            let y = Self::pcd_value_to_f64(&y_values[0])?;
635            let z = Self::pcd_value_to_f64(&z_values[0])?;
636
637            cloud_points.push(Point3f::new(x as f32, y as f32, z as f32));
638        }
639
640        Ok(PointCloud::from_points(cloud_points))
641    }
642
643    /// Convert PcdValue to f64
644    fn pcd_value_to_f64(value: &PcdValue) -> Result<f64> {
645        match value {
646            PcdValue::I8(v) => Ok(*v as f64),
647            PcdValue::U8(v) => Ok(*v as f64),
648            PcdValue::I16(v) => Ok(*v as f64),
649            PcdValue::U16(v) => Ok(*v as f64),
650            PcdValue::I32(v) => Ok(*v as f64),
651            PcdValue::U32(v) => Ok(*v as f64),
652            PcdValue::F32(v) => Ok(*v as f64),
653            PcdValue::F64(v) => Ok(*v),
654        }
655    }
656}
657
658/// Enhanced PCD writer with comprehensive format support
659pub struct RobustPcdWriter;
660
661impl RobustPcdWriter {
662    /// Write point cloud to PCD file with options
663    pub fn write_point_cloud<P: AsRef<Path>>(
664        cloud: &PointCloud<Point3f>,
665        path: P,
666        options: &PcdWriteOptions,
667    ) -> Result<()> {
668        let file = File::create(path)?;
669        let mut writer = std::io::BufWriter::new(file);
670        Self::write_point_cloud_to_writer(cloud, &mut writer, options)
671    }
672
673    /// Write point cloud to writer with options
674    pub fn write_point_cloud_to_writer<W: Write>(
675        cloud: &PointCloud<Point3f>,
676        writer: &mut W,
677        options: &PcdWriteOptions,
678    ) -> Result<()> {
679        // Build PCD header
680        let mut fields = vec![
681            PcdField {
682                name: "x".to_string(),
683                field_type: PcdFieldType::F32,
684                count: 1,
685            },
686            PcdField {
687                name: "y".to_string(),
688                field_type: PcdFieldType::F32,
689                count: 1,
690            },
691            PcdField {
692                name: "z".to_string(),
693                field_type: PcdFieldType::F32,
694                count: 1,
695            },
696        ];
697
698        // Add additional fields
699        fields.extend(options.additional_fields.clone());
700
701        let header = PcdHeader {
702            version: options.version.clone(),
703            fields,
704            width: cloud.len(),
705            height: 1,
706            viewpoint: options
707                .viewpoint
708                .unwrap_or([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]),
709            data_format: options.data_format,
710        };
711
712        // Write header
713        Self::write_header(writer, &header)?;
714
715        // Write data
716        match options.data_format {
717            PcdDataFormat::Ascii => Self::write_ascii_data(writer, cloud, &header),
718            PcdDataFormat::Binary => Self::write_binary_data(writer, cloud, &header),
719            PcdDataFormat::BinaryCompressed => Err(Error::Unsupported(
720                "Binary compressed PCD format not yet supported".to_string(),
721            )),
722        }
723    }
724
725    /// Write PCD header
726    fn write_header<W: Write>(writer: &mut W, header: &PcdHeader) -> Result<()> {
727        writeln!(
728            writer,
729            "# .PCD v{} - Point Cloud Data file format",
730            header.version
731        )?;
732        writeln!(writer, "VERSION {}", header.version)?;
733        write!(writer, "FIELDS")?;
734        for field in &header.fields {
735            write!(writer, " {}", field.name)?;
736        }
737        writeln!(writer)?;
738
739        write!(writer, "SIZE")?;
740        for field in &header.fields {
741            let size = match field.field_type {
742                PcdFieldType::I8 | PcdFieldType::U8 => 1,
743                PcdFieldType::I16 | PcdFieldType::U16 => 2,
744                PcdFieldType::I32 | PcdFieldType::U32 | PcdFieldType::F32 => 4,
745                PcdFieldType::F64 => 8,
746            };
747            write!(writer, " {}", size)?;
748        }
749        writeln!(writer)?;
750
751        write!(writer, "TYPE")?;
752        for field in &header.fields {
753            let type_char = match field.field_type {
754                PcdFieldType::I8 | PcdFieldType::I16 | PcdFieldType::I32 => "I",
755                PcdFieldType::U8 | PcdFieldType::U16 | PcdFieldType::U32 => "U",
756                PcdFieldType::F32 | PcdFieldType::F64 => "F",
757            };
758            write!(writer, " {}", type_char)?;
759        }
760        writeln!(writer)?;
761
762        write!(writer, "COUNT")?;
763        for field in &header.fields {
764            write!(writer, " {}", field.count)?;
765        }
766        writeln!(writer)?;
767
768        writeln!(writer, "WIDTH {}", header.width)?;
769        writeln!(writer, "HEIGHT {}", header.height)?;
770        writeln!(
771            writer,
772            "VIEWPOINT {} {} {} {} {} {} {}",
773            header.viewpoint[0],
774            header.viewpoint[1],
775            header.viewpoint[2],
776            header.viewpoint[3],
777            header.viewpoint[4],
778            header.viewpoint[5],
779            header.viewpoint[6]
780        )?;
781        writeln!(writer, "POINTS {}", header.width * header.height)?;
782
783        let data_str = match header.data_format {
784            PcdDataFormat::Ascii => "ascii",
785            PcdDataFormat::Binary => "binary",
786            PcdDataFormat::BinaryCompressed => "binary_compressed",
787        };
788        writeln!(writer, "DATA {}", data_str)?;
789
790        Ok(())
791    }
792
793    /// Write ASCII format data
794    fn write_ascii_data<W: Write>(
795        writer: &mut W,
796        cloud: &PointCloud<Point3f>,
797        _header: &PcdHeader,
798    ) -> Result<()> {
799        for point in cloud.iter() {
800            // Write x, y, z
801            write!(writer, "{} {} {}", point.x, point.y, point.z)?;
802
803            // Write additional fields (all zeros for now)
804            // TODO: Implement additional field writing when needed
805            // for field in &header.fields[3..] {
806            //     for _ in 0..field.count {
807            //         write!(writer, " 0")?;
808            //     }
809            // }
810
811            writeln!(writer)?;
812        }
813
814        Ok(())
815    }
816
817    /// Write binary format data
818    fn write_binary_data<W: Write>(
819        writer: &mut W,
820        cloud: &PointCloud<Point3f>,
821        _header: &PcdHeader,
822    ) -> Result<()> {
823        for point in cloud.iter() {
824            // Write x, y, z as f32 little endian
825            writer.write_all(&point.x.to_le_bytes())?;
826            writer.write_all(&point.y.to_le_bytes())?;
827            writer.write_all(&point.z.to_le_bytes())?;
828
829            // Write additional fields (all zeros for now)
830            // TODO: Implement additional field writing when needed
831            // for field in &header.fields[3..] {
832            //     for _ in 0..field.count {
833            //         match field.field_type {
834            //             PcdFieldType::I8 | PcdFieldType::U8 => writer.write_all(&[0u8])?,
835            //             PcdFieldType::I16 | PcdFieldType::U16 => writer.write_all(&[0u8, 0u8])?,
836            //             PcdFieldType::I32 | PcdFieldType::U32 | PcdFieldType::F32 => writer.write_all(&[0u8, 0u8, 0u8, 0u8])?,
837            //             PcdFieldType::F64 => writer.write_all(&[0u8, 0u8, 0u8, 0u8, 0u8, 0u8, 0u8, 0u8])?,
838            //         }
839            //     }
840            // }
841        }
842
843        Ok(())
844    }
845}
846
847/// PCD reader implementing the registry trait
848pub struct PcdReader;
849
850impl RegistryPointCloudReader for PcdReader {
851    fn read_point_cloud(&self, path: &Path) -> Result<PointCloud<Point3f>> {
852        let (header, points) = RobustPcdReader::read_pcd_file(path)?;
853        RobustPcdReader::pcd_to_point_cloud(&header, &points)
854    }
855
856    fn can_read(&self, path: &Path) -> bool {
857        path.extension()
858            .and_then(|ext| ext.to_str())
859            .map(|ext| ext.to_lowercase() == "pcd")
860            .unwrap_or(false)
861    }
862
863    fn format_name(&self) -> &'static str {
864        "pcd"
865    }
866}
867
868/// PCD writer implementing the registry trait
869pub struct PcdWriter;
870
871impl RegistryPointCloudWriter for PcdWriter {
872    fn write_point_cloud(&self, cloud: &PointCloud<Point3f>, path: &Path) -> Result<()> {
873        let options = PcdWriteOptions::default();
874        RobustPcdWriter::write_point_cloud(cloud, path, &options)
875    }
876
877    fn format_name(&self) -> &'static str {
878        "pcd"
879    }
880}
881
882// Keep the legacy trait implementations for backward compatibility
883impl PointCloudReader for PcdReader {
884    fn read_point_cloud<P: AsRef<Path>>(path: P) -> Result<PointCloud<Point3f>> {
885        let reader = PcdReader;
886        RegistryPointCloudReader::read_point_cloud(&reader, path.as_ref())
887    }
888}
889
890impl PointCloudWriter for PcdWriter {
891    fn write_point_cloud<P: AsRef<Path>>(cloud: &PointCloud<Point3f>, path: P) -> Result<()> {
892        let writer = PcdWriter;
893        RegistryPointCloudWriter::write_point_cloud(&writer, cloud, path.as_ref())
894    }
895}
threecrate_io/pcd.rs

threecrate_io/
pcd.rs
