Skip to main content

rust_robotics_slam/
slam_benchmark.rs

1//! Relative-pose SLAM metric of Kümmerle et al., "On measuring the accuracy of
2//! SLAM algorithms" (Autonomous Robots, 2009).
3//!
4//! Instead of comparing a trajectory against a global ground truth, the
5//! benchmark provides *relations*: verified relative poses `δ*ᵢⱼ` between the
6//! poses at two timestamps. For an estimate the error of each relation is
7//! `δᵢⱼ ⊖ δ*ᵢⱼ` with `δᵢⱼ = xᵢ⁻¹ ⊕ xⱼ`, and translational and rotational parts
8//! are averaged separately (absolute and squared). The metric is invariant to
9//! the global frame, so any SLAM output can be scored.
10//!
11//! Relations files have one relation per line:
12//!
13//! ```text
14//! timestamp_i timestamp_j x y z roll pitch yaw
15//! ```
16
17use rust_robotics_core::Pose2D;
18
19use crate::scan_to_map::relative_pose;
20
21/// A reference relative pose between the poses at two timestamps.
22#[derive(Debug, Clone, Copy, PartialEq)]
23pub struct Relation {
24    pub timestamp_from: f64,
25    pub timestamp_to: f64,
26    /// Relative pose `x_from⁻¹ ⊕ x_to`.
27    pub relative: Pose2D,
28}
29
30/// Aggregated relation errors.
31#[derive(Debug, Clone, Copy, PartialEq)]
32pub struct RelationErrors {
33    /// Relations whose both timestamps matched a trajectory pose.
34    pub matched: usize,
35    /// Relations skipped because a timestamp had no pose within tolerance.
36    pub unmatched: usize,
37    /// Mean absolute translational error \[m\].
38    pub translation_mean: f64,
39    /// Standard deviation of the absolute translational error \[m\].
40    pub translation_std: f64,
41    /// Mean squared translational error \[m²\].
42    pub translation_mean_squared: f64,
43    /// Mean absolute rotational error \[rad\].
44    pub rotation_mean: f64,
45    /// Standard deviation of the absolute rotational error \[rad\].
46    pub rotation_std: f64,
47    /// Mean squared rotational error \[rad²\].
48    pub rotation_mean_squared: f64,
49}
50
51/// Parses a relations file; blank lines and `#` comments are skipped.
52pub fn parse_relations(text: &str) -> Result<Vec<Relation>, String> {
53    text.lines()
54        .enumerate()
55        .filter(|(_, line)| {
56            let line = line.trim();
57            !line.is_empty() && !line.starts_with('#')
58        })
59        .map(|(index, line)| {
60            let values: Vec<f64> = line
61                .split_whitespace()
62                .map(str::parse::<f64>)
63                .collect::<Result<_, _>>()
64                .map_err(|error| format!("line {}: {error}", index + 1))?;
65            let [t1, t2, x, y, _z, _roll, _pitch, yaw] = values.as_slice() else {
66                return Err(format!(
67                    "line {}: expected 8 values, found {}",
68                    index + 1,
69                    values.len()
70                ));
71            };
72            Ok(Relation {
73                timestamp_from: *t1,
74                timestamp_to: *t2,
75                relative: Pose2D::new(*x, *y, *yaw),
76            })
77        })
78        .collect()
79}
80
81/// Formats relations in the benchmark file format.
82pub fn format_relations(relations: &[Relation]) -> String {
83    relations
84        .iter()
85        .map(|relation| {
86            format!(
87                "{:.6} {:.6} {:.6} {:.6} 0 0 0 {:.6}\n",
88                relation.timestamp_from,
89                relation.timestamp_to,
90                relation.relative.x,
91                relation.relative.y,
92                relation.relative.yaw
93            )
94        })
95        .collect()
96}
97
98/// Pose of a trajectory sorted by timestamp closest to `timestamp`, if within
99/// `tolerance` seconds.
100fn pose_at(trajectory: &[(f64, Pose2D)], timestamp: f64, tolerance: f64) -> Option<Pose2D> {
101    let index = trajectory.partition_point(|(time, _)| *time < timestamp);
102    [index.checked_sub(1), Some(index)]
103        .into_iter()
104        .flatten()
105        .filter_map(|index| trajectory.get(index))
106        .min_by(|a, b| (a.0 - timestamp).abs().total_cmp(&(b.0 - timestamp).abs()))
107        .filter(|(time, _)| (time - timestamp).abs() <= tolerance)
108        .map(|(_, pose)| *pose)
109}
110
111fn mean_and_std(values: &[f64]) -> (f64, f64) {
112    if values.is_empty() {
113        return (0.0, 0.0);
114    }
115    let mean = values.iter().sum::<f64>() / values.len() as f64;
116    let variance = values.iter().map(|v| (v - mean).powi(2)).sum::<f64>() / values.len() as f64;
117    (mean, variance.sqrt())
118}
119
120/// Scores `trajectory` (timestamped poses, any order) against `relations`.
121///
122/// A relation is used when both of its timestamps have a pose within
123/// `time_tolerance` seconds.
124pub fn evaluate_relations(
125    trajectory: &[(f64, Pose2D)],
126    relations: &[Relation],
127    time_tolerance: f64,
128) -> RelationErrors {
129    let mut sorted = trajectory.to_vec();
130    sorted.sort_by(|a, b| a.0.total_cmp(&b.0));
131
132    let mut translation = Vec::new();
133    let mut rotation = Vec::new();
134    for relation in relations {
135        let (Some(from), Some(to)) = (
136            pose_at(&sorted, relation.timestamp_from, time_tolerance),
137            pose_at(&sorted, relation.timestamp_to, time_tolerance),
138        ) else {
139            continue;
140        };
141        let error = relative_pose(relation.relative, relative_pose(from, to));
142        translation.push(error.x.hypot(error.y));
143        rotation.push(error.yaw.abs());
144    }
145
146    let (translation_mean, translation_std) = mean_and_std(&translation);
147    let (rotation_mean, rotation_std) = mean_and_std(&rotation);
148    let squared =
149        |values: &[f64]| values.iter().map(|v| v * v).sum::<f64>() / values.len().max(1) as f64;
150    RelationErrors {
151        matched: translation.len(),
152        unmatched: relations.len() - translation.len(),
153        translation_mean,
154        translation_std,
155        translation_mean_squared: squared(&translation),
156        rotation_mean,
157        rotation_std,
158        rotation_mean_squared: squared(&rotation),
159    }
160}
161
162#[cfg(test)]
163mod tests {
164    use super::*;
165    use crate::scan_to_map::compose_pose;
166
167    fn trajectory() -> Vec<(f64, Pose2D)> {
168        let mut pose = Pose2D::origin();
169        (0..20)
170            .map(|i| {
171                pose = compose_pose(pose, Pose2D::new(0.5, 0.0, 0.1));
172                (f64::from(i), pose)
173            })
174            .collect()
175    }
176
177    #[test]
178    fn exact_trajectory_scores_zero_in_any_frame() {
179        let truth = trajectory();
180        let relations: Vec<Relation> = (0..15)
181            .map(|i| Relation {
182                timestamp_from: truth[i].0,
183                timestamp_to: truth[i + 5].0,
184                relative: relative_pose(truth[i].1, truth[i + 5].1),
185            })
186            .collect();
187        // Same trajectory expressed in a different global frame.
188        let offset = Pose2D::new(3.0, -1.0, 0.7);
189        let moved: Vec<(f64, Pose2D)> = truth
190            .iter()
191            .map(|(time, pose)| (time + 0.01, compose_pose(offset, *pose)))
192            .collect();
193        let errors = evaluate_relations(&moved, &relations, 0.05);
194        assert_eq!(errors.matched, 15);
195        assert_eq!(errors.unmatched, 0);
196        assert!(errors.translation_mean < 1e-9);
197        assert!(errors.rotation_mean < 1e-9);
198    }
199
200    #[test]
201    fn measures_a_known_error_and_skips_unmatched_relations() {
202        let truth = trajectory();
203        let relations = vec![
204            Relation {
205                timestamp_from: 0.0,
206                timestamp_to: 1.0,
207                relative: relative_pose(truth[0].1, truth[1].1),
208            },
209            Relation {
210                timestamp_from: 0.0,
211                timestamp_to: 99.0,
212                relative: Pose2D::origin(),
213            },
214        ];
215        let mut estimate = truth.clone();
216        // Push the second pose 0.3 m forward in its own frame.
217        estimate[1].1 = compose_pose(estimate[1].1, Pose2D::new(0.3, 0.0, 0.0));
218        let errors = evaluate_relations(&estimate, &relations, 0.1);
219        assert_eq!(errors.matched, 1);
220        assert_eq!(errors.unmatched, 1);
221        assert!((errors.translation_mean - 0.3).abs() < 1e-9);
222        assert!((errors.translation_mean_squared - 0.09).abs() < 1e-9);
223    }
224
225    #[test]
226    fn relations_round_trip_through_the_file_format() {
227        let relations = vec![Relation {
228            timestamp_from: 1.5,
229            timestamp_to: 7.25,
230            relative: Pose2D::new(0.25, -1.5, 0.125),
231        }];
232        let parsed = parse_relations(&format!("# header\n\n{}", format_relations(&relations)))
233            .expect("valid file");
234        assert_eq!(parsed, relations);
235        assert!(parse_relations("1 2 3\n").is_err());
236    }
237}