spatialrust_vision/
spatial.rs

1//! Bridges dense vision outputs into SpatialRust camera and point-cloud APIs.
2
3use spatialrust_camera::{depth_to_point_cloud, DepthConversionOptions, PinholeCamera};
4use spatialrust_core::{PointBuffer, PointBufferSet, PointCloud, SpatialMetadata, StandardSchemas};
5
6use crate::{ConfidenceMap, DepthMap, PointMap, VisionError, VisionResult};
7
8/// Unprojects a depth map with a calibrated camera into an XYZ point cloud.
9pub fn depth_map_to_point_cloud(
10    depth: &DepthMap,
11    camera: &PinholeCamera,
12    options: DepthConversionOptions,
13) -> VisionResult<PointCloud> {
14    depth_to_point_cloud(depth.image().view(), camera, options)
15        .map_err(|error| VisionError::InvalidParameter(error.to_string()))
16}
17
18/// Flattens valid point-map pixels into an XYZ cloud, optionally filtering by confidence.
19pub fn point_map_to_point_cloud(
20    point_map: &PointMap,
21    confidence: Option<&ConfidenceMap>,
22    min_confidence: f32,
23) -> VisionResult<PointCloud> {
24    if !min_confidence.is_finite() || !(0.0..=1.0).contains(&min_confidence) {
25        return Err(VisionError::InvalidParameter(
26            "minimum confidence must be finite and in [0, 1]".to_owned(),
27        ));
28    }
29    if let Some(confidence) = confidence {
30        if confidence.width() != point_map.width() || confidence.height() != point_map.height() {
31            return Err(VisionError::ShapeMismatch(
32                "point map and confidence map dimensions must match".to_owned(),
33            ));
34        }
35    }
36
37    let capacity = point_map.width().saturating_mul(point_map.height());
38    let mut xs = Vec::with_capacity(capacity);
39    let mut ys = Vec::with_capacity(capacity);
40    let mut zs = Vec::with_capacity(capacity);
41    for y in 0..point_map.height() {
42        for x in 0..point_map.width() {
43            let point = point_map.image().get(x, y).expect("point-map coordinate in bounds");
44            if !point.iter().all(|value| value.is_finite()) {
45                continue;
46            }
47            if let Some(confidence) = confidence {
48                let score =
49                    confidence.image().get(x, y).expect("confidence coordinate in bounds")[0];
50                if score < min_confidence {
51                    continue;
52                }
53            }
54            xs.push(point[0]);
55            ys.push(point[1]);
56            zs.push(point[2]);
57        }
58    }
59
60    let mut buffers = PointBufferSet::new();
61    buffers.insert("x", PointBuffer::from_f32(xs));
62    buffers.insert("y", PointBuffer::from_f32(ys));
63    buffers.insert("z", PointBuffer::from_f32(zs));
64    PointCloud::try_from_parts(StandardSchemas::point_xyz(), buffers, SpatialMetadata::default())
65        .map_err(|error| VisionError::InvalidParameter(error.to_string()))
66}
67
68#[cfg(test)]
69mod tests {
70    use super::{depth_map_to_point_cloud, point_map_to_point_cloud};
71    use crate::{ConfidenceMap, DepthMap, PointMap};
72    use spatialrust_camera::{CameraIntrinsics, PinholeCamera};
73
74    #[test]
75    fn depth_map_uses_camera_unprojection() {
76        let depth = DepthMap::try_new(2, 1, vec![1.0, 2.0]).unwrap();
77        let camera =
78            PinholeCamera::new(CameraIntrinsics::try_new(2.0, 2.0, 0.0, 0.0, 2, 1).unwrap());
79        let cloud = depth_map_to_point_cloud(&depth, &camera, Default::default()).unwrap();
80        assert_eq!(cloud.field("x").unwrap().as_f32().unwrap(), &[0.0, 1.0]);
81        assert_eq!(cloud.field("z").unwrap().as_f32().unwrap(), &[1.0, 2.0]);
82    }
83
84    #[test]
85    fn point_map_filters_invalid_and_low_confidence_points() {
86        let points =
87            PointMap::try_new(3, 1, vec![0.0, 0.0, 1.0, 1.0, 0.0, 1.0, f32::NAN, 0.0, 1.0])
88                .unwrap();
89        let confidence = ConfidenceMap::try_new(3, 1, vec![0.9, 0.1, 1.0]).unwrap();
90        let cloud = point_map_to_point_cloud(&points, Some(&confidence), 0.5).unwrap();
91        assert_eq!(cloud.len(), 1);
92        assert_eq!(cloud.field("x").unwrap().as_f32().unwrap(), &[0.0]);
93    }
94}
spatialrust_vision/spatial.rs

spatialrust_vision/
spatial.rs
