1use std::collections::{BTreeMap, HashMap};
31use std::fs::File;
32use std::path::Path;
33use std::sync::Arc;
34
35use parquet::basic::{Compression, Encoding, ZstdLevel};
36use parquet::column::reader::ColumnReader;
37use parquet::data_type::{BoolType, DataType, DoubleType, FloatType, Int32Type, Int64Type};
38use parquet::file::metadata::KeyValue;
39use parquet::file::properties::WriterProperties;
40use parquet::file::reader::{FileReader, SerializedFileReader};
41use parquet::file::writer::{SerializedFileWriter, SerializedRowGroupWriter};
42use parquet::schema::parser::parse_message_type;
43
44use crate::error::{Error, Result};
45use crate::pointcloud::Attr;
46use crate::{Point, Shots};
47
48const VERSION: &str = "1";
49const MAX_SCANS: usize = 1 << 16;
50
51impl From<parquet::errors::ParquetError> for Error {
52 fn from(e: parquet::errors::ParquetError) -> Self {
53 Error::invalid(format!("parquet: {e}"))
54 }
55}
56
57#[derive(Debug, Clone, Copy, PartialEq)]
58pub struct ShotsWriteOptions {
60 pub double: bool,
62 pub row_group_size: usize,
64 pub zstd_level: i32,
66 pub origin_tolerance: f64,
72}
73
74impl Default for ShotsWriteOptions {
75 fn default() -> Self {
76 ShotsWriteOptions { double: false, row_group_size: 1 << 20, zstd_level: 3, origin_tolerance: 1e-3 }
77 }
78}
79
80fn physical(a: &Attr) -> &'static str {
81 match a {
82 Attr::F64(_) => "double",
83 Attr::F32(_) => "float",
84 Attr::I64(_) => "int64",
85 Attr::I32(_) => "int32",
86 Attr::U32(_) => "int32 element (INTEGER(32,false))",
87 Attr::U16(_) => "int32 element (INTEGER(16,false))",
88 Attr::U8(_) => "int32 element (INTEGER(8,false))",
89 Attr::I8(_) => "int32 element (INTEGER(8,true))",
90 Attr::Bool(_) => "boolean",
91 }
92}
93
94fn list_field(name: &str, element: &str) -> String {
95 let element = if element.contains("element") { element.to_string() } else { format!("{element} element") };
96 format!(" required group {name} (LIST) {{ repeated group list {{ required {element}; }} }}\n")
97}
98
99fn angles(d: &Point) -> (f64, f64) {
101 (d[2].clamp(-1.0, 1.0).acos(), d[0].atan2(d[1]))
102}
103
104fn direction(zenith: f64, azimuth: f64) -> Point {
105 let (sz, cz) = zenith.sin_cos();
106 let (sa, ca) = azimuth.sin_cos();
107 [sz * sa, sz * ca, cz]
108}
109
110fn write_col<T: DataType>(rg: &mut SerializedRowGroupWriter<'_, File>, values: &[T::T], levels: Option<(&[i16], &[i16])>) -> Result<()> {
111 let mut col = rg.next_column()?.ok_or_else(|| Error::invalid("parquet: schema has fewer columns than the data"))?;
112 col.typed::<T>().write_batch(values, levels.map(|l| l.0), levels.map(|l| l.1))?;
113 col.close()?;
114 Ok(())
115}
116
117pub fn write_shots(shots: &Shots, path: impl AsRef<Path>, opts: &ShotsWriteOptions) -> Result<()> {
119 let path = path.as_ref();
120 let n = shots.n_shots();
121 for name in shots.echo_attrs.keys() {
122 let ok = !name.is_empty() && name.chars().all(|c| c.is_ascii_alphanumeric() || c == '_');
123 if !ok || matches!(name.as_str(), "scan" | "zenith" | "azimuth" | "range" | "origin_x" | "origin_y" | "origin_z" | "list" | "element") {
124 return Err(Error::invalid(format!("echo attribute {name:?} cannot be a column name")));
125 }
126 }
127
128 let tol = opts.origin_tolerance.max(0.0);
130 let key_of = |o: &Point| -> [u64; 3] {
131 if tol > 0.0 {
132 [(o[0] / tol).round() as i64 as u64, (o[1] / tol).round() as i64 as u64, (o[2] / tol).round() as i64 as u64]
133 } else {
134 [o[0].to_bits(), o[1].to_bits(), o[2].to_bits()]
135 }
136 };
137 let mut sums: Vec<(Point, f64)> = Vec::new();
138 let mut first: Vec<(Point, bool)> = Vec::new();
140 let mut scan_of: Vec<i32> = Vec::with_capacity(n);
141 let mut seen: HashMap<[u64; 3], i32> = HashMap::new();
142 for o in &shots.origin {
143 let next = sums.len() as i32;
144 let id = *seen.entry(key_of(o)).or_insert(next);
145 if id == next {
146 if sums.len() == MAX_SCANS {
147 break;
148 }
149 sums.push(([0.0; 3], 0.0));
150 first.push((*o, true));
151 }
152 first[id as usize].1 &= first[id as usize].0 == *o;
153 let s = &mut sums[id as usize];
154 for k in 0..3 {
155 s.0[k] += o[k];
156 }
157 s.1 += 1.0;
158 scan_of.push(id);
159 }
160 let per_shot_origin = scan_of.len() < n;
161 let scans: Vec<Point> = sums.iter().zip(&first).map(|((s, c), (o, same))| if *same { *o } else { [s[0] / c, s[1] / c, s[2] / c] }).collect();
162
163 let snapped;
165 let shots = if per_shot_origin || tol == 0.0 {
166 shots
167 } else {
168 let mut out = shots.clone();
169 for s in 0..n {
170 let (o, d, scan) = (shots.origin[s], shots.direction[s], scans[scan_of[s] as usize]);
171 let (e0, c) = (shots.echo_start[s], shots.echo_count[s] as usize);
172 out.origin[s] = scan;
173 for e in e0..e0 + c {
174 let r = shots.echo_range[e];
175 let v = [o[0] + d[0] * r - scan[0], o[1] + d[1] * r - scan[1], o[2] + d[2] * r - scan[2]];
176 out.echo_range[e] = (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt();
177 if e + 1 == e0 + c && out.echo_range[e] > 0.0 {
178 out.direction[s] = [v[0] / out.echo_range[e], v[1] / out.echo_range[e], v[2] / out.echo_range[e]];
179 }
180 }
181 }
182 snapped = out;
183 &snapped
184 };
185
186 let real = if opts.double { "double" } else { "float" };
187 let mut schema = String::from("message sylva_shots {\n");
188 if per_shot_origin {
189 schema += " required int32 origin_x;\n required int32 origin_y;\n required int32 origin_z;\n";
190 } else {
191 schema += " required int32 scan;\n";
192 }
193 schema += &format!(" required {real} zenith;\n required {real} azimuth;\n");
194 schema += &list_field("range", real);
195 for (name, a) in &shots.echo_attrs {
196 schema += &list_field(name, physical(a));
197 }
198 schema += "}\n";
199 let schema = Arc::new(parse_message_type(&schema)?);
200
201 let xyz_bounds = {
202 let xyz = shots.echo_xyz();
203 (crate::util::spatial::min_corner(&xyz), crate::util::spatial::max_corner(&xyz))
204 };
205 let join = |p: &Point| format!("{} {} {}", p[0], p[1], p[2]);
206 let origin_offset = shots.origin.first().copied().unwrap_or([0.0; 3]);
209 let reach = shots.origin.iter().flat_map(|o| (0..3).map(move |k| (o[k] - origin_offset[k]).abs())).fold(0.0f64, f64::max);
210 let origin_scale = (reach / 2.0e9).max(if opts.double { 1e-6 } else { 1e-4 });
211 let kv = vec![
212 KeyValue::new("sylva.origin_offset".into(), join(&origin_offset)),
213 KeyValue::new("sylva.origin_scale".into(), origin_scale.to_string()),
214 KeyValue::new("sylva.shots".into(), VERSION.to_string()),
215 KeyValue::new("sylva.precision".into(), if opts.double { "double" } else { "single" }.to_string()),
216 KeyValue::new("sylva.scans".into(), if per_shot_origin { String::new() } else { scans.iter().map(join).collect::<Vec<_>>().join(";") }),
217 KeyValue::new("sylva.attrs".into(), shots.echo_attrs.iter().map(|(k, a)| format!("{k}:{}", a.dtype())).collect::<Vec<_>>().join(",")),
218 KeyValue::new("sylva.bounds".into(), format!("{} {}", join(&xyz_bounds.0), join(&xyz_bounds.1))),
219 KeyValue::new("sylva.n_echoes".into(), shots.n_echoes().to_string()),
220 ];
221 let props = WriterProperties::builder()
222 .set_compression(Compression::ZSTD(ZstdLevel::try_new(opts.zstd_level)?))
223 .set_key_value_metadata(Some(kv))
224 .set_column_encoding("origin_x".into(), Encoding::DELTA_BINARY_PACKED)
226 .set_column_dictionary_enabled("origin_x".into(), false)
227 .set_column_encoding("origin_y".into(), Encoding::DELTA_BINARY_PACKED)
228 .set_column_dictionary_enabled("origin_y".into(), false)
229 .set_column_encoding("origin_z".into(), Encoding::DELTA_BINARY_PACKED)
230 .set_column_dictionary_enabled("origin_z".into(), false)
231 .set_column_encoding("zenith".into(), Encoding::BYTE_STREAM_SPLIT)
232 .set_column_dictionary_enabled("zenith".into(), false)
233 .set_column_encoding("azimuth".into(), Encoding::BYTE_STREAM_SPLIT)
234 .set_column_dictionary_enabled("azimuth".into(), false)
235 .set_column_encoding("range.list.element".into(), Encoding::BYTE_STREAM_SPLIT)
236 .set_column_dictionary_enabled("range.list.element".into(), false)
237 .build();
238 let file = File::create(path).map_err(|e| Error::file(path, e.to_string()))?;
239 let mut writer = SerializedFileWriter::new(file, schema, Arc::new(props))?;
240
241 let group = opts.row_group_size.max(1);
242 for s0 in (0..n.max(1)).step_by(group) {
243 let s1 = (s0 + group).min(n);
244 let mut rg = writer.next_row_group()?;
245 if per_shot_origin {
246 for k in 0..3 {
247 let v: Vec<i32> = shots.origin[s0..s1].iter().map(|o| ((o[k] - origin_offset[k]) / origin_scale).round() as i32).collect();
248 write_col::<Int32Type>(&mut rg, &v, None)?;
249 }
250 } else {
251 write_col::<Int32Type>(&mut rg, &scan_of[s0..s1], None)?;
252 }
253 for k in 0..2 {
254 let v: Vec<f64> = shots.direction[s0..s1].iter().map(|d| if k == 0 { angles(d).0 } else { angles(d).1 }).collect();
255 if opts.double {
256 write_col::<DoubleType>(&mut rg, &v, None)?;
257 } else {
258 write_col::<FloatType>(&mut rg, &v.iter().map(|&x| x as f32).collect::<Vec<_>>(), None)?;
259 }
260 }
261 let (e0, e1) = if s0 < n { (shots.echo_start[s0], shots.echo_start[s1 - 1] + shots.echo_count[s1 - 1] as usize) } else { (0, 0) };
264 let mut def = Vec::with_capacity(e1 - e0);
265 let mut rep = Vec::with_capacity(e1 - e0);
266 for &c in &shots.echo_count[s0..s1] {
267 if c == 0 {
268 def.push(0);
269 rep.push(0);
270 }
271 for j in 0..c {
272 def.push(1);
273 rep.push((j > 0) as i16);
274 }
275 }
276 let lv = Some((&def[..], &rep[..]));
277 if opts.double {
278 write_col::<DoubleType>(&mut rg, &shots.echo_range[e0..e1], lv)?;
279 } else {
280 write_col::<FloatType>(&mut rg, &shots.echo_range[e0..e1].iter().map(|&x| x as f32).collect::<Vec<_>>(), lv)?;
281 }
282 for a in shots.echo_attrs.values() {
283 let as_i32 = |it: &mut dyn Iterator<Item = i32>| it.collect::<Vec<i32>>();
284 match a {
285 Attr::F64(v) => write_col::<DoubleType>(&mut rg, &v[e0..e1], lv)?,
286 Attr::F32(v) => write_col::<FloatType>(&mut rg, &v[e0..e1], lv)?,
287 Attr::I64(v) => write_col::<Int64Type>(&mut rg, &v[e0..e1], lv)?,
288 Attr::I32(v) => write_col::<Int32Type>(&mut rg, &v[e0..e1], lv)?,
289 Attr::U32(v) => write_col::<Int32Type>(&mut rg, &as_i32(&mut v[e0..e1].iter().map(|&x| x as i32)), lv)?,
290 Attr::U16(v) => write_col::<Int32Type>(&mut rg, &as_i32(&mut v[e0..e1].iter().map(|&x| x as i32)), lv)?,
291 Attr::U8(v) => write_col::<Int32Type>(&mut rg, &as_i32(&mut v[e0..e1].iter().map(|&x| x as i32)), lv)?,
292 Attr::I8(v) => write_col::<Int32Type>(&mut rg, &as_i32(&mut v[e0..e1].iter().map(|&x| x as i32)), lv)?,
293 Attr::Bool(v) => write_col::<BoolType>(&mut rg, &v[e0..e1], lv)?,
294 }
295 }
296 rg.close()?;
297 }
298 writer.close()?;
299 Ok(())
300}
301
302pub struct ShotsFile {
304 path: std::path::PathBuf,
305 reader: SerializedFileReader<File>,
306 scans: Vec<Point>,
307 per_pulse: bool,
309 attrs: Vec<(String, String)>,
310 double: bool,
311 origin_offset: Point,
312 origin_scale: f64,
313 pub bounds: (Point, Point),
315 pub n_shots: usize,
317 pub n_echoes: usize,
319}
320
321fn read_values<T: DataType>(col: ColumnReader, rows: usize, lists: bool, get: impl FnOnce(ColumnReader) -> Option<parquet::column::reader::ColumnReaderImpl<T>>) -> Result<(Vec<T::T>, Vec<i16>)> {
322 let mut r = get(col).ok_or_else(|| Error::invalid("parquet: unexpected column type"))?;
323 let (mut values, mut def, mut rep) = (Vec::new(), Vec::new(), Vec::new());
324 let mut done = 0;
325 while done < rows {
326 let (records, _, _) = r.read_records(rows - done, lists.then_some(&mut def), lists.then_some(&mut rep), &mut values)?;
327 if records == 0 {
328 break;
329 }
330 done += records;
331 }
332 if done != rows {
333 return Err(Error::invalid("parquet: column is shorter than its row group"));
334 }
335 Ok((values, rep.iter().zip(&def).map(|(&r, &d)| if r == 0 { d } else { 2 }).collect()))
336}
337
338macro_rules! reader_of {
339 ($variant:ident) => {
340 |c| match c {
341 ColumnReader::$variant(r) => Some(r),
342 _ => None,
343 }
344 };
345}
346
347impl ShotsFile {
348 pub fn open(path: impl AsRef<Path>) -> Result<Self> {
351 let path = path.as_ref();
352 let file = File::open(path).map_err(|e| Error::file(path, e.to_string()))?;
353 let reader = SerializedFileReader::new(file)?;
354 let meta = reader.metadata().file_metadata();
355 let kv: BTreeMap<&str, &str> = meta.key_value_metadata().into_iter().flatten().map(|k| (k.key.as_str(), k.value.as_deref().unwrap_or(""))).collect();
356 let bad = |what: &str| Error::file(path, format!("not a sylva shots file ({what})"));
357 if kv.get("sylva.shots") != Some(&VERSION) {
358 return Err(bad("missing or unknown sylva.shots version"));
359 }
360 let point = |s: &str| -> Option<Point> {
361 let v: Vec<f64> = s.split_whitespace().map(|t| t.parse().ok()).collect::<Option<_>>()?;
362 (v.len() == 3).then(|| [v[0], v[1], v[2]])
363 };
364 let scans: Vec<Point> = kv.get("sylva.scans").copied().unwrap_or("").split(';').filter(|s| !s.is_empty()).map(point).collect::<Option<_>>().ok_or_else(|| bad("scans"))?;
365 let attrs: Vec<(String, String)> = kv.get("sylva.attrs").copied().unwrap_or("").split(',').filter(|s| !s.is_empty()).map(|s| s.split_once(':').map(|(a, b)| (a.to_string(), b.to_string()))).collect::<Option<_>>().ok_or_else(|| bad("attrs"))?;
366 let b: Vec<f64> = kv.get("sylva.bounds").copied().unwrap_or("").split_whitespace().filter_map(|t| t.parse().ok()).collect();
367 if b.len() != 6 {
368 return Err(bad("bounds"));
369 }
370 let per_pulse = meta.schema_descr().columns().iter().any(|c| c.name() == "origin_x");
373 Ok(ShotsFile {
374 path: path.to_path_buf(),
375 scans,
376 per_pulse,
377 attrs,
378 double: kv.get("sylva.precision") == Some(&"double"),
379 origin_offset: kv.get("sylva.origin_offset").and_then(|s| point(s)).ok_or_else(|| bad("origin_offset"))?,
380 origin_scale: kv.get("sylva.origin_scale").and_then(|s| s.parse().ok()).ok_or_else(|| bad("origin_scale"))?,
381 bounds: ([b[0], b[1], b[2]], [b[3], b[4], b[5]]),
382 n_shots: meta.num_rows() as usize,
383 n_echoes: kv.get("sylva.n_echoes").and_then(|s| s.parse().ok()).ok_or_else(|| bad("n_echoes"))?,
384 reader,
385 })
386 }
387
388 pub fn reopen(&self) -> Result<Self> {
391 ShotsFile::open(&self.path)
392 }
393
394 pub fn n_groups(&self) -> usize {
396 self.reader.num_row_groups()
397 }
398
399 pub fn scans(&self) -> &[Point] {
401 &self.scans
402 }
403
404 pub fn attr_names(&self) -> impl Iterator<Item = &str> {
406 self.attrs.iter().map(|a| a.0.as_str())
407 }
408
409 pub fn read_group(&self, group: usize) -> Result<Shots> {
411 let rg = self.reader.get_row_group(group)?;
412 let rows = rg.metadata().num_rows() as usize;
413 let mut col = 0;
414 let mut next = || {
415 col += 1;
416 rg.get_column_reader(col - 1)
417 };
418 let reals = |c: ColumnReader, lists: bool| -> Result<(Vec<f64>, Vec<i16>)> {
419 if self.double {
420 read_values::<DoubleType>(c, rows, lists, reader_of!(DoubleColumnReader))
421 } else {
422 let (v, l) = read_values::<FloatType>(c, rows, lists, reader_of!(FloatColumnReader))?;
423 Ok((v.into_iter().map(|x| x as f64).collect(), l))
424 }
425 };
426 let mut shots = Shots::default();
427 if self.per_pulse {
428 let mut xyz = Vec::new();
429 for _ in 0..3 {
430 xyz.push(read_values::<Int32Type>(next()?, rows, false, reader_of!(Int32ColumnReader))?.0);
431 }
432 let at = |k: usize, i: usize| self.origin_offset[k] + xyz[k][i] as f64 * self.origin_scale;
433 shots.origin = (0..rows).map(|i| [at(0, i), at(1, i), at(2, i)]).collect();
434 } else {
435 let (ids, _) = read_values::<Int32Type>(next()?, rows, false, reader_of!(Int32ColumnReader))?;
436 shots.origin = ids.iter().map(|&i| self.scans.get(i as usize).copied().ok_or_else(|| Error::invalid("shots file: scan index out of range"))).collect::<Result<_>>()?;
437 }
438 let zenith = reals(next()?, false)?.0;
439 let azimuth = reals(next()?, false)?.0;
440 shots.direction = zenith.iter().zip(&azimuth).map(|(&z, &a)| direction(z, a)).collect();
441
442 let (range, levels) = reals(next()?, true)?;
444 shots.echo_range = range;
445 let mut at = 0;
446 for &l in &levels {
447 match l {
448 2 => *shots.echo_count.last_mut().ok_or_else(|| Error::invalid("shots file: bad list levels"))? += 1,
449 _ => {
450 shots.echo_start.push(at);
451 shots.echo_count.push(l as u32);
452 }
453 }
454 at += (l > 0) as usize;
455 }
456 for (name, dtype) in &self.attrs {
457 let c = next()?;
458 let ints = |c| read_values::<Int32Type>(c, rows, true, reader_of!(Int32ColumnReader)).map(|v| v.0);
459 let attr = match dtype.as_str() {
460 "float64" => Attr::F64(read_values::<DoubleType>(c, rows, true, reader_of!(DoubleColumnReader))?.0),
461 "float32" => Attr::F32(read_values::<FloatType>(c, rows, true, reader_of!(FloatColumnReader))?.0),
462 "int64" => Attr::I64(read_values::<Int64Type>(c, rows, true, reader_of!(Int64ColumnReader))?.0),
463 "int32" => Attr::I32(ints(c)?),
464 "uint32" => Attr::U32(ints(c)?.into_iter().map(|x| x as u32).collect()),
465 "uint16" => Attr::U16(ints(c)?.into_iter().map(|x| x as u16).collect()),
466 "uint8" => Attr::U8(ints(c)?.into_iter().map(|x| x as u8).collect()),
467 "int8" => Attr::I8(ints(c)?.into_iter().map(|x| x as i8).collect()),
468 "bool" => Attr::Bool(read_values::<BoolType>(c, rows, true, reader_of!(BoolColumnReader))?.0),
469 other => return Err(Error::invalid(format!("shots file: unknown dtype {other:?}"))),
470 };
471 if attr.len() != shots.echo_range.len() {
472 return Err(Error::invalid(format!("shots file: attribute {name:?} does not match the echoes")));
473 }
474 shots.echo_attrs.insert(name.clone(), attr);
475 }
476 Ok(shots)
477 }
478
479 pub fn read_all(&self) -> Result<Shots> {
481 self.read_groups(&(0..self.n_groups()).collect::<Vec<_>>())
482 }
483
484 pub fn read_groups(&self, groups: &[usize]) -> Result<Shots> {
486 let mut out = Shots::default();
487 for (name, dtype) in &self.attrs {
488 let empty = match dtype.as_str() {
489 "float64" => Attr::F64(vec![]),
490 "float32" => Attr::F32(vec![]),
491 "int64" => Attr::I64(vec![]),
492 "int32" => Attr::I32(vec![]),
493 "uint32" => Attr::U32(vec![]),
494 "uint16" => Attr::U16(vec![]),
495 "uint8" => Attr::U8(vec![]),
496 "int8" => Attr::I8(vec![]),
497 _ => Attr::Bool(vec![]),
498 };
499 out.echo_attrs.insert(name.clone(), empty);
500 }
501 for &g in groups {
502 if g >= self.n_groups() {
503 return Err(Error::invalid(format!("shots file has {} row groups, asked for {g}", self.n_groups())));
504 }
505 append(&mut out, self.read_group(g)?)?;
506 }
507 Ok(out)
508 }
509}
510
511pub(crate) fn append(out: &mut Shots, part: Shots) -> Result<()> {
513 if out.n_shots() == 0 {
514 *out = part;
515 return Ok(());
516 }
517 let offset = out.echo_range.len();
518 out.origin.extend(part.origin);
519 out.direction.extend(part.direction);
520 out.echo_start.extend(part.echo_start.iter().map(|s| s + offset));
521 out.echo_count.extend(part.echo_count);
522 out.echo_range.extend(part.echo_range);
523 for (k, a) in &part.echo_attrs {
524 out.echo_attrs.get_mut(k).ok_or_else(|| Error::invalid(format!("shots file: attribute {k:?} is not in every row group")))?.extend(a)?;
525 }
526 Ok(())
527}
528
529pub fn read_shots(path: impl AsRef<Path>) -> Result<Shots> {
531 ShotsFile::open(path)?.read_all()
532}