1use std::borrow::Cow;
39
40use base64::alphabet;
41use base64::engine::general_purpose::{GeneralPurpose, GeneralPurposeConfig};
42use base64::engine::{DecodePaddingMode, Engine};
43use base64::DecodeSliceError;
44use serde_json::Value;
45
46use crate::json::{object, points3};
47use crate::math::Vec3;
48use crate::{Attributes, DecodeError};
49
50pub const POINTS_ARRAY: &str = "points::array";
52pub const POINTS_BASE64: &str = "points::base64";
54pub const PCD_BASE64: &str = "pcd::base64";
56
57#[derive(Debug, Clone, PartialEq, Default)]
59pub struct PointCloud {
60 pub positions: Vec<Vec3>,
62 pub colors: Option<Vec<[f32; 3]>>,
65}
66
67impl PointCloud {
68 pub fn len(&self) -> usize {
70 self.positions.len()
71 }
72
73 pub fn is_empty(&self) -> bool {
75 self.positions.is_empty()
76 }
77
78 pub fn from_attributes<A: Attributes + ?Sized>(
82 attributes: &A,
83 ) -> Result<Option<Self>, DecodeError> {
84 if let Some(value) = attributes.attribute(PCD_BASE64) {
85 Self::from_pcd_base64(value).map(Some)
86 } else if let Some(value) = attributes.attribute(POINTS_ARRAY) {
87 Self::from_points_array(value).map(Some)
88 } else if let Some(value) = attributes.attribute(POINTS_BASE64) {
89 Self::from_points_base64(value).map(Some)
90 } else {
91 Ok(None)
92 }
93 }
94
95 pub fn from_points_array(value: &Value) -> Result<Self, DecodeError> {
99 let fields = object(value, POINTS_ARRAY)?;
100 let positions = points3(positions(fields)?, "positions")?;
101 let colors = match fields.get("colors").filter(|v| !v.is_null()) {
102 Some(colors) => Some(
103 points3(colors, "colors")?
104 .into_iter()
105 .map(|c| c.map(|v| v as f32))
106 .collect(),
107 ),
108 None => None,
109 };
110 Self::checked(positions, colors)
111 }
112
113 pub fn from_points_base64(value: &Value) -> Result<Self, DecodeError> {
117 let fields = object(value, POINTS_BASE64)?;
118 let positions =
119 base64_triples(string(positions(fields)?, "positions")?, "positions", |t| {
120 t.map(f64::from)
121 })?;
122 let colors = match fields.get("colors").filter(|v| !v.is_null()) {
123 Some(colors) => Some(base64_triples(string(colors, "colors")?, "colors", |t| t)?),
124 None => None,
125 };
126 Self::checked(positions, colors)
127 }
128
129 pub fn from_pcd_base64(value: &Value) -> Result<Self, DecodeError> {
132 let bytes = FORGIVING
133 .decode(&*without_whitespace(string(value, PCD_BASE64)?))
134 .map_err(|e| DecodeError::InvalidBase64 {
135 at: PCD_BASE64.to_owned(),
136 reason: e.to_string(),
137 })?;
138 Self::from_pcd(&bytes)
139 }
140
141 pub fn from_pcd(bytes: &[u8]) -> Result<Self, DecodeError> {
154 let header = PcdHeader::parse(bytes)?;
155 let body = &bytes[header.len..];
156 match header.data.as_str() {
157 "ascii" => read_pcd_ascii(&header, body),
158 "binary" => read_pcd_binary(&header, body, Layout::Rows),
159 "binary_compressed" => {
160 let decompressed = read_pcd_compressed(body)?;
161 read_pcd_binary(&header, &decompressed, Layout::Columns)
162 }
163 other => Err(unsupported(format!("DATA {other}"))),
164 }
165 }
166
167 fn checked(positions: Vec<Vec3>, colors: Option<Vec<[f32; 3]>>) -> Result<Self, DecodeError> {
168 if let Some(colors) = &colors {
169 if colors.len() != positions.len() {
170 return Err(DecodeError::LengthMismatch {
171 field: "colors",
172 len: colors.len(),
173 expected: positions.len(),
174 });
175 }
176 }
177 Ok(Self { positions, colors })
178 }
179}
180
181fn positions(fields: &serde_json::Map<String, Value>) -> Result<&Value, DecodeError> {
182 fields
183 .get("positions")
184 .filter(|v| !v.is_null())
185 .ok_or(DecodeError::MissingField { field: "positions" })
186}
187
188fn string<'v>(value: &'v Value, at: &str) -> Result<&'v str, DecodeError> {
189 value.as_str().ok_or_else(|| DecodeError::WrongShape {
190 at: at.to_owned(),
191 expected: "base64 string",
192 })
193}
194
195const FORGIVING: GeneralPurpose = GeneralPurpose::new(
198 &alphabet::STANDARD,
199 GeneralPurposeConfig::new()
200 .with_decode_padding_mode(DecodePaddingMode::Indifferent)
201 .with_decode_allow_trailing_bits(true),
202);
203
204const CHUNK_SYMBOLS: usize = 16 * 1024;
207const CHUNK_BYTES: usize = CHUNK_SYMBOLS / 4 * 3;
208
209fn without_whitespace(text: &str) -> Cow<'_, [u8]> {
210 let bytes = text.as_bytes();
211 if bytes.iter().any(u8::is_ascii_whitespace) {
212 Cow::Owned(
213 bytes
214 .iter()
215 .copied()
216 .filter(|b| !b.is_ascii_whitespace())
217 .collect(),
218 )
219 } else {
220 Cow::Borrowed(bytes)
221 }
222}
223
224fn base64_triples<T>(
226 text: &str,
227 at: &str,
228 convert: impl Fn([f32; 3]) -> T,
229) -> Result<Vec<T>, DecodeError> {
230 let invalid = |reason: String| DecodeError::InvalidBase64 {
231 at: at.to_owned(),
232 reason,
233 };
234 let input = without_whitespace(text);
235 let mut symbols = input.len();
236 for _ in 0..2 {
237 if symbols > 0 && input[symbols - 1] == b'=' {
238 symbols -= 1;
239 }
240 }
241 let bytes = symbols / 4 * 3
242 + match symbols % 4 {
243 0 => 0,
244 2 => 1,
245 3 => 2,
246 _ => {
247 return Err(invalid(format!(
248 "{symbols} symbols cannot encode whole bytes"
249 )))
250 }
251 };
252 let byte_length = || DecodeError::ByteLength {
253 at: at.to_owned(),
254 bytes,
255 element_size: 12,
256 };
257 if bytes % 12 != 0 {
258 return Err(byte_length());
259 }
260
261 let mut out = Vec::with_capacity(bytes / 12);
262 let mut buffer = [0u8; CHUNK_BYTES];
263 for chunk in input.chunks(CHUNK_SYMBOLS) {
264 let written = FORGIVING
265 .decode_slice(chunk, &mut buffer)
266 .map_err(|e| match e {
267 DecodeSliceError::DecodeError(e) => invalid(e.to_string()),
268 DecodeSliceError::OutputSliceTooSmall => invalid("padding inside the data".into()),
269 })?;
270 if written % 12 != 0 {
271 return Err(byte_length());
272 }
273 out.extend(
274 buffer[..written]
275 .chunks_exact(12)
276 .map(|t| convert([f32_le(&t[0..4]), f32_le(&t[4..8]), f32_le(&t[8..12])])),
277 );
278 }
279 Ok(out)
280}
281
282fn f32_le(b: &[u8]) -> f32 {
283 f32::from_le_bytes([b[0], b[1], b[2], b[3]])
284}
285
286fn u32_le(b: &[u8]) -> u32 {
287 u32::from_le_bytes([b[0], b[1], b[2], b[3]])
288}
289
290fn srgb_to_linear(c: f64) -> f64 {
292 if c < 0.04045 {
293 c * 0.0773993808
294 } else {
295 (c * 0.9478672986 + 0.0521327014).powf(2.4)
296 }
297}
298
299fn packed_rgb(packed: u32) -> [f32; 3] {
301 let channel = |shift: u32| srgb_to_linear(f64::from((packed >> shift) & 0xff) / 255.0) as f32;
302 [channel(16), channel(8), channel(0)]
303}
304
305fn invalid(reason: impl Into<String>) -> DecodeError {
306 DecodeError::InvalidPcd {
307 reason: reason.into(),
308 }
309}
310
311fn unsupported(reason: impl Into<String>) -> DecodeError {
312 DecodeError::UnsupportedPcd {
313 reason: reason.into(),
314 }
315}
316
317#[derive(Debug)]
319struct PcdHeader {
320 fields: Vec<String>,
321 size: Option<Vec<usize>>,
322 kind: Option<Vec<String>>,
323 count: Vec<usize>,
324 points: Option<usize>,
325 data: String,
326 len: usize,
328}
329
330impl PcdHeader {
331 fn parse(bytes: &[u8]) -> Result<Self, DecodeError> {
332 let mut fields = None;
333 let mut size = None;
334 let mut kind = None;
335 let mut count = None;
336 let mut width = None;
337 let mut height = None;
338 let mut points = None;
339 let mut pos = 0;
340 loop {
341 if pos >= bytes.len() {
342 return Err(invalid("no DATA line"));
343 }
344 let end = bytes[pos..]
345 .iter()
346 .position(|&b| b == b'\n')
347 .map_or(bytes.len(), |i| pos + i);
348 let line = std::str::from_utf8(&bytes[pos..end])
349 .map_err(|_| invalid("header line is not UTF-8"))?;
350 let next = (end + 1).min(bytes.len());
351 let line = line.split('#').next().unwrap_or_default();
352 let mut tokens = line.split_ascii_whitespace();
353 let Some(keyword) = tokens.next() else {
354 pos = next;
355 continue;
356 };
357 let values = || tokens.clone().map(str::to_owned).collect::<Vec<_>>();
358 match keyword.to_ascii_uppercase().as_str() {
359 "FIELDS" => fields = Some(values()),
360 "SIZE" => size = Some(numbers("SIZE", tokens.clone())?),
361 "TYPE" => kind = Some(values()),
362 "COUNT" => count = Some(numbers("COUNT", tokens.clone())?),
363 "WIDTH" => width = Some(number("WIDTH", tokens.clone())?),
364 "HEIGHT" => height = Some(number("HEIGHT", tokens.clone())?),
365 "POINTS" => points = Some(number("POINTS", tokens.clone())?),
366 "DATA" => {
367 let data = tokens.next().ok_or_else(|| invalid("empty DATA line"))?;
368 let fields: Vec<String> = fields.ok_or_else(|| invalid("no FIELDS line"))?;
369 let count = count.unwrap_or_else(|| vec![1; fields.len()]);
370 for (name, len) in [
371 ("SIZE", size.as_ref().map(Vec::len)),
372 ("TYPE", kind.as_ref().map(Vec::len)),
373 ("COUNT", Some(count.len())),
374 ] {
375 if let Some(len) = len.filter(|&len| len != fields.len()) {
376 return Err(invalid(format!(
377 "{name} has {len} entries for {} fields",
378 fields.len()
379 )));
380 }
381 }
382 let points = match (points, width, height) {
383 (Some(points), _, _) => Some(points),
384 (None, Some(w), Some(h)) => Some(
385 usize::checked_mul(w, h)
386 .ok_or_else(|| invalid("WIDTH × HEIGHT overflows"))?,
387 ),
388 _ => None,
389 };
390 return Ok(Self {
391 fields,
392 size,
393 kind,
394 count,
395 points,
396 data: data.to_owned(),
397 len: next,
398 });
399 }
400 _ => {}
401 }
402 pos = next;
403 }
404 }
405
406 fn index(&self, name: &str) -> Option<usize> {
407 self.fields.iter().position(|f| f == name)
408 }
409
410 fn type_of(&self, field: usize) -> Option<&str> {
411 self.kind.as_ref().map(|kind| kind[field].as_str())
412 }
413
414 fn xyz(&self) -> Result<[usize; 3], DecodeError> {
415 match (self.index("x"), self.index("y"), self.index("z")) {
416 (Some(x), Some(y), Some(z)) => Ok([x, y, z]),
417 _ => Err(invalid("FIELDS lacks x, y, or z")),
418 }
419 }
420}
421
422fn number<'a>(name: &str, mut tokens: impl Iterator<Item = &'a str>) -> Result<usize, DecodeError> {
423 tokens
424 .next()
425 .and_then(|t| t.parse().ok())
426 .ok_or_else(|| invalid(format!("{name} is not a count")))
427}
428
429fn numbers<'a>(
430 name: &str,
431 tokens: impl Iterator<Item = &'a str>,
432) -> Result<Vec<usize>, DecodeError> {
433 tokens
434 .map(|t| {
435 t.parse()
436 .map_err(|_| invalid(format!("{name} entry {t:?} is not a count")))
437 })
438 .collect()
439}
440
441fn read_pcd_ascii(header: &PcdHeader, body: &[u8]) -> Result<PointCloud, DecodeError> {
442 let text = std::str::from_utf8(body).map_err(|_| invalid("ASCII data is not UTF-8"))?;
443 let column = |field: usize| {
446 header.count[..field]
447 .iter()
448 .try_fold(0usize, |sum, &n| sum.checked_add(n))
449 .ok_or_else(|| invalid("COUNT values overflow"))
450 };
451 let mut xyz = [(0, false); 3];
454 for (slot, f) in xyz.iter_mut().zip(header.xyz()?) {
455 let double = header.size.as_ref().is_some_and(|size| size[f] == 8);
456 *slot = (column(f)?, double);
457 }
458 let [x, y, z] = xyz;
459 let rgb = match header.index("rgb") {
460 Some(i) => Some((column(i)?, header.type_of(i) == Some("F"))),
461 None => None,
462 };
463
464 let mut positions = Vec::new();
465 let mut colors = rgb.map(|_| Vec::new());
466 let mut tokens: Vec<&str> = Vec::new();
467 for (line_no, line) in text.lines().enumerate() {
468 tokens.clear();
469 tokens.extend(line.split_ascii_whitespace());
470 if tokens.is_empty() {
471 continue;
472 }
473 let value = |col: usize| -> Result<f64, DecodeError> {
474 tokens
475 .get(col)
476 .and_then(|t| t.parse::<f64>().ok())
477 .ok_or_else(|| {
478 invalid(format!(
479 "data line {} column {} is not a number",
480 line_no + 1,
481 col + 1
482 ))
483 })
484 };
485 let coordinate = |(col, double): (usize, bool)| {
486 value(col).map(|v| if double { v } else { f64::from(v as f32) })
487 };
488 positions.push([coordinate(x)?, coordinate(y)?, coordinate(z)?]);
489 if let (Some((col, is_float)), Some(colors)) = (rgb, colors.as_mut()) {
490 let v = value(col)?;
491 let packed = if is_float {
492 (v as f32).to_bits()
493 } else {
494 js_to_uint32(v)
495 };
496 colors.push(packed_rgb(packed));
497 }
498 }
499 Ok(PointCloud { positions, colors })
500}
501
502fn js_to_uint32(v: f64) -> u32 {
504 if v.is_finite() {
505 v.trunc().rem_euclid(4_294_967_296.0) as u32
506 } else {
507 0
508 }
509}
510
511#[derive(Clone, Copy)]
512enum Layout {
513 Rows,
515 Columns,
517}
518
519fn read_pcd_binary(
520 header: &PcdHeader,
521 data: &[u8],
522 layout: Layout,
523) -> Result<PointCloud, DecodeError> {
524 let size = header
525 .size
526 .as_ref()
527 .ok_or_else(|| invalid("binary data without SIZE"))?;
528 let points = header
529 .points
530 .ok_or_else(|| invalid("binary data without POINTS or WIDTH and HEIGHT"))?;
531 let xyz = header.xyz()?;
532 for (axis, &field) in ["x", "y", "z"].iter().zip(&xyz) {
533 let ty = header.type_of(field);
534 if size[field] != 4 || ty != Some("F") || header.count[field] != 1 {
535 return Err(unsupported(format!(
536 "{axis} is SIZE {} TYPE {} COUNT {}; only 4-byte floats are read",
537 size[field],
538 ty.unwrap_or("?"),
539 header.count[field]
540 )));
541 }
542 }
543 let rgb = header.index("rgb");
544 if let Some(field) = rgb {
545 if size[field] != 4 || header.count[field] != 1 {
546 return Err(unsupported("rgb must be one packed 4-byte value"));
547 }
548 }
549
550 let mut offsets = Vec::with_capacity(size.len());
551 let mut row = 0usize;
552 for (&s, &c) in size.iter().zip(&header.count) {
553 offsets.push(row);
554 row = s
555 .checked_mul(c)
556 .and_then(|n| row.checked_add(n))
557 .ok_or_else(|| invalid("record size overflows"))?;
558 }
559 let needed = points
560 .checked_mul(row)
561 .ok_or_else(|| invalid("POINTS × record size overflows"))?;
562 if data.len() < needed {
563 return Err(invalid(format!(
564 "{points} points need {needed} data bytes, found {}",
565 data.len()
566 )));
567 }
568
569 let at = |f: usize, i: usize| match layout {
571 Layout::Rows => i * row + offsets[f],
572 Layout::Columns => points * offsets[f] + i * size[f],
573 };
574 let positions = (0..points)
575 .map(|i| xyz.map(|f| f64::from(f32_le(&data[at(f, i)..]))))
576 .collect();
577 let colors = rgb.map(|f| {
578 (0..points)
579 .map(|i| packed_rgb(u32_le(&data[at(f, i)..])))
580 .collect()
581 });
582 Ok(PointCloud { positions, colors })
583}
584
585fn read_pcd_compressed(body: &[u8]) -> Result<Vec<u8>, DecodeError> {
586 if body.len() < 8 {
587 return Err(invalid("binary_compressed data lacks its size words"));
588 }
589 let compressed = u32_le(&body[0..4]) as usize;
590 let decompressed = u32_le(&body[4..8]) as usize;
591 let input = body
592 .get(8..8usize.saturating_add(compressed))
593 .ok_or_else(|| {
594 invalid(format!(
595 "{compressed} compressed bytes announced, {} present",
596 body.len() - 8
597 ))
598 })?;
599 decompress_lzf(input, decompressed)
600}
601
602const LZF_MAX_RATIO: usize = 88;
604
605fn decompress_lzf(input: &[u8], out_len: usize) -> Result<Vec<u8>, DecodeError> {
609 let corrupt = || invalid("corrupt LZF data");
610 if out_len > input.len().saturating_mul(LZF_MAX_RATIO) {
611 return Err(invalid(format!(
612 "{} LZF bytes cannot expand to {out_len}",
613 input.len()
614 )));
615 }
616 let mut out = Vec::with_capacity(out_len);
617 let mut i = 0;
618 while i < input.len() {
619 let ctrl = usize::from(input[i]);
620 i += 1;
621 if ctrl < 32 {
622 let n = ctrl + 1;
623 if out.len() + n > out_len || i + n > input.len() {
624 return Err(corrupt());
625 }
626 out.extend_from_slice(&input[i..i + n]);
627 i += n;
628 } else {
629 let mut len = ctrl >> 5;
630 if i >= input.len() {
631 return Err(corrupt());
632 }
633 if len == 7 {
634 len += usize::from(input[i]);
635 i += 1;
636 if i >= input.len() {
637 return Err(corrupt());
638 }
639 }
640 let back = ((ctrl & 0x1f) << 8) + usize::from(input[i]) + 1;
641 i += 1;
642 if out.len() + len + 2 > out_len || back > out.len() {
643 return Err(corrupt());
644 }
645 let start = out.len() - back;
646 for k in start..start + len + 2 {
648 out.push(out[k]);
649 }
650 }
651 }
652 if out.len() != out_len {
653 return Err(invalid(format!(
654 "LZF data expands to {} bytes, header says {out_len}",
655 out.len()
656 )));
657 }
658 Ok(out)
659}
660
661#[cfg(test)]
662mod tests {
663 use super::*;
664
665 #[test]
666 fn lzf_back_reference_repeats_bytes() {
667 let input = [1, b'a', b'b', (2 << 5), 1];
669 assert_eq!(decompress_lzf(&input, 6).unwrap(), b"ababab");
670 }
671
672 #[test]
673 fn lzf_rejects_references_before_start_and_overlong_output() {
674 assert!(matches!(
675 decompress_lzf(&[(1 << 5), 0], 3),
676 Err(DecodeError::InvalidPcd { .. })
677 ));
678 assert!(decompress_lzf(&[0, b'a'], 2).is_err());
679 assert!(decompress_lzf(&[0], 10_000).is_err());
680 }
681
682 #[test]
685 fn ascii_count_overflow_is_an_error() {
686 let pcd = b"FIELDS a x y z\nCOUNT 18446744073709551615 1 1 1\nDATA ascii\n1 2 3 4\n";
687 assert_eq!(
688 PointCloud::from_pcd(pcd),
689 Err(invalid("COUNT values overflow"))
690 );
691 }
692
693 #[test]
694 fn srgb_endpoints_stay_put() {
695 assert_eq!(packed_rgb(0x00ff_0000), [1.0, 0.0, 0.0]);
696 assert_eq!(packed_rgb(0x0000_ff00), [0.0, 1.0, 0.0]);
697 assert_eq!(packed_rgb(0x0000_00ff), [0.0, 0.0, 1.0]);
698 let [r, _, _] = packed_rgb(0x0080_8080);
700 assert!((r - 0.2158605).abs() < 1e-6, "{r}");
701 }
702
703 #[test]
704 fn to_uint32_wraps_like_javascript() {
705 assert_eq!(js_to_uint32(16_711_680.0), 0x00ff_0000);
706 assert_eq!(js_to_uint32(-1.0), u32::MAX);
707 assert_eq!(js_to_uint32(f64::NAN), 0);
708 }
709}