spatialrust_mapping/
motion.rs

1//! Relative motion estimators used by localization pipelines.
2
3use spatialrust_math::{Isometry3, Pose3, Quat, Vec3};
4use spatialrust_sync::StampedTime;
5
6use crate::{MappingResult, StampedPose};
7
8/// Relative motion between two times.
9#[derive(Clone, Debug, PartialEq)]
10pub struct DeltaMotion {
11    /// Start stamp.
12    pub from: StampedTime,
13    /// End stamp.
14    pub to: StampedTime,
15    /// Transform that maps `from` coordinates into `to` coordinates.
16    pub to_t_from: Isometry3<f32>,
17}
18
19/// Estimates relative motion for odometry / keyframe tracking.
20pub trait RelativeMotionEstimator {
21    /// Estimates motion aligning `previous` toward `current` coordinates.
22    fn estimate(&self, previous: &StampedPose, current: &StampedPose)
23        -> MappingResult<DeltaMotion>;
24}
25
26/// Synthetic odometry that trusts successive pose stamps and emits their delta.
27#[derive(Clone, Copy, Debug, Default)]
28pub struct SyntheticOdometry;
29
30impl RelativeMotionEstimator for SyntheticOdometry {
31    fn estimate(
32        &self,
33        previous: &StampedPose,
34        current: &StampedPose,
35    ) -> MappingResult<DeltaMotion> {
36        let to_t_from = current.pose.isometry.compose(previous.pose.isometry.inverse());
37        Ok(DeltaMotion { from: previous.stamp.clone(), to: current.stamp.clone(), to_t_from })
38    }
39}
40
41impl SyntheticOdometry {
42    /// Integrates a translation-only delta onto a pose.
43    #[must_use]
44    pub fn integrate_translation(pose: Pose3<f32>, delta: Vec3<f32>) -> Pose3<f32> {
45        let translation = pose.isometry.translation() + delta;
46        Pose3::new(Isometry3::new(pose.isometry.rotation(), translation))
47    }
48
49    /// Builds a translation-only delta as an isometry.
50    #[must_use]
51    pub fn translation_delta(delta: Vec3<f32>) -> Isometry3<f32> {
52        Isometry3::new(Quat::new(0.0, 0.0, 0.0, 1.0), delta)
53    }
54}
55
56#[cfg(test)]
57mod tests {
58    use super::{RelativeMotionEstimator, SyntheticOdometry};
59    use crate::StampedPose;
60    use spatialrust_core::Timestamp;
61    use spatialrust_math::{Isometry3, Pose3, Quat, Vec3};
62    use spatialrust_sync::{ClockDomain, StampedTime};
63
64    #[test]
65    fn synthetic_odometry_recovers_translation_delta() {
66        let previous = StampedPose::new(
67            StampedTime::exact("host", ClockDomain::HostSteady, Timestamp::from_nanos(0)),
68            Pose3::new(Isometry3::new(Quat::new(0.0, 0.0, 0.0, 1.0), Vec3::new(0.0, 0.0, 0.0))),
69        );
70        let current = StampedPose::new(
71            StampedTime::exact("host", ClockDomain::HostSteady, Timestamp::from_nanos(1)),
72            Pose3::new(Isometry3::new(Quat::new(0.0, 0.0, 0.0, 1.0), Vec3::new(1.0, 2.0, 0.0))),
73        );
74        let delta = SyntheticOdometry.estimate(&previous, &current).unwrap();
75        assert!((delta.to_t_from.translation().x - 1.0).abs() < 1e-5);
76        assert!((delta.to_t_from.translation().y - 2.0).abs() < 1e-5);
77    }
78}
spatialrust_mapping/motion.rs

spatialrust_mapping/
motion.rs
