stochastic geometry and bayesian slam - cecmartin/icra_2012_workshop... · 2012-05-02 · integral...
TRANSCRIPT
![Page 1: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/1.jpg)
Stochastic Geometry and Bayesian SLAM
St Paul, US, May 2012
Ba-Ngu VoSchool of EECEUniversity of Western AustraliaPerth, Australiahttp://www.ee.uwa.edu.au/~bnvo/
![Page 2: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/2.jpg)
Introduction
Map representation
Stochastic Geometry
Bayesian SLAM
Conclusions
Outline
![Page 3: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/3.jpg)
SLAM (Simultaneous Localisation and Mapping)
Objective: Jointly estimate robot pose & map
Introduction
![Page 4: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/4.jpg)
Estimates of landmarks are correlated with each other because of the common error in estimated vehicle location [Smith, Self & Cheeseman]
SLAM requires a joint state composed of pose and every landmark position, to be updated following each landmark observation.
Statistical basis: [Smith & Cheeseman]
Essential theory on convergence [Csorba]
Algorithms [Bailey & Durrant-Whyte], [Montermelo et al]
Introduction
Key problem: Geometric uncertainty [Durrent-Whyte]
![Page 5: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/5.jpg)
t = 0 Initial State and Uncertainty
Using Range Measurements
Introduction
![Page 6: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/6.jpg)
t =1 Predict Pose and Uncertainty to time 1
Introduction
![Page 7: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/7.jpg)
Correct pose and pose uncertainty
Predict new feature positions and their uncertainties
t =1
Introduction
![Page 8: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/8.jpg)
Predict pose and uncertainty of pose at time 2
Predict feature measurements and their uncertainties
t = 2
Introduction
![Page 9: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/9.jpg)
Correct pose and mapped features
Update uncertainties for mapped features
Estimate uncertainty of new features
t = 2
Introduction
![Page 10: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/10.jpg)
1 2 3 4 5 6 7
1 2 3 4 5 6 7
[ , , , , , , ]
[ , , , , , , ]
k
k
Z z z z z z z z
M m m m m m m m
=
=
Traditional “Bayes” SLAM:
pk-1(Mk-1, xk-1|Z1:k-1) pk(Mk, xk|Z1:k)pk|k-1(Mk, xk|Z1:k-1)⋅⋅⋅ ⋅⋅⋅prediction data-update
“Bayes”-SLAM filter
| 1 1 1: 1( , | , ) ( , | )k k k k k kf M x M x p M x Z dMdx− − −∫ ∫ | 1 1: 1
| 1 1: 1
( | , ) ( , | )( | , ) ( , | )
k k k k k k k k k
k k k k k
g Z M x p M x Zg Z M x p M x Z dMdx
− −
− −∫ ∫
Vector representation of map
Performs data association
Applies “Bayes”-SLAM filter
Introduction
![Page 11: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/11.jpg)
Map Representation
Q: What is the purpose of estimation?
A: To get good estimate!
What is the type object that we’re trying to estimate?
What is a “good” estimate?
Error metric:
Quantifies how close an estimate is to the true value
Fundamental in estimation
Well-understood for localization: Euclidean distance, MSE, …
What about mapping?
![Page 12: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/12.jpg)
Q: Why do we need mapping error, localisation error alone is sufficient, since goodlocalization implies good mapping anyway?
A: How do we know it’s a good mapping if we don’t know how to quantify mapping error?
Map Representation
![Page 13: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/13.jpg)
0011
M
=
11ˆ00
M
=
Estimate is correct but estimation error ?
True Map Estimated Map
ˆ|| || 2M M− =
2 landmarks
Traditional feature-based SLAM: stack landmarks into a large vector!
00
11
00
11
Map Representation
2 landmarks
+
+
( )ˆmin || || 0
perm MM M− =Remedy: use
![Page 14: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/14.jpg)
What are the estimation errors?
Which map estimate is better?
O : True landmarks + : Estimated landmarks
(a) (b) (c)
Map Representation
![Page 15: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/15.jpg)
Need the mapping error metric to
be a metric
have meaningful interpretation
capture errors in number of landmarks and their positions
Map Representation
![Page 16: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/16.jpg)
Metric: d(. , .)
(identity) d(x, y) = 0 iff x = y;
(symmetry) d(x, y) = d(y, x) for all x, y
(triangle inequality) d(x, y) < d(x, z) + d(z; y) for all x, y, z.
Why triangle inequality?
Suppose estimate z is “close” to the true state x.
If estimate y is “close” to z, then y is also “close” to xx
z y
Q: Why do we need a metric?
A: Necessary for comparisons/bounds/convergence
Map Representation
![Page 17: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/17.jpg)
Q: Why do we even care about error in the number of landmarks?
Catastrophic consequences in applications such as search & rescue, obstacle avoidance, UAV mission…
A:
Map Representation
![Page 18: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/18.jpg)
Map Representation
Vector representation doesn’t admit map error metric!
Finite set representation admits map error metric, e.g.
Hausdorff, Wasserstein, OSPA
The realization that the map is a set is found in [Durrant-Whyte]
The map is fundamentally a set (of landmarks)
![Page 19: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/19.jpg)
What about grid-based maps?
True Map Estimate 1 Estimate 2
Map Representation
When treated as vectors, estimates 1 and 2 have the same error, even though intuitively estimate 1 is better than 2
![Page 20: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/20.jpg)
Stochastic Geometry
Essence: Connections between Geometry and Probability
(Buffon’s needle 1777) What is the chance that a needle dropped randomly
on a floor marked with equally spaced parallel lines crosses 1 of the lines?
Origin 18th century: geometric probability
D > L
L2LπD
Ans =
![Page 21: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/21.jpg)
Stochastic Geometry
What is the mean length of a random chord of a unit circle?
What is the chance that 3 random points in the plane form an acute triangle?
What is the mean area of the polygonal regions formed when randomly-oriented lines are spread over the plane?
Monograph: [H. Solomon, Geometric Probability, Philadelphia, PA: SIAM,1978]
Other well-known Geometric Probability problems
(cf. Bertrand’s paradox)
![Page 22: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/22.jpg)
Stochastic Geometry
Development of expected values associated with geometric objects derived from random points
Theory of measures that are invariant under symmetry groups
Geometric Probability
Integral Geometry Stochastic Geometry
Focus on the random geometrical objects, e.g. models for random lines, random tessellations, random sets.
Study of random processes whose outcomes are geometrical objects or spatial patterns
The terms Stochastic Geometry and Geometric Probability are some times used interchangeably
![Page 23: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/23.jpg)
Stochastic Geometry
Modern stochastic geometry deals with random subsets of arbitrary forms, even randomly generated fractals
D. Kendall (1918-2007)
Foundation (1960s-1970s): mostly due to independent work by Matheronand Kendall, both of whom gave credits to earlier work by Choquet
G. Matheron (1930-2000) G. Choquet (1915-2006)
Applications: physics, biology, sampling theory, stereology, spatial statistics, agriculture, forestry, geology, epidemiology, material science, image analysis, telecommunications, data fusion, target tracking …
![Page 24: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/24.jpg)
The number of points is random, The points have no ordering and are randomAn RFS is a finite-set-valued random variableAKA: (simple finite) point process or random point pattern
Pine saplings in a Finish forest [Kelomaki & Penttinen]
Childhood leukaemia & lymphoma in North Humberland [Cuzich & Edwards]
Random Finite Set: Special case of Matheron’s random closed setExamples of point pattern data (realisations of RFS)
Stochastic Geometry
![Page 25: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/25.jpg)
intensity measure or1st moment measure
VΣ(S) = E[|Σ∩ S|]E
S = expected No. points of Σ in S
E[|Σ∩ S|] = vΣ(x)dx∫S
( ) 0
( ) ( )( ) lim( ) ( )x
x
volx
V V dxv xvol vol dx
Σ ΣΣ ∆ →
∆= =
∆
intensity function or PHD (Probability Hypothesis Density)
What is the expectation of a random finite set?
E∆x
Stochastic Geometry
![Page 26: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/26.jpg)
x0 state space
PHD of an RFS
S
v(x)dx = expected No. points in S∫S
v(x0) = density/concentration ofexpected No. points at x0
Physical interpretation of the PHD
Stochastic Geometry
![Page 27: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/27.jpg)
intensity function (PHD)
vΣ(x) = E[δΣ(x)]
E
Engineering interpretation of the PHD as the “expected set”
Stochastic Geometry
![Page 28: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/28.jpg)
Map = finite set of landmarks
Bayesian SLAM requires modelling uncertainty in maps by RFS
| 1 1: 1 1: 1 01: 1: 1 0
| 1 1: 1 1: 1 0
( | , ) ( , | , , )( , | , , )
( | , ) ( , | , , )k k k k k k k k k k
k k k k kk k k k k k k k k k k k
g Z x M p x M Z u xp x M Z u x
g Z x M p x M Z u x M dxδ− − −
−− − −
=∫ ∫
| 1 1: 1 1: 1 0 | 1 1 1 1 1 1 1 1: 1 1: 2 0 1 1( , | , , ) ( , | , , ) ( , | , , )k k k k k k k k k k k k k k k k k k k kp x M Z u x f x M x M u p x M Z u x M dxδ− − − − − − − − − − − − − −= ∫ ∫
Robot poseMap
MeasurementsControls
Measurement likelihood
Set integralTransition density
Bayes-SLAM prediction
Bayes-SLAM update
Set integral
RFS-SLAM [Mullane et. al. 08]
Bayesian SLAM
![Page 29: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/29.jpg)
Bayes Risk: Expected posterior cost/penalty of incorrect estimate
( ) ( )1: 1: 1: 1:( ) ( ( ), ) ( ( ), ) |E k k k kR M C M Z M C M Z M p Z M p M M Zδ δ = ∫
Penalty of usingas an estimate of M
1:( )kM Z
Optimal Bayes estimator:
Bayes risk
1: 1: 1:ˆ ˆ: ( ) arg min ( )Bayes Bayes
k k kM
M Z M Z R M Z=
set integrals
Bayesian SLAM
Joint multi-target estimator [Mahler07]: given a D| |
1:ˆ arg sup ( | )
| | !
MJoMD k
M
DM p M ZM
=
Bayes Optimal & converges as k tends to infinity
![Page 30: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/30.jpg)
Using the PHD as the expected map
Bayesian SLAM
RFS-SLAM [Mullane et. al. 08]
![Page 31: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/31.jpg)
Bayesian SLAM
SLAM SMC-PHD [Kalyan et. al. 2010]
SLAM with cluster processes [Clark et. al. 2012]
Collaborative SLAM [Moratuwage et. al. 2010, 2012]
Mapping [Lundquist et. al. 2011]
SLAM Formulation & Solutions [Mullane et. al. 2008, 2010, 2011]
![Page 32: Stochastic Geometry and Bayesian SLAM - CECmartin/icra_2012_workshop... · 2012-05-02 · Integral Geometry Stochastic Geometry Focus on the random geometrical objects, e.g. models](https://reader031.vdocuments.net/reader031/viewer/2022040612/5edc7cc2ad6a402d66672a3c/html5/thumbnails/32.jpg)
Conclusion
Mapping error is of fundamental importance
The (feature) map is a finite set
Bayesian SLAM requires random finite set
Borne out of practical & fundamental necessity
Fully integrates uncertainty in data association & landmarks under one umbrella.
The rest is up to you ...
Thank You!