1use nalgebra::Vector2;
23use rand::{rngs::StdRng, Rng, SeedableRng};
24use rand_distr::{Distribution, Normal};
25use rust_robotics_core::Pose2D;
26
27use crate::lidar_occupancy::{CellState, OccupancyGrid};
28use crate::scan_to_map::compose_pose;
29
30#[derive(Debug, Clone, Copy, PartialEq)]
32pub struct LidarMclConfig {
33 pub particles: usize,
35 pub translation_noise: f64,
37 pub rotation_noise: f64,
39 pub rotation_noise_per_meter: f64,
41 pub hit_sigma: f64,
43 pub random_weight: f64,
45 pub beam_stride: usize,
47 pub max_field_distance: f64,
49 pub independent_beams: f64,
53 pub poor_fit: f64,
56 pub reset_fraction: f64,
58 pub reset_candidates: usize,
61 pub min_injection: f64,
65 pub global_oversampling: usize,
68 pub scan_match_candidates: usize,
73 pub scan_match_interval: usize,
75 pub scan_match_share: f64,
78 pub alpha_slow: f64,
81 pub alpha_fast: f64,
82 pub seed: u64,
84}
85
86impl Default for LidarMclConfig {
87 fn default() -> Self {
88 Self {
89 particles: 600,
90 translation_noise: 0.1,
91 rotation_noise: 0.1,
92 rotation_noise_per_meter: 0.05,
93 hit_sigma: 0.12,
94 random_weight: 0.05,
95 beam_stride: 6,
96 max_field_distance: 1.5,
97 independent_beams: 8.0,
98 poor_fit: 0.6,
99 reset_fraction: 0.1,
100 reset_candidates: 20,
101 min_injection: 0.01,
102 global_oversampling: 10,
103 scan_match_candidates: 8,
104 scan_match_interval: 20,
105 scan_match_share: 0.5,
106 alpha_slow: 0.01,
107 alpha_fast: 0.3,
108 seed: 3,
109 }
110 }
111}
112
113#[derive(Debug, Clone, Copy, PartialEq)]
115pub struct Particle {
116 pub pose: Pose2D,
117 pub weight: f64,
118}
119
120#[derive(Debug, Clone)]
122pub struct LidarMcl {
123 config: LidarMclConfig,
124 grid: OccupancyGrid,
125 free_cells: Vec<(usize, usize)>,
126 particles: Vec<Particle>,
127 rng: StdRng,
128 w_slow: f64,
129 w_fast: f64,
130 fit: f64,
131 global_pending: bool,
133 log_fine: Vec<f32>,
136 log_coarse: Vec<f32>,
137 candidates: Vec<(Pose2D, f64)>,
138 updates_since_candidates: usize,
139 match_job: Option<MatchJob>,
141 alternative_better: bool,
144}
145
146const BETTER_FIT_MARGIN: f64 = 0.03;
149
150const PROBE_PRIOR: f64 = 0.05;
152
153const MATCH_CELLS_PER_UPDATE: usize = 250;
156
157#[derive(Debug, Clone)]
159struct MatchJob {
160 fine: Vec<Vector2<f64>>,
161 coarse: Vec<Vector2<f64>>,
162 cells: Vec<(usize, usize)>,
163 cursor: usize,
164 scored: Vec<(Pose2D, f64)>,
165 count: usize,
166 motion: Pose2D,
168}
169
170const MATCH_STEP: f64 = 0.3;
172const MATCH_HEADINGS: usize = 48;
174const MATCH_COARSE_SIGMA: f64 = 0.35;
177const MATCH_DISTINCT_XY: f64 = 0.6;
179const MATCH_DISTINCT_YAW: f64 = 0.35;
180
181impl LidarMcl {
182 pub fn new(grid: OccupancyGrid, config: LidarMclConfig) -> Self {
185 let field = grid.distance_field(config.max_field_distance);
186 let log_table = |sigma: f64| -> Vec<f32> {
187 field
188 .iter()
189 .map(|&distance| {
190 let distance = f64::from(distance);
191 let hit = (-distance * distance / (2.0 * sigma * sigma)).exp();
192 (hit + config.random_weight).ln() as f32
193 })
194 .collect()
195 };
196 let log_fine = log_table(config.hit_sigma);
197 let log_coarse = log_table(MATCH_COARSE_SIGMA);
198 let (width, height) = grid.size();
199 let free_cells = (0..height)
200 .flat_map(|y| (0..width).map(move |x| (x, y)))
201 .filter(|&(x, y)| grid.state(x, y) == CellState::Free)
202 .collect();
203 let mut mcl = Self {
204 config,
205 grid,
206 free_cells,
207 particles: Vec::new(),
208 rng: StdRng::seed_from_u64(config.seed),
209 w_slow: 0.0,
210 w_fast: 0.0,
211 fit: 0.0,
212 global_pending: false,
213 log_fine,
214 log_coarse,
215 candidates: Vec::new(),
216 updates_since_candidates: 0,
217 match_job: None,
218 alternative_better: false,
219 };
220 mcl.initialize_global();
221 mcl
222 }
223
224 pub fn grid(&self) -> &OccupancyGrid {
226 &self.grid
227 }
228
229 pub fn particles(&self) -> &[Particle] {
231 &self.particles
232 }
233
234 fn random_free_pose(&mut self) -> Pose2D {
235 if self.free_cells.is_empty() {
236 return Pose2D::origin();
237 }
238 let (x, y) = self.free_cells[self.rng.random_range(0..self.free_cells.len())];
239 let center = self.grid.cell_center(x, y);
240 let half = self.grid.resolution() / 2.0;
241 Pose2D::new(
242 center.x + self.rng.random_range(-half..half),
243 center.y + self.rng.random_range(-half..half),
244 self.rng
245 .random_range(-std::f64::consts::PI..std::f64::consts::PI),
246 )
247 }
248
249 pub fn initialize_global(&mut self) {
251 self.alternative_better = false;
253 let count = self.config.particles.max(1);
254 let weight = 1.0 / count as f64;
255 self.particles = (0..count)
256 .map(|_| Particle {
257 pose: self.random_free_pose(),
258 weight,
259 })
260 .collect();
261 self.w_slow = 0.0;
262 self.w_fast = 0.0;
263 self.global_pending = true;
264 }
265
266 pub fn initialize_at(&mut self, pose: Pose2D, xy: f64, yaw: f64) {
268 self.alternative_better = false;
270 let count = self.config.particles.max(1);
271 let position = Normal::new(0.0, xy.max(1.0e-9)).expect("finite sigma");
272 let heading = Normal::new(0.0, yaw.max(1.0e-9)).expect("finite sigma");
273 self.particles = (0..count)
274 .map(|_| Particle {
275 pose: Pose2D::new(
276 pose.x + position.sample(&mut self.rng),
277 pose.y + position.sample(&mut self.rng),
278 pose.yaw + heading.sample(&mut self.rng),
279 ),
280 weight: 1.0 / count as f64,
281 })
282 .collect();
283 self.global_pending = false;
284 }
285
286 pub fn predict(&mut self, odom_delta: Pose2D) {
288 let distance = odom_delta.x.hypot(odom_delta.y);
289 let turn = odom_delta.yaw.abs();
290 let sigma_xy = self.config.translation_noise * distance;
291 let sigma_yaw =
292 self.config.rotation_noise * turn + self.config.rotation_noise_per_meter * distance;
293 let xy = Normal::new(0.0, sigma_xy.max(1.0e-9)).expect("finite sigma");
294 let yaw = Normal::new(0.0, sigma_yaw.max(1.0e-9)).expect("finite sigma");
295 for particle in &mut self.particles {
296 let noisy = Pose2D::new(
297 odom_delta.x + xy.sample(&mut self.rng),
298 odom_delta.y + xy.sample(&mut self.rng),
299 odom_delta.yaw + yaw.sample(&mut self.rng),
300 );
301 particle.pose = compose_pose(particle.pose, noisy);
302 }
303 if let Some(job) = &mut self.match_job {
304 job.motion = compose_pose(job.motion, odom_delta);
305 }
306 }
307
308 fn mean_log(&self, table: &[f32], pose: Pose2D, beams: &[Vector2<f64>]) -> f64 {
310 let (width, _) = self.grid.size();
311 let (sin, cos) = pose.yaw.sin_cos();
312 let outside = {
313 let distance = self.config.max_field_distance;
314 let sigma = self.config.hit_sigma;
315 let hit = (-distance * distance / (2.0 * sigma * sigma)).exp();
316 (hit + self.config.random_weight).ln()
317 };
318 let sum: f64 = beams
319 .iter()
320 .map(|beam| {
321 let world = Vector2::new(
322 pose.x + cos * beam.x - sin * beam.y,
323 pose.y + sin * beam.x + cos * beam.y,
324 );
325 self.grid
326 .cell_of(world)
327 .map_or(outside, |(x, y)| f64::from(table[y * width + x]))
328 })
329 .sum();
330 sum / beams.len().max(1) as f64
331 }
332
333 fn beam_likelihood(&self, pose: Pose2D, beams: &[Vector2<f64>]) -> f64 {
335 if beams.is_empty() {
336 return 1.0;
337 }
338 self.mean_log(&self.log_fine, pose, beams).exp() / (1.0 + self.config.random_weight)
339 }
340
341 pub fn scan_match_candidates(
347 &self,
348 scan_body: &[Vector2<f64>],
349 count: usize,
350 ) -> Vec<(Pose2D, f64)> {
351 let Some(mut job) = self.start_match(scan_body, count) else {
352 return Vec::new();
353 };
354 while !self.advance_match(&mut job, usize::MAX) {}
355 self.finish_match(job)
356 }
357
358 fn start_match(&self, scan_body: &[Vector2<f64>], count: usize) -> Option<MatchJob> {
360 let stride = self.config.beam_stride.max(1);
361 let fine: Vec<Vector2<f64>> = scan_body.iter().step_by(stride).copied().collect();
362 let coarse: Vec<Vector2<f64>> = scan_body.iter().step_by(stride * 2).copied().collect();
363 if fine.is_empty() || count == 0 {
364 return None;
365 }
366 let lattice = ((MATCH_STEP / self.grid.resolution()).round() as usize).max(1);
367 let cells = self
368 .free_cells
369 .iter()
370 .filter(|(x, y)| x % lattice == 0 && y % lattice == 0)
371 .copied()
372 .collect();
373 Some(MatchJob {
374 fine,
375 coarse,
376 cells,
377 cursor: 0,
378 scored: Vec::new(),
379 count,
380 motion: Pose2D::origin(),
381 })
382 }
383
384 fn advance_match(&self, job: &mut MatchJob, max_cells: usize) -> bool {
387 let end = job.cursor.saturating_add(max_cells).min(job.cells.len());
388 for &(x, y) in &job.cells[job.cursor..end] {
389 let center = self.grid.cell_center(x, y);
390 for i in 0..MATCH_HEADINGS {
391 let yaw = -std::f64::consts::PI
392 + std::f64::consts::TAU * i as f64 / MATCH_HEADINGS as f64;
393 let pose = Pose2D::new(center.x, center.y, yaw);
394 job.scored
395 .push((pose, self.mean_log(&self.log_coarse, pose, &job.coarse)));
396 }
397 }
398 job.cursor = end;
399 let keep = (job.count * 6).min(job.scored.len());
401 if keep > 0 && job.scored.len() > keep {
402 job.scored
403 .select_nth_unstable_by(keep - 1, |a, b| b.1.total_cmp(&a.1));
404 job.scored.truncate(keep);
405 }
406 job.cursor == job.cells.len()
407 }
408
409 fn finish_match(&self, job: MatchJob) -> Vec<(Pose2D, f64)> {
412 let MatchJob {
413 fine,
414 scored,
415 count,
416 motion,
417 ..
418 } = job;
419 let mut refined: Vec<(Pose2D, f64)> = scored
420 .into_iter()
421 .map(|(pose, _)| self.refine(pose, &fine))
422 .map(|(pose, score)| (compose_pose(pose, motion), score))
423 .collect();
424 refined.sort_by(|a, b| b.1.total_cmp(&a.1));
425 let mut distinct: Vec<(Pose2D, f64)> = Vec::with_capacity(count);
426 for (pose, score) in refined {
427 let duplicate = distinct.iter().any(|(kept, _)| {
428 let yaw = (kept.yaw - pose.yaw)
429 .sin()
430 .atan2((kept.yaw - pose.yaw).cos());
431 (kept.x - pose.x).hypot(kept.y - pose.y) < MATCH_DISTINCT_XY
432 && yaw.abs() < MATCH_DISTINCT_YAW
433 });
434 if !duplicate {
435 distinct.push((pose, score));
436 if distinct.len() == count {
437 break;
438 }
439 }
440 }
441 distinct
442 .into_iter()
443 .map(|(pose, score)| (pose, score.exp() / (1.0 + self.config.random_weight)))
444 .collect()
445 }
446
447 fn refine(&self, mut pose: Pose2D, beams: &[Vector2<f64>]) -> (Pose2D, f64) {
449 let mut score = self.mean_log(&self.log_fine, pose, beams);
450 let (mut step_xy, mut step_yaw) = (
451 MATCH_STEP / 2.0,
452 std::f64::consts::PI / MATCH_HEADINGS as f64,
453 );
454 for _ in 0..4 {
455 loop {
456 let mut improved = false;
457 for (dx, dy, dyaw) in [
458 (step_xy, 0.0, 0.0),
459 (-step_xy, 0.0, 0.0),
460 (0.0, step_xy, 0.0),
461 (0.0, -step_xy, 0.0),
462 (0.0, 0.0, step_yaw),
463 (0.0, 0.0, -step_yaw),
464 ] {
465 let candidate = Pose2D::new(pose.x + dx, pose.y + dy, pose.yaw + dyaw);
466 let candidate_score = self.mean_log(&self.log_fine, candidate, beams);
467 if candidate_score > score + 1.0e-9 {
468 pose = candidate;
469 score = candidate_score;
470 improved = true;
471 }
472 }
473 if !improved {
474 break;
475 }
476 }
477 step_xy /= 2.0;
478 step_yaw /= 2.0;
479 }
480 (pose, score)
481 }
482
483 pub fn candidates(&self) -> &[(Pose2D, f64)] {
485 &self.candidates
486 }
487
488 fn candidate_pose(&mut self) -> Option<Pose2D> {
490 if self.candidates.is_empty() {
491 return None;
492 }
493 let (pose, _) = self.candidates[self.rng.random_range(0..self.candidates.len())];
494 let xy = Normal::new(0.0, 0.08).expect("finite sigma");
495 let yaw = Normal::new(0.0, 0.04).expect("finite sigma");
496 Some(Pose2D::new(
497 pose.x + xy.sample(&mut self.rng),
498 pose.y + xy.sample(&mut self.rng),
499 pose.yaw + yaw.sample(&mut self.rng),
500 ))
501 }
502
503 fn alternative_is_better(&self, beams: &[Vector2<f64>]) -> bool {
507 let (estimate, _) = self.estimate();
508 let current = self.refine(estimate, beams).1.exp() / (1.0 + self.config.random_weight);
509 self.candidates
510 .iter()
511 .filter(|(pose, _)| {
512 let yaw = (pose.yaw - estimate.yaw)
513 .sin()
514 .atan2((pose.yaw - estimate.yaw).cos());
515 (pose.x - estimate.x).hypot(pose.y - estimate.y) > MATCH_DISTINCT_XY
516 || yaw.abs() > MATCH_DISTINCT_YAW
517 })
518 .any(|(_, fit)| *fit > current + BETTER_FIT_MARGIN)
519 }
520
521 fn injected_pose(&mut self, beams: &[Vector2<f64>]) -> Pose2D {
527 if self.alternative_better
528 && self
529 .rng
530 .random_bool(self.config.scan_match_share.clamp(0.0, 1.0))
531 {
532 if let Some(pose) = self.candidate_pose() {
533 return pose;
534 }
535 }
536 self.scored_free_pose(beams)
537 }
538
539 pub fn update(&mut self, scan_body: &[Vector2<f64>]) {
541 let beams: Vec<Vector2<f64>> = scan_body
542 .iter()
543 .step_by(self.config.beam_stride.max(1))
544 .copied()
545 .collect();
546 if beams.is_empty() || self.particles.is_empty() {
547 return;
548 }
549 self.updates_since_candidates += 1;
550 if self.config.scan_match_candidates > 0 {
551 if self.global_pending {
552 self.match_job = None;
554 self.candidates =
555 self.scan_match_candidates(scan_body, self.config.scan_match_candidates);
556 self.updates_since_candidates = 0;
557 } else if let Some(mut job) = self.match_job.take() {
558 if self.advance_match(&mut job, MATCH_CELLS_PER_UPDATE) {
560 self.candidates = self.finish_match(job);
561 self.alternative_better = self.alternative_is_better(&beams);
562 } else {
563 self.match_job = Some(job);
564 }
565 } else if self.updates_since_candidates >= self.config.scan_match_interval.max(1) {
566 self.match_job = self.start_match(scan_body, self.config.scan_match_candidates);
567 self.updates_since_candidates = 0;
568 }
569 }
570 if self.global_pending {
571 self.seed_from_scan(&beams);
572 return;
573 }
574 let probes =
575 (self.particles.len() as f64 * self.config.min_injection.clamp(0.0, 1.0)) as usize;
576 for _ in 0..probes {
577 let slot = self.rng.random_range(0..self.particles.len());
578 let pose = self.injected_pose(&beams);
579 self.particles[slot] = Particle {
584 pose,
585 weight: PROBE_PRIOR / self.particles.len() as f64,
586 };
587 }
588 let exponent = self.config.independent_beams.max(1.0);
592 let likelihoods: Vec<f64> = self
593 .particles
594 .iter()
595 .map(|particle| {
596 let position = Vector2::new(particle.pose.x, particle.pose.y);
597 let likelihood = self.beam_likelihood(particle.pose, &beams);
598 if self.grid.state_at(position) == CellState::Occupied {
599 likelihood * 0.1
600 } else {
601 likelihood
602 }
603 })
604 .collect();
605 let average = likelihoods.iter().sum::<f64>() / likelihoods.len() as f64;
606 let best = likelihoods
607 .iter()
608 .copied()
609 .fold(0.0, f64::max)
610 .max(1.0e-300);
611 for (particle, likelihood) in self.particles.iter_mut().zip(&likelihoods) {
613 particle.weight *= (likelihood / best).powf(exponent);
614 }
615 let total: f64 = self.particles.iter().map(|p| p.weight).sum();
616 if total > 0.0 && total.is_finite() {
617 for particle in &mut self.particles {
618 particle.weight /= total;
619 }
620 } else {
621 let uniform = 1.0 / self.particles.len() as f64;
622 for particle in &mut self.particles {
623 particle.weight = uniform;
624 }
625 }
626
627 if self.w_slow == 0.0 {
628 self.w_slow = average;
629 self.w_fast = average;
630 } else {
631 self.w_slow += self.config.alpha_slow * (average - self.w_slow);
632 self.w_fast += self.config.alpha_fast * (average - self.w_fast);
633 }
634 let fit: f64 = self
636 .particles
637 .iter()
638 .zip(&likelihoods)
639 .map(|(particle, likelihood)| particle.weight * likelihood)
640 .sum();
641 self.fit = fit;
642 let reset = if fit < self.config.poor_fit {
643 self.config.reset_fraction
644 } else {
645 0.0
646 };
647 let mut inject = (1.0 - self.w_fast / self.w_slow).max(reset);
648 if self.config.scan_match_candidates > 0 && !self.alternative_better {
653 inject = 0.0;
654 }
655 let effective = 1.0
656 / self
657 .particles
658 .iter()
659 .map(|p| p.weight * p.weight)
660 .sum::<f64>();
661 if inject > 0.0 || effective < 0.5 * self.particles.len() as f64 {
662 self.resample(inject, &beams);
663 }
664 }
665
666 fn seed_from_scan(&mut self, beams: &[Vector2<f64>]) {
669 let count = self.config.particles.max(1);
670 let exponent = self.config.independent_beams.max(1.0);
671 let candidates: Vec<(Pose2D, f64)> = (0..count * self.config.global_oversampling.max(1))
672 .map(|_| {
673 let pose = self.random_free_pose();
674 (pose, self.beam_likelihood(pose, beams))
675 })
676 .collect();
677 let best = candidates
678 .iter()
679 .map(|(_, likelihood)| *likelihood)
680 .fold(1.0e-300, f64::max);
681 let total: f64 = candidates
682 .iter()
683 .map(|(_, likelihood)| (likelihood / best).powf(exponent))
684 .sum();
685 self.particles = candidates
686 .iter()
687 .map(|(pose, likelihood)| Particle {
688 pose: *pose,
689 weight: (likelihood / best).powf(exponent) / total,
690 })
691 .collect();
692 self.resample_to(count, 0.0, beams);
693 let share = (count as f64 * self.config.scan_match_share.clamp(0.0, 1.0) / 2.0) as usize;
696 for slot in 0..share.min(count) {
697 if let Some(pose) = self.candidate_pose() {
698 self.particles[slot].pose = pose;
699 }
700 }
701 let average = candidates.iter().map(|(_, l)| l).sum::<f64>() / candidates.len() as f64;
702 self.w_slow = average;
703 self.w_fast = average;
704 self.fit = best;
705 self.global_pending = false;
706 }
707
708 fn scored_free_pose(&mut self, beams: &[Vector2<f64>]) -> Pose2D {
710 let mut best = (f64::NEG_INFINITY, Pose2D::origin());
711 for _ in 0..self.config.reset_candidates.max(1) {
712 let pose = self.random_free_pose();
713 let score = self.beam_likelihood(pose, beams);
714 if score > best.0 {
715 best = (score, pose);
716 }
717 }
718 best.1
719 }
720
721 fn resample(&mut self, inject: f64, beams: &[Vector2<f64>]) {
724 self.resample_to(self.particles.len(), inject, beams);
725 }
726
727 fn resample_to(&mut self, count: usize, inject: f64, beams: &[Vector2<f64>]) {
730 let step = 1.0 / count as f64;
731 let mut target = self.rng.random_range(0.0..step);
732 let mut cumulative = self.particles[0].weight;
733 let mut index = 0;
734 let mut next = Vec::with_capacity(count);
735 for _ in 0..count {
736 while target > cumulative && index + 1 < self.particles.len() {
737 index += 1;
738 cumulative += self.particles[index].weight;
739 }
740 let pose = if self.rng.random_bool(inject.clamp(0.0, 1.0)) {
741 self.injected_pose(beams)
742 } else {
743 self.particles[index].pose
744 };
745 next.push(Particle { pose, weight: step });
746 target += step;
747 }
748 self.particles = next;
749 }
750
751 pub fn fit(&self) -> f64 {
754 self.fit
755 }
756
757 pub fn estimate(&self) -> (Pose2D, f64) {
760 const BIN_XY: f64 = 1.0;
765 const BIN_YAW: f64 = std::f64::consts::FRAC_PI_4;
766 let bin = |pose: &Pose2D| {
767 (
768 (pose.x / BIN_XY).floor() as i64,
769 (pose.y / BIN_XY).floor() as i64,
770 (pose.yaw.rem_euclid(std::f64::consts::TAU) / BIN_YAW).floor() as i64,
771 )
772 };
773 let mut bins: std::collections::HashMap<(i64, i64, i64), f64> =
774 std::collections::HashMap::new();
775 for particle in &self.particles {
776 *bins.entry(bin(&particle.pose)).or_insert(0.0) += particle.weight;
777 }
778 let Some(&heaviest) = bins
779 .iter()
780 .max_by(|a, b| a.1.total_cmp(b.1))
781 .map(|(key, _)| key)
782 else {
783 return (Pose2D::origin(), f64::INFINITY);
784 };
785 let seed = self
786 .particles
787 .iter()
788 .filter(|particle| bin(&particle.pose) == heaviest)
789 .fold((0.0, 0.0, 0.0, 0.0, 0.0), |(x, y, s, c, w), p| {
790 (
791 x + p.weight * p.pose.x,
792 y + p.weight * p.pose.y,
793 s + p.weight * p.pose.yaw.sin(),
794 c + p.weight * p.pose.yaw.cos(),
795 w + p.weight,
796 )
797 });
798 let seed_weight = seed.4.max(1.0e-300);
799 let center = Pose2D::new(
800 seed.0 / seed_weight,
801 seed.1 / seed_weight,
802 seed.2.atan2(seed.3),
803 );
804 let (mut x, mut y, mut sin, mut cos, mut total) = (0.0, 0.0, 0.0, 0.0, 0.0);
805 for particle in &self.particles {
806 let yaw = (particle.pose.yaw - center.yaw)
807 .sin()
808 .atan2((particle.pose.yaw - center.yaw).cos());
809 let near = (particle.pose.x - center.x).hypot(particle.pose.y - center.y) < BIN_XY
810 && yaw.abs() < BIN_YAW;
811 if near {
812 let w = particle.weight;
813 x += w * particle.pose.x;
814 y += w * particle.pose.y;
815 sin += w * particle.pose.yaw.sin();
816 cos += w * particle.pose.yaw.cos();
817 total += w;
818 }
819 }
820 let total = total.max(1.0e-300);
821 let mean = Pose2D::new(x / total, y / total, sin.atan2(cos));
822 let all: f64 = self
823 .particles
824 .iter()
825 .map(|p| p.weight)
826 .sum::<f64>()
827 .max(1.0e-300);
828 let spread = (self
829 .particles
830 .iter()
831 .map(|p| p.weight * ((p.pose.x - mean.x).powi(2) + (p.pose.y - mean.y).powi(2)))
832 .sum::<f64>()
833 / all)
834 .sqrt();
835 (mean, spread)
836 }
837}
838
839#[cfg(test)]
840mod tests {
841 use super::*;
842 use crate::lidar_occupancy::OccupancyConfig;
843 use crate::scan_to_map::{ranges_to_points, ray_cast_ranges, relative_pose, LineSegment};
844
845 fn polygon(corners: &[(f64, f64)]) -> Vec<LineSegment> {
846 (0..corners.len())
847 .map(|i| {
848 let (ax, ay) = corners[i];
849 let (bx, by) = corners[(i + 1) % corners.len()];
850 LineSegment::new(Vector2::new(ax, ay), Vector2::new(bx, by))
851 })
852 .collect()
853 }
854
855 fn world() -> Vec<LineSegment> {
857 let mut walls = polygon(&[(-6.0, -4.0), (6.0, -4.0), (6.0, 4.0), (-6.0, 4.0)]);
858 walls.extend(polygon(&[(1.0, 1.0), (2.0, 1.4), (1.6, 2.4), (0.6, 2.0)]));
859 walls.extend(polygon(&[(-4.5, -3.0), (-3.7, -3.0), (-3.7, -2.2)]));
860 walls.push(LineSegment::new(
861 Vector2::new(3.5, -4.0),
862 Vector2::new(3.5, -1.5),
863 ));
864 walls
865 }
866
867 fn scan(pose: Pose2D) -> Vec<Vector2<f64>> {
868 ranges_to_points(&ray_cast_ranges(pose, &world(), 180, 12.0))
869 }
870
871 fn map() -> OccupancyGrid {
872 let poses = [
873 Pose2D::new(-3.0, 0.0, 0.0),
874 Pose2D::new(0.0, -2.0, 1.0),
875 Pose2D::new(3.0, 2.0, -2.0),
876 Pose2D::new(-1.0, 3.0, 0.5),
877 ];
878 let scans: Vec<_> = poses.iter().map(|pose| scan(*pose)).collect();
879 OccupancyGrid::from_scans(
880 poses.iter().copied().zip(scans.iter().map(Vec::as_slice)),
881 0.5,
882 OccupancyConfig::default(),
883 )
884 }
885
886 #[test]
888 fn reinitializing_drops_the_old_verdict_on_alternatives() {
889 let mut mcl = LidarMcl::new(map(), LidarMclConfig::default());
890 mcl.alternative_better = true;
891 mcl.initialize_at(Pose2D::new(1.0, 0.0, 0.0), 0.1, 0.05);
892 assert!(!mcl.alternative_better);
893 mcl.alternative_better = true;
894 mcl.initialize_global();
895 assert!(!mcl.alternative_better);
896 }
897
898 fn drive(mcl: &mut LidarMcl, start: Pose2D, steps: usize) -> Pose2D {
899 let mut truth = start;
900 let step = Pose2D::new(0.1, 0.0, 0.04);
901 for _ in 0..steps {
902 truth = compose_pose(truth, step);
903 mcl.predict(step);
904 mcl.update(&scan(truth));
905 }
906 truth
907 }
908
909 #[test]
910 fn scan_matching_finds_the_pose_and_its_look_alikes() {
911 let mcl = LidarMcl::new(map(), LidarMclConfig::default());
912 let truth = Pose2D::new(-2.0, -1.0, 0.3);
913 let candidates = mcl.scan_match_candidates(&scan(truth), 4);
914 let (best, fit) = candidates[0];
915 let error = relative_pose(truth, best);
916 assert!(error.x.hypot(error.y) < 0.15, "best {best:?}");
917 assert!(error.yaw.abs() < 0.05, "best {best:?}");
918 assert!(fit > 0.8, "fit {fit}");
919
920 let room = polygon(&[(-5.0, -3.0), (5.0, -3.0), (5.0, 3.0), (-5.0, 3.0)]);
922 let pose = Pose2D::new(2.0, 1.0, 0.4);
923 let scan_of = |pose: Pose2D| ranges_to_points(&ray_cast_ranges(pose, &room, 180, 12.0));
924 let grid = OccupancyGrid::from_scans(
925 [(pose, scan_of(pose).as_slice())],
926 0.5,
927 OccupancyConfig::default(),
928 );
929 let mcl = LidarMcl::new(grid, LidarMclConfig::default());
930 let candidates = mcl.scan_match_candidates(&scan_of(pose), 4);
931 let mirror = Pose2D::new(-2.0, -1.0, 0.4 + std::f64::consts::PI);
932 for expected in [pose, mirror] {
933 assert!(
934 candidates.iter().any(|(candidate, _)| {
935 let error = relative_pose(expected, *candidate);
936 error.x.hypot(error.y) < 0.2 && error.yaw.abs() < 0.1
937 }),
938 "{expected:?} missing from {candidates:?}"
939 );
940 }
941 }
942
943 #[test]
944 fn global_localization_converges_from_uniform_particles() {
945 let mut mcl = LidarMcl::new(map(), LidarMclConfig::default());
946 let truth = drive(&mut mcl, Pose2D::new(-2.0, -1.0, 0.3), 40);
947 let (estimate, spread) = mcl.estimate();
948 let error = relative_pose(truth, estimate);
949 assert!(error.x.hypot(error.y) < 0.2, "error {error:?}");
950 assert!(error.yaw.abs() < 0.1, "yaw error {}", error.yaw);
951 assert!(spread < 0.3, "spread {spread}");
952 }
953
954 #[test]
955 fn recovers_from_a_kidnapping() {
956 let mut mcl = LidarMcl::new(map(), LidarMclConfig::default());
957 let start = Pose2D::new(-2.0, -1.0, 0.3);
958 mcl.initialize_at(start, 0.05, 0.02);
959 let _ = drive(&mut mcl, start, 10);
960 let truth = drive(&mut mcl, Pose2D::new(3.0, 1.0, 2.5), 80);
962 let error = relative_pose(truth, mcl.estimate().0);
963 assert!(error.x.hypot(error.y) < 0.3, "error {error:?}");
964 }
965}