spatialrust_camera/
rgbd.rs

1use crate::{CameraError, PinholeCamera};
2use spatialrust_core::{
3    PointBuffer, PointBufferSet, PointCloud, SpatialError, SpatialMetadata, StandardSchemas,
4};
5use spatialrust_image::ImageView;
6use spatialrust_math::Vec2;
7
8/// Errors from RGB-D conversion.
9#[derive(Debug, thiserror::Error)]
10pub enum RgbdError {
11    /// Camera dimensions and depth image dimensions differ.
12    #[error("depth dimensions {image_width}x{image_height} do not match camera dimensions {camera_width}x{camera_height}")]
13    DepthDimensionMismatch {
14        /// Depth image width.
15        image_width: usize,
16        /// Depth image height.
17        image_height: usize,
18        /// Calibrated camera width.
19        camera_width: usize,
20        /// Calibrated camera height.
21        camera_height: usize,
22    },
23    /// RGB and depth image dimensions differ.
24    #[error("color dimensions {color_width}x{color_height} do not match depth dimensions {depth_width}x{depth_height}")]
25    ColorDimensionMismatch {
26        /// Color image width.
27        color_width: usize,
28        /// Color image height.
29        color_height: usize,
30        /// Depth image width.
31        depth_width: usize,
32        /// Depth image height.
33        depth_height: usize,
34    },
35    /// Conversion settings were invalid.
36    #[error("{0}")]
37    InvalidOptions(String),
38    /// Camera geometry failed.
39    #[error(transparent)]
40    Camera(#[from] CameraError),
41    /// Point cloud construction failed.
42    #[error(transparent)]
43    Spatial(#[from] SpatialError),
44}
45
46/// Controls conversion from stored depth values to metric camera coordinates.
47#[derive(Clone, Copy, Debug, PartialEq)]
48pub struct DepthConversionOptions {
49    /// Multiplier converting each stored depth value to meters.
50    pub depth_scale: f32,
51    /// Inclusive minimum accepted metric depth.
52    pub min_depth: f32,
53    /// Inclusive maximum accepted metric depth.
54    pub max_depth: f32,
55}
56
57impl Default for DepthConversionOptions {
58    fn default() -> Self {
59        Self { depth_scale: 1.0, min_depth: f32::EPSILON, max_depth: f32::INFINITY }
60    }
61}
62
63impl DepthConversionOptions {
64    fn validate(self) -> Result<(), RgbdError> {
65        if !self.depth_scale.is_finite() || self.depth_scale <= 0.0 {
66            return Err(RgbdError::InvalidOptions(
67                "depth_scale must be finite and positive".to_owned(),
68            ));
69        }
70        if !self.min_depth.is_finite()
71            || self.min_depth < 0.0
72            || self.max_depth.is_nan()
73            || self.max_depth < self.min_depth
74        {
75            return Err(RgbdError::InvalidOptions(
76                "depth range must be ordered, non-negative, and not NaN".to_owned(),
77            ));
78        }
79        Ok(())
80    }
81}
82
83fn validate_depth(depth: ImageView<'_, f32, 1>, camera: &PinholeCamera) -> Result<(), RgbdError> {
84    let intrinsics = camera.intrinsics;
85    if depth.width() != intrinsics.width || depth.height() != intrinsics.height {
86        return Err(RgbdError::DepthDimensionMismatch {
87            image_width: depth.width(),
88            image_height: depth.height(),
89            camera_width: intrinsics.width,
90            camera_height: intrinsics.height,
91        });
92    }
93    Ok(())
94}
95
96#[derive(Clone, Copy)]
97struct FastPinholeF32 {
98    inv_fx: f32,
99    inv_fy: f32,
100    cx: f32,
101    cy: f32,
102}
103
104impl FastPinholeF32 {
105    fn from_camera(camera: &PinholeCamera) -> Self {
106        Self {
107            inv_fx: (1.0 / camera.intrinsics.fx) as f32,
108            inv_fy: (1.0 / camera.intrinsics.fy) as f32,
109            cx: camera.intrinsics.cx as f32,
110            cy: camera.intrinsics.cy as f32,
111        }
112    }
113}
114
115/// Converts depth into a dense `H×W×3` XYZ buffer (row-major interleaved).
116///
117/// Invalid / out-of-range depths become `NaN` triples, matching OpenCV
118/// `rgbd.depthTo3d` dense semantics for undistorted pinhole cameras.
119pub fn depth_to_xyz_dense(
120    depth: ImageView<'_, f32, 1>,
121    camera: &PinholeCamera,
122    options: DepthConversionOptions,
123) -> Result<Vec<f32>, RgbdError> {
124    let len = depth.width().saturating_mul(depth.height()).saturating_mul(3);
125    let mut out = vec![0.0f32; len];
126    depth_to_xyz_dense_into(depth, camera, options, &mut out)?;
127    Ok(out)
128}
129
130/// Fills a caller-provided dense `H×W×3` XYZ buffer (length must be `H*W*3`).
131pub fn depth_to_xyz_dense_into(
132    depth: ImageView<'_, f32, 1>,
133    camera: &PinholeCamera,
134    options: DepthConversionOptions,
135    out: &mut [f32],
136) -> Result<(), RgbdError> {
137    validate_depth(depth, camera)?;
138    options.validate()?;
139    let expected = depth.width().saturating_mul(depth.height()).saturating_mul(3);
140    if out.len() != expected {
141        return Err(RgbdError::InvalidOptions(format!(
142            "dense XYZ buffer length must be {expected}, found {}",
143            out.len()
144        )));
145    }
146    if camera.distortion.is_identity() {
147        fill_xyz_dense_identity(depth, camera, options, out);
148    } else {
149        fill_xyz_dense_distorted(depth, camera, options, out)?;
150    }
151    Ok(())
152}
153
154fn fill_xyz_dense_identity(
155    depth: ImageView<'_, f32, 1>,
156    camera: &PinholeCamera,
157    options: DepthConversionOptions,
158    out: &mut [f32],
159) {
160    let pin = FastPinholeF32::from_camera(camera);
161    let width = depth.width();
162    let height = depth.height();
163    let scale = options.depth_scale;
164    let min_d = options.min_depth;
165    let max_d = options.max_depth;
166    let scale_is_one = scale == 1.0;
167    let max_is_inf = !max_d.is_finite();
168    // Per-column `(x - cx) / fx` factors so the inner loop is multiply-add only.
169    let x_mul: Vec<f32> = (0..width).map(|x| (x as f32 - pin.cx) * pin.inv_fx).collect();
170    let pixels = width.saturating_mul(height);
171    let threads = std::thread::available_parallelism().map(|n| n.get().clamp(1, 8)).unwrap_or(1);
172    // Thread spawn overhead dominates below ~2M pixels on typical hosts.
173    if threads == 1 || pixels < 2_000_000 {
174        fill_xyz_dense_identity_rows(
175            depth,
176            pin,
177            &x_mul,
178            0,
179            height,
180            width,
181            scale,
182            min_d,
183            max_d,
184            scale_is_one,
185            max_is_inf,
186            out,
187        );
188        return;
189    }
190
191    let row_stride = width * 3;
192    let chunk = height.div_ceil(threads);
193    std::thread::scope(|scope| {
194        let mut rest = out;
195        let mut y0 = 0usize;
196        while y0 < height {
197            let y1 = (y0 + chunk).min(height);
198            let take = (y1 - y0) * row_stride;
199            let (chunk_out, next) = rest.split_at_mut(take);
200            rest = next;
201            let x_mul = &x_mul;
202            scope.spawn(move || {
203                fill_xyz_dense_identity_rows(
204                    depth,
205                    pin,
206                    x_mul,
207                    y0,
208                    y1,
209                    width,
210                    scale,
211                    min_d,
212                    max_d,
213                    scale_is_one,
214                    max_is_inf,
215                    chunk_out,
216                );
217            });
218            y0 = y1;
219        }
220    });
221}
222
223#[allow(clippy::too_many_arguments)]
224fn fill_xyz_dense_identity_rows(
225    depth: ImageView<'_, f32, 1>,
226    pin: FastPinholeF32,
227    x_mul: &[f32],
228    y0: usize,
229    y1: usize,
230    width: usize,
231    scale: f32,
232    min_d: f32,
233    max_d: f32,
234    scale_is_one: bool,
235    max_is_inf: bool,
236    out: &mut [f32],
237) {
238    #[cfg(any(target_arch = "x86_64", target_arch = "x86"))]
239    {
240        if scale_is_one && max_is_inf && is_x86_feature_detected!("avx2") {
241            // SAFETY: feature detection above; slices are row-validated.
242            unsafe {
243                fill_xyz_dense_identity_rows_avx2(depth, pin, x_mul, y0, y1, width, min_d, out);
244            }
245            return;
246        }
247    }
248
249    let mut o = 0usize;
250    for y in y0..y1 {
251        let row = depth.row(y).expect("validated row");
252        let y_mul = (y as f32 - pin.cy) * pin.inv_fy;
253        for x in 0..width {
254            let raw = row[x];
255            let meters = if scale_is_one { raw } else { raw * scale };
256            // Ordered compares treat NaN as invalid (NaN >= x is false).
257            let z = if max_is_inf {
258                if meters >= min_d {
259                    meters
260                } else {
261                    f32::NAN
262                }
263            } else if meters >= min_d && meters <= max_d {
264                meters
265            } else {
266                f32::NAN
267            };
268            out[o] = x_mul[x] * z;
269            out[o + 1] = y_mul * z;
270            out[o + 2] = z;
271            o += 3;
272        }
273    }
274}
275
276#[cfg(any(target_arch = "x86_64", target_arch = "x86"))]
277#[target_feature(enable = "avx2")]
278#[allow(unsafe_code, clippy::too_many_arguments)]
279unsafe fn fill_xyz_dense_identity_rows_avx2(
280    depth: ImageView<'_, f32, 1>,
281    pin: FastPinholeF32,
282    x_mul: &[f32],
283    y0: usize,
284    y1: usize,
285    width: usize,
286    min_d: f32,
287    out: &mut [f32],
288) {
289    #[cfg(target_arch = "x86")]
290    use std::arch::x86::*;
291    #[cfg(target_arch = "x86_64")]
292    use std::arch::x86_64::*;
293
294    let min_v = _mm256_set1_ps(min_d);
295    let nan_v = _mm256_set1_ps(f32::NAN);
296    let mut o = 0usize;
297    for y in y0..y1 {
298        let row = depth.row(y).expect("validated row");
299        let y_mul = _mm256_set1_ps((y as f32 - pin.cy) * pin.inv_fy);
300        let mut x = 0usize;
301        while x + 8 <= width {
302            let z_raw = _mm256_loadu_ps(row.as_ptr().add(x));
303            // _CMP_GE_OQ: ordered → NaN lanes become false.
304            let valid = _mm256_cmp_ps(z_raw, min_v, _CMP_GE_OQ);
305            let z = _mm256_blendv_ps(nan_v, z_raw, valid);
306            let xm = _mm256_loadu_ps(x_mul.as_ptr().add(x));
307            let xs = _mm256_mul_ps(xm, z);
308            let ys = _mm256_mul_ps(y_mul, z);
309
310            let mut xa = [0.0f32; 8];
311            let mut ya = [0.0f32; 8];
312            let mut za = [0.0f32; 8];
313            _mm256_storeu_ps(xa.as_mut_ptr(), xs);
314            _mm256_storeu_ps(ya.as_mut_ptr(), ys);
315            _mm256_storeu_ps(za.as_mut_ptr(), z);
316            for i in 0..8 {
317                *out.get_unchecked_mut(o) = xa[i];
318                *out.get_unchecked_mut(o + 1) = ya[i];
319                *out.get_unchecked_mut(o + 2) = za[i];
320                o += 3;
321            }
322            x += 8;
323        }
324        let y_mul_s = (y as f32 - pin.cy) * pin.inv_fy;
325        while x < width {
326            let meters = *row.get_unchecked(x);
327            let z = if meters >= min_d { meters } else { f32::NAN };
328            *out.get_unchecked_mut(o) = *x_mul.get_unchecked(x) * z;
329            *out.get_unchecked_mut(o + 1) = y_mul_s * z;
330            *out.get_unchecked_mut(o + 2) = z;
331            o += 3;
332            x += 1;
333        }
334    }
335}
336
337fn fill_xyz_dense_distorted(
338    depth: ImageView<'_, f32, 1>,
339    camera: &PinholeCamera,
340    options: DepthConversionOptions,
341    out: &mut [f32],
342) -> Result<(), RgbdError> {
343    let width = depth.width();
344    let mut o = 0usize;
345    for y in 0..depth.height() {
346        for x in 0..width {
347            let meters =
348                depth.get(x, y).expect("validated image coordinates")[0] * options.depth_scale;
349            if meters.is_finite() && meters >= options.min_depth && meters <= options.max_depth {
350                let point = camera.unproject(Vec2 { x: x as f64, y: y as f64 }, meters as f64)?;
351                out[o] = point.x as f32;
352                out[o + 1] = point.y as f32;
353                out[o + 2] = point.z as f32;
354            } else {
355                out[o] = f32::NAN;
356                out[o + 1] = f32::NAN;
357                out[o + 2] = f32::NAN;
358            }
359            o += 3;
360        }
361    }
362    Ok(())
363}
364
365/// Converts an aligned depth image into an XYZ point cloud.
366///
367/// Zero, non-finite, and out-of-range depths are omitted.
368pub fn depth_to_point_cloud(
369    depth: ImageView<'_, f32, 1>,
370    camera: &PinholeCamera,
371    options: DepthConversionOptions,
372) -> Result<PointCloud, RgbdError> {
373    validate_depth(depth, camera)?;
374    options.validate()?;
375    let capacity = depth.width().saturating_mul(depth.height());
376    let mut xs = Vec::with_capacity(capacity);
377    let mut ys = Vec::with_capacity(capacity);
378    let mut zs = Vec::with_capacity(capacity);
379    if camera.distortion.is_identity() {
380        pack_xyz_identity(depth, camera, options, &mut xs, &mut ys, &mut zs);
381    } else {
382        for y in 0..depth.height() {
383            for x in 0..depth.width() {
384                let meters =
385                    depth.get(x, y).expect("validated image coordinates")[0] * options.depth_scale;
386                if !meters.is_finite() || meters < options.min_depth || meters > options.max_depth {
387                    continue;
388                }
389                let point = camera.unproject(Vec2 { x: x as f64, y: y as f64 }, meters as f64)?;
390                xs.push(point.x as f32);
391                ys.push(point.y as f32);
392                zs.push(point.z as f32);
393            }
394        }
395    }
396    let mut buffers = PointBufferSet::new();
397    buffers.insert("x", PointBuffer::from_f32(xs));
398    buffers.insert("y", PointBuffer::from_f32(ys));
399    buffers.insert("z", PointBuffer::from_f32(zs));
400    Ok(PointCloud::try_from_parts(
401        StandardSchemas::point_xyz(),
402        buffers,
403        SpatialMetadata::default(),
404    )?)
405}
406
407fn pack_xyz_identity(
408    depth: ImageView<'_, f32, 1>,
409    camera: &PinholeCamera,
410    options: DepthConversionOptions,
411    xs: &mut Vec<f32>,
412    ys: &mut Vec<f32>,
413    zs: &mut Vec<f32>,
414) {
415    let pin = FastPinholeF32::from_camera(camera);
416    let width = depth.width();
417    let x_mul: Vec<f32> = (0..width).map(|x| (x as f32 - pin.cx) * pin.inv_fx).collect();
418    let scale = options.depth_scale;
419    let min_d = options.min_depth;
420    let max_d = options.max_depth;
421    let scale_is_one = scale == 1.0;
422    let max_is_inf = !max_d.is_finite();
423    let capacity = xs.capacity();
424    debug_assert_eq!(xs.len(), 0);
425    // SAFETY: reserved capacity; write at most `capacity` then set lengths.
426    unsafe {
427        let xs_p = xs.as_mut_ptr();
428        let ys_p = ys.as_mut_ptr();
429        let zs_p = zs.as_mut_ptr();
430        let mut n = 0usize;
431        for y in 0..depth.height() {
432            let row = depth.row(y).expect("validated row");
433            let y_mul = (y as f32 - pin.cy) * pin.inv_fy;
434            for x in 0..width {
435                let meters = if scale_is_one {
436                    *row.get_unchecked(x)
437                } else {
438                    *row.get_unchecked(x) * scale
439                };
440                let valid =
441                    if max_is_inf { meters >= min_d } else { meters >= min_d && meters <= max_d };
442                if !valid {
443                    continue;
444                }
445                debug_assert!(n < capacity);
446                *xs_p.add(n) = *x_mul.get_unchecked(x) * meters;
447                *ys_p.add(n) = y_mul * meters;
448                *zs_p.add(n) = meters;
449                n += 1;
450            }
451        }
452        xs.set_len(n);
453        ys.set_len(n);
454        zs.set_len(n);
455    }
456}
457
458/// Converts aligned RGB and depth images into an XYZRGB point cloud.
459///
460/// RGB channel order is preserved as `r`, `g`, and `b` `u8` fields.
461pub fn rgbd_to_point_cloud(
462    depth: ImageView<'_, f32, 1>,
463    color: ImageView<'_, u8, 3>,
464    camera: &PinholeCamera,
465    options: DepthConversionOptions,
466) -> Result<PointCloud, RgbdError> {
467    validate_depth(depth, camera)?;
468    options.validate()?;
469    if color.width() != depth.width() || color.height() != depth.height() {
470        return Err(RgbdError::ColorDimensionMismatch {
471            color_width: color.width(),
472            color_height: color.height(),
473            depth_width: depth.width(),
474            depth_height: depth.height(),
475        });
476    }
477
478    let capacity = depth.width().saturating_mul(depth.height());
479    let mut xs = Vec::with_capacity(capacity);
480    let mut ys = Vec::with_capacity(capacity);
481    let mut zs = Vec::with_capacity(capacity);
482    let mut rs = Vec::with_capacity(capacity);
483    let mut gs = Vec::with_capacity(capacity);
484    let mut bs = Vec::with_capacity(capacity);
485    if camera.distortion.is_identity() {
486        pack_xyzrgb_identity(
487            depth, color, camera, options, &mut xs, &mut ys, &mut zs, &mut rs, &mut gs, &mut bs,
488        );
489    } else {
490        for y in 0..depth.height() {
491            for x in 0..depth.width() {
492                let meters =
493                    depth.get(x, y).expect("validated image coordinates")[0] * options.depth_scale;
494                if !meters.is_finite() || meters < options.min_depth || meters > options.max_depth {
495                    continue;
496                }
497                let point = camera.unproject(Vec2 { x: x as f64, y: y as f64 }, meters as f64)?;
498                let rgb = color.get(x, y).expect("validated image coordinates");
499                xs.push(point.x as f32);
500                ys.push(point.y as f32);
501                zs.push(point.z as f32);
502                rs.push(rgb[0]);
503                gs.push(rgb[1]);
504                bs.push(rgb[2]);
505            }
506        }
507    }
508    let mut buffers = PointBufferSet::new();
509    buffers.insert("x", PointBuffer::from_f32(xs));
510    buffers.insert("y", PointBuffer::from_f32(ys));
511    buffers.insert("z", PointBuffer::from_f32(zs));
512    buffers.insert("r", PointBuffer::U8(rs));
513    buffers.insert("g", PointBuffer::U8(gs));
514    buffers.insert("b", PointBuffer::U8(bs));
515    Ok(PointCloud::try_from_parts(
516        StandardSchemas::point_xyzrgb(),
517        buffers,
518        SpatialMetadata::default(),
519    )?)
520}
521
522#[allow(clippy::too_many_arguments)]
523fn pack_xyzrgb_identity(
524    depth: ImageView<'_, f32, 1>,
525    color: ImageView<'_, u8, 3>,
526    camera: &PinholeCamera,
527    options: DepthConversionOptions,
528    xs: &mut Vec<f32>,
529    ys: &mut Vec<f32>,
530    zs: &mut Vec<f32>,
531    rs: &mut Vec<u8>,
532    gs: &mut Vec<u8>,
533    bs: &mut Vec<u8>,
534) {
535    let pin = FastPinholeF32::from_camera(camera);
536    let width = depth.width();
537    let x_mul: Vec<f32> = (0..width).map(|x| (x as f32 - pin.cx) * pin.inv_fx).collect();
538    let scale = options.depth_scale;
539    let min_d = options.min_depth;
540    let max_d = options.max_depth;
541    let scale_is_one = scale == 1.0;
542    let max_is_inf = !max_d.is_finite();
543
544    // Bulk pointer writes beat six `push` hot paths once capacity is reserved.
545    let count = pack_xyzrgb_identity_raw(
546        depth,
547        color,
548        &x_mul,
549        pin,
550        scale,
551        min_d,
552        max_d,
553        scale_is_one,
554        max_is_inf,
555        xs,
556        ys,
557        zs,
558        rs,
559        gs,
560        bs,
561    );
562    debug_assert_eq!(count, xs.len());
563}
564
565#[allow(clippy::too_many_arguments, unsafe_code)]
566fn pack_xyzrgb_identity_raw(
567    depth: ImageView<'_, f32, 1>,
568    color: ImageView<'_, u8, 3>,
569    x_mul: &[f32],
570    pin: FastPinholeF32,
571    scale: f32,
572    min_d: f32,
573    max_d: f32,
574    scale_is_one: bool,
575    max_is_inf: bool,
576    xs: &mut Vec<f32>,
577    ys: &mut Vec<f32>,
578    zs: &mut Vec<f32>,
579    rs: &mut Vec<u8>,
580    gs: &mut Vec<u8>,
581    bs: &mut Vec<u8>,
582) -> usize {
583    let width = depth.width();
584    let height = depth.height();
585    let capacity = xs.capacity();
586    debug_assert!(ys.capacity() >= capacity);
587    debug_assert!(zs.capacity() >= capacity);
588    debug_assert!(rs.capacity() >= capacity);
589    debug_assert!(gs.capacity() >= capacity);
590    debug_assert!(bs.capacity() >= capacity);
591    debug_assert_eq!(xs.len(), 0);
592
593    // SAFETY: all vectors have `capacity` reserved and start empty; we write at
594    // most `capacity` elements then set lengths together.
595    unsafe {
596        let xs_p = xs.as_mut_ptr();
597        let ys_p = ys.as_mut_ptr();
598        let zs_p = zs.as_mut_ptr();
599        let rs_p = rs.as_mut_ptr();
600        let gs_p = gs.as_mut_ptr();
601        let bs_p = bs.as_mut_ptr();
602        let mut n = 0usize;
603        for y in 0..height {
604            let depth_row = depth.row(y).expect("validated row");
605            let color_row = color.row(y).expect("validated row");
606            let y_mul = (y as f32 - pin.cy) * pin.inv_fy;
607            for x in 0..width {
608                let meters = if scale_is_one {
609                    *depth_row.get_unchecked(x)
610                } else {
611                    *depth_row.get_unchecked(x) * scale
612                };
613                let valid =
614                    if max_is_inf { meters >= min_d } else { meters >= min_d && meters <= max_d };
615                if !valid {
616                    continue;
617                }
618                debug_assert!(n < capacity);
619                *xs_p.add(n) = *x_mul.get_unchecked(x) * meters;
620                *ys_p.add(n) = y_mul * meters;
621                *zs_p.add(n) = meters;
622                let c = x * 3;
623                *rs_p.add(n) = *color_row.get_unchecked(c);
624                *gs_p.add(n) = *color_row.get_unchecked(c + 1);
625                *bs_p.add(n) = *color_row.get_unchecked(c + 2);
626                n += 1;
627            }
628        }
629        xs.set_len(n);
630        ys.set_len(n);
631        zs.set_len(n);
632        rs.set_len(n);
633        gs.set_len(n);
634        bs.set_len(n);
635        n
636    }
637}
638
639#[cfg(test)]
640mod tests {
641    use super::{
642        depth_to_point_cloud, depth_to_xyz_dense, rgbd_to_point_cloud, DepthConversionOptions,
643    };
644    use crate::{CameraIntrinsics, PinholeCamera};
645    use spatialrust_core::PointBuffer;
646    use spatialrust_image::Image;
647
648    fn camera() -> PinholeCamera {
649        PinholeCamera::new(CameraIntrinsics::try_new(2.0, 2.0, 0.0, 0.0, 2, 2).unwrap())
650    }
651
652    #[test]
653    fn skips_invalid_depth_and_unprojects() {
654        let depth = Image::<f32, 1>::try_new(2, 2, vec![1.0, 0.0, f32::NAN, 2.0]).unwrap();
655        let cloud = depth_to_point_cloud(depth.view(), &camera(), Default::default()).unwrap();
656        assert_eq!(cloud.len(), 2);
657        assert_eq!(cloud.field("x").unwrap().as_f32().unwrap(), &[0.0, 1.0]);
658        assert_eq!(cloud.field("y").unwrap().as_f32().unwrap(), &[0.0, 1.0]);
659        assert_eq!(cloud.field("z").unwrap().as_f32().unwrap(), &[1.0, 2.0]);
660    }
661
662    #[test]
663    fn dense_xyz_uses_nan_for_invalid() {
664        let depth = Image::<f32, 1>::try_new(2, 2, vec![1.0, 0.0, f32::NAN, 2.0]).unwrap();
665        let xyz = depth_to_xyz_dense(depth.view(), &camera(), Default::default()).unwrap();
666        assert_eq!(xyz.len(), 12);
667        assert_eq!(&xyz[0..3], &[0.0, 0.0, 1.0]);
668        assert!(xyz[3].is_nan() && xyz[4].is_nan() && xyz[5].is_nan());
669        assert!(xyz[6].is_nan());
670        assert_eq!(&xyz[9..12], &[1.0, 1.0, 2.0]);
671    }
672
673    #[test]
674    fn rgb_fields_follow_valid_depths() {
675        let depth = Image::<f32, 1>::try_new(2, 2, vec![1.0, 0.0, 3.0, 2.0]).unwrap();
676        let color =
677            Image::<u8, 3>::try_new(2, 2, vec![10, 11, 12, 20, 21, 22, 30, 31, 32, 40, 41, 42])
678                .unwrap();
679        let options = DepthConversionOptions { max_depth: 2.0, ..Default::default() };
680        let cloud = rgbd_to_point_cloud(depth.view(), color.view(), &camera(), options).unwrap();
681        assert_eq!(cloud.len(), 2);
682        assert_eq!(cloud.field("r").unwrap(), &PointBuffer::U8(vec![10, 40]));
683        assert_eq!(cloud.field("g").unwrap(), &PointBuffer::U8(vec![11, 41]));
684        assert_eq!(cloud.field("b").unwrap(), &PointBuffer::U8(vec![12, 42]));
685    }
686}
spatialrust_camera/rgbd.rs

spatialrust_camera/
rgbd.rs
