Robot Mapping Grid-based...
Transcript of Robot Mapping Grid-based...
![Page 1: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/1.jpg)
1
Robot Mapping
Grid-based FastSLAM
Cyrill Stachniss
![Page 2: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/2.jpg)
2
Motivation
§ So far, we addressed landmark-based SLAM (EKF, SEIF, FastSLAM)
§ We learned how to build grid maps assuming “known poses”
Today: SLAM for building grid maps
![Page 3: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/3.jpg)
3
Mapping With Raw Odometry
Courtesy: Dirk Hähnel
![Page 4: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/4.jpg)
4
Observation
§ Assuming known poses fails!
Questions § Can we solve the SLAM problem if no
pre-defined landmarks are available? § Can we use the ideas of FastSLAM to
build grid maps?
![Page 5: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/5.jpg)
5
Rao-Blackwellization for SLAM
§ Factorization of the SLAM posterior
First introduced for SLAM by Murphy in 1999
poses map observations & movements
![Page 6: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/6.jpg)
6
Rao-Blackwellization for SLAM
§ Factorization of the SLAM posterior
First introduced for SLAM by Murphy in 1999
poses map observations & movements
path posterior (particle filter)
map posterior (given the path)
![Page 7: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/7.jpg)
7
Grid-based SLAM
§ As with landmarks, the map depends on the poses of the robot during data acquisition
§ If the poses are known, grid-based mapping is easy (“mapping with known poses”)
![Page 8: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/8.jpg)
8
A Graphical Model for Grid-Based SLAM
m
x
z
u
x
z
u
2
2
x
z
u
... t
t
x 1
1
0
1 0 t-1
![Page 9: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/9.jpg)
9
Grid-Based Mapping with Rao-Blackwellized Particle Filters § Each particle represents a possible
trajectory of the robot § Each particle maintains its own map § Each particle updates it using
“mapping with known poses”
![Page 10: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/10.jpg)
10
Particle Filter Example
map of particle 1 map of particle 3
map of particle 2
3 particles
![Page 11: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/11.jpg)
11
Performance of Grid-based FastSLAM 1.0
![Page 12: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/12.jpg)
12
Problem § Too many samples are needed to
sufficiently model the motion noise § Increasing the number of samples is
difficult as each map is quite large
§ Idea: Improve the proposal to generate a better prediction. This reduces the required number of particles
![Page 13: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/13.jpg)
13
Improved Proposal
§ Compute an improved proposal that considers the most recent observation
Goals: § More precise sampling § More accurate maps § Less particles needed
![Page 14: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/14.jpg)
14
For lasers is typically peaked and dominates the product
[Arulampalam et al., 01]
The Optimal Proposal Distribution
![Page 15: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/15.jpg)
15
Proposal Distribution
![Page 16: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/16.jpg)
16
Proposal Distribution
![Page 17: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/17.jpg)
17
Proposal Distribution
![Page 18: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/18.jpg)
18
Proposal Distribution
globally limits the area over which to integrate (odometry)
locally limits the area over which to integrate (measurement)
![Page 19: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/19.jpg)
19
Proposal Distribution
global local
![Page 20: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/20.jpg)
20
Proposal Distribution
Gaussian approximation:
How to sample from this term?
with
![Page 21: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/21.jpg)
21
Gaussian Proposal Distribution
Approximate this equation by a Gaussian:
Sampled points around the maximum
maximum reported by a scan matcher
Gaussian approximation
Draw next generation of samples
![Page 22: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/22.jpg)
22
Estimating the Parameters of the Gaussian for Each Particle
xj are points sampled around the location x* to which the scan matching has converged to
![Page 23: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/23.jpg)
23
Computing the Importance Weight
[Arulampalam et al., 01]
![Page 24: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/24.jpg)
24
Computing the Importance Weight
![Page 25: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/25.jpg)
25
Computing the Importance Weight
![Page 26: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/26.jpg)
26
Computing the Importance Weight
![Page 27: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/27.jpg)
27
Computing the Importance Weight
Sampled points around the maximum of the likelihood function found by scan-matching
![Page 28: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/28.jpg)
28
Improved Proposal
§ The proposal adapts to the structure of the environment
![Page 29: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/29.jpg)
29
Resampling
§ Resampling at each step limits the “memory” of our filter
§ Suppose we loose each time 25% of the particles, this may lead to:
§ Goal: Reduce the resampling actions
![Page 30: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/30.jpg)
30
Selective Resampling
§ Resampling is necessary to achieve convergence
§ Resampling is dangerous, since important samples might get lost (“particle depletion”)
§ Resampling makes only sense if particle weights differ significantly
§ Key question: When to resample?
![Page 31: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/31.jpg)
31
Number of Effective Particles
§ Empirical measure of how well the target distribution is approximated by samples drawn from the proposal
§ nef describes “the inverse variance of
the normalized particle weights” § For equal weights, the sample
approximation is close to the target
![Page 32: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/32.jpg)
32
Resampling with
§ If our approximation is close to the target, no resampling is needed
§ We only resample when drops below a given threshold ( )
§ Note: weights need to be normalized [Doucet, ’98; Arulampalam, ’01]
![Page 33: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/33.jpg)
33
Typical Evolution of
visiting new areas closing the
first loop
second loop closure
visiting known areas
![Page 34: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/34.jpg)
34
Intel Lab § 15 particles § four times faster
than real-time P4, 2.8GHz
§ 5cm resolution during scan matching
§ 1cm resolution in final map
![Page 35: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/35.jpg)
35
Intel Lab
![Page 36: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/36.jpg)
36
Outdoor Campus Map § 30 particles § 250x250m2
§ 1.75 km (odometry)
§ 30cm resolution in final map
![Page 37: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/37.jpg)
37
MIT Killian Court
§ The “infinite-corridor-dataset” at MIT
![Page 38: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/38.jpg)
38
MIT Killian Court
![Page 39: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/39.jpg)
39
MIT Killian Court – Video
![Page 40: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/40.jpg)
40
Real World Application
§ This guy uses a similar technique…
![Page 41: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/41.jpg)
41
Problems of Gaussian Proposals
§ Gaussians are uni-model distributions § In case of loop-closures, the likelihood
function might be multi-modal
![Page 42: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/42.jpg)
42
Problems of Gaussian Proposals
§ Multi-modal likelihood function can cause filter divergence
![Page 43: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/43.jpg)
43
Efficient Multi-Modal Sampling § Approximate the likelihood in a better way!
§ Sample from odometry first and the use
this as the start point for scan matching
odometry
mode 1 mode 2
odometry with uncertainty
![Page 44: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/44.jpg)
44
The Two-Step Sampling Works!
…with nearly zero overhead
![Page 45: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/45.jpg)
45
Difference Between the Optimal Proposal and the Approximations
Two-Step Sampling
Gaussian Proposal
+
+
+
+
+
+
![Page 46: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/46.jpg)
46
Is a Gaussian an Accurate Choice for the Proposal?
![Page 47: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/47.jpg)
47
Gaussian Proposal: Yes or No?
§ Gaussian allow for efficient sampling § Problematic in multi-model cases § Laser-baser SLAM: 3-6% multi-modal
distribution (for the datasets here) § Gaussian proposals can lead to
divergence § Two-step sampling process overcomes
this problem effectively and efficiently
![Page 48: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/48.jpg)
48
Conclusion
§ The ideas of FastSLAM can also be applied in the context of grid maps
§ Improved proposals are essential § Similar to scan-matching on a per-
particle base § Selective resamples reduces the risk
of particle depletion § Substantial reduction of the required
number of particles
![Page 49: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/49.jpg)
49
Literature Grid-FastSLAM with Improved Proposals § Grisetti, Stachniss, Burgard: Improved
Techniques for Grid Mapping with Rao-Blackwellized Particle Filters, 2007
§ Stachniss, Grisetti, Burgard, Roy. Analyzing Gaussian Proposal Distributions for Mapping with Rao-Blackwellized Particle Filters, 2007
![Page 50: Robot Mapping Grid-based FastSLAMais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/...“mapping with known poses” 10 Particle Filter Example map of particle 1 map of particle](https://reader033.fdocuments.us/reader033/viewer/2022051805/5ff8775bbf4c4f05093ac032/html5/thumbnails/50.jpg)
50
GMapping
§ Efficient open source implementation of the presented method (2005-2008)
§ C++ Code available via svn co https://svn.openslam.org/data/svn/gmapping