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

threecrate_io/
pcd.rs
