Skip to main content

rust_robotics_slam/
lidar_loop_scenario.rs

1//! Deterministic corridor-loop scenario for [`LidarGraphSlam`].
2//!
3//! A robot drives slightly more than one lap of a 3 m wide rectangular
4//! corridor (30 × 20 m outer walls, 24 × 14 m inner block) with pillars along
5//! the walls — except the top corridor, which is left featureless so the
6//! scan-to-map front end cannot observe forward motion there. Wheel odometry
7//! over-reports distance and drifts in yaw; the simulated 2D LiDAR adds range
8//! noise. [`run_corridor_loop`] records every step so examples, gallery
9//! renderers, and the playground can replay the same run.
10
11use 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/// Pillar arrangement along the bottom corridor.
22#[derive(Debug, Clone, Copy, PartialEq)]
23pub enum CorridorLayout {
24    /// Irregularly spaced pillars: every place looks different.
25    Irregular,
26    /// Identical pillars every `spacing` meters on both walls of the bottom
27    /// corridor: neighboring places look the same (perceptual aliasing).
28    Periodic { spacing: f64 },
29}
30
31/// Scenario parameters for [`run_corridor_loop`].
32#[derive(Debug, Clone, Copy, PartialEq)]
33pub struct CorridorLoopConfig {
34    /// Pillar arrangement along the bottom corridor.
35    pub layout: CorridorLayout,
36    /// Distance along the centerline (from the bottom-left start) at which
37    /// the robot starts \[m\].
38    pub start_offset: f64,
39    /// Distance travelled per step \[m\].
40    pub step: f64,
41    /// Turning radius at the corridor corners \[m\].
42    pub corner_radius: f64,
43    /// Distance driven past one full lap \[m\].
44    pub extra_distance: f64,
45    /// Odometry distance scale factor (1.03 = over-reports by 3 %).
46    pub odometry_scale: f64,
47    /// Odometry yaw drift \[rad/m\].
48    pub odometry_yaw_drift: f64,
49    /// Standard deviation of per-step odometry noise \[m, rad\].
50    pub odometry_noise: f64,
51    /// Standard deviation of LiDAR range noise \[m\].
52    pub range_noise: f64,
53    /// LiDAR beams over 360°.
54    pub beams: usize,
55    /// LiDAR maximum range \[m\].
56    pub max_range: f64,
57    /// RNG seed for odometry and range noise.
58    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/// State recorded after one [`LidarGraphSlam::update`].
81#[derive(Debug, Clone, PartialEq)]
82pub struct CorridorLoopFrame {
83    /// Ground-truth pose.
84    pub truth: Pose2D,
85    /// Dead-reckoned wheel odometry.
86    pub odometry: Pose2D,
87    /// Scan-to-map front-end pose (no loop closure).
88    pub front_end: Pose2D,
89    /// Graph SLAM pose estimate.
90    pub estimate: Pose2D,
91    /// Optimized node poses at this step.
92    pub node_poses: Vec<Pose2D>,
93    /// Loop closure accepted at this step, if any.
94    pub loop_closure: Option<LoopClosure>,
95    /// Whether the graph was re-optimized at this step.
96    pub optimized: bool,
97    /// Noisy body-frame scan observed at this step.
98    pub scan: Vec<Vector2<f64>>,
99}
100
101/// A full recorded run of the corridor-loop scenario.
102#[derive(Debug, Clone)]
103pub struct CorridorLoopRun {
104    /// Environment walls.
105    pub walls: Vec<LineSegment>,
106    /// Length of one lap of the corridor centerline \[m\].
107    pub lap_length: f64,
108    /// Total distance driven \[m\].
109    pub distance: f64,
110    /// One frame per update, starting with the initial scan.
111    pub frames: Vec<CorridorLoopFrame>,
112    /// Body-frame scan of each node.
113    pub node_scans: Vec<Vec<Vector2<f64>>>,
114    /// Ground truth at each node.
115    pub node_truth: Vec<Pose2D>,
116    /// Wheel odometry at each node.
117    pub node_odometry: Vec<Pose2D>,
118    /// Front-end pose at each node.
119    pub node_front_end: Vec<Pose2D>,
120    /// Node poses after a final optimization over all edges.
121    pub final_node_poses: Vec<Pose2D>,
122    /// Pose estimate after the final optimization.
123    pub final_estimate: Pose2D,
124    /// All accepted loop closures.
125    pub loop_closures: Vec<LoopClosure>,
126    /// Loop matches rejected as ambiguous (perceptual aliasing).
127    pub ambiguous_loop_rejections: usize,
128}
129
130impl CorridorLoopRun {
131    /// Whether `closure`'s measured relative pose is off from ground truth by
132    /// more than `tolerance` meters (a false loop closure).
133    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    /// Number of false loop closures (see [`Self::is_wrong_loop`]).
140    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    /// Index of the first frame that accepted a loop closure.
148    pub fn first_loop_frame(&self) -> Option<usize> {
149        self.frames
150            .iter()
151            .position(|frame| frame.loop_closure.is_some())
152    }
153}
154
155/// Perceptual-aliasing variant: identical pillars every 2.5 m along the
156/// bottom corridor and a start in its middle, where no corner is in LiDAR
157/// range, so loop matches can lock onto the wrong pillar.
158pub 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
167/// Planar position RMSE between two equally indexed pose sequences \[m\].
168pub 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
189/// Walls of the corridor loop: outer 30 × 20 m, inner block 24 × 14 m, and
190/// pillars at irregular spacing except along the top corridor.
191pub fn corridor_loop_walls() -> Vec<LineSegment> {
192    corridor_loop_walls_for(CorridorLayout::Irregular)
193}
194
195/// Walls of the corridor loop with the given bottom-corridor `layout`.
196pub 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    // The top corridor is left featureless: in its middle the LiDAR sees two
220    // parallel walls only, so along-corridor motion is unobservable and the
221    // odometry scale error leaks into the scan-to-map front end.
222    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
233/// Start pose of the scenario (bottom corridor, heading +x).
234pub fn corridor_loop_start() -> Pose2D {
235    Pose2D::new(-12.0, -8.5, 0.0)
236}
237
238/// Length of one lap of the corridor centerline \[m\].
239pub 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
243/// Start pose of `config`: `start_offset` meters along the centerline.
244pub 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
250/// Body-frame step deltas along the corridor centerline (rounded rectangle),
251/// starting `start_offset` meters along it and covering one lap plus
252/// `extra_distance`.
253pub 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
262/// Deltas from the bottom-left start covering at least `total` meters.
263fn 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        // `half_lap` covers half the rectangle; the other half mirrors it.
279        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
294/// Runs [`LidarGraphSlam`] through the corridor loop and records every step.
295///
296/// # Panics
297///
298/// Panics if `odometry_noise` or `range_noise` is negative or not finite.
299pub 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    // Fold in loop edges that were consistent with the graph when added.
361    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        // No revisit yet, so nothing to close.
401        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}