spatialrust_mapping/
vision.rs

1//! Explicit conversion from vision odometry estimates into mapping motion.
2
3use spatialrust_math::{Isometry3, Mat3, Quat, Vec3};
4use spatialrust_sync::StampedTime;
5use spatialrust_vision::{MonocularOdometryEstimate, RgbdOdometryEstimate};
6
7use crate::{DeltaMotion, MappingError, MappingResult};
8
9/// Converts scale-ambiguous monocular motion after the caller supplies scale.
10pub fn delta_from_monocular_odometry(
11    from: StampedTime,
12    to: StampedTime,
13    estimate: &MonocularOdometryEstimate,
14    translation_scale: f32,
15) -> MappingResult<DeltaMotion> {
16    if !translation_scale.is_finite() || translation_scale <= 0.0 {
17        return Err(MappingError::InvalidConfiguration(
18            "monocular translation scale must be finite and positive".into(),
19        ));
20    }
21    let pose = estimate.pose;
22    Ok(delta(from, to, pose.rotation(), pose.translation(), translation_scale))
23}
24
25/// Converts a metric RGB-D source-to-target pose into mapping motion.
26pub fn delta_from_rgbd_odometry(
27    from: StampedTime,
28    to: StampedTime,
29    estimate: &RgbdOdometryEstimate,
30) -> DeltaMotion {
31    let pose = estimate.pose;
32    delta(from, to, pose.rotation(), pose.translation(), 1.0)
33}
34
35fn delta(
36    from: StampedTime,
37    to: StampedTime,
38    rotation: Mat3<f64>,
39    translation: Vec3<f64>,
40    scale: f32,
41) -> DeltaMotion {
42    let matrix = Mat3::from_rows(
43        [rotation.m[0][0] as f32, rotation.m[0][1] as f32, rotation.m[0][2] as f32],
44        [rotation.m[1][0] as f32, rotation.m[1][1] as f32, rotation.m[1][2] as f32],
45        [rotation.m[2][0] as f32, rotation.m[2][1] as f32, rotation.m[2][2] as f32],
46    );
47    let value = matrix_to_quaternion(matrix);
48    let translation = Vec3::new(
49        translation.x as f32 * scale,
50        translation.y as f32 * scale,
51        translation.z as f32 * scale,
52    );
53    DeltaMotion { from, to, to_t_from: Isometry3::new(value, translation) }
54}
55
56fn matrix_to_quaternion(matrix: Mat3<f32>) -> Quat<f32> {
57    let trace = matrix.m[0][0] + matrix.m[1][1] + matrix.m[2][2];
58    let quaternion = if trace > 0.0 {
59        let s = (trace + 1.0).sqrt() * 2.0;
60        Quat::new(
61            (matrix.m[2][1] - matrix.m[1][2]) / s,
62            (matrix.m[0][2] - matrix.m[2][0]) / s,
63            (matrix.m[1][0] - matrix.m[0][1]) / s,
64            0.25 * s,
65        )
66    } else if matrix.m[0][0] > matrix.m[1][1] && matrix.m[0][0] > matrix.m[2][2] {
67        let s = (1.0 + matrix.m[0][0] - matrix.m[1][1] - matrix.m[2][2]).sqrt() * 2.0;
68        Quat::new(
69            0.25 * s,
70            (matrix.m[0][1] + matrix.m[1][0]) / s,
71            (matrix.m[0][2] + matrix.m[2][0]) / s,
72            (matrix.m[2][1] - matrix.m[1][2]) / s,
73        )
74    } else if matrix.m[1][1] > matrix.m[2][2] {
75        let s = (1.0 + matrix.m[1][1] - matrix.m[0][0] - matrix.m[2][2]).sqrt() * 2.0;
76        Quat::new(
77            (matrix.m[0][1] + matrix.m[1][0]) / s,
78            0.25 * s,
79            (matrix.m[1][2] + matrix.m[2][1]) / s,
80            (matrix.m[0][2] - matrix.m[2][0]) / s,
81        )
82    } else {
83        let s = (1.0 + matrix.m[2][2] - matrix.m[0][0] - matrix.m[1][1]).sqrt() * 2.0;
84        Quat::new(
85            (matrix.m[0][2] + matrix.m[2][0]) / s,
86            (matrix.m[1][2] + matrix.m[2][1]) / s,
87            0.25 * s,
88            (matrix.m[1][0] - matrix.m[0][1]) / s,
89        )
90    };
91    quaternion.normalize()
92}
93
94#[cfg(test)]
95mod tests {
96    use super::delta_from_monocular_odometry;
97    use spatialrust_core::Timestamp;
98    use spatialrust_math::{Mat3, Vec3};
99    use spatialrust_sync::{ClockDomain, StampedTime};
100    use spatialrust_vision::{MonocularOdometryEstimate, RelativePose};
101
102    #[test]
103    fn monocular_bridge_applies_caller_scale() {
104        let estimate = MonocularOdometryEstimate {
105            pose: RelativePose::try_new(Mat3::<f64>::identity(), Vec3::new(1.0, 0.0, 0.0)).unwrap(),
106            inliers: vec![true; 8],
107            positive_depth_count: 8,
108        };
109        let stamp = |nanos| {
110            StampedTime::exact("camera", ClockDomain::HostSteady, Timestamp::from_nanos(nanos))
111        };
112        let motion = delta_from_monocular_odometry(stamp(1), stamp(2), &estimate, 0.25).unwrap();
113        assert!((motion.to_t_from.translation().x - 0.25).abs() < 1e-6);
114        assert_eq!(motion.to_t_from.rotation(), spatialrust_math::Quat::<f32>::identity());
115    }
116}
spatialrust_mapping/vision.rs

spatialrust_mapping/
vision.rs
