robot mapping fastslam feature-based slam with particle...
TRANSCRIPT
![Page 1: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/1.jpg)
1
Robot Mapping
FastSLAM – Feature-Based SLAM with Particle Filters
Gian Diego Tipaldi, Wolfram Burgard
![Page 2: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/2.jpg)
2
Particle Filter
Non-parametric recursive Bayes filter
Posterior is represented by a set of weighted samples
Can model arbitrary distributions
Works well in low-dimensional spaces
3-Step procedure
Sampling from proposal
Importance Weighting
Resampling
![Page 3: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/3.jpg)
3
Particle Filter Algorithm
1. Sample the particles from the
proposal distribution
2. Compute the importance weights
1. Resampling: Draw sample with
probability and repeat times
![Page 4: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/4.jpg)
4
Particle Representation
A set of weighted samples
Think of a sample as one hypothesis about the state
For feature-based SLAM:
poses landmarks
![Page 5: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/5.jpg)
5
Dimensionality Problem
Particle filters are effective in low dimensional spaces. The likely regions of the state space need to be covered with samples.
Higher dimensions -> more samples.
high-dimensional
![Page 6: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/6.jpg)
6
Can We Exploit Dependencies Between the Different
Dimensions of the State Space?
![Page 7: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/7.jpg)
7
If We Know the Poses of the Robot, Mapping is Easy!
![Page 8: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/8.jpg)
8
Key Idea
If we use the particle set only to model the robot’s path, each sample is a path hypothesis. For each sample, we can
compute an individual map of landmarks.
![Page 9: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/9.jpg)
9
Rao-Blackwellization
Factorization to exploit dependencies between variables:
If can be computed in closed form, represent only with samples and compute for every sample
![Page 10: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/10.jpg)
10
Rao-Blackwellization for SLAM
Factorization of the SLAM posterior
First introduced for SLAM by Murphy in 1999
poses map observations & movements
![Page 11: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/11.jpg)
11
Rao-Blackwellization for SLAM
Factorization of the SLAM posterior
First introduced for SLAM by Murphy in 1999
poses map observations & movements
path posterior map posterior
![Page 12: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/12.jpg)
12
Rao-Blackwellization for SLAM
Factorization of the SLAM posterior
First introduced for SLAM by Murphy in 1999
How to compute this term efficiently?
![Page 13: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/13.jpg)
13
Revisit the Graphical Model
Courtesy: Thrun, Burgard, Fox
![Page 14: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/14.jpg)
14
Revisit the Graphical Model
known
Courtesy: Thrun, Burgard, Fox
![Page 15: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/15.jpg)
15
Landmarks are Conditionally Independent Given the Poses
Landmark variables are all disconnected (i.e. independent) given the robot’s path
![Page 16: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/16.jpg)
16
Rao-Blackwellization for SLAM
Factorization of the SLAM posterior
Landmarks are conditionally independent given the poses
First exploited in FastSLAM by Montemerlo et al., 2002
![Page 17: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/17.jpg)
17
Rao-Blackwellization for SLAM
Factorization of the SLAM posterior
First exploited in FastSLAM by Montemerlo et al., 2002
![Page 18: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/18.jpg)
18
Rao-Blackwellization for SLAM
Factorization of the SLAM posterior
First exploited in FastSLAM by Montemerlo et al., 2002
2-dimensional EKFs!
![Page 19: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/19.jpg)
19
Rao-Blackwellization for SLAM
Factorization of the SLAM posterior
First exploited in FastSLAM by Montemerlo et al., 2002
particle filter similar to MCL
2-dimensional EKFs!
![Page 20: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/20.jpg)
20
Sample-based representation for
Each sample is a path hypothesis
Past poses of a sample are not revised
No need to maintain past poses in the sample set
Modeling the Robot’s Path
starting location, typically (0,0,0)
pose hypothesis at time t=1
![Page 21: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/21.jpg)
21
FastSLAM
Proposed by Montemerlo et al. in 2002
Each landmark is represented by a 2x2 EKF
Each particle therefore has to maintain M individual EKFs
Landmark 1 Landmark 2 Landmark M …
Landmark 1 Landmark 2 Landmark M … Particle
1
Landmark 1 Landmark 2 Landmark M … Particle
2
Particle N
…
![Page 22: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/22.jpg)
22
FastSLAM – Action Update
Particle #1
Particle #2
Particle #3
Landmark 1
2x2 EKF
Landmark 2
2x2 EKF
Courtesy: M. Montemerlo
![Page 23: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/23.jpg)
23
FastSLAM – Sensor Update
Particle #1
Particle #2
Particle #3
Landmark 1
2x2 EKF
Landmark 2
2x2 EKF
Courtesy: M. Montemerlo
![Page 24: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/24.jpg)
24
FastSLAM – Sensor Update
Particle #1
Particle #2
Particle #3
Weight = 0.8
Weight = 0.4
Weight = 0.1
Courtesy: M. Montemerlo
![Page 25: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/25.jpg)
25
FastSLAM – Sensor Update
Particle #1
Particle #2
Particle #3
Update map
of particle 1
Update map
of particle 2
Update map
of particle 3
Courtesy: M. Montemerlo
![Page 26: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/26.jpg)
26
Key Steps of FastSLAM 1.0
Extend the path posterior by sampling a new pose for each sample
Compute particle weight
Update belief of observed landmarks
(EKF update rule)
Resample
measurement covariance
exp. observation
![Page 27: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/27.jpg)
27
FastSLAM 1.0 – Part 1
![Page 28: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/28.jpg)
28
FastSLAM 1.0 – Part 1
![Page 29: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/29.jpg)
29
FastSLAM 1.0 – Part 2
measurement cov. exp. observation
![Page 30: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/30.jpg)
30
FastSLAM 1.0 – Part 2 (long)
EKF update
![Page 31: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/31.jpg)
31
FastSLAM in Action
Courtesy: M. Montemerlo
![Page 32: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/32.jpg)
32
The Weight is a Result From the Importance Sampling Principle
Importance weight is given by the ratio of target and proposal in
See: importance sampling principle
![Page 33: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/33.jpg)
33
The Importance Weight
The target distribution is
The proposal distribution is
Proposal is used step-by-step
![Page 34: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/34.jpg)
34
The Importance Weight
![Page 35: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/35.jpg)
35
The Importance Weight
Bayes rule + factorization
![Page 36: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/36.jpg)
36
The Importance Weight
![Page 37: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/37.jpg)
37
The Importance Weight
![Page 38: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/38.jpg)
38
The Importance Weight
![Page 39: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/39.jpg)
39
The Importance Weight
Integrating over the pose of the observed landmark leads to
![Page 40: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/40.jpg)
40
The Importance Weight
Integrating over the pose of the observed landmark leads to
![Page 41: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/41.jpg)
41
The Importance Weight
Integrating over the pose of the observed landmark leads to
![Page 42: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/42.jpg)
42
The Importance Weight
This leads to
measurement covariance (pose uncertainty of the landmark estimate plus measurement noise)
![Page 43: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/43.jpg)
43
The Importance Weight
This leads to
![Page 44: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/44.jpg)
44
FastSLAM 1.0 – Part 2
![Page 45: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/45.jpg)
45
Data Association Problem
Which observation belongs to which landmark?
More than one possible association
Potential data associations depend on the pose of the robot
Courtesy: M. Montemerlo
![Page 46: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/46.jpg)
46
Particles Support for Multi-Hypotheses Data Association
Decisions on a per-particle basis
Robot pose error is factored out of data association decisions
Courtesy: M. Montemerlo
![Page 47: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/47.jpg)
47
Per-Particle Data Association
Was the observation
generated by the red or by the brown landmark?
P(observation|red) = 0.3 P(observation|brown) = 0.7
Courtesy: M. Montemerlo
![Page 48: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/48.jpg)
48
Per-Particle Data Association
P(observation|red) = 0.3 P(observation|brown) = 0.7
Two options for per-particle data association
Pick the most probable match
Pick an random association weighted by the observation likelihoods
If the probability for an assignment is too low, generate a new landmark
Was the observation
generated by the red or by the brown landmark?
Courtesy: M. Montemerlo
![Page 49: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/49.jpg)
49
Per-Particle Data Association
Multi-modal belief
Pose error is factored out of data association decisions
Simple but effective data association
Big advantage of FastSLAM over EKF
Was the observation
generated by the red or by the brown landmark?
Courtesy: M. Montemerlo
![Page 50: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/50.jpg)
50
Results – Victoria Park
4 km traverse
< 2.5 m RMS position error
100 particles
Blue = GPS
Yellow = FastSLAM Courtesy: M. Montemerlo
![Page 51: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/51.jpg)
51
Results – Victoria Park (Video)
Courtesy: M. Montemerlo
![Page 52: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/52.jpg)
52
Results (Sample Size)
Courtesy: M. Montemerlo
![Page 53: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/53.jpg)
53
Results (Motion Uncertainty)
Courtesy: M. Montemerlo
![Page 54: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/54.jpg)
54
FastSLAM 1.0 Summary
Use a particle filter to model the belief
Factors the SLAM posterior into low-dimensional estimation problems
Model only the robot’s path by sampling
Compute the landmarks given the path
Per-particle data association
No robot pose uncertainty in the per-particle data association
![Page 55: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/55.jpg)
55
FastSLAM Complexity – Simple Implementation
Update robot particles based on the control
Incorporate an observation into the Kalman filters
Resample particle set
N = Number of particles
M = Number of map features
O(N)
O(N)
O(N M)
![Page 56: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/56.jpg)
56
A Better Data Structure for FastSLAM
Courtesy: M. Montemerlo
![Page 57: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/57.jpg)
57
A Better Data Structure for FastSLAM
Courtesy: M. Montemerlo
![Page 58: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/58.jpg)
58
FastSLAM Complexity
Update robot particles based on the control
Incorporate an observation into the Kalman filters
Resample particle set
N = Number of particles
M = Number of map features
O(N log(M))
O(N log(M))
![Page 59: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/59.jpg)
59
Memory Complexity
Courtesy: M. Montemerlo
![Page 60: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/60.jpg)
60
FastSLAM 1.0
FastSLAM 1.0 uses the motion model as the proposal distribution
Is there a better distribution to sample from?
[Montemerlo et al., 2002]
![Page 61: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/61.jpg)
61
FastSLAM 1.0 to FastSLAM 2.0
FastSLAM 1.0 uses the motion model as the proposal distribution
FastSLAM 2.0 considers also the measurements during sampling
Especially useful if an accurate sensor is used (compared to the motion noise)
[Montemerlo et al., 2003]
![Page 62: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/62.jpg)
62
FastSLAM 2.0 (Informally)
FastSLAM 2.0 samples from
Results in a more peaked proposal distribution
Less particles are required
More robust and accurate
But more complex…
[Montemerlo et al., 2003]
![Page 63: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/63.jpg)
63
FastSLAM Problems
How to determine the sample size?
Particle deprivation, especially when closing (multiple) loops
FastSLAM 1.0 FastSLAM 2.0
Court
esy:
M.
Monte
merlo
![Page 64: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/64.jpg)
64
FastSLAM Summary
Particle filter-based SLAM
Rao-Blackwellization: model the robot’s path by sampling and compute the landmarks given the poses
Allow for per-particle data association
FastSLAM 1.0 and 2.0 differ in the proposal distribution
Complexity
![Page 65: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/65.jpg)
65
FastSLAM Results
Scales well (1 million+ features)
Robust to ambiguities in the data association
Advantages compared to the classical EKF approach (especially with non-linearities)
![Page 66: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/66.jpg)
66
Literature
FastSLAM
Thrun et al.: “Probabilistic Robotics”, Chapter 13.1-13.3 + 13.8 (see errata!)
Montemerlo, Thrun, Kollar, Wegbreit: FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem, 2002
Montemerlo and Thrun: Simultaneous Localization and Mapping with Unknown Data Association Using FastSLAM, 2003
![Page 67: Robot Mapping FastSLAM Feature-Based SLAM with Particle ...ais.informatik.uni-freiburg.de/teaching/ws17/... · 17 Rao-Blackwellization for SLAM Factorization of the SLAM posterior](https://reader034.vdocuments.net/reader034/viewer/2022051914/600504c3a529824d5e54e87b/html5/thumbnails/67.jpg)
67
Slide Information
These slides have been created by Cyrill Stachniss as part of the robot mapping course taught in 2012/13 and 2013/14. I created this set of slides partially extending existing material of Edwin Olson, Pratik Agarwal, and myself.
I tried to acknowledge all people that contributed image or video material. In case I missed something, please let me know. If you adapt this course material, please make sure you keep the acknowledgements.
Feel free to use and change the slides. If you use them, I would appreciate an acknowledgement as well. To satisfy my own curiosity, I appreciate a short email notice in case you use the material in your course.
My video recordings are available through YouTube: http://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_&feature=g-list
Cyrill Stachniss, 2014 [email protected]
bonn.de