align3d/
metrics.rs

1use crate::{error::A3dError, trajectory::Trajectory, transform::Transform};
2
3/// Metrics for comparing two transforms.
4#[derive(Clone, Debug)]
5pub struct TransformMetrics {
6    /// Angle between the two transforms in radians.
7    pub angle: f32,
8    /// Translation vector size between the two transforms.
9    pub translation: f32,
10}
11
12impl Default for TransformMetrics {
13    fn default() -> Self {
14        Self {
15            angle: 0.0,
16            translation: 0.0,
17        }
18    }
19}
20
21impl TransformMetrics {
22    /// Creates a new `TransformMetrics` from two transforms.
23    pub fn new(lfs: &Transform, rhs: &Transform) -> Self {
24        let lfs_inv = lfs.inverse();
25        let diff = &lfs_inv * rhs;
26
27        Self {
28            angle: diff.angle(),
29            translation: diff.translation().norm(),
30        }
31    }
32
33    pub fn mean_trajectory_error(
34        pred_trajectory: &Trajectory,
35        gt_trajectory: &Trajectory,
36    ) -> Result<Self, A3dError> {
37        if pred_trajectory.len() != gt_trajectory.len() {
38            return Err(A3dError::invalid_parameter(
39                "Pred and GT trajectories have different lengths.",
40            ));
41        }
42
43        let mut accum_metrics = TransformMetrics::default();
44        for (pred, gt) in pred_trajectory.iter().zip(gt_trajectory.iter()) {
45            let metrics = Self::new(&pred.0, &gt.0);
46            accum_metrics.angle += metrics.angle;
47            accum_metrics.translation += metrics.translation;
48        }
49        accum_metrics.angle /= pred_trajectory.len() as f32;
50        accum_metrics.translation /= pred_trajectory.len() as f32;
51        Ok(accum_metrics)
52    }
53
54    /// Returns the total error of the two transforms.
55    pub fn total(&self) -> f32 {
56        self.angle + self.translation
57    }
58}
59
60impl std::fmt::Display for TransformMetrics {
61    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
62        write!(
63            f,
64            "angle: {:.2}°, translation: {:.5}",
65            self.angle.to_degrees(),
66            self.translation
67        )
68    }
69}
70
71#[cfg(test)]
72mod tests {
73    use nalgebra::{Quaternion, Vector3};
74
75    use super::*;
76
77    #[test]
78    fn test_transform_metrics() {
79        let sample0 = Transform::new(
80            &Vector3::new(0.00022050377, 7.3633055e-5, -1.51071e-5),
81            &Quaternion::new(2.059626e-5, 0.00888227, 0.0008264509, 0.99996024),
82        );
83        let sample1 = Transform::new(
84            &Vector3::new(0.00022050377, 7.3633055e-5, -1.51071e-5),
85            &Quaternion::new(2.059626e-5, 0.00888227, 0.0008264509, 0.99996024),
86        );
87
88        let metrics = TransformMetrics::new(&sample0, &sample1);
89
90        assert_eq!(metrics.angle, 0.0);
91        assert_eq!(metrics.translation, 0.0);
92        assert_eq!(metrics.total(), 0.0);
93    }
94}
align3d/metrics.rs

align3d/
metrics.rs
