1. 9-1 SA-1 Probabilistic Robotics: SLAM = Simultaneous Localization and Mapping Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian Plagemann, Dirk Haehnel, Mike Montemerlo, Nick Roy, Kai Arras, Patrick Pfaff and others Sebastian Thrun & Alex Teichman Stanford Artificial Intelligence Lab

2. 9-2 Given: The robot’s controls Observations of nearby features Estimate: Map of features Path of the robot The SLAM Problem A robot is exploring an unknown, static environment.

3. 9-3 Chicken-or-Egg SLAM is a chicken-or-egg problem A map is needed for localizing a robot A good pose estimate is needed to build a map Thus, SLAM is regarded as a hard problem in robotics A variety of different approaches to address the SLAM problem have been presented Probabilistic methods outperform most other techniques

4. 9-4 Structure of the Landmark- based SLAM-Problem

5. 9-5 SLAM Applications Indoors Space Undersea Underground

6. 9-6 Map Representations Examples: City map, subway map, landmark-based map

7. 9-7 Map Representations Grid maps or scans [Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras, 99; Haehnel, 01;…] Landmark-based [Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002;…

8. 9-8 Why is SLAM a hard problem? SLAM: robot path and map are both unknown Robot path error correlates errors in the map

9. 9-9 Why is SLAM a hard problem? In the real world, the mapping between observations and landmarks is unknown Picking wrong data associations can have catastrophic consequences Pose error correlates data associations Robot pose uncertainty

10. 9-10 SLAM: Simultaneous Localization and Mapping Full SLAM: Online SLAM: Integrations typically done one at a time Estimates most recent pose and map! Estimates entire path and map!

11. 9-11 Graphical Model of Full SLAM:

12. 9-12 Graphical Model of Online SLAM:

13. 9-13 Techniques for Generating Consistent Maps Scan matching EKF SLAM FastSLAM Probabilistic mapping with a single map and a posterior about poses Mapping + Localization GraphSLAM, SEIF

14. 9-14 Kalman Filter Algorithm 1. Algorithm Kalman_filter(  t-1,  t-1, u t, z t ): 2. Prediction: 3. 4. 5. Correction: 6. 7. 8. 9. Return  t,  t

15. 9-15 Map with N landmarks:(3+2N)-dimensional Gaussian Can handle hundreds of dimensions (E)KF-SLAM

16. 9-16 EKF-SLAM Map Correlation matrix

17. 9-17 EKF-SLAM Map Correlation matrix

18. 9-18 EKF-SLAM Map Correlation matrix

19. 9-19 Properties of KF-SLAM (Linear Case) Theorem: The determinant of any sub-matrix of the map covariance matrix decreases monotonically as successive observations are made. Theorem: In the limit the landmark estimates become fully correlated [Dissanayake et al., 2001]

20. 9-20 Victoria Park Data Set [courtesy by E. Nebot]

21. 9-21 Victoria Park Data Set Vehicle [courtesy by E. Nebot]

22. 9-22 Data Acquisition [courtesy by E. Nebot]

23. 9-23 Raw Odometry (no SLAM) Odometry GPS (for reference)

24. 9-24 Estimated Trajectory [courtesy by E. Nebot]

25. 9-25 EKF SLAM Application [courtesy by J. Leonard]

26. 9-26 EKF SLAM Application odometry estimated trajectory [courtesy by John Leonard]

27. 9-27 Local submaps [Leonard et al.99, Bosse et al. 02, Newman et al. 03] Sparse links (correlations) [Lu & Milios 97, Guivant & Nebot 01] Sparse extended information filters [Frese et al. 01, Thrun et al. 02] Thin junction tree filters [Paskin 03] Rao-Blackwellisation (FastSLAM) [Murphy 99, Montemerlo et al. 02, Eliazar et al. 03, Haehnel et al. 03] Approximations for SLAM

28. 9-28 EKF-SLAM: Complexity Cost per step: O(n 2 ), quadratic in the number of landmarks: O(n 2 ) Total cost to build a map with n landmarks: O(n 3 ) Memory: O(n 2 ) Approaches exist that make EKF-SLAM O(n^1.5) / O(n 2.5 ) / O(n)

29. 9-29 EKF-SLAM: Summary Convergence for linear case! Can diverge if nonlinearities are large Has been applied successfully in large-scale environments Approximations reduce the computational complexity

30. 9-30 Data Association for SLAM Interpretation tree

31. 9-31 Data Association for SLAM Env. Dyn.

32. 9-32 Data Association for SLAM Geometric Constraints Location independent constraints Unary constraint: intrinsic property of feature e.g. type, color, size Binary constraint: relative measure between features e.g. relative position, angle Location dependent constraints Rigidity constraint: "is the feature where I expect it given my position?" Visibility constraint: "is the feature visible from my position?" Extension constraint: "do the features overlap at my position?" All decisions on a significance level 

33. 9-33 Data Association for SLAM Interpretation Tree [Grimson 1987], [Drumheller 1987], [Castellanos 1996], [Lim 2000] Algorithm backtracking depth-first recursive uses geometric constraints exponential complexity absence of feature: no info. presence of feature: info. perhaps

34. 9-34 Data Association for EKF SLAM Pygmalion a = 0.95, p = 2

35. 9-35 Data Association for EKF SLAM a = 0.95, p = 3 Pygmalion

36. 9-36 Data Association for EKF SLAM a = 0.95, p = 4 a = 0.95, p = 5 t exe : 633 ms PowerPC at 300 MHz Pygmalion

37. 9-37 Summary: EKF SLAM Extends EKF localization by additional state variables (landmark locations) Converges in linear-Gaussian world Data association problem: which measurement corresponds to which landmark? Data association solved by tree search