spatialrust_io/copc/
writer.rs

1use std::path::Path;
2
3use copc_core::{Bounds as CopcBounds, Error as CopcCoreError, Result as CopcCoreResult};
4use copc_writer::{write_source, CopcPointFields, CopcPointSource, CopcWriterParams};
5use spatialrust_core::{DType, FieldSemantic, HasPositions3, PointCloud, PointField, PointSchema};
6
7use crate::error::{copc_format, copc_parse, IoError};
8use crate::{PointWriter, WriteOptions};
9
10/// Writes point clouds to COPC files.
11pub struct CopcWriter;
12
13impl PointWriter for CopcWriter {
14    fn write(
15        &mut self,
16        _cloud: &PointCloud,
17        _options: &WriteOptions,
18    ) -> spatialrust_core::SpatialResult<()> {
19        Err(spatialrust_core::SpatialError::InvalidArgument(
20            "CopcWriter requires write_copc_file with a path ending in .copc.laz".to_owned(),
21        ))
22    }
23}
24
25/// Writes a point cloud to a COPC file on disk.
26pub fn write_copc(path: impl AsRef<Path>, cloud: &PointCloud) -> Result<(), IoError> {
27    write_copc_file(path, cloud)
28}
29
30/// Writes a point cloud to a COPC file on disk.
31pub fn write_copc_file(path: impl AsRef<Path>, cloud: &PointCloud) -> Result<(), IoError> {
32    write_copc_file_with_params(path, cloud, &CopcWriterParams::default())
33}
34
35/// Writes a point cloud to a COPC file using custom octree builder parameters.
36pub fn write_copc_file_with_params(
37    path: impl AsRef<Path>,
38    cloud: &PointCloud,
39    params: &CopcWriterParams,
40) -> Result<(), IoError> {
41    validate_copc_output_path(path.as_ref())?;
42    cloud.validate()?;
43    if cloud.is_empty() {
44        return Err(copc_format("cannot write an empty point cloud to COPC".to_owned()));
45    }
46
47    let has_color = cloud.schema().fields().iter().any(|field| {
48        matches!(
49            field.semantic,
50            FieldSemantic::ColorR | FieldSemantic::ColorG | FieldSemantic::ColorB
51        )
52    });
53    let bounds = bounds_from_cloud(cloud)?;
54    let source = PointCloudCopcSource { cloud };
55    write_source(path.as_ref(), &source, has_color, bounds, params).map_err(map_copc_writer_error)
56}
57
58fn validate_copc_output_path(path: &Path) -> Result<(), IoError> {
59    let file_stem = path.file_stem().and_then(|stem| stem.to_str());
60    let extension = path.extension().and_then(|ext| ext.to_str());
61    match (file_stem, extension) {
62        (Some(stem), Some(ext)) if ext.eq_ignore_ascii_case("laz") => {
63            Path::new(stem)
64                .extension()
65                .and_then(|copc| copc.to_str())
66                .filter(|copc| copc.eq_ignore_ascii_case("copc"))
67                .ok_or_else(|| {
68                    copc_format(format!(
69                        "COPC output path must end with `.copc.laz`, got `{}`",
70                        path.display()
71                    ))
72                })?;
73            Ok(())
74        }
75        _ => Err(copc_format(format!(
76            "COPC output path must end with `.copc.laz`, got `{}`",
77            path.display()
78        ))),
79    }
80}
81
82fn bounds_from_cloud(cloud: &PointCloud) -> Result<CopcBounds, IoError> {
83    let (x, y, z) = cloud.positions3()?;
84    let mut min = [f64::INFINITY; 3];
85    let mut max = [f64::NEG_INFINITY; 3];
86    for index in 0..cloud.len() {
87        min[0] = min[0].min(f64::from(x[index]));
88        min[1] = min[1].min(f64::from(y[index]));
89        min[2] = min[2].min(f64::from(z[index]));
90        max[0] = max[0].max(f64::from(x[index]));
91        max[1] = max[1].max(f64::from(y[index]));
92        max[2] = max[2].max(f64::from(z[index]));
93    }
94    expand_degenerate_bounds(&mut min, &mut max);
95    Ok(CopcBounds::new((min[0], min[1], min[2]), (max[0], max[1], max[2])))
96}
97
98fn expand_degenerate_bounds(min: &mut [f64; 3], max: &mut [f64; 3]) {
99    const EPS: f64 = 0.001;
100    for axis in 0..3 {
101        if !(max[axis] - min[axis]).is_normal() {
102            min[axis] -= EPS;
103            max[axis] += EPS;
104        }
105    }
106}
107
108fn map_copc_writer_error(error: CopcCoreError) -> IoError {
109    match error {
110        CopcCoreError::InvalidInput(message) => copc_format(message),
111        other => copc_parse(other.to_string()),
112    }
113}
114
115struct PointCloudCopcSource<'a> {
116    cloud: &'a PointCloud,
117}
118
119impl CopcPointSource for PointCloudCopcSource<'_> {
120    fn len(&self) -> usize {
121        self.cloud.len()
122    }
123
124    fn xyz(&self, index: usize) -> (f64, f64, f64) {
125        let (x, y, z) = self.cloud.positions3().expect("validated cloud positions");
126        (f64::from(x[index]), f64::from(y[index]), f64::from(z[index]))
127    }
128
129    fn fields(&self, index: usize) -> CopcCoreResult<CopcPointFields> {
130        point_fields_from_cloud(self.cloud, index)
131    }
132}
133
134fn point_fields_from_cloud(cloud: &PointCloud, index: usize) -> CopcCoreResult<CopcPointFields> {
135    let (x, y, z) =
136        cloud.positions3().map_err(|error| CopcCoreError::InvalidInput(error.to_string()))?;
137    let schema = cloud.schema();
138    Ok(CopcPointFields {
139        x: f64::from(x[index]),
140        y: f64::from(y[index]),
141        z: f64::from(z[index]),
142        intensity: read_optional_u16(cloud, schema, FieldSemantic::Intensity, "intensity", index)?
143            .unwrap_or(0),
144        return_number: read_optional_u8(
145            cloud,
146            schema,
147            FieldSemantic::Unknown,
148            "return_number",
149            index,
150        )?
151        .unwrap_or(1),
152        number_of_returns: read_optional_u8(
153            cloud,
154            schema,
155            FieldSemantic::Unknown,
156            "number_of_returns",
157            index,
158        )?
159        .unwrap_or(1),
160        synthetic: 0,
161        key_point: 0,
162        withheld: 0,
163        overlap: 0,
164        scan_channel: 0,
165        scan_direction_flag: 0,
166        edge_of_flight_line: 0,
167        classification: read_optional_u8(
168            cloud,
169            schema,
170            FieldSemantic::Label,
171            "classification",
172            index,
173        )?
174        .unwrap_or(0),
175        user_data: 0,
176        scan_angle: 0.0,
177        point_source_id: read_optional_u16(
178            cloud,
179            schema,
180            FieldSemantic::Unknown,
181            "point_source_id",
182            index,
183        )?
184        .unwrap_or(0),
185        gps_time: read_optional_f64(cloud, schema, FieldSemantic::TimeOffset, "gps_time", index)?
186            .unwrap_or(0.0),
187        red: read_optional_u16(cloud, schema, FieldSemantic::ColorR, "red", index)?.unwrap_or(0),
188        green: read_optional_u16(cloud, schema, FieldSemantic::ColorG, "green", index)?
189            .unwrap_or(0),
190        blue: read_optional_u16(cloud, schema, FieldSemantic::ColorB, "blue", index)?.unwrap_or(0),
191    })
192}
193
194fn read_optional_u8(
195    cloud: &PointCloud,
196    schema: &PointSchema,
197    semantic: FieldSemantic,
198    fallback_name: &str,
199    index: usize,
200) -> CopcCoreResult<Option<u8>> {
201    let Some(field) = find_field(schema, semantic, fallback_name) else {
202        return Ok(None);
203    };
204    read_scalar_as_u8(cloud, field, index).map(Some)
205}
206
207fn read_optional_u16(
208    cloud: &PointCloud,
209    schema: &PointSchema,
210    semantic: FieldSemantic,
211    fallback_name: &str,
212    index: usize,
213) -> CopcCoreResult<Option<u16>> {
214    let Some(field) = find_field(schema, semantic, fallback_name) else {
215        return Ok(None);
216    };
217    read_scalar_as_u16(cloud, field, index).map(Some)
218}
219
220fn read_optional_f64(
221    cloud: &PointCloud,
222    schema: &PointSchema,
223    semantic: FieldSemantic,
224    fallback_name: &str,
225    index: usize,
226) -> CopcCoreResult<Option<f64>> {
227    let Some(field) = find_field(schema, semantic, fallback_name) else {
228        return Ok(None);
229    };
230    read_scalar_as_f64(cloud, field, index).map(Some)
231}
232
233fn find_field<'a>(
234    schema: &'a PointSchema,
235    semantic: FieldSemantic,
236    fallback_name: &str,
237) -> Option<&'a PointField> {
238    schema
239        .find_semantic(semantic)
240        .or_else(|| schema.fields().iter().find(|field| field.name == fallback_name))
241}
242
243fn read_scalar_as_u8(cloud: &PointCloud, field: &PointField, index: usize) -> CopcCoreResult<u8> {
244    let value = read_scalar_as_f64(cloud, field, index)?;
245    Ok(value.round() as u8)
246}
247
248fn read_scalar_as_u16(cloud: &PointCloud, field: &PointField, index: usize) -> CopcCoreResult<u16> {
249    let value = read_scalar_as_f64(cloud, field, index)?;
250    Ok(value.round() as u16)
251}
252
253fn read_scalar_as_f64(cloud: &PointCloud, field: &PointField, index: usize) -> CopcCoreResult<f64> {
254    use spatialrust_core::PointBuffer;
255
256    let buffer =
257        cloud.field(&field.name).map_err(|error| CopcCoreError::InvalidInput(error.to_string()))?;
258    match field.dtype {
259        DType::F32 | DType::F16 => Ok(f64::from(
260            buffer.as_f32().map_err(|error| CopcCoreError::InvalidInput(error.to_string()))?[index],
261        )),
262        DType::F64 => {
263            let PointBuffer::F64(values) = buffer else {
264                return Err(CopcCoreError::InvalidInput(format!(
265                    "unsupported dtype {:?} for field `{}`",
266                    field.dtype, field.name
267                )));
268            };
269            Ok(values[index])
270        }
271        DType::U8 => {
272            let PointBuffer::U8(values) = buffer else {
273                return Err(CopcCoreError::InvalidInput(format!(
274                    "unsupported dtype {:?} for field `{}`",
275                    field.dtype, field.name
276                )));
277            };
278            Ok(f64::from(values[index]))
279        }
280        DType::U16 => {
281            let PointBuffer::U16(values) = buffer else {
282                return Err(CopcCoreError::InvalidInput(format!(
283                    "unsupported dtype {:?} for field `{}`",
284                    field.dtype, field.name
285                )));
286            };
287            Ok(f64::from(values[index]))
288        }
289        DType::I32 => {
290            let PointBuffer::I32(values) = buffer else {
291                return Err(CopcCoreError::InvalidInput(format!(
292                    "unsupported dtype {:?} for field `{}`",
293                    field.dtype, field.name
294                )));
295            };
296            Ok(f64::from(values[index]))
297        }
298        DType::U32 => {
299            let PointBuffer::U32(values) = buffer else {
300                return Err(CopcCoreError::InvalidInput(format!(
301                    "unsupported dtype {:?} for field `{}`",
302                    field.dtype, field.name
303                )));
304            };
305            Ok(f64::from(values[index]))
306        }
307    }
308}
309
310#[cfg(test)]
311mod tests {
312    use std::path::Path;
313
314    use super::{validate_copc_output_path, write_copc_file};
315    use crate::copc::read_copc_file;
316    use spatialrust_core::{HasPositions3, PointCloudBuilder};
317
318    #[test]
319    fn rejects_non_copc_extension() {
320        let mut builder = PointCloudBuilder::xyz();
321        builder.push_point([0.0, 0.0, 0.0]).unwrap();
322        let cloud = builder.build().unwrap();
323        let path =
324            std::env::temp_dir().join(format!("spatialrust_bad_ext_{}.laz", std::process::id()));
325        let error = write_copc_file(&path, &cloud).unwrap_err();
326        assert!(matches!(error, crate::IoError::CopcFormat(_)));
327    }
328
329    #[test]
330    fn validate_copc_output_path_accepts_copc_laz_suffix() {
331        assert!(validate_copc_output_path(Path::new("scan.copc.laz")).is_ok());
332        assert!(validate_copc_output_path(Path::new("scan.laz")).is_err());
333    }
334
335    #[test]
336    fn roundtrip_xyzi_cloud() {
337        use spatialrust_core::{HasIntensity, StandardSchemas};
338
339        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyzi());
340        builder.push_point([1.0, 2.0, 3.0, 128.0]).unwrap();
341        builder.push_point([4.0, 5.0, 6.0, 64.0]).unwrap();
342        let cloud = builder.build().unwrap();
343
344        let path = std::env::temp_dir()
345            .join(format!("spatialrust_copc_xyzi_{}.copc.laz", std::process::id()));
346        write_copc_file(&path, &cloud).expect("write copc");
347        let loaded = read_copc_file(&path).expect("read copc");
348        let _ = std::fs::remove_file(&path);
349
350        assert_eq!(loaded.len(), cloud.len());
351        let (src_x, src_y, src_z) = cloud.positions3().unwrap();
352        let (out_x, out_y, out_z) = loaded.positions3().unwrap();
353        let src_i = cloud.intensity().unwrap();
354        let out_i = loaded.intensity().unwrap();
355        for index in 0..cloud.len() {
356            assert!((out_x[index] - src_x[index]).abs() < 1e-3);
357            assert!((out_y[index] - src_y[index]).abs() < 1e-3);
358            assert!((out_z[index] - src_z[index]).abs() < 1e-3);
359            assert!((out_i[index] - src_i[index]).abs() < 1.0);
360        }
361    }
362
363    #[test]
364    fn roundtrip_xyzrgb_cloud() {
365        use spatialrust_core::{PointBuffer, StandardSchemas};
366
367        let mut builder = PointCloudBuilder::new(StandardSchemas::point_xyzrgb());
368        builder.push_point([0.0, 0.0, 0.0, 10.0, 20.0, 30.0]).unwrap();
369        builder.push_point([1.0, 1.0, 1.0, 40.0, 50.0, 60.0]).unwrap();
370        let cloud = builder.build().unwrap();
371
372        let path = std::env::temp_dir()
373            .join(format!("spatialrust_copc_xyzrgb_{}.copc.laz", std::process::id()));
374        write_copc_file(&path, &cloud).expect("write copc");
375        let loaded = read_copc_file(&path).expect("read copc");
376        let _ = std::fs::remove_file(&path);
377
378        assert_eq!(loaded.len(), cloud.len());
379        let (src_x, src_y, src_z) = cloud.positions3().unwrap();
380        let (out_x, out_y, out_z) = loaded.positions3().unwrap();
381        for index in 0..cloud.len() {
382            assert!((out_x[index] - src_x[index]).abs() < 1e-3);
383            assert!((out_y[index] - src_y[index]).abs() < 1e-3);
384            assert!((out_z[index] - src_z[index]).abs() < 1e-3);
385        }
386
387        let PointBuffer::U8(src_r) = cloud.field("r").unwrap() else {
388            panic!("expected u8 red channel");
389        };
390        let PointBuffer::U8(src_g) = cloud.field("g").unwrap() else {
391            panic!("expected u8 green channel");
392        };
393        let PointBuffer::U8(src_b) = cloud.field("b").unwrap() else {
394            panic!("expected u8 blue channel");
395        };
396        let PointBuffer::U16(out_r) = loaded.field("red").unwrap() else {
397            panic!("expected u16 red channel");
398        };
399        let PointBuffer::U16(out_g) = loaded.field("green").unwrap() else {
400            panic!("expected u16 green channel");
401        };
402        let PointBuffer::U16(out_b) = loaded.field("blue").unwrap() else {
403            panic!("expected u16 blue channel");
404        };
405        for index in 0..cloud.len() {
406            assert_eq!(u16::from(src_r[index]), out_r[index]);
407            assert_eq!(u16::from(src_g[index]), out_g[index]);
408            assert_eq!(u16::from(src_b[index]), out_b[index]);
409        }
410    }
411
412    #[test]
413    fn roundtrip_xyz_cloud() {
414        let mut builder = PointCloudBuilder::xyz();
415        builder.push_point([1.0, 2.0, 3.0]).unwrap();
416        builder.push_point([1.5, 2.5, 3.5]).unwrap();
417        let cloud = builder.build().unwrap();
418
419        let path = std::env::temp_dir()
420            .join(format!("spatialrust_copc_roundtrip_{}.copc.laz", std::process::id()));
421        write_copc_file(&path, &cloud).expect("write copc");
422        let loaded = read_copc_file(&path).expect("read copc");
423        let _ = std::fs::remove_file(&path);
424
425        assert_eq!(loaded.len(), cloud.len());
426        let (src_x, src_y, src_z) = cloud.positions3().unwrap();
427        let (out_x, out_y, out_z) = loaded.positions3().unwrap();
428        for index in 0..cloud.len() {
429            assert!((out_x[index] - src_x[index]).abs() < 1e-3);
430            assert!((out_y[index] - src_y[index]).abs() < 1e-3);
431            assert!((out_z[index] - src_z[index]).abs() < 1e-3);
432        }
433    }
434
435    #[test]
436    fn bounds_query_excludes_out_of_region_points() {
437        use crate::copc::{read_copc_file_with_query, CopcBounds, CopcQuery};
438
439        let mut builder = PointCloudBuilder::xyz();
440        for x in 0..10 {
441            for y in 0..10 {
442                builder.push_point([x as f32 * 0.1, y as f32 * 0.1, 0.0]).unwrap();
443            }
444        }
445        builder.push_point([0.0, 0.0, 0.5]).unwrap();
446        let cloud = builder.build().unwrap();
447
448        let path = std::env::temp_dir()
449            .join(format!("spatialrust_copc_bounds_query_{}.copc.laz", std::process::id()));
450        write_copc_file(&path, &cloud).expect("write copc");
451
452        let bounds = CopcBounds::from_ranges((0.0, 0.85), (0.0, 0.85), (-0.01, 0.01));
453        let loaded = read_copc_file_with_query(&path, &CopcQuery::bounds(bounds)).expect("query");
454        let _ = std::fs::remove_file(&path);
455
456        assert!(loaded.len() < cloud.len());
457        assert!(loaded.len() >= 10);
458        assert!(loaded
459            .schema()
460            .find_semantic(spatialrust_core::FieldSemantic::PositionX)
461            .is_some());
462        let (x, _, _) = loaded.positions3().expect("positions3");
463        assert_eq!(x.len(), loaded.len());
464    }
465}
spatialrust_io/copc/writer.rs

spatialrust_io/copc/
writer.rs
