[fmrico]

Fixes #150

The costmap AMCL now measures how well the particles explain the scan in absolute terms. A lost filter is detected even when its cloud is compact, and the worst particles are replaced with random ones when the match drops.

Changes

• LocalizationQuality keeps, from each correction, the best particle's score and the mean score: the fraction of scan points that land on obstacles of the map, before the weights are normalized.

  • match_score: the best score, smoothed.
  • quality_ratio: fast / slow average of the mean score (Augmented MCL). Near 1 while the match keeps its usual level, well below 1 right after it drops.
  • The averages start as the plain mean of the scores so far, so the first, unconverged corrections do not set the level.

• AMCLLocalizer:

  • publishes localizer.amcl.match_score and localizer.amcl.quality_ratio in the NavState;
  • in each reseed, the worst particles go to random free poses of map.base. Their number is 1 - quality_ratio times the population, capped by random_particles_max_fraction (0.25) and by the worse half. The population size does not change;
  • new parameters recovery_alpha_slow (0.001) and recovery_alpha_fast (0.1); either set to 0 disables the averages and the random particles;
  • a new initial pose resets the scores.

• AmclConvergenceEvaluator: an ERROR also for:

  • match_score below match_threshold (0.5; 0 disables it): the cloud does not match the map, a lost filter;
  • quality_ratio below quality_ratio_threshold (0.5): the match dropped, e.g. a kidnapping.

  Both are held for debounce_duration (2 s) first. The covariance check is unchanged and is still immediate. The diagnostic always carries the three values.

• AmclRelocalizeMitigation: the robot counts as relocalized when the covariance is low and, if the localizer reports it, match_score is at least match_threshold (0.5).

Measured in the Kobuki playground (home map)

Robot kidnapped in Gazebo, 7 m away, into another room:

	covariance	match_score	quality_ratio	true error
localized	0.001	0.75	1.07	0.06 m
right after the kidnapping	0.003-0.8	0.25-0.29	0.29	6.9 m
converged at a wrong pose	0.003	0.35	0.6-0.75	1.5 m

• The drop of the ratio flags the kidnapping within a few seconds.
• The random particles took the estimate from 6.9 m to 1.5 m of the true pose.
• The filter then converged at that wrong pose. The covariance reports "converged" there, while match_score (0.35 < 0.5) keeps reporting it lost.

match_threshold depends on the map, the sensor and the resolution. 0.5 fits this case and can be tuned per robot.

Tests

• costmap_localizer_quality_tests:
  • LocalizationQuality: the first level, the warm-up, a steady match, a drop and how the slow average catches up, a better match, the alphas set to 0, non-finite scores, reset;
  • the scores on a map telling a right pose from a compact wrong cloud;
  • the reseed placing the worst particles at random free cells, never on obstacles, keeping the population and the better half;
  • the caps.
• amcl_convergence_evaluator_tests:
  • a compact but lost cloud reported only after the debounce;
  • a good match clears it, and a short dip is not an error;
  • match_threshold: 0 disables the check;
  • a kidnapping;
  • a spread cloud is still an error at once.
• amcl_relocalize_mitigation_tests:
  • a compact cloud must also match the map;
  • without a score, only the covariance counts.
• colcon test of the package passes, lint included.