Understanding SLAM Algorithm for Robot Localization and Mapping

1. SLAM: Simultaneous Localization and Mapping: Part IIBY TIM BAILEY AND HUGH DURRANT-WHYTE Presented by Chang Young Kim These slides are based on: Probabilistic Robotics, S. Thrun, W. Burgard, D. Fox, MIT Press, 2005 Many images are also taken from Probabilistic Robotics. http://www.probabilistic-robotics.com

2. Overview • Review • SLAM • Reducing complexity • State Augmentation • Partitioned Updates • Sparsification • Data association • Batch Gating • SIFT • Multi-Hypothesis • Future works

3. What is SLAM? A robot is exploring an unknown, static environment. Given: • The robot’s controls • Observations of nearby features Estimate: • Map of features • Path of the robot

4. f g f f g u u u u z z z z = = x x x x 1 1 2 t t 1 1 2 = t t 1 1 2 t t : : ; ; : : : ; ; ; : : : ; : ; ; : : : ; ( ) µ z u x x y = t t t ; ; Terminology • Robot State (or pose): • Position and heading • Robot Controls: • Robot motion and manipulation • Sensor Measurements: • Range scans, images, etc. • Landmark or Map: • Landmarks or Map }

5. Terminology • Observation model: or • The probability of a measurement zt given that the robot is at position xt and map m. • Motion Model: • The posterior probability that action ut carries the robot from xt-1 to xt.

6. SLAM algorithm • Prediction • Update

7. EKF State Space Model • Prediction • Update where 7

8. Maintaining values: Bel(xt,m) and its covariance matrix Pt. Map with N landmarks:(3+2N)-dimensional Gaussian. EKF-SLAM 8

9. Overview • Review • SLAM • Reducing complexity • State Augmentation • Partitioned Updates • Sparsification • Data association • Batch Gating • SIFT • Multi-Hypothesis • Future works

10. Complexity O(N3) with N landmarks due to the covariance matrix and matrix multiplication of Jacobian. Can handle hundreds of dimensions? It can be reduced by approximation methods: State Augmentation for the prediction stage Partitioned Updates for the update stage Sparsification using an information form EKF-SLAM : Complexity 10

11. State Augmentation Prediction : • Solution : State Augmentation • Separating the state into an augmented states • Update only affected matrixes Static 11

12. State Augmentation O(N3) Covariance prediction State Augmentation O(N) Covariance prediction Static

13. Partitioned Updates Update : • Solution : Partitioned Update with local submap. • Confines the map to a small local region. • Only Updates the small local region. • Updates the whole map only at a much lower frequency 13

14. Partitioned Updates Updated by Local SLAM Local State : Global State: Periodically registers

15. Sparsification • State Bel(xt ,m) and covariance matrix Ptare Gaussian probability density which, • implicitly describes the two central moments of Gaussian • Using Moment or Information Form • Sparsification Pt Yt • Many of none diagonal components are very close to 0 •  they can be set to zero.

16. Sparsification O(N3) Covariance prediction Sparsification using the information form O(N) Covariance prediction

17. Overview • Review • SLAM • Computational complexity • State Augmentation • Partitioned Updates • Sparsification • Data association • Batch Gating • SIFT • Multi-Hypothesis • Future works

18. Data Association Problem • Which observation belongs to which landmark? • A robust SLAM must consider possible data associations • Solutions: three key methods : • Batch Gating • SIFT • Multi-Hypothesis

19. Batch Gating • Basic Principle of Batch: RANSAC • Gating : constrained by robot position estimation < taken from T. Bailey, “Mobile robot localization and mapping in extensive outdoor environments,” Ph.D. dissertation> • If true robot movement is ==> the left case is chosen by using the gating

20. SIFT • Batch Gating is not enough for reliable data association • SIFT features have “landmark-quality” for SLAM • SIFT correspondences tend to be reliable and recognizable under variable conditions < taken from “Distinctive Image Featuresfrom Scale-Invariant Keypoints”, David G. Lowe – IJCV 2004 > • Gating • If true robot movement is ==> the left case is chosen by using the gating

21. x, y,  Landmark 1 Landmark 2 Landmark M Particle #1 … x, y,  Landmark 1 Landmark 2 Landmark M Particle #2 … … x, y,  Landmark 1 Landmark 2 Landmark M Particle N … Multi-Hypothesis Data Association • Multi-hypothesis data association • Generate a separate track estimate for each association hypothesis. • Low-likelihood tracks are pruned • FastSLAM is inherently a Multi-hypothesis solution because its data association is done on a per-particle basis.

22. Per-Particle Data Association Was the observation generated by the red or the blue landmark? P(observation|red) = 0.3 P(observation|blue) = 0.7 • Per-particle data association • Pick the most probable match • If the probability is too low, generate a new landmark

23. Future Woks • Large scale mapping • including many vehicles • in mixed environments • with sensor networks and dynamic landmark. • The delayed data-fusion concept instead of batch association and iterative smoothing to improve estimation quality and robustness