use crate::error::{Error, Result};
use crate::format::Format;
use crate::io::{PcdEncoding, PcdOptions};
use crate::types::{Color, Geometry, Point, PointCloud, Vec3};
use std::io::{BufRead, Read, Write};
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
enum DataEncoding {
Ascii,
Binary,
BinaryCompressed,
}
#[derive(Debug, Clone)]
struct PcdHeader {
fields: Vec<String>,
size: Vec<usize>,
ty: Vec<char>,
count: Vec<usize>,
width: usize,
height: usize,
points: usize,
data: DataEncoding,
comments: Vec<String>,
}
impl PcdHeader {
fn specs(&self) -> Result<Vec<FieldSpec>> {
if self.fields.is_empty() {
return Err(Error::parse(Format::Pcd, None, "FIELDS header is required"));
}
if self.size.len() != self.fields.len()
|| self.ty.len() != self.fields.len()
|| self.count.len() != self.fields.len()
{
return Err(Error::parse(
Format::Pcd,
None,
"FIELDS, SIZE, TYPE and COUNT must have equal lengths",
));
}
let mut offset = 0usize;
let mut specs = Vec::with_capacity(self.fields.len());
for idx in 0..self.fields.len() {
let bytes = self.size[idx]
.checked_mul(self.count[idx])
.ok_or_else(|| Error::parse(Format::Pcd, None, "field byte size overflow"))?;
specs.push(FieldSpec {
name: self.fields[idx].clone(),
size: self.size[idx],
ty: self.ty[idx],
count: self.count[idx],
offset,
});
offset += bytes;
}
Ok(specs)
}
fn point_step(&self) -> Result<usize> {
Ok(self
.specs()?
.iter()
.map(|spec| spec.size * spec.count)
.sum())
}
}
#[derive(Debug, Clone)]
struct FieldSpec {
name: String,
size: usize,
ty: char,
count: usize,
offset: usize,
}
pub fn read<R: BufRead>(reader: &mut R) -> Result<Geometry> {
let header = read_header(reader)?;
if header.data == DataEncoding::BinaryCompressed {
return Err(Error::unsupported(
Format::Pcd,
"read",
"binary_compressed PCD requires PCL/LZF-compatible decompression; convert to ascii/binary first or add an adapter",
));
}
let specs = header.specs()?;
let mut cloud = PointCloud::empty();
cloud.metadata.source_format = Some(Format::Pcd);
cloud.metadata.point_count_hint = Some(header.points);
cloud.metadata.comments = header.comments.clone();
match header.data {
DataEncoding::Ascii => read_ascii_points(reader, &header, &specs, &mut cloud)?,
DataEncoding::Binary => read_binary_points(reader, &header, &specs, &mut cloud)?,
DataEncoding::BinaryCompressed => {
return Err(Error::unsupported(
Format::Pcd,
"read",
"binary_compressed PCD requires PCL/LZF-compatible decompression",
));
}
}
Ok(Geometry::PointCloud(cloud))
}
pub fn write<W: Write>(writer: &mut W, cloud: &PointCloud, options: &PcdOptions) -> Result<()> {
let fields = write_fields(cloud);
writeln!(
writer,
"# .PCD v0.7 - Point Cloud Data file generated by point-formats"
)?;
writeln!(writer, "VERSION 0.7")?;
writeln!(
writer,
"FIELDS {}",
fields.iter().map(|f| f.name).collect::<Vec<_>>().join(" ")
)?;
writeln!(
writer,
"SIZE {}",
fields
.iter()
.map(|f| f.size.to_string())
.collect::<Vec<_>>()
.join(" ")
)?;
writeln!(
writer,
"TYPE {}",
fields
.iter()
.map(|f| f.ty.to_string())
.collect::<Vec<_>>()
.join(" ")
)?;
writeln!(
writer,
"COUNT {}",
fields
.iter()
.map(|f| f.count.to_string())
.collect::<Vec<_>>()
.join(" ")
)?;
writeln!(writer, "WIDTH {}", cloud.points.len())?;
writeln!(writer, "HEIGHT 1")?;
writeln!(writer, "VIEWPOINT 0 0 0 1 0 0 0")?;
writeln!(writer, "POINTS {}", cloud.points.len())?;
match options.encoding {
PcdEncoding::Ascii => {
writeln!(writer, "DATA ascii")?;
write_ascii(writer, cloud, &fields, options.precision)
}
PcdEncoding::Binary => {
writeln!(writer, "DATA binary")?;
write_binary(writer, cloud, &fields)
}
}
}
fn read_header<R: BufRead>(reader: &mut R) -> Result<PcdHeader> {
let mut header = PcdHeader {
fields: Vec::new(),
size: Vec::new(),
ty: Vec::new(),
count: Vec::new(),
width: 0,
height: 1,
points: 0,
data: DataEncoding::Ascii,
comments: Vec::new(),
};
for line_no in 1usize.. {
let mut line = String::new();
if reader.read_line(&mut line)? == 0 {
return Err(Error::parse(
Format::Pcd,
line_no,
"unexpected EOF before DATA line",
));
}
let trimmed = line.trim();
if trimmed.is_empty() {
continue;
}
if trimmed.starts_with('#') {
header
.comments
.push(trimmed.trim_start_matches('#').trim().to_string());
continue;
}
let parts: Vec<&str> = trimmed.split_whitespace().collect();
if parts.is_empty() {
continue;
}
match parts[0].to_ascii_uppercase().as_str() {
"VERSION" => {}
"FIELDS" => header.fields = parts[1..].iter().map(|s| (*s).to_string()).collect(),
"SIZE" => header.size = parse_usize_list(Format::Pcd, line_no, &parts[1..], "SIZE")?,
"TYPE" => {
header.ty = parts[1..]
.iter()
.map(|value| match *value {
"F" | "f" => Ok('F'),
"U" | "u" => Ok('U'),
"I" | "i" => Ok('I'),
_ => Err(Error::parse(
Format::Pcd,
line_no,
format!("TYPE must be F/U/I, got '{value}'"),
)),
})
.collect::<Result<Vec<_>>>()?;
}
"COUNT" => header.count = parse_usize_list(Format::Pcd, line_no, &parts[1..], "COUNT")?,
"WIDTH" => header.width = parse_usize(Format::Pcd, line_no, parts.get(1), "WIDTH")?,
"HEIGHT" => header.height = parse_usize(Format::Pcd, line_no, parts.get(1), "HEIGHT")?,
"POINTS" => header.points = parse_usize(Format::Pcd, line_no, parts.get(1), "POINTS")?,
"VIEWPOINT" => {}
"DATA" => {
let value = parts.get(1).ok_or_else(|| {
Error::parse(Format::Pcd, line_no, "DATA line requires encoding")
})?;
header.data = match value.to_ascii_lowercase().as_str() {
"ascii" => DataEncoding::Ascii,
"binary" => DataEncoding::Binary,
"binary_compressed" => DataEncoding::BinaryCompressed,
_ => {
return Err(Error::parse(
Format::Pcd,
line_no,
format!("unknown DATA encoding '{value}'"),
))
}
};
break;
}
_ => {
header.comments.push(trimmed.to_string());
}
}
}
if header.count.is_empty() {
header.count = vec![1; header.fields.len()];
}
if header.points == 0 {
header.points = header.width.saturating_mul(header.height);
}
if header.points == 0 {
return Err(Error::parse(
Format::Pcd,
None,
"POINTS or WIDTH/HEIGHT must be non-zero",
));
}
Ok(header)
}
fn read_ascii_points<R: BufRead>(
reader: &mut R,
header: &PcdHeader,
specs: &[FieldSpec],
cloud: &mut PointCloud,
) -> Result<()> {
let (layout, flat_fields) = PcdLayout::from_specs(specs);
let values_per_point = flat_fields.len();
let mut line = String::new();
for row in 0..header.points {
line.clear();
if reader.read_line(&mut line)? == 0 {
return Err(Error::parse(
Format::Pcd,
None,
format!("unexpected EOF while reading point {row}"),
));
}
let trimmed = line.trim();
let mut values_buf = [""; 64];
let mut count = 0;
for part in trimmed.split_whitespace() {
if count < 64 {
values_buf[count] = part;
count += 1;
} else {
break;
}
}
let values = &values_buf[..count];
if values.len() < values_per_point {
return Err(Error::parse(
Format::Pcd,
None,
format!(
"point {row} has {} values but {values_per_point} were expected",
values.len()
),
));
}
let get_scalar = |idx: usize| -> Result<Scalar> {
let field = &flat_fields[idx];
let val_str = values[idx];
parse_scalar_text_field(field, val_str)
};
cloud.points.push(point_from_scalars(&layout, get_scalar)?);
}
Ok(())
}
fn read_binary_points<R: Read>(
reader: &mut R,
header: &PcdHeader,
specs: &[FieldSpec],
cloud: &mut PointCloud,
) -> Result<()> {
let step = header.point_step()?;
let (layout, flat_fields) = PcdLayout::from_specs(specs);
let mut buffer = vec![0_u8; step];
for _ in 0..header.points {
reader.read_exact(&mut buffer)?;
let get_scalar = |idx: usize| -> Result<Scalar> {
let field = &flat_fields[idx];
parse_scalar_bytes_field(field, &buffer)
};
cloud.points.push(point_from_scalars(&layout, get_scalar)?);
}
Ok(())
}
#[derive(Debug, Clone, Default)]
struct PcdLayout {
x_idx: Option<usize>,
y_idx: Option<usize>,
z_idx: Option<usize>,
intensity_idx: Option<usize>,
rgb_idx: Option<usize>,
red_idx: Option<usize>,
green_idx: Option<usize>,
blue_idx: Option<usize>,
classification_idx: Option<usize>,
nx_idx: Option<usize>,
ny_idx: Option<usize>,
nz_idx: Option<usize>,
}
#[derive(Debug, Clone, Copy)]
struct FlatField {
kind: char,
size: usize,
offset: usize,
}
impl PcdLayout {
fn from_specs(specs: &[FieldSpec]) -> (Self, Vec<FlatField>) {
let mut layout = Self::default();
let mut flat_fields = Vec::new();
let mut scalar_idx = 0;
for spec in specs {
for count_idx in 0..spec.count {
flat_fields.push(FlatField {
kind: spec.ty,
size: spec.size,
offset: spec.offset + count_idx * spec.size,
});
match (spec.name.to_ascii_lowercase().as_str(), count_idx) {
("x", 0) => layout.x_idx = Some(scalar_idx),
("y", 0) => layout.y_idx = Some(scalar_idx),
("z", 0) => layout.z_idx = Some(scalar_idx),
("intensity" | "i", 0) => layout.intensity_idx = Some(scalar_idx),
("rgb" | "rgba", 0) => layout.rgb_idx = Some(scalar_idx),
("red" | "r", 0) => layout.red_idx = Some(scalar_idx),
("green" | "g", 0) => layout.green_idx = Some(scalar_idx),
("blue" | "b", 0) => layout.blue_idx = Some(scalar_idx),
("classification" | "class" | "label", 0) => {
layout.classification_idx = Some(scalar_idx)
}
("normal_x" | "nx", 0) => layout.nx_idx = Some(scalar_idx),
("normal_y" | "ny", 0) => layout.ny_idx = Some(scalar_idx),
("normal_z" | "nz", 0) => layout.nz_idx = Some(scalar_idx),
_ => {}
}
scalar_idx += 1;
}
}
(layout, flat_fields)
}
}
#[derive(Debug, Clone, Copy)]
enum Scalar {
Float(f64),
Signed(i64),
Unsigned(u64),
}
impl Scalar {
#[inline]
fn as_f64(self) -> f64 {
match self {
Self::Float(v) => v,
Self::Signed(v) => v as f64,
Self::Unsigned(v) => v as f64,
}
}
#[inline]
fn as_u64(self) -> Option<u64> {
match self {
Self::Unsigned(v) => Some(v),
Self::Signed(v) if v >= 0 => Some(v as u64),
Self::Float(v) if v.is_finite() && v.fract() == 0.0 && v >= 0.0 => Some(v as u64),
_ => None,
}
}
}
fn point_from_scalars(
layout: &PcdLayout,
get_scalar: impl Fn(usize) -> Result<Scalar>,
) -> Result<Point> {
let x = get_scalar(
layout
.x_idx
.ok_or_else(|| Error::parse(Format::Pcd, None, "missing x"))?,
)?
.as_f64();
let y = get_scalar(
layout
.y_idx
.ok_or_else(|| Error::parse(Format::Pcd, None, "missing y"))?,
)?
.as_f64();
let z = get_scalar(
layout
.z_idx
.ok_or_else(|| Error::parse(Format::Pcd, None, "missing z"))?,
)?
.as_f64();
let mut point = Point::new(x, y, z);
if let Some(intensity_idx) = layout.intensity_idx {
point.intensity = Some(get_scalar(intensity_idx)?.as_f64() as f32);
}
let rgb = layout.rgb_idx;
let explicit_color = (layout.red_idx, layout.green_idx, layout.blue_idx);
if let Some(rgb_idx) = rgb {
point.color = Some(decode_rgb(get_scalar(rgb_idx)?));
} else if let (Some(r_idx), Some(g_idx), Some(b_idx)) = explicit_color {
point.color = Some(Color::new(
scalar_to_u16(get_scalar(r_idx)?, "red")?,
scalar_to_u16(get_scalar(g_idx)?, "green")?,
scalar_to_u16(get_scalar(b_idx)?, "blue")?,
));
}
if let Some(class_idx) = layout.classification_idx {
let value = get_scalar(class_idx)?.as_u64().ok_or_else(|| {
Error::parse(
Format::Pcd,
None,
"classification must be a non-negative integer",
)
})?;
if value > u8::MAX as u64 {
return Err(Error::parse(
Format::Pcd,
None,
"classification exceeds u8 range",
));
}
point.classification = Some(value as u8);
}
if let (Some(nx_idx), Some(ny_idx), Some(nz_idx)) =
(layout.nx_idx, layout.ny_idx, layout.nz_idx)
{
point.normal = Some(Vec3::new(
get_scalar(nx_idx)?.as_f64(),
get_scalar(ny_idx)?.as_f64(),
get_scalar(nz_idx)?.as_f64(),
));
}
Ok(point)
}
fn parse_scalar_text_field(field: &FlatField, value: &str) -> Result<Scalar> {
match field.kind {
'F' => Ok(Scalar::Float(value.parse::<f64>().map_err(|_| {
Error::parse(Format::Pcd, None, format!("invalid float '{value}'"))
})?)),
'U' => Ok(Scalar::Unsigned(value.parse::<u64>().map_err(|_| {
Error::parse(
Format::Pcd,
None,
format!("invalid unsigned integer '{value}'"),
)
})?)),
'I' => Ok(Scalar::Signed(value.parse::<i64>().map_err(|_| {
Error::parse(
Format::Pcd,
None,
format!("invalid signed integer '{value}'"),
)
})?)),
_ => Err(Error::parse(Format::Pcd, None, "unknown PCD TYPE")),
}
}
fn parse_scalar_bytes_field(field: &FlatField, bytes: &[u8]) -> Result<Scalar> {
let offset = field.offset;
let end = offset + field.size;
let field_bytes = bytes.get(offset..end).ok_or_else(|| {
Error::parse(
Format::Pcd,
None,
format!(
"slice bounds out of range for binary field of size {}",
field.size
),
)
})?;
Ok(match (field.kind, field.size) {
('F', 4) => Scalar::Float(f32::from_le_bytes(array(field_bytes)?) as f64),
('F', 8) => Scalar::Float(f64::from_le_bytes(array(field_bytes)?)),
('U', 1) => Scalar::Unsigned(field_bytes[0] as u64),
('U', 2) => Scalar::Unsigned(u16::from_le_bytes(array(field_bytes)?) as u64),
('U', 4) => Scalar::Unsigned(u32::from_le_bytes(array(field_bytes)?) as u64),
('U', 8) => Scalar::Unsigned(u64::from_le_bytes(array(field_bytes)?)),
('I', 1) => Scalar::Signed(field_bytes[0] as i8 as i64),
('I', 2) => Scalar::Signed(i16::from_le_bytes(array(field_bytes)?) as i64),
('I', 4) => Scalar::Signed(i32::from_le_bytes(array(field_bytes)?) as i64),
('I', 8) => Scalar::Signed(i64::from_le_bytes(array(field_bytes)?)),
_ => {
return Err(Error::parse(
Format::Pcd,
None,
format!(
"unsupported field TYPE/SIZE combination {}{}",
field.kind, field.size
),
))
}
})
}
fn array<const N: usize>(bytes: &[u8]) -> Result<[u8; N]> {
bytes.try_into().map_err(|_| {
Error::parse(
Format::Pcd,
None,
format!("expected {N} bytes for scalar, got {}", bytes.len()),
)
})
}
fn scalar_to_u16(value: Scalar, name: &str) -> Result<u16> {
let value = value.as_u64().ok_or_else(|| {
Error::parse(
Format::Pcd,
None,
format!("{name} must be a non-negative integer"),
)
})?;
if value > u16::MAX as u64 {
return Err(Error::parse(
Format::Pcd,
None,
format!("{name} exceeds u16 range"),
));
}
Ok(value as u16)
}
fn decode_rgb(value: Scalar) -> Color {
let packed = match value {
Scalar::Float(v) => (v as f32).to_bits(),
Scalar::Unsigned(v) => v as u32,
Scalar::Signed(v) => v as u32,
};
let red = ((packed >> 16) & 0xff) as u16;
let green = ((packed >> 8) & 0xff) as u16;
let blue = (packed & 0xff) as u16;
Color::new(red, green, blue)
}
#[derive(Debug, Clone, Copy)]
struct WriteField {
name: &'static str,
size: usize,
ty: char,
count: usize,
kind: WriteFieldKind,
}
#[derive(Debug, Clone, Copy)]
enum WriteFieldKind {
X,
Y,
Z,
Intensity,
Red,
Green,
Blue,
Classification,
NormalX,
NormalY,
NormalZ,
}
fn write_fields(cloud: &PointCloud) -> Vec<WriteField> {
let mut fields = vec![
WriteField {
name: "x",
size: 8,
ty: 'F',
count: 1,
kind: WriteFieldKind::X,
},
WriteField {
name: "y",
size: 8,
ty: 'F',
count: 1,
kind: WriteFieldKind::Y,
},
WriteField {
name: "z",
size: 8,
ty: 'F',
count: 1,
kind: WriteFieldKind::Z,
},
];
if cloud.has_intensity() {
fields.push(WriteField {
name: "intensity",
size: 4,
ty: 'F',
count: 1,
kind: WriteFieldKind::Intensity,
});
}
if cloud.has_color() {
fields.extend([
WriteField {
name: "red",
size: 2,
ty: 'U',
count: 1,
kind: WriteFieldKind::Red,
},
WriteField {
name: "green",
size: 2,
ty: 'U',
count: 1,
kind: WriteFieldKind::Green,
},
WriteField {
name: "blue",
size: 2,
ty: 'U',
count: 1,
kind: WriteFieldKind::Blue,
},
]);
}
if cloud.has_classification() {
fields.push(WriteField {
name: "classification",
size: 1,
ty: 'U',
count: 1,
kind: WriteFieldKind::Classification,
});
}
if cloud.has_normals() {
fields.extend([
WriteField {
name: "normal_x",
size: 8,
ty: 'F',
count: 1,
kind: WriteFieldKind::NormalX,
},
WriteField {
name: "normal_y",
size: 8,
ty: 'F',
count: 1,
kind: WriteFieldKind::NormalY,
},
WriteField {
name: "normal_z",
size: 8,
ty: 'F',
count: 1,
kind: WriteFieldKind::NormalZ,
},
]);
}
fields
}
fn write_ascii<W: Write>(
writer: &mut W,
cloud: &PointCloud,
fields: &[WriteField],
precision: usize,
) -> Result<()> {
for point in &cloud.points {
for (idx, field) in fields.iter().enumerate() {
if idx > 0 {
write!(writer, " ")?;
}
write_field_text(writer, point, field, precision)?;
}
writeln!(writer)?;
}
Ok(())
}
fn write_field_text<W: Write>(
writer: &mut W,
point: &Point,
field: &WriteField,
precision: usize,
) -> Result<()> {
match field.kind {
WriteFieldKind::X => crate::io::write_fmt_f64(writer, point.position.x, precision)?,
WriteFieldKind::Y => crate::io::write_fmt_f64(writer, point.position.y, precision)?,
WriteFieldKind::Z => crate::io::write_fmt_f64(writer, point.position.z, precision)?,
WriteFieldKind::Intensity => {
write!(writer, "{:.*}", precision, point.intensity.unwrap_or(0.0))?
}
WriteFieldKind::Red => {
write!(writer, "{}", point.color.unwrap_or(Color::new(0, 0, 0)).red)?
}
WriteFieldKind::Green => write!(
writer,
"{}",
point.color.unwrap_or(Color::new(0, 0, 0)).green
)?,
WriteFieldKind::Blue => write!(
writer,
"{}",
point.color.unwrap_or(Color::new(0, 0, 0)).blue
)?,
WriteFieldKind::Classification => write!(writer, "{}", point.classification.unwrap_or(0))?,
WriteFieldKind::NormalX => {
crate::io::write_fmt_f64(writer, point.normal.unwrap_or(Vec3::ZERO).x, precision)?
}
WriteFieldKind::NormalY => {
crate::io::write_fmt_f64(writer, point.normal.unwrap_or(Vec3::ZERO).y, precision)?
}
WriteFieldKind::NormalZ => {
crate::io::write_fmt_f64(writer, point.normal.unwrap_or(Vec3::ZERO).z, precision)?
}
}
Ok(())
}
fn write_binary<W: Write>(writer: &mut W, cloud: &PointCloud, fields: &[WriteField]) -> Result<()> {
for point in &cloud.points {
for field in fields {
write_field_binary(writer, point, field)?;
}
}
Ok(())
}
fn write_field_binary<W: Write>(writer: &mut W, point: &Point, field: &WriteField) -> Result<()> {
match field.kind {
WriteFieldKind::X => writer.write_all(&point.position.x.to_le_bytes())?,
WriteFieldKind::Y => writer.write_all(&point.position.y.to_le_bytes())?,
WriteFieldKind::Z => writer.write_all(&point.position.z.to_le_bytes())?,
WriteFieldKind::Intensity => {
writer.write_all(&point.intensity.unwrap_or(0.0).to_le_bytes())?
}
WriteFieldKind::Red => {
writer.write_all(&point.color.unwrap_or(Color::new(0, 0, 0)).red.to_le_bytes())?
}
WriteFieldKind::Green => writer.write_all(
&point
.color
.unwrap_or(Color::new(0, 0, 0))
.green
.to_le_bytes(),
)?,
WriteFieldKind::Blue => writer.write_all(
&point
.color
.unwrap_or(Color::new(0, 0, 0))
.blue
.to_le_bytes(),
)?,
WriteFieldKind::Classification => writer.write_all(&[point.classification.unwrap_or(0)])?,
WriteFieldKind::NormalX => {
writer.write_all(&point.normal.unwrap_or(Vec3::ZERO).x.to_le_bytes())?
}
WriteFieldKind::NormalY => {
writer.write_all(&point.normal.unwrap_or(Vec3::ZERO).y.to_le_bytes())?
}
WriteFieldKind::NormalZ => {
writer.write_all(&point.normal.unwrap_or(Vec3::ZERO).z.to_le_bytes())?
}
}
Ok(())
}
fn parse_usize_list(
format: Format,
line: usize,
values: &[&str],
name: &str,
) -> Result<Vec<usize>> {
values
.iter()
.map(|value| {
value
.parse::<usize>()
.map_err(|_| Error::parse(format, line, format!("invalid {name} value '{value}'")))
})
.collect()
}
fn parse_usize(format: Format, line: usize, value: Option<&&str>, name: &str) -> Result<usize> {
let value = value.ok_or_else(|| Error::parse(format, line, format!("missing {name}")))?;
value
.parse::<usize>()
.map_err(|_| Error::parse(format, line, format!("invalid {name} '{value}'")))
}