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(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(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        // If POINTS is specified and different from WIDTH * HEIGHT, validate it
400        if let Some(points) = points {
401            if points != width * height {
402                return Err(Error::InvalidData(format!(
403                    "POINTS ({}) doesn't match WIDTH * HEIGHT ({})",
404                    points,
405                    width * height
406                )));
407            }
408        }
409
410        Ok(PcdHeader {
411            version,
412            fields,
413            width,
414            height,
415            viewpoint,
416            data_format,
417        })
418    }
419
420    /// Read point data based on header format
421    fn read_points<R: BufRead>(reader: &mut R, header: &PcdHeader) -> Result<Vec<PcdPoint>> {
422        match header.data_format {
423            PcdDataFormat::Ascii => Self::read_ascii_points(reader, header),
424            PcdDataFormat::Binary => Self::read_binary_points(reader, header),
425            PcdDataFormat::BinaryCompressed => Err(Error::Unsupported(
426                "Binary compressed PCD format not yet supported".to_string(),
427            )),
428        }
429    }
430
431    /// Read ASCII format points
432    fn read_ascii_points<R: BufRead>(reader: &mut R, header: &PcdHeader) -> Result<Vec<PcdPoint>> {
433        let mut points = Vec::with_capacity(header.width * header.height);
434
435        for _ in 0..(header.width * header.height) {
436            let mut line = String::new();
437            reader.read_line(&mut line)?;
438            let line = line.trim();
439
440            if line.is_empty() {
441                continue;
442            }
443
444            let values: Vec<&str> = line.split_whitespace().collect();
445            let mut value_idx = 0;
446            let mut point = PcdPoint::new();
447
448            for field in &header.fields {
449                let field_values = Self::read_ascii_field_values(&values, &mut value_idx, field)?;
450                point.insert(field.name.clone(), field_values);
451            }
452
453            points.push(point);
454        }
455
456        Ok(points)
457    }
458
459    /// Read binary format points
460    fn read_binary_points<R: Read>(reader: &mut R, header: &PcdHeader) -> Result<Vec<PcdPoint>> {
461        let mut points = Vec::with_capacity(header.width * header.height);
462
463        for _ in 0..(header.width * header.height) {
464            let mut point = PcdPoint::new();
465
466            for field in &header.fields {
467                let field_values = Self::read_binary_field_values(reader, field)?;
468                point.insert(field.name.clone(), field_values);
469            }
470
471            points.push(point);
472        }
473
474        Ok(points)
475    }
476
477    /// Read ASCII field values
478    fn read_ascii_field_values(
479        values: &[&str],
480        value_idx: &mut usize,
481        field: &PcdField,
482    ) -> Result<Vec<PcdValue>> {
483        let mut field_values = Vec::with_capacity(field.count);
484
485        for _ in 0..field.count {
486            if *value_idx >= values.len() {
487                return Err(Error::InvalidData(
488                    "Not enough values in ASCII PCD line".to_string(),
489                ));
490            }
491
492            let value = match field.field_type {
493                PcdFieldType::I8 => {
494                    PcdValue::I8(values[*value_idx].parse::<i8>().map_err(|_| {
495                        Error::InvalidData(format!("Invalid I8 value: {}", values[*value_idx]))
496                    })?)
497                }
498                PcdFieldType::U8 => {
499                    PcdValue::U8(values[*value_idx].parse::<u8>().map_err(|_| {
500                        Error::InvalidData(format!("Invalid U8 value: {}", values[*value_idx]))
501                    })?)
502                }
503                PcdFieldType::I16 => {
504                    PcdValue::I16(values[*value_idx].parse::<i16>().map_err(|_| {
505                        Error::InvalidData(format!("Invalid I16 value: {}", values[*value_idx]))
506                    })?)
507                }
508                PcdFieldType::U16 => {
509                    PcdValue::U16(values[*value_idx].parse::<u16>().map_err(|_| {
510                        Error::InvalidData(format!("Invalid U16 value: {}", values[*value_idx]))
511                    })?)
512                }
513                PcdFieldType::I32 => {
514                    PcdValue::I32(values[*value_idx].parse::<i32>().map_err(|_| {
515                        Error::InvalidData(format!("Invalid I32 value: {}", values[*value_idx]))
516                    })?)
517                }
518                PcdFieldType::U32 => {
519                    PcdValue::U32(values[*value_idx].parse::<u32>().map_err(|_| {
520                        Error::InvalidData(format!("Invalid U32 value: {}", values[*value_idx]))
521                    })?)
522                }
523                PcdFieldType::F32 => {
524                    PcdValue::F32(values[*value_idx].parse::<f32>().map_err(|_| {
525                        Error::InvalidData(format!("Invalid F32 value: {}", values[*value_idx]))
526                    })?)
527                }
528                PcdFieldType::F64 => {
529                    PcdValue::F64(values[*value_idx].parse::<f64>().map_err(|_| {
530                        Error::InvalidData(format!("Invalid F64 value: {}", values[*value_idx]))
531                    })?)
532                }
533            };
534
535            field_values.push(value);
536            *value_idx += 1;
537        }
538
539        Ok(field_values)
540    }
541
542    /// Read binary field values
543    fn read_binary_field_values<R: Read>(
544        reader: &mut R,
545        field: &PcdField,
546    ) -> Result<Vec<PcdValue>> {
547        let mut field_values = Vec::with_capacity(field.count);
548
549        for _ in 0..field.count {
550            let value = match field.field_type {
551                PcdFieldType::I8 => {
552                    let mut buf = [0u8; 1];
553                    reader.read_exact(&mut buf)?;
554                    PcdValue::I8(buf[0] as i8)
555                }
556                PcdFieldType::U8 => {
557                    let mut buf = [0u8; 1];
558                    reader.read_exact(&mut buf)?;
559                    PcdValue::U8(buf[0])
560                }
561                PcdFieldType::I16 => {
562                    let mut buf = [0u8; 2];
563                    reader.read_exact(&mut buf)?;
564                    PcdValue::I16(i16::from_le_bytes(buf))
565                }
566                PcdFieldType::U16 => {
567                    let mut buf = [0u8; 2];
568                    reader.read_exact(&mut buf)?;
569                    PcdValue::U16(u16::from_le_bytes(buf))
570                }
571                PcdFieldType::I32 => {
572                    let mut buf = [0u8; 4];
573                    reader.read_exact(&mut buf)?;
574                    PcdValue::I32(i32::from_le_bytes(buf))
575                }
576                PcdFieldType::U32 => {
577                    let mut buf = [0u8; 4];
578                    reader.read_exact(&mut buf)?;
579                    PcdValue::U32(u32::from_le_bytes(buf))
580                }
581                PcdFieldType::F32 => {
582                    let mut buf = [0u8; 4];
583                    reader.read_exact(&mut buf)?;
584                    PcdValue::F32(f32::from_le_bytes(buf))
585                }
586                PcdFieldType::F64 => {
587                    let mut buf = [0u8; 8];
588                    reader.read_exact(&mut buf)?;
589                    PcdValue::F64(f64::from_le_bytes(buf))
590                }
591            };
592
593            field_values.push(value);
594        }
595
596        Ok(field_values)
597    }
598
599    /// Convert PCD data to PointCloud
600    pub fn pcd_to_point_cloud(
601        _header: &PcdHeader,
602        points: &[PcdPoint],
603    ) -> Result<PointCloud<Point3f>> {
604        let mut cloud_points = Vec::with_capacity(points.len());
605
606        for point in points {
607            // Extract x, y, z coordinates
608            let x_values = point.get("x").ok_or_else(|| {
609                Error::InvalidData("Missing x coordinate in PCD point".to_string())
610            })?;
611            let y_values = point.get("y").ok_or_else(|| {
612                Error::InvalidData("Missing y coordinate in PCD point".to_string())
613            })?;
614            let z_values = point.get("z").ok_or_else(|| {
615                Error::InvalidData("Missing z coordinate in PCD point".to_string())
616            })?;
617
618            if x_values.is_empty() || y_values.is_empty() || z_values.is_empty() {
619                return Err(Error::InvalidData(
620                    "Empty coordinate values in PCD point".to_string(),
621                ));
622            }
623
624            let x = Self::pcd_value_to_f64(&x_values[0])?;
625            let y = Self::pcd_value_to_f64(&y_values[0])?;
626            let z = Self::pcd_value_to_f64(&z_values[0])?;
627
628            cloud_points.push(Point3f::new(x as f32, y as f32, z as f32));
629        }
630
631        Ok(PointCloud::from_points(cloud_points))
632    }
633
634    /// Convert PcdValue to f64
635    fn pcd_value_to_f64(value: &PcdValue) -> Result<f64> {
636        match value {
637            PcdValue::I8(v) => Ok(*v as f64),
638            PcdValue::U8(v) => Ok(*v as f64),
639            PcdValue::I16(v) => Ok(*v as f64),
640            PcdValue::U16(v) => Ok(*v as f64),
641            PcdValue::I32(v) => Ok(*v as f64),
642            PcdValue::U32(v) => Ok(*v as f64),
643            PcdValue::F32(v) => Ok(*v as f64),
644            PcdValue::F64(v) => Ok(*v),
645        }
646    }
647}
648
649/// Enhanced PCD writer with comprehensive format support
650pub struct RobustPcdWriter;
651
652impl RobustPcdWriter {
653    /// Write point cloud to PCD file with options
654    pub fn write_point_cloud<P: AsRef<Path>>(
655        cloud: &PointCloud<Point3f>,
656        path: P,
657        options: &PcdWriteOptions,
658    ) -> Result<()> {
659        let file = File::create(path)?;
660        let mut writer = std::io::BufWriter::new(file);
661        Self::write_point_cloud_to_writer(cloud, &mut writer, options)
662    }
663
664    /// Write point cloud to writer with options
665    pub fn write_point_cloud_to_writer<W: Write>(
666        cloud: &PointCloud<Point3f>,
667        writer: &mut W,
668        options: &PcdWriteOptions,
669    ) -> Result<()> {
670        // Build PCD header
671        let mut fields = vec![
672            PcdField {
673                name: "x".to_string(),
674                field_type: PcdFieldType::F32,
675                count: 1,
676            },
677            PcdField {
678                name: "y".to_string(),
679                field_type: PcdFieldType::F32,
680                count: 1,
681            },
682            PcdField {
683                name: "z".to_string(),
684                field_type: PcdFieldType::F32,
685                count: 1,
686            },
687        ];
688
689        // Add additional fields
690        fields.extend(options.additional_fields.clone());
691
692        let header = PcdHeader {
693            version: options.version.clone(),
694            fields,
695            width: cloud.len(),
696            height: 1,
697            viewpoint: options
698                .viewpoint
699                .unwrap_or([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]),
700            data_format: options.data_format,
701        };
702
703        // Write header
704        Self::write_header(writer, &header)?;
705
706        // Write data
707        match options.data_format {
708            PcdDataFormat::Ascii => Self::write_ascii_data(writer, cloud, &header),
709            PcdDataFormat::Binary => Self::write_binary_data(writer, cloud, &header),
710            PcdDataFormat::BinaryCompressed => Err(Error::Unsupported(
711                "Binary compressed PCD format not yet supported".to_string(),
712            )),
713        }
714    }
715
716    /// Write PCD header
717    fn write_header<W: Write>(writer: &mut W, header: &PcdHeader) -> Result<()> {
718        writeln!(
719            writer,
720            "# .PCD v{} - Point Cloud Data file format",
721            header.version
722        )?;
723        writeln!(writer, "VERSION {}", header.version)?;
724        write!(writer, "FIELDS")?;
725        for field in &header.fields {
726            write!(writer, " {}", field.name)?;
727        }
728        writeln!(writer)?;
729
730        write!(writer, "SIZE")?;
731        for field in &header.fields {
732            let size = match field.field_type {
733                PcdFieldType::I8 | PcdFieldType::U8 => 1,
734                PcdFieldType::I16 | PcdFieldType::U16 => 2,
735                PcdFieldType::I32 | PcdFieldType::U32 | PcdFieldType::F32 => 4,
736                PcdFieldType::F64 => 8,
737            };
738            write!(writer, " {}", size)?;
739        }
740        writeln!(writer)?;
741
742        write!(writer, "TYPE")?;
743        for field in &header.fields {
744            let type_char = match field.field_type {
745                PcdFieldType::I8 | PcdFieldType::I16 | PcdFieldType::I32 => "I",
746                PcdFieldType::U8 | PcdFieldType::U16 | PcdFieldType::U32 => "U",
747                PcdFieldType::F32 | PcdFieldType::F64 => "F",
748            };
749            write!(writer, " {}", type_char)?;
750        }
751        writeln!(writer)?;
752
753        write!(writer, "COUNT")?;
754        for field in &header.fields {
755            write!(writer, " {}", field.count)?;
756        }
757        writeln!(writer)?;
758
759        writeln!(writer, "WIDTH {}", header.width)?;
760        writeln!(writer, "HEIGHT {}", header.height)?;
761        writeln!(
762            writer,
763            "VIEWPOINT {} {} {} {} {} {} {}",
764            header.viewpoint[0],
765            header.viewpoint[1],
766            header.viewpoint[2],
767            header.viewpoint[3],
768            header.viewpoint[4],
769            header.viewpoint[5],
770            header.viewpoint[6]
771        )?;
772        writeln!(writer, "POINTS {}", header.width * header.height)?;
773
774        let data_str = match header.data_format {
775            PcdDataFormat::Ascii => "ascii",
776            PcdDataFormat::Binary => "binary",
777            PcdDataFormat::BinaryCompressed => "binary_compressed",
778        };
779        writeln!(writer, "DATA {}", data_str)?;
780
781        Ok(())
782    }
783
784    /// Write ASCII format data
785    fn write_ascii_data<W: Write>(
786        writer: &mut W,
787        cloud: &PointCloud<Point3f>,
788        _header: &PcdHeader,
789    ) -> Result<()> {
790        for point in cloud.iter() {
791            // Write x, y, z
792            write!(writer, "{} {} {}", point.x, point.y, point.z)?;
793
794            // Write additional fields (all zeros for now)
795            // TODO: Implement additional field writing when needed
796            // for field in &header.fields[3..] {
797            //     for _ in 0..field.count {
798            //         write!(writer, " 0")?;
799            //     }
800            // }
801
802            writeln!(writer)?;
803        }
804
805        Ok(())
806    }
807
808    /// Write binary format data
809    fn write_binary_data<W: Write>(
810        writer: &mut W,
811        cloud: &PointCloud<Point3f>,
812        _header: &PcdHeader,
813    ) -> Result<()> {
814        for point in cloud.iter() {
815            // Write x, y, z as f32 little endian
816            writer.write_all(&point.x.to_le_bytes())?;
817            writer.write_all(&point.y.to_le_bytes())?;
818            writer.write_all(&point.z.to_le_bytes())?;
819
820            // Write additional fields (all zeros for now)
821            // TODO: Implement additional field writing when needed
822            // for field in &header.fields[3..] {
823            //     for _ in 0..field.count {
824            //         match field.field_type {
825            //             PcdFieldType::I8 | PcdFieldType::U8 => writer.write_all(&[0u8])?,
826            //             PcdFieldType::I16 | PcdFieldType::U16 => writer.write_all(&[0u8, 0u8])?,
827            //             PcdFieldType::I32 | PcdFieldType::U32 | PcdFieldType::F32 => writer.write_all(&[0u8, 0u8, 0u8, 0u8])?,
828            //             PcdFieldType::F64 => writer.write_all(&[0u8, 0u8, 0u8, 0u8, 0u8, 0u8, 0u8, 0u8])?,
829            //         }
830            //     }
831            // }
832        }
833
834        Ok(())
835    }
836}
837
838/// PCD reader implementing the registry trait
839pub struct PcdReader;
840
841impl RegistryPointCloudReader for PcdReader {
842    fn read_point_cloud(&self, path: &Path) -> Result<PointCloud<Point3f>> {
843        let (header, points) = RobustPcdReader::read_pcd_file(path)?;
844        RobustPcdReader::pcd_to_point_cloud(&header, &points)
845    }
846
847    fn can_read(&self, path: &Path) -> bool {
848        path.extension()
849            .and_then(|ext| ext.to_str())
850            .map(|ext| ext.to_lowercase() == "pcd")
851            .unwrap_or(false)
852    }
853
854    fn format_name(&self) -> &'static str {
855        "pcd"
856    }
857}
858
859/// PCD writer implementing the registry trait
860pub struct PcdWriter;
861
862impl RegistryPointCloudWriter for PcdWriter {
863    fn write_point_cloud(&self, cloud: &PointCloud<Point3f>, path: &Path) -> Result<()> {
864        let options = PcdWriteOptions::default();
865        RobustPcdWriter::write_point_cloud(cloud, path, &options)
866    }
867
868    fn format_name(&self) -> &'static str {
869        "pcd"
870    }
871}
872
873// Keep the legacy trait implementations for backward compatibility
874impl PointCloudReader for PcdReader {
875    fn read_point_cloud<P: AsRef<Path>>(path: P) -> Result<PointCloud<Point3f>> {
876        let reader = PcdReader;
877        RegistryPointCloudReader::read_point_cloud(&reader, path.as_ref())
878    }
879}
880
881impl PointCloudWriter for PcdWriter {
882    fn write_point_cloud<P: AsRef<Path>>(cloud: &PointCloud<Point3f>, path: P) -> Result<()> {
883        let writer = PcdWriter;
884        RegistryPointCloudWriter::write_point_cloud(&writer, cloud, path.as_ref())
885    }
886}
threecrate_io/pcd.rs

threecrate_io/
pcd.rs
