1use rust_robotics_core::Pose2D;
18
19use crate::scan_to_map::relative_pose;
20
21#[derive(Debug, Clone, Copy, PartialEq)]
23pub struct Relation {
24 pub timestamp_from: f64,
25 pub timestamp_to: f64,
26 pub relative: Pose2D,
28}
29
30#[derive(Debug, Clone, Copy, PartialEq)]
32pub struct RelationErrors {
33 pub matched: usize,
35 pub unmatched: usize,
37 pub translation_mean: f64,
39 pub translation_std: f64,
41 pub translation_mean_squared: f64,
43 pub rotation_mean: f64,
45 pub rotation_std: f64,
47 pub rotation_mean_squared: f64,
49}
50
51pub 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
81pub 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
98fn 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
120pub 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 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 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}