rigidity_io/
pcd.rs

1//! Reading and writing PCD, the Point Cloud Library's own format.
2//!
3//! Written here rather than taken from a crate for the same reason PLY is:
4//! the format is a text header and a block of numbers, the whole of it fits
5//! on one page, and a dependency for that costs more than the code does.
6//! It is also the format most likely to arrive from a robot, since
7//! everything built on PCL writes it.
8//!
9//! Supported: `ascii` and `binary` data, `F`/`U`/`I` fields of one, two,
10//! four or eight bytes, and any field order. Not supported: `binary_compressed`,
11//! which is an LZF stream nobody writes by hand and which no file this
12//! application has been shown has used.
13
14use std::fs::File;
15use std::io::{BufRead, BufReader, BufWriter, Read, Write};
16use std::path::Path;
17
18use nalgebra::Vector3;
19use rigidity_core::PointCloud;
20
21use crate::IoError;
22
23/// One column of the header.
24#[derive(Clone)]
25struct Field {
26    name: String,
27    kind: char,
28    size: usize,
29    count: usize,
30}
31
32/// What the header said.
33struct Header {
34    fields: Vec<Field>,
35    points: usize,
36    binary: bool,
37}
38
39/// Reads a cloud from PCD.
40///
41/// The origin is placed at the centre of the bounding box, as for every
42/// other georeferenced format: `f32` storage cannot hold absolute
43/// coordinates in the hundreds of thousands and millimetres at the same
44/// time.
45pub fn read_pcd(path: &Path) -> Result<PointCloud, IoError> {
46    let mut reader = BufReader::new(File::open(path)?);
47    let header = read_header(&mut reader)?;
48    let (x, y, z) = coordinates(&header)?;
49
50    let stride: usize = header
51        .fields
52        .iter()
53        .map(|field| field.size * field.count)
54        .sum();
55    let mut rows: Vec<[f64; 3]> = Vec::with_capacity(header.points);
56
57    if header.binary {
58        let mut row = vec![0u8; stride];
59        for _ in 0..header.points {
60            reader.read_exact(&mut row)?;
61            rows.push([
62                value(&row, &header.fields, x)?,
63                value(&row, &header.fields, y)?,
64                value(&row, &header.fields, z)?,
65            ]);
66        }
67    } else {
68        for line in reader.lines() {
69            let line = line?;
70            let numbers: Vec<&str> = line.split_whitespace().collect();
71            if numbers.is_empty() {
72                continue;
73            }
74            let pick = |at: usize| -> Result<f64, IoError> {
75                numbers
76                    .get(at)
77                    .ok_or_else(|| IoError::BadPcd("a row is shorter than the header".into()))?
78                    .parse()
79                    .map_err(|_| IoError::BadNumber(numbers[at].to_owned()))
80            };
81            rows.push([pick(x)?, pick(y)?, pick(z)?]);
82            if rows.len() == header.points {
83                break;
84            }
85        }
86    }
87
88    if rows.is_empty() {
89        return Ok(PointCloud::new());
90    }
91    let mut min = Vector3::repeat(f64::INFINITY);
92    let mut max = Vector3::repeat(f64::NEG_INFINITY);
93    for row in &rows {
94        let point = Vector3::new(row[0], row[1], row[2]);
95        min = min.inf(&point);
96        max = max.sup(&point);
97    }
98
99    let mut cloud = PointCloud::with_origin((min + max) * 0.5);
100    for row in rows {
101        cloud.push(Vector3::new(row[0], row[1], row[2]));
102    }
103    Ok(cloud)
104}
105
106/// Writes a cloud as binary PCD.
107///
108/// Binary rather than ascii: a million points in text is thirty megabytes
109/// of decimal digits and a lossy round trip unless every one is printed to
110/// seventeen significant figures.
111///
112/// # Why the coordinates are `f64`
113///
114/// PCL's own point types are `f32`, and writing `f32` here would be the
115/// compatible choice. It is also wrong for the data this application
116/// exists to handle: at a UTM northing of four million metres the `f32`
117/// step is a quarter of a metre, and a survey written that way comes back
118/// having moved further than the thing it was measuring. The storage keeps
119/// `f32` *offsets from an `f64` origin* precisely to avoid that, and a
120/// writer that flattens the two throws the invariant away at the last
121/// step. The format allows `SIZE 8`; tools that only read `f32` will say
122/// so, which is better than silently rounding.
123pub fn write_pcd(cloud: &PointCloud, path: &Path) -> Result<(), IoError> {
124    let mut out = BufWriter::new(File::create(path)?);
125    writeln!(out, "# .PCD v0.7 - Point Cloud Data file format")?;
126    writeln!(out, "VERSION 0.7")?;
127    writeln!(out, "FIELDS x y z")?;
128    writeln!(out, "SIZE 8 8 8")?;
129    writeln!(out, "TYPE F F F")?;
130    writeln!(out, "COUNT 1 1 1")?;
131    writeln!(out, "WIDTH {}", cloud.len())?;
132    writeln!(out, "HEIGHT 1")?;
133    writeln!(out, "VIEWPOINT 0 0 0 1 0 0 0")?;
134    writeln!(out, "POINTS {}", cloud.len())?;
135    writeln!(out, "DATA binary")?;
136
137    for index in 0..cloud.len() {
138        let point = cloud.point(index);
139        for value in [point.x, point.y, point.z] {
140            out.write_all(&value.to_le_bytes())?;
141        }
142    }
143    out.flush()?;
144    Ok(())
145}
146
147fn read_header<R: BufRead>(reader: &mut R) -> Result<Header, IoError> {
148    let mut fields: Vec<String> = Vec::new();
149    let mut sizes: Vec<usize> = Vec::new();
150    let mut kinds: Vec<char> = Vec::new();
151    let mut counts: Vec<usize> = Vec::new();
152    let mut points = None;
153    let mut width = None;
154    let mut height = None;
155
156    loop {
157        let mut line = String::new();
158        if reader.read_line(&mut line)? == 0 {
159            return Err(IoError::BadPcd("the header never ended".into()));
160        }
161        let line = line.trim();
162        if line.is_empty() || line.starts_with('#') {
163            continue;
164        }
165        let (key, rest) = line.split_once(char::is_whitespace).unwrap_or((line, ""));
166        let words: Vec<&str> = rest.split_whitespace().collect();
167        let numbers = |words: &[&str]| -> Result<Vec<usize>, IoError> {
168            words
169                .iter()
170                .map(|word| {
171                    word.parse()
172                        .map_err(|_| IoError::BadNumber((*word).to_owned()))
173                })
174                .collect()
175        };
176        match key.to_ascii_uppercase().as_str() {
177            "FIELDS" => fields = words.iter().map(|word| (*word).to_owned()).collect(),
178            "SIZE" => sizes = numbers(&words)?,
179            "TYPE" => {
180                kinds = words
181                    .iter()
182                    .filter_map(|word| word.chars().next())
183                    .collect()
184            }
185            "COUNT" => counts = numbers(&words)?,
186            "WIDTH" => width = numbers(&words)?.first().copied(),
187            "HEIGHT" => height = numbers(&words)?.first().copied(),
188            "POINTS" => points = numbers(&words)?.first().copied(),
189            "DATA" => {
190                let binary = match words.first().map(|word| word.to_ascii_lowercase()) {
191                    Some(word) if word == "binary" => true,
192                    Some(word) if word == "ascii" => false,
193                    other => {
194                        return Err(IoError::UnsupportedPcd(
195                            other.unwrap_or_else(|| "nothing".into()),
196                        ));
197                    }
198                };
199                // `POINTS` is optional in the specification; `WIDTH ×
200                // HEIGHT` is not, and an organised cloud states its shape
201                // there.
202                let points = points
203                    .or_else(|| Some(width? * height?))
204                    .ok_or_else(|| IoError::BadPcd("the header gives no point count".into()))?;
205                if fields.len() != sizes.len() || fields.len() != kinds.len() {
206                    return Err(IoError::BadPcd(
207                        "FIELDS, SIZE and TYPE disagree about how many columns there are".into(),
208                    ));
209                }
210                let fields = fields
211                    .iter()
212                    .enumerate()
213                    .map(|(index, name)| Field {
214                        name: name.clone(),
215                        kind: kinds[index],
216                        size: sizes[index],
217                        count: counts.get(index).copied().unwrap_or(1),
218                    })
219                    .collect();
220                return Ok(Header {
221                    fields,
222                    points,
223                    binary,
224                });
225            }
226            _ => {}
227        }
228    }
229}
230
231/// Where x, y and z are, by name rather than by position: PCL writes them
232/// first by convention and not by rule.
233fn coordinates(header: &Header) -> Result<(usize, usize, usize), IoError> {
234    let find = |wanted: &str| {
235        header
236            .fields
237            .iter()
238            .position(|field| field.name.eq_ignore_ascii_case(wanted))
239    };
240    match (find("x"), find("y"), find("z")) {
241        (Some(x), Some(y), Some(z)) => Ok((x, y, z)),
242        _ => Err(IoError::BadPcd("no x, y and z fields".into())),
243    }
244}
245
246/// One field of one binary row, as `f64`.
247fn value(row: &[u8], fields: &[Field], wanted: usize) -> Result<f64, IoError> {
248    let at: usize = fields[..wanted]
249        .iter()
250        .map(|field| field.size * field.count)
251        .sum();
252    let field = &fields[wanted];
253    let bytes = row
254        .get(at..at + field.size)
255        .ok_or_else(|| IoError::BadPcd("a row is shorter than the header".into()))?;
256    let signed = |bytes: &[u8]| -> i64 {
257        let mut wide = [0u8; 8];
258        wide[..bytes.len()].copy_from_slice(bytes);
259        let raw = u64::from_le_bytes(wide);
260        // Sign-extend from the field's own width.
261        let shift = 64 - bytes.len() * 8;
262        ((raw << shift) as i64) >> shift
263    };
264    Ok(match (field.kind.to_ascii_uppercase(), field.size) {
265        ('F', 4) => f64::from(f32::from_le_bytes(bytes.try_into().unwrap())),
266        ('F', 8) => f64::from_le_bytes(bytes.try_into().unwrap()),
267        ('U', _) => {
268            let mut wide = [0u8; 8];
269            wide[..bytes.len()].copy_from_slice(bytes);
270            u64::from_le_bytes(wide) as f64
271        }
272        ('I', _) => signed(bytes) as f64,
273        (kind, size) => {
274            return Err(IoError::UnsupportedPcd(format!("{kind}{}", size * 8)));
275        }
276    })
277}
rigidity_io/pcd.rs

rigidity_io/
pcd.rs
