Transcript Monte Carlo Hidden Markov Models

Probabilistic Algorithms for
Mobile Robot Mapping
Sebastian Thrun
Carnegie Mellon & Stanford
Wolfram Burgard
University of Freiburg
and Dieter Fox
University of Washington
Based on the paper
A Real-Time Algorithm for Mobile Robot Mapping
With Applications to Multi-Robot and 3D Mapping
Best paper award at 2000 IEEE International Conference on Robotics
and Automation (~1,100 submissions)
Sponsored by DARPA (TMR-J.Blitch, MARS-D.Gage, MICA-S.Heise)
and NSF (ITR(2), CAREER-E.Glinert, IIS-V.Lumelsky)
Other contributors: Yufeng Liu, Rosemary Emery, Deepayan Charkrabarti, Frank Dellaert, Michael
Montemerlo, Reid Simmons, Hugh Durrant-Whyte, Somajyoti Majnuder, Nick Roy, Joelle Pineau, …
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Motivation
SLAM
(Kalman filters)
Expectation
Maximization
Real Time
Hybrid
3D Mapping
with EM
Open
Problems
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Museum Tour-Guide Robots
With: Greg Armstrong, Michael Beetz, Maren Benewitz,
Wolfram Burgard, Armin Cremers, Frank Dellaert, Dieter
Fox, Dirk Haenel, Chuck Rosenberg, Nicholas Roy,
Jamie Schulte, Dirk Schulz
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
The Nursebot Initiative
With: Greg Armstrong, Greg Baltus, Jacqueline DunbarJacob, Jennifer Goetz, Sara Kiesler, Judith Matthews,
Colleen McCarthy, Michael Montemerlo, Joelle Pineau,
Martha Pollack, Nicholas Roy, Jamie Schulte
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping: The Problem


Concurrent Mapping and Localization (CML)
Simultaneous Localization and Mapping (SLAM)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Mapping: The Problem




Continuous variables
High-dimensional (eg, 1,000,000+ dimensions)
Multiple sources of noise
Simulation not acceptable
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Milestone Approaches
Mataric 1990
Elfes/Moravec 1986
Kuipers et al 1991
Lu/Milios/Gutmann 1997
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
3D Mapping
Moravec et al, 2000
Konolige et al, 2001
Teller et al, 2000
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Take-Home Message
Mapping is the
Every state-of-the-art
holy grail in
mapping algorithm
mobile robotics.
is probabilistic.
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Motivation
SLAM
(Kalman filters)
Expectation
Maximization
Real Time
Hybrid
3D Mapping
with EM
Open
Problems
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Bayes Filters
p( xt | z1...t , u1...t )   p( zt | xt )  p( xt | ut , xt 1 ) p( xt 1 | z1...t 1 , u1...t 1 ) dxt 1
x = state
t = time
z = measurement
u = control
 = constant
Special cases:
HMMs
DBNs
POMDPs
Kalman filters
Condensation
...
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Bayes Filters in Localization
p( xt | z1...t , u1...t )   p( zt | xt )  p( xt | ut , xt 1 ) p( xt 1 | z1...t 1 , u1...t 1 ) dxt 1
[Simmons/Koenig 95]
[Kaelbling et al 96]
[Burgard, Fox, et al 96]
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Bayes Filters for Mapping
s = robot pose
m = map
t = time
 = constant
z = measurement
u = control
Localization:
p( st | z1...t , u1...t )   p( zt | st , m)  p( st | ut , st 1 ) p( st 1 | z1...t 1 , u1...t 1 ) dst 1
Mapping?
p(mt | z1t , u1t )   p( zt | mt )  p(mt | ut , mt 1 ) p(mt 1 | z1t 1 , u1t 1 ) dmt 1
p(mt , st | z1t , u1t )   p( zt | mt , st )  p(mt , st | ut , mt 1 , st 1 ) p(mt 1 , st 1 | z1t 1 , u1t 1 )dmt 1 dst
p(m, st | z1t , u1t )   p( zt | m, st )  p( st | ut , st 1 ) p(m, st 1 | z1t 1 , u1t 1 )dst
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Kalman Filters (SLAM)
p(m, st | z1t , u1t ) 
 l1 
 
 l2 

 
 lN  ,
x
 
 y
 
 
  l21

  l1l2

 
  l1lN

  l1x
 l y
 1
 l
 1
 l1l2
 l22

 l2l N
 l2 x
 l2 y
 l2







 l1lN
 l2l N

 l2N
 lN x
 lN y
 l N
 l1x
 l2 x

 lN x
 x2
 xy
 x
 l1 y
 l2 y

 lN y
 xy
 y2
 y
 l1 

 l2 

 
 l N 

 x 
 y 

2 
 
[Smith, Self, Cheeseman, 1990]
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Underwater Mapping with SLAM
Courtesy of Hugh Durrant-Whyte, Univ of Sydney
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Large-Scale SLAM Mapping
Courtesy of John Leonard, MIT
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
SLAM: Limitations

Linear

Scaling: O(N4) in number of features in map
Can’t solve data
association problem

Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Motivation
SLAM
(Kalman filters)
Expectation
Maximization
Real Time
Hybrid
3D Mapping
with EM
Open
Problems
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Unknown Data Association: EM
p(m, st | z1t , u1t )   p( zt | m, st )  p( st | ut , st 1 ) p(m, st 1 | z1t 1 , u1t 1 )dst
 E [log p( z1t , u1t , s1t | m)]
     p(s , s 1 | m , z1t , u1t ) log p(s | u , s 1 ) ds , s 1

   p(s | m , z1t , u1t ) log p( z | s , m) ds

E-Step: Localization
M-Step: Mapping with known poses
[Dempster et al, 77] [Thrun et al, 1998] [Shatkay/Kaelbling 1997]
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
CMU’s Wean Hall (80 x 25 meters)
15 landmarks
17 landmarks
16 landmarks
27 landmarks
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
EM Mapping, Example (width 45 m)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
EM Mapping: Limitations

Local Minima

Not Real-Time
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Motivation
SLAM
(Kalman filters)
Expectation
Maximization
Real Time
Hybrid
3D Mapping
with EM
Open
Problems
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
The Goal
EM:
data association
Not real-time
Kalman filters:
real-time
No data association
?
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Real-Time Approximation (ICRA paper)
p( st , m | z1t , u1t )   p( zt | st , m)  p( st | st 1 , ut ) p( st 1 , m | z1t 1 , u1t 1 ) dst 1

mt , st  argmax p( zt | s, m) p(s | st 1 , ut )
m, s

Incremental ML
p( st | z1t 1 , u1t 1 , mt )   p( zt | st , mt )  p( st | st 1 , ut ) p( st 1 | z1t 1 , u1t 1 , mt 1 ) dst 1
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Incremental ML: Not A Good Idea
mismatch
path
robot
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Real-Time Approximation
p( st , m | z1t , u1t )   p( zt | st , m)  p( st | st 1 , ut ) p( st 1 , m | z1t 1 , u1t 1 ) dst 1

mt , st  argmax p( zt | s, m) p(s | st 1 , ut )
m, s

p( st | z1t 1 , u1t 1 , mt )   p( zt | st , mt )  p( st | st 1 , ut ) p( st 1 | z1t 1 , u1t 1 , mt 1 ) dst 1
Our ICRA Paper 
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Real-Time Approximation
Yellow flashes:
artificially distorted
map (30 deg, 50 cm)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Importance of Posterior Pose Estimate
With pose posterior
Without pose posterior
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Online Mapping with Posterior
Courtesy of Kurt Konolige, SRI, DARPA-TMR
[Gutmann & Konolige, 00]
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Accuracy: “The Tech” Museum, San Jose
2D Map, learned
CAD map
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Multi-Robot Mapping


map
…
map
…
map
map
map
Aligned map
Pre-aligned scans
Cascaded architecture
Every module maximizes likelihood
Pre-aligned scans are passed up in hierarchy
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Multi-Robot Exploration
DARPA TMR Texas 7/99
DARPA TMR Maryland 7/00
(July. Texas. No air conditioning.
Req to dress up. Rattlesnakes)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
3D Volumetric Mapping
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
3D Structure Mapping
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
3D Texture Mapping
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Fine-Grained Structure:
Can We Do Better?
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Motivation
SLAM
(Kalman filters)
Expectation
Maximization
Real Time
Hybrid
3D Mapping
with EM
Open
Problems
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Multi-Planar 3D Mapping
Idea: Exploit fact that buildings posses many planar surfaces
 Compact models
 High Accuracy
 Objects instead of pixels
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
3D Multi-Plane Mapping Problem
Entails five problems
– Generative model with priors: Not all of the world is
planar
– Parameter estimation: Location and angle of planar
surfaces unknown
– Outlier identification: Not all measurements
correspond to planar surfaces (other objects, noise)
– Correspondence: Different measurements correspond
to different planar surfaces
– Model selection: Number of planar surfaces unknown
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Expected Log-Likelihood Function
t
p( z1t | m, c1t )  
 1
Elog p( z1t , c1t | m)

1
2
( j  z   j )
z max 2 J
1 
 c * log

c
j
2
2
2

2
j

1

2
e
2




1


log


2
( J  1) 2



2
t 
zmax
1


     E[c * ] log
2


2
2

 1

2 
J
(


z


)
j
  1 E[c ] j 



j
2
 2 j 1




[Liu et al, ICML-01]
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
EM To The Rescue!
*
*
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Results
With EM
Without EM
(95% of data explained by 7 surfaces)
error
With: Deepayan Chakrabarti, Rosemary Emery, Yufeng Liu, Wolfram Burgard, ICML-01
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
The Obvious Next Step
EM for
concurrent
localization
EM for
object
mapping

Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Underwater Mapping
(with University of Sydney)
With: Hugh Durrant-Whyte, Somajyoti Majunder, Marc de Battista, Steve Scheding
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Motivation
SLAM
(Kalman filters)
Expectation
Maximization
Real Time
Hybrid
3D Mapping
with EM
Open
Problems
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Take-Home Message
Mapping is the
Every state-of-the-art
holy grail in
mapping algorithm
mobile robotics.
is probabilistic.
Sebastian has
one cool animation!
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Open Problems






2D Indoor mapping and exploration
3D mapping (real-time, multi-robot)
Object mapping (desks, chairs, doors, …)
Outdoors, underwater, planetary
Dynamic environments (people, retail stores)
Full posterior with data association (real-time, optimal)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
Open Problems, con’t




Mapping, localization
Control/Planning under uncertainty
Integration of symbolic making
Human robot interaction
Literature Pointers:
 “Robotic Mapping” at
www.thrun.org
 “Probabilistic Robotics” AI Magazine 21(4)
Sebastian Thrun, Carnegie Mellon, IJCAI-2001
www.appliedautonomy.com
Sebastian Thrun, Carnegie Mellon, IJCAI-2001