1use nalgebra::Vector2;
12use rand::{rngs::StdRng, SeedableRng};
13use rand_distr::{Distribution, Normal};
14use rust_robotics_core::Pose2D;
15
16use crate::lidar_graph_slam::{LidarGraphSlam, LidarGraphSlamConfig, LoopClosure};
17use crate::scan_to_map::{
18 compose_pose, ranges_to_points, ray_cast_ranges, relative_pose, LineSegment,
19};
20
21#[derive(Debug, Clone, Copy, PartialEq)]
23pub enum CorridorLayout {
24 Irregular,
26 Periodic { spacing: f64 },
29}
30
31#[derive(Debug, Clone, Copy, PartialEq)]
33pub struct CorridorLoopConfig {
34 pub layout: CorridorLayout,
36 pub start_offset: f64,
39 pub step: f64,
41 pub corner_radius: f64,
43 pub extra_distance: f64,
45 pub odometry_scale: f64,
47 pub odometry_yaw_drift: f64,
49 pub odometry_noise: f64,
51 pub range_noise: f64,
53 pub beams: usize,
55 pub max_range: f64,
57 pub seed: u64,
59}
60
61impl Default for CorridorLoopConfig {
62 fn default() -> Self {
63 Self {
64 layout: CorridorLayout::Irregular,
65 start_offset: 0.0,
66 step: 0.2,
67 corner_radius: 1.5,
68 extra_distance: 12.0,
69 odometry_scale: 1.03,
70 odometry_yaw_drift: 1.0_f64.to_radians(),
71 odometry_noise: 0.003,
72 range_noise: 0.02,
73 beams: 180,
74 max_range: 8.0,
75 seed: 11,
76 }
77 }
78}
79
80#[derive(Debug, Clone, PartialEq)]
82pub struct CorridorLoopFrame {
83 pub truth: Pose2D,
85 pub odometry: Pose2D,
87 pub front_end: Pose2D,
89 pub estimate: Pose2D,
91 pub node_poses: Vec<Pose2D>,
93 pub loop_closure: Option<LoopClosure>,
95 pub optimized: bool,
97 pub scan: Vec<Vector2<f64>>,
99}
100
101#[derive(Debug, Clone)]
103pub struct CorridorLoopRun {
104 pub walls: Vec<LineSegment>,
106 pub lap_length: f64,
108 pub distance: f64,
110 pub frames: Vec<CorridorLoopFrame>,
112 pub node_scans: Vec<Vec<Vector2<f64>>>,
114 pub node_truth: Vec<Pose2D>,
116 pub node_odometry: Vec<Pose2D>,
118 pub node_front_end: Vec<Pose2D>,
120 pub final_node_poses: Vec<Pose2D>,
122 pub final_estimate: Pose2D,
124 pub loop_closures: Vec<LoopClosure>,
126 pub ambiguous_loop_rejections: usize,
128}
129
130impl CorridorLoopRun {
131 pub fn is_wrong_loop(&self, closure: &LoopClosure, tolerance: f64) -> bool {
134 let truth = relative_pose(self.node_truth[closure.from], self.node_truth[closure.to]);
135 let error = relative_pose(truth, closure.relative);
136 error.x.hypot(error.y) > tolerance
137 }
138
139 pub fn wrong_loop_closures(&self, tolerance: f64) -> usize {
141 self.loop_closures
142 .iter()
143 .filter(|closure| self.is_wrong_loop(closure, tolerance))
144 .count()
145 }
146
147 pub fn first_loop_frame(&self) -> Option<usize> {
149 self.frames
150 .iter()
151 .position(|frame| frame.loop_closure.is_some())
152 }
153}
154
155pub fn aliased_corridor_config() -> CorridorLoopConfig {
159 CorridorLoopConfig {
160 layout: CorridorLayout::Periodic { spacing: 2.5 },
161 start_offset: 12.0,
162 extra_distance: 14.0,
163 ..CorridorLoopConfig::default()
164 }
165}
166
167pub fn position_rmse(estimates: &[Pose2D], truth: &[Pose2D]) -> f64 {
169 let sum: f64 = estimates
170 .iter()
171 .zip(truth)
172 .map(|(estimate, truth)| (estimate.x - truth.x).powi(2) + (estimate.y - truth.y).powi(2))
173 .sum();
174 (sum / truth.len().max(1) as f64).sqrt()
175}
176
177fn rectangle(min: (f64, f64), max: (f64, f64)) -> Vec<LineSegment> {
178 let corners = [
179 Vector2::new(min.0, min.1),
180 Vector2::new(max.0, min.1),
181 Vector2::new(max.0, max.1),
182 Vector2::new(min.0, max.1),
183 ];
184 (0..4)
185 .map(|i| LineSegment::new(corners[i], corners[(i + 1) % 4]))
186 .collect()
187}
188
189pub fn corridor_loop_walls() -> Vec<LineSegment> {
192 corridor_loop_walls_for(CorridorLayout::Irregular)
193}
194
195pub fn corridor_loop_walls_for(layout: CorridorLayout) -> Vec<LineSegment> {
197 let mut walls = rectangle((-15.0, -10.0), (15.0, 10.0));
198 walls.extend(rectangle((-12.0, -7.0), (12.0, 7.0)));
199 let pillar = |x: f64, y: f64| rectangle((x - 0.2, y - 0.2), (x + 0.2, y + 0.2));
200 match layout {
201 CorridorLayout::Irregular => {
202 for x in [-9.3, -4.1, 1.2, 6.7, 10.4] {
203 walls.extend(pillar(x, -9.8));
204 }
205 for x in [-7.6, -1.9, 3.8, 8.9] {
206 walls.extend(pillar(x, -7.2));
207 }
208 }
209 CorridorLayout::Periodic { spacing } => {
210 let spacing = spacing.max(0.5);
211 let count = (22.0 / spacing).floor() as i32;
212 for i in 0..=count {
213 let x = -11.0 + f64::from(i) * spacing;
214 walls.extend(pillar(x, -9.8));
215 walls.extend(pillar(x, -7.2));
216 }
217 }
218 }
219 for y in [-4.4, 1.7, 6.2] {
223 walls.extend(pillar(-14.8, y));
224 walls.extend(pillar(14.8, -y + 0.9));
225 }
226 for y in [-2.8, 3.9] {
227 walls.extend(pillar(-12.2, y));
228 walls.extend(pillar(12.2, -y));
229 }
230 walls
231}
232
233pub fn corridor_loop_start() -> Pose2D {
235 Pose2D::new(-12.0, -8.5, 0.0)
236}
237
238pub fn corridor_loop_lap_length(corner_radius: f64) -> f64 {
240 2.0 * (27.0 + 17.0) - 8.0 * corner_radius + 2.0 * std::f64::consts::PI * corner_radius
241}
242
243pub fn corridor_loop_start_for(config: &CorridorLoopConfig) -> Pose2D {
245 centerline_deltas(config, config.start_offset)
246 .into_iter()
247 .fold(corridor_loop_start(), compose_pose)
248}
249
250pub fn corridor_loop_deltas(config: &CorridorLoopConfig) -> Vec<Pose2D> {
254 let skipped = centerline_deltas(config, config.start_offset).len();
255 let total = corridor_loop_lap_length(config.corner_radius) + config.extra_distance;
256 centerline_deltas(config, config.start_offset + total)
257 .into_iter()
258 .skip(skipped)
259 .collect()
260}
261
262fn centerline_deltas(config: &CorridorLoopConfig, total: f64) -> Vec<Pose2D> {
264 if total <= 0.0 {
265 return Vec::new();
266 }
267 let radius = config.corner_radius;
268 let quarter = std::f64::consts::FRAC_PI_2 * radius;
269 let half_lap = [
270 (27.0 - 2.0 * radius, 0.0),
271 (quarter, 1.0 / radius),
272 (17.0 - 2.0 * radius, 0.0),
273 (quarter, 1.0 / radius),
274 ];
275 let mut deltas = Vec::new();
276 let mut travelled = 0.0;
277 'outer: loop {
278 for (length, curvature) in half_lap.iter().chain(&half_lap) {
280 let steps = ((length / config.step).round() as usize).max(1);
281 let step = length / steps as f64;
282 for _ in 0..steps {
283 deltas.push(Pose2D::new(step, 0.0, step * curvature));
284 travelled += step;
285 if travelled >= total {
286 break 'outer;
287 }
288 }
289 }
290 }
291 deltas
292}
293
294pub fn run_corridor_loop(
300 config: &CorridorLoopConfig,
301 slam_config: LidarGraphSlamConfig,
302) -> CorridorLoopRun {
303 let walls = corridor_loop_walls_for(config.layout);
304 let deltas = corridor_loop_deltas(config);
305 let mut rng = StdRng::seed_from_u64(config.seed);
306 let range_noise = Normal::new(0.0, config.range_noise).expect("valid range noise");
307 let odom_noise = Normal::new(0.0, config.odometry_noise).expect("valid odometry noise");
308 let scan = |pose: Pose2D, rng: &mut StdRng| {
309 let ranges: Vec<f64> = ray_cast_ranges(pose, &walls, config.beams, config.max_range)
310 .into_iter()
311 .map(|range| range + range_noise.sample(rng))
312 .collect();
313 ranges_to_points(&ranges)
314 };
315
316 let start = corridor_loop_start_for(config);
317 let mut truth = start;
318 let mut odometry = start;
319 let mut slam = LidarGraphSlam::new(slam_config, start);
320 let mut frames = Vec::with_capacity(deltas.len() + 1);
321 let mut node_truth = Vec::new();
322 let mut node_odometry = Vec::new();
323
324 let mut record = |slam: &mut LidarGraphSlam,
325 truth: Pose2D,
326 odometry: Pose2D,
327 odom_delta: Pose2D,
328 scan: Vec<Vector2<f64>>| {
329 let update = slam.update(odom_delta, &scan);
330 if update.new_node.is_some() {
331 node_truth.push(truth);
332 node_odometry.push(odometry);
333 }
334 frames.push(CorridorLoopFrame {
335 truth,
336 odometry,
337 front_end: slam.front_end_pose(),
338 estimate: update.pose,
339 node_poses: slam.node_poses(),
340 loop_closure: update.loop_closure,
341 optimized: update.optimized,
342 scan,
343 });
344 };
345
346 let initial_scan = scan(truth, &mut rng);
347 record(&mut slam, truth, odometry, Pose2D::origin(), initial_scan);
348 for true_delta in &deltas {
349 truth = compose_pose(truth, *true_delta);
350 let odom_delta = Pose2D::new(
351 true_delta.x * config.odometry_scale + odom_noise.sample(&mut rng),
352 odom_noise.sample(&mut rng),
353 true_delta.yaw + config.odometry_yaw_drift * true_delta.x + odom_noise.sample(&mut rng),
354 );
355 odometry = compose_pose(odometry, odom_delta);
356 let current_scan = scan(truth, &mut rng);
357 record(&mut slam, truth, odometry, odom_delta, current_scan);
358 }
359
360 slam.optimize();
362 let node_count = slam.node_poses().len();
363 CorridorLoopRun {
364 lap_length: corridor_loop_lap_length(config.corner_radius),
365 distance: deltas.iter().map(|delta| delta.x).sum(),
366 frames,
367 node_scans: (0..node_count)
368 .map(|index| slam.node_scan(index).unwrap_or_default().to_vec())
369 .collect(),
370 node_truth,
371 node_odometry,
372 node_front_end: slam.node_front_end_poses(),
373 final_node_poses: slam.node_poses(),
374 final_estimate: slam.pose(),
375 loop_closures: slam.loop_closures().to_vec(),
376 ambiguous_loop_rejections: slam.ambiguous_loop_rejections(),
377 walls,
378 }
379}
380
381#[cfg(test)]
382mod tests {
383 use super::*;
384
385 #[test]
386 fn short_run_records_frames_and_nodes_consistently() {
387 let config = CorridorLoopConfig {
388 extra_distance: -70.0,
389 ..CorridorLoopConfig::default()
390 };
391 let run = run_corridor_loop(&config, LidarGraphSlamConfig::default());
392 assert_eq!(run.frames.len(), corridor_loop_deltas(&config).len() + 1);
393 let nodes = run.final_node_poses.len();
394 assert!(nodes > 5);
395 assert_eq!(run.node_truth.len(), nodes);
396 assert_eq!(run.node_odometry.len(), nodes);
397 assert_eq!(run.node_front_end.len(), nodes);
398 assert_eq!(run.node_scans.len(), nodes);
399 assert_eq!(run.frames.last().map(|f| f.node_poses.len()), Some(nodes));
400 assert!(run.loop_closures.is_empty());
402 assert!(run.first_loop_frame().is_none());
403 }
404
405 #[test]
406 fn lap_deltas_close_the_rectangle() {
407 let config = CorridorLoopConfig {
408 extra_distance: 0.0,
409 ..CorridorLoopConfig::default()
410 };
411 let end = corridor_loop_deltas(&config)
412 .into_iter()
413 .fold(corridor_loop_start(), compose_pose);
414 let start = corridor_loop_start();
415 assert!((end.x - start.x).hypot(end.y - start.y) < 0.25);
416 }
417}