spatialrust_mapping/
trajectory.rs

1//! Stamped pose trajectories.
2
3use spatialrust_math::{Isometry3, Pose3};
4use spatialrust_sync::StampedTime;
5
6use crate::{MappingError, MappingResult};
7
8/// One timed pose sample.
9#[derive(Clone, Debug, PartialEq)]
10pub struct StampedPose {
11    /// Observation / estimate time.
12    pub stamp: StampedTime,
13    /// Pose in the trajectory frame.
14    pub pose: Pose3<f32>,
15}
16
17impl StampedPose {
18    /// Creates a stamped pose.
19    #[must_use]
20    pub fn new(stamp: StampedTime, pose: Pose3<f32>) -> Self {
21        Self { stamp, pose }
22    }
23}
24
25/// Ordered trajectory of stamped poses.
26#[derive(Clone, Debug, Default, PartialEq)]
27pub struct Trajectory {
28    samples: Vec<StampedPose>,
29}
30
31impl Trajectory {
32    /// Creates an empty trajectory.
33    #[must_use]
34    pub fn new() -> Self {
35        Self::default()
36    }
37
38    /// Appends a sample; timestamps must be non-decreasing.
39    pub fn push(&mut self, sample: StampedPose) -> MappingResult<()> {
40        if let Some(last) = self.samples.last() {
41            if sample.stamp.as_nanos() < last.stamp.as_nanos() {
42                return Err(MappingError::InvalidConfiguration(
43                    "trajectory timestamps must be non-decreasing".into(),
44                ));
45            }
46        }
47        self.samples.push(sample);
48        Ok(())
49    }
50
51    /// Returns all samples.
52    #[must_use]
53    pub fn samples(&self) -> &[StampedPose] {
54        &self.samples
55    }
56
57    /// Returns the latest sample.
58    #[must_use]
59    pub fn last(&self) -> Option<&StampedPose> {
60        self.samples.last()
61    }
62
63    /// Linearly interpolates translation between bracketing samples (rotation holds the earlier).
64    pub fn interpolate(&self, nanos: u64) -> MappingResult<Pose3<f32>> {
65        if self.samples.is_empty() {
66            return Err(MappingError::Missing("trajectory samples".into()));
67        }
68        if nanos <= self.samples[0].stamp.as_nanos() {
69            return Ok(self.samples[0].pose);
70        }
71        if let Some(last) = self.samples.last() {
72            if nanos >= last.stamp.as_nanos() {
73                return Ok(last.pose);
74            }
75        }
76        for window in self.samples.windows(2) {
77            let a = &window[0];
78            let b = &window[1];
79            if nanos >= a.stamp.as_nanos() && nanos <= b.stamp.as_nanos() {
80                let span = b.stamp.as_nanos() - a.stamp.as_nanos();
81                if span == 0 {
82                    return Ok(a.pose);
83                }
84                let t = (nanos - a.stamp.as_nanos()) as f32 / span as f32;
85                let ta = a.pose.isometry.translation();
86                let tb = b.pose.isometry.translation();
87                let translation = spatialrust_math::Vec3::new(
88                    ta.x + (tb.x - ta.x) * t,
89                    ta.y + (tb.y - ta.y) * t,
90                    ta.z + (tb.z - ta.z) * t,
91                );
92                return Ok(Pose3::new(Isometry3::new(a.pose.isometry.rotation(), translation)));
93            }
94        }
95        Err(MappingError::Graph("interpolation failed".into()))
96    }
97}
98
99#[cfg(test)]
100mod tests {
101    use super::{StampedPose, Trajectory};
102    use spatialrust_core::Timestamp;
103    use spatialrust_math::{Isometry3, Pose3, Quat, Vec3};
104    use spatialrust_sync::{ClockDomain, StampedTime};
105
106    #[test]
107    fn interpolates_translation() {
108        let mut traj = Trajectory::new();
109        traj.push(StampedPose::new(
110            StampedTime::exact("host", ClockDomain::HostSteady, Timestamp::from_nanos(0)),
111            Pose3::new(Isometry3::new(Quat::new(0.0, 0.0, 0.0, 1.0), Vec3::new(0.0, 0.0, 0.0))),
112        ))
113        .unwrap();
114        traj.push(StampedPose::new(
115            StampedTime::exact("host", ClockDomain::HostSteady, Timestamp::from_nanos(10)),
116            Pose3::new(Isometry3::new(Quat::new(0.0, 0.0, 0.0, 1.0), Vec3::new(10.0, 0.0, 0.0))),
117        ))
118        .unwrap();
119        let mid = traj.interpolate(5).unwrap();
120        assert!((mid.isometry.translation().x - 5.0).abs() < 1e-5);
121    }
122}
spatialrust_mapping/trajectory.rs

spatialrust_mapping/
trajectory.rs
