spatialrust_viewer/
timeline.rs

1use spatialrust_viz::PointCloudView;
2
3use crate::{ViewerError, ViewerResult};
4
5/// RGB, depth, and cloud timestamps associated with one synchronized frame.
6#[derive(Clone, Copy, Debug, PartialEq, Eq)]
7#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
8pub struct FrameTimestamps {
9    /// RGB timestamp in nanoseconds.
10    pub rgb_nanos: u64,
11    /// Depth timestamp in nanoseconds.
12    pub depth_nanos: u64,
13    /// Optional point-cloud timestamp in nanoseconds.
14    pub cloud_nanos: Option<u64>,
15}
16
17impl FrameTimestamps {
18    fn range(self) -> (u64, u64) {
19        let mut min = self.rgb_nanos.min(self.depth_nanos);
20        let mut max = self.rgb_nanos.max(self.depth_nanos);
21        if let Some(cloud) = self.cloud_nanos {
22            min = min.min(cloud);
23            max = max.max(cloud);
24        }
25        (min, max)
26    }
27
28    /// Deterministic display timestamp at the midpoint of the sensor range.
29    #[must_use]
30    pub fn display_nanos(self) -> u64 {
31        let (min, max) = self.range();
32        min + (max - min) / 2
33    }
34}
35
36/// Borrowed synchronized RGB-D/cloud frame.
37#[derive(Clone, Copy, Debug, PartialEq)]
38pub struct RgbdFrameView<'a> {
39    /// Stable coordinate-frame identifier.
40    pub frame_id: &'a str,
41    /// Sensor timestamps.
42    pub timestamps: FrameTimestamps,
43    /// Image width.
44    pub width: usize,
45    /// Image height.
46    pub height: usize,
47    /// Interleaved RGB8 pixels.
48    pub rgb: &'a [u8],
49    /// Metric row-major depth.
50    pub depth: &'a [f32],
51    /// Optional synchronized point-cloud view.
52    pub cloud: Option<PointCloudView<'a>>,
53}
54
55impl<'a> RgbdFrameView<'a> {
56    /// Validates dimensions, finite depth values, timestamps, and cloud alignment.
57    pub fn validate(self, max_skew_nanos: u64) -> ViewerResult<()> {
58        if self.frame_id.trim().is_empty() || self.width == 0 || self.height == 0 {
59            return Err(ViewerError::InvalidState(
60                "RGB-D frame ID and dimensions must be non-empty".into(),
61            ));
62        }
63        let pixels = self
64            .width
65            .checked_mul(self.height)
66            .ok_or_else(|| ViewerError::InvalidState("RGB-D pixel count overflow".into()))?;
67        let rgb_len = pixels
68            .checked_mul(3)
69            .ok_or_else(|| ViewerError::InvalidState("RGB byte count overflow".into()))?;
70        if self.rgb.len() != rgb_len || self.depth.len() != pixels {
71            return Err(ViewerError::InvalidState(
72                "RGB/depth lengths must exactly match frame dimensions".into(),
73            ));
74        }
75        if self.depth.iter().any(|depth| depth.is_infinite() || *depth < 0.0) {
76            return Err(ViewerError::InvalidState(
77                "depth values must be non-negative and not infinite".into(),
78            ));
79        }
80        let (min, max) = self.timestamps.range();
81        if max - min > max_skew_nanos {
82            return Err(ViewerError::InvalidState(format!(
83                "RGB-D/cloud timestamp skew {} exceeds {} ns",
84                max - min,
85                max_skew_nanos
86            )));
87        }
88        if self.cloud.is_some() != self.timestamps.cloud_nanos.is_some() {
89            return Err(ViewerError::InvalidState(
90                "cloud payload and cloud timestamp must either both be present or absent".into(),
91            ));
92        }
93        Ok(())
94    }
95}
96
97/// Pixel-level RGB-D projection inspection result.
98#[derive(Clone, Copy, Debug, PartialEq)]
99pub struct RgbdPixelSample {
100    /// Pixel x coordinate.
101    pub x: usize,
102    /// Pixel y coordinate.
103    pub y: usize,
104    /// RGB value.
105    pub rgb: [u8; 3],
106    /// Metric depth.
107    pub depth: f32,
108    /// Camera-space XYZ when depth is finite and positive.
109    pub camera_point: Option<spatialrust_math::Vec3<f64>>,
110}
111
112/// Ordered, borrowed synchronized sensor timeline.
113#[derive(Clone, Debug, PartialEq)]
114pub struct RgbdTimeline<'a> {
115    frames: Vec<RgbdFrameView<'a>>,
116    max_skew_nanos: u64,
117}
118
119impl<'a> RgbdTimeline<'a> {
120    /// Creates a validated timeline in non-decreasing display timestamp order.
121    pub fn try_new(
122        frames: impl IntoIterator<Item = RgbdFrameView<'a>>,
123        max_skew_nanos: u64,
124    ) -> ViewerResult<Self> {
125        let frames: Vec<_> = frames.into_iter().collect();
126        let mut previous = None;
127        for frame in &frames {
128            frame.validate(max_skew_nanos)?;
129            let timestamp = frame.timestamps.display_nanos();
130            if previous.is_some_and(|value| timestamp < value) {
131                return Err(ViewerError::InvalidState(
132                    "RGB-D timeline timestamps must be non-decreasing".into(),
133                ));
134            }
135            previous = Some(timestamp);
136        }
137        Ok(Self { frames, max_skew_nanos })
138    }
139
140    /// Ordered frames.
141    #[must_use]
142    pub fn frames(&self) -> &[RgbdFrameView<'a>] {
143        &self.frames
144    }
145
146    /// Configured maximum sensor skew.
147    #[must_use]
148    pub const fn max_skew_nanos(&self) -> u64 {
149        self.max_skew_nanos
150    }
151
152    /// Selects the nearest frame, preferring the earlier frame on ties.
153    #[must_use]
154    pub fn nearest(&self, timestamp_nanos: u64) -> Option<RgbdFrameView<'a>> {
155        self.frames.iter().copied().min_by_key(|frame| {
156            let stamp = frame.timestamps.display_nanos();
157            (stamp.abs_diff(timestamp_nanos), stamp)
158        })
159    }
160
161    /// Inspects one pixel and unprojects valid depth with a pinhole camera.
162    #[cfg(feature = "camera")]
163    pub fn inspect_pixel(
164        &self,
165        frame_index: usize,
166        x: usize,
167        y: usize,
168        camera: &spatialrust_camera::PinholeCamera,
169    ) -> ViewerResult<RgbdPixelSample> {
170        let frame = self
171            .frames
172            .get(frame_index)
173            .ok_or_else(|| ViewerError::InvalidState("RGB-D frame index out of bounds".into()))?;
174        if camera.intrinsics.width != frame.width || camera.intrinsics.height != frame.height {
175            return Err(ViewerError::InvalidState(
176                "camera dimensions must match RGB-D frame".into(),
177            ));
178        }
179        if x >= frame.width || y >= frame.height {
180            return Err(ViewerError::InvalidState("RGB-D pixel out of bounds".into()));
181        }
182        let index = y * frame.width + x;
183        let rgb_offset = index * 3;
184        let depth = frame.depth[index];
185        let camera_point = if depth.is_finite() && depth > 0.0 {
186            Some(
187                camera
188                    .unproject(spatialrust_math::Vec2 { x: x as f64, y: y as f64 }, depth as f64)
189                    .map_err(|error| ViewerError::InvalidState(error.to_string()))?,
190            )
191        } else {
192            None
193        };
194        Ok(RgbdPixelSample {
195            x,
196            y,
197            rgb: [frame.rgb[rgb_offset], frame.rgb[rgb_offset + 1], frame.rgb[rgb_offset + 2]],
198            depth,
199            camera_point,
200        })
201    }
202}
203
204#[cfg(test)]
205mod tests {
206    use super::{FrameTimestamps, RgbdFrameView, RgbdTimeline};
207
208    fn frame<'a>(
209        rgb: &'a [u8],
210        depth: &'a [f32],
211        rgb_nanos: u64,
212        depth_nanos: u64,
213    ) -> RgbdFrameView<'a> {
214        RgbdFrameView {
215            frame_id: "camera",
216            timestamps: FrameTimestamps { rgb_nanos, depth_nanos, cloud_nanos: None },
217            width: 2,
218            height: 1,
219            rgb,
220            depth,
221            cloud: None,
222        }
223    }
224
225    #[test]
226    fn validates_alignment_and_selects_nearest_with_earlier_tie() {
227        let rgb = [1, 2, 3, 4, 5, 6];
228        let depth = [1.0, f32::NAN];
229        let timeline =
230            RgbdTimeline::try_new([frame(&rgb, &depth, 98, 102), frame(&rgb, &depth, 198, 202)], 5)
231                .unwrap();
232        assert_eq!(timeline.nearest(150).unwrap().timestamps.display_nanos(), 100);
233        assert_eq!(timeline.max_skew_nanos(), 5);
234    }
235
236    #[test]
237    fn rejects_skew_lengths_order_and_unpaired_cloud_timestamp() {
238        let rgb = [0; 6];
239        let depth = [1.0, 2.0];
240        assert!(RgbdTimeline::try_new([frame(&rgb, &depth, 0, 10)], 5).is_err());
241        assert!(RgbdTimeline::try_new(
242            [frame(&rgb, &depth, 200, 200), frame(&rgb, &depth, 100, 100)],
243            0
244        )
245        .is_err());
246        let mut invalid = frame(&rgb, &depth, 0, 0);
247        invalid.timestamps.cloud_nanos = Some(0);
248        assert!(invalid.validate(0).is_err());
249        assert!(frame(&rgb[..3], &depth, 0, 0).validate(0).is_err());
250    }
251
252    #[cfg(feature = "camera")]
253    #[test]
254    fn inspects_rgb_depth_and_projection_deterministically() {
255        let rgb = [10, 20, 30, 40, 50, 60];
256        let depth = [2.0, f32::NAN];
257        let timeline = RgbdTimeline::try_new([frame(&rgb, &depth, 0, 0)], 0).unwrap();
258        let camera = spatialrust_camera::PinholeCamera::new(
259            spatialrust_camera::CameraIntrinsics::try_new(2.0, 2.0, 0.0, 0.0, 2, 1).unwrap(),
260        );
261        let sample = timeline.inspect_pixel(0, 0, 0, &camera).unwrap();
262        assert_eq!(sample.rgb, [10, 20, 30]);
263        assert_eq!(sample.camera_point.unwrap(), spatialrust_math::Vec3::new(0.0, 0.0, 2.0));
264        assert!(timeline.inspect_pixel(0, 1, 0, &camera).unwrap().camera_point.is_none());
265        assert!(timeline.inspect_pixel(0, 2, 0, &camera).is_err());
266    }
267}
spatialrust_viewer/timeline.rs

spatialrust_viewer/
timeline.rs
