1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94

use crate::{error::A3dError, trajectory::Trajectory, transform::Transform};

/// Metrics for comparing two transforms.
#[derive(Clone, Debug)]
pub struct TransformMetrics {
    /// Angle between the two transforms in radians.
    pub angle: f32,
    /// Translation vector size between the two transforms.
    pub translation: f32,
}

impl Default for TransformMetrics {
    fn default() -> Self {
        Self {
            angle: 0.0,
            translation: 0.0,
        }
    }
}

impl TransformMetrics {
    /// Creates a new `TransformMetrics` from two transforms.
    pub fn new(lfs: &Transform, rhs: &Transform) -> Self {
        let lfs_inv = lfs.inverse();
        let diff = &lfs_inv * rhs;

        Self {
            angle: diff.angle(),
            translation: diff.translation().norm(),
        }
    }

    pub fn mean_trajectory_error(
        pred_trajectory: &Trajectory,
        gt_trajectory: &Trajectory,
    ) -> Result<Self, A3dError> {
        if pred_trajectory.len() != gt_trajectory.len() {
            return Err(A3dError::invalid_parameter(
                "Pred and GT trajectories have different lengths.",
            ));
        }

        let mut accum_metrics = TransformMetrics::default();
        for (pred, gt) in pred_trajectory.iter().zip(gt_trajectory.iter()) {
            let metrics = Self::new(&pred.0, &gt.0);
            accum_metrics.angle += metrics.angle;
            accum_metrics.translation += metrics.translation;
        }
        accum_metrics.angle /= pred_trajectory.len() as f32;
        accum_metrics.translation /= pred_trajectory.len() as f32;
        Ok(accum_metrics)
    }

    /// Returns the total error of the two transforms.
    pub fn total(&self) -> f32 {
        self.angle + self.translation
    }
}

impl std::fmt::Display for TransformMetrics {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(
            f,
            "angle: {:.2}°, translation: {:.5}",
            self.angle.to_degrees(),
            self.translation
        )
    }
}

#[cfg(test)]
mod tests {
    use nalgebra::{Quaternion, Vector3};

    use super::*;

    #[test]
    fn test_transform_metrics() {
        let sample0 = Transform::new(
            &Vector3::new(0.00022050377, 7.3633055e-5, -1.51071e-5),
            &Quaternion::new(2.059626e-5, 0.00888227, 0.0008264509, 0.99996024),
        );
        let sample1 = Transform::new(
            &Vector3::new(0.00022050377, 7.3633055e-5, -1.51071e-5),
            &Quaternion::new(2.059626e-5, 0.00888227, 0.0008264509, 0.99996024),
        );

        let metrics = TransformMetrics::new(&sample0, &sample1);

        assert_eq!(metrics.angle, 0.0);
        assert_eq!(metrics.translation, 0.0);
        assert_eq!(metrics.total(), 0.0);
    }
}