1
Sanjiban Choudhury
Occupancy Grid Mapping
Slides adapted from Eric Westman, Cyrill Stachniss
TAs: Matthew Rockett, Gilwoo Lee, Matt Schmittle
What is an occupancy map?
2
What is an occupancy map?
2
Probabilistic representation of world
from
noisy, uncertain sensor measurement data,
with the assumption
that the robot pose is known.
What is an occupancy grid map?
316-833, Spring 20185
Example
Courtesy: C. Stachniss
Discretize world into cells
Assign a probability [0,1]to each cell
When do need to map online?
4
When do need to map online?
4
1. Cant rely on floor plans
When do need to map online?
4
1. Cant rely on floor plans 2. Mapping disaster regions
When do need to map online?
4
1. Cant rely on floor plans
3. Mapping unstructured environments
2. Mapping disaster regions
Even floor plans can be wrong…
516-833, Spring 20188
Even when we have prior maps, they can be inaccurate…
CAD map occupancy grid mapTech Museum, San Jose
Why do we need estimation?
616-833, Spring 20189
Occupancy Grids: From scans to maps
False “hits” from people in the environment
Scans are noisy - if you just added them they will contradict each other and create a mess!
Bayes filter is a powerful tool
7Localization Mapping SLAM POMDP
Assembling Bayes filter
8
TasksLocalization
P(pose | data)
Mapping P(map | data)
SLAM P(pose, map | data)
Belief Representations
Probabilistic Models
Bayes Filter
Different tasks as Bayes filtering
9
Tasks Belief Representation Probabilistic Models
Localization P(pose | data) Gaussian / Particles Motion model
Measurement model
Mapping P(map | data) Discrete (binary) Inverse measurement model
SLAM P(pose, map |
data)
Gaussian (pose, landmarks) Motion model, measurement model, correspondence model
Today’s objective
10
1. Understand occupancy grid mapping intuitively
2. Work through Bayes filter derivation
3. Examine when assumptions get violated
Spilling the beans on mapping
11
Spilling the beans on mapping
11
Step 1: Start with an empty map. 0.5 prob of being free.
Spilling the beans on mapping
11
Step 1: Start with an empty map. 0.5 prob of being free.
zt
xt
Step 2: Accept the latest measurement and pose
Spilling the beans on mapping
11
Step 1: Start with an empty map. 0.5 prob of being free.
zt
xt
Step 2: Accept the latest measurement and pose
Step 3: Raycast every beam. Group cells as HIT and MISS.
HIT: Bump down probability. MISS: Bump up.
zt
xt
Spilling the beans on mapping
11
Step 1: Start with an empty map. 0.5 prob of being free.
zt
xt
Step 2: Accept the latest measurement and pose
Step 3: Raycast every beam. Group cells as HIT and MISS.
HIT: Bump down probability. MISS: Bump up.
zt
xt
Step 4: Get new measurement.
Update the map.
12
Mapping as just another Bayes filtering problem
12
Mapping as just another Bayes filtering problem
Task: Mapping P(map | data)
12
Mapping as just another Bayes filtering problem
Task: Mapping P(map | data)
What is the data?
Stream of pose and laser scans x1:t, z1:t
zt
zt�1
xt�1
xt
12
Mapping as just another Bayes filtering problem
Task: Mapping P(map | data)
What is the belief representation?
16-833, Spring 20189
Occupancy Grids: From scans to maps
False “hits” from people in the environment
Represent world as a
collection of cells
Each belief is [0,1]0 is occupied, 1 is free,
0.5 unknown
P (m) = P (m1,m2, . . . ,mn)
What is the data?
Stream of pose and laser scans x1:t, z1:t
zt
zt�1
xt�1
xt
Graphical model of mapping
13
xt�1 xt xt+1
zt�1 zt zt+1
m
Problem: Space of maps is huge!!
14
Problem: Space of maps is huge!!
14
The state is a matrix of binary values
The belief assigns a probability to all states
Problem: Space of maps is huge!!
14
Map m
mi
The state is a matrix of binary values
The belief assigns a probability to all states
Problem: Space of maps is huge!!
14
Map m
mi
Example: We are mapping 25m x 25m area at 25 cm resolution.
100 x 100 grid = 10,000 cells
How many possible maps can there be?
2^10000 !!!
The state is a matrix of binary values
The belief assigns a probability to all states
Solution: Approximate by independent cells
15
16-833, Spring 201818
Representation
• The probability distribution of the map is given by the product over the cells
example map(4-dim state)
4 individual cellsCourtesy: C. Stachniss
4 individual cells
Structured map (4-dim state)
Joint probability is approximated by product of individual probabilities
P (m) =Y
i
P (mi)
P (m|x1:t, z1:t) =Y
i
P (mi|x1:t, z1:t)
Binary r.v.
Let’s crank through Bayes filter
16
Let’s crank through Bayes filter
16
P (mi|z1:t, x1:t) =P (mi|z1:t�1, x1:t�1, zt, xt)
[old data] [new data]
Let’s crank through Bayes filter
16
⌘P (zt|mi, z1:t�1, x1:t�1, xt)P (mi|z1:t�1, x1:t�1, xt)=(Bayes)
P (mi|z1:t, x1:t) =P (mi|z1:t�1, x1:t�1, zt, xt)
[old data] [new data]
Let’s crank through Bayes filter
16
⌘P (zt|mi, z1:t�1, x1:t�1, xt)P (mi|z1:t�1, x1:t�1, xt)=(Bayes)
P (mi|z1:t, x1:t) =P (mi|z1:t�1, x1:t�1, zt, xt)
[old data] [new data]
(Cond Ind.) ⌘P (zt|mi, z1:t�1, x1:t�1, xt)P (mi|z1:t�1, x1:t�1)=
[old filter value][old data]
Problem 1: Can we apply conditional indep?
17
P (zt|mi, xt)P (zt|mi, z1:t�1, x1:t�1, xt)=[old data][map value] [sensor pose]
Let’s crank through Bayes filter
18
P (mi|z1:t, x1:t) =P (mi|z1:t�1, x1:t�1, zt, xt)
⌘P (zt|mi, z1:t�1, x1:t�1, xt)P (mi|z1:t�1, x1:t�1, xt)=(Bayes)
(Cond Ind.) ⌘P (zt|mi, z1:t�1, x1:t�1, xt)P (mi|z1:t�1, x1:t�1)=
⌘P (zt|mi, xt)P (mi|z1:t�1, x1:t�1)=(Cond Ind.)
Problem 2: Sensor model is hard to define
19
Why is this hard to specify?P (zt|mi, xt)
ztxt
mi
[ray] [cell]
Problem 2: Sensor model is hard to define
19
Why is this hard to specify?P (zt|mi, xt)
ztxt
mi
[ray] [cell]
P (zt|mi, xt) =P (mi|zt, xt)P (zt|xt)
P (mi|xt)[cell] [ray]
Is this easier to specify?
Problem 2: Sensor model is hard to define
20
We can’t specify this…
P (zt|mi, xt)
Problem 2: Sensor model is hard to define
20
We can’t specify this…
P (zt|mi, xt)
Solution: Apply Bayes to get an inverse sensor model
P (zt|mi, xt) =P (mi|zt, xt)P (zt|xt)
P (mi|xt)
Let’s crank through Bayes filter
21
P (mi|z1:t, x1:t) =P (mi|z1:t�1, x1:t�1, zt, xt)
⌘P (zt|mi, z1:t�1, x1:t�1, xt)P (mi|z1:t�1, x1:t�1, xt)=(Bayes)
(Cond Ind.) ⌘P (zt|mi, z1:t�1, x1:t�1, xt)P (mi|z1:t�1, x1:t�1)=
⌘P (zt|mi, xt)P (mi|z1:t�1, x1:t�1)=(Cond Ind.)
Let’s crank through Bayes filter
21
P (mi|z1:t, x1:t) =P (mi|z1:t�1, x1:t�1, zt, xt)
⌘P (zt|mi, z1:t�1, x1:t�1, xt)P (mi|z1:t�1, x1:t�1, xt)=(Bayes)
(Cond Ind.) ⌘P (zt|mi, z1:t�1, x1:t�1, xt)P (mi|z1:t�1, x1:t�1)=
⌘P (zt|mi, xt)P (mi|z1:t�1, x1:t�1)=(Cond Ind.)
=⌘
P (mi|zt, xt)P (zt|xt)
P (mi|xt)P (mi|z1:t�1, x1:t�1)(Bayes.)
Let’s crank through Bayes filter
21
P (mi|z1:t, x1:t) =P (mi|z1:t�1, x1:t�1, zt, xt)
⌘P (zt|mi, z1:t�1, x1:t�1, xt)P (mi|z1:t�1, x1:t�1, xt)=(Bayes)
(Cond Ind.) ⌘P (zt|mi, z1:t�1, x1:t�1, xt)P (mi|z1:t�1, x1:t�1)=
⌘P (zt|mi, xt)P (mi|z1:t�1, x1:t�1)=(Cond Ind.)
=⌘
P (mi|zt, xt)P (zt|xt)
P (mi|xt)P (mi|z1:t�1, x1:t�1)(Bayes.)
(Cond Ind.) ⌘
P (mi|zt, xt)P (zt|xt)
P (mi)P (mi|z1:t�1, x1:t�1)=
Let’s look at likelihood ratios
22
P (mi|z1:t, x1:t) ⌘
P (mi|zt, xt)P (zt|xt)
P (mi)P (mi|z1:t�1, x1:t�1)=
There are terms we don’t know and would not like to calculate!
[cell=1]
Let’s look at likelihood ratios
22
P (mi|z1:t, x1:t) ⌘
P (mi|zt, xt)P (zt|xt)
P (mi)P (mi|z1:t�1, x1:t�1)=
There are terms we don’t know and would not like to calculate!
[cell=1]
⌘
P (¬mi|zt, xt)P (zt|xt)
P (¬mi)P (¬mi|z1:t�1, x1:t�1)=
P (¬mi|z1:t, x1:t)
Let’s look at the opposite probability!
[cell=0]
Let’s look at likelihood ratios
22
P (mi|z1:t, x1:t) ⌘
P (mi|zt, xt)P (zt|xt)
P (mi)P (mi|z1:t�1, x1:t�1)=
There are terms we don’t know and would not like to calculate!
Let’s look at the ratio
P (mi|z1:t, x1:t)
P (¬mi|z1:t, x1:t)=
P (mi|zt, xt)
P (¬mi|zt, xt)
P (¬mi)
P (mi)
P (mi|z1:t�1, x1:t�1)
P (¬mi|z1:t�1, x1:t�1)
[cell=1]
⌘
P (¬mi|zt, xt)P (zt|xt)
P (¬mi)P (¬mi|z1:t�1, x1:t�1)=
P (¬mi|z1:t, x1:t)
Let’s look at the opposite probability!
[cell=0]
Log likelihood ratios
23
P (mi|z1:t, x1:t)
P (¬mi|z1:t, x1:t)=
P (mi|zt, xt)
P (¬mi|zt, xt)
P (¬mi)
P (mi)
P (mi|z1:t�1, x1:t�1)
P (¬mi|z1:t�1, x1:t�1)
(inverse sensor model) (new ratio)(prior)
Log likelihood ratios
23
P (mi|z1:t, x1:t)
P (¬mi|z1:t, x1:t)=
P (mi|zt, xt)
P (¬mi|zt, xt)
P (¬mi)
P (mi)
P (mi|z1:t�1, x1:t�1)
P (¬mi|z1:t�1, x1:t�1)
(inverse sensor model) (new ratio)(prior)
Taking logs of all terms
log
✓P (mi|z1:t, z1:t)P (¬mi|z1:t, z1:t)
◆log
✓P (mi|zt, xt)
P (¬mi|zt, xt)
◆log
✓P (mi)
P (¬mi)
◆log
✓P (mi|z1:t�1, x1:t�1)
P (¬mi|z1:t�1, x1:t�1)
◆= � +
Log likelihood ratios
23
P (mi|z1:t, x1:t)
P (¬mi|z1:t, x1:t)=
P (mi|zt, xt)
P (¬mi|zt, xt)
P (¬mi)
P (mi)
P (mi|z1:t�1, x1:t�1)
P (¬mi|z1:t�1, x1:t�1)
(inverse sensor model) (new ratio)(prior)
Taking logs of all terms
log
✓P (mi|z1:t, z1:t)P (¬mi|z1:t, z1:t)
◆log
✓P (mi|zt, xt)
P (¬mi|zt, xt)
◆log
✓P (mi)
P (¬mi)
◆log
✓P (mi|z1:t�1, x1:t�1)
P (¬mi|z1:t�1, x1:t�1)
◆= � +
lt l(mi|zt, xt) lt�1l0= +�(updated belief) (old belief)(inverse model) (prior)
Pseudo code of occupancy mapping
24
zt
xt
for every ray r in (xt, zt)
for every cell mi in r
if mi is MISS
li = li + l(HIT)� l0
li = li + l(MISS)� l0else
What is the inverse sensor model P(m|z,x)?
25
P (zt|mi, xt) =P (mi|zt, xt)P (zt|xt)
P (mi|xt)
P (zt|mi, xt) =P (mi|zt, xt)P (zt|xt)
P (mi|xt)
P (mi)� �
P (mi) + �
P (mi)=
if ray passes through cell
if ray does not intersect
cell
if ray stops in cell
[cell] [ray]
Problem: Dynamic obstacles
26
t=0 t=2 t=4 t=6 t=8 t=10 t=12
Laser array
What will the occupancy map look like?
Problem: Dynamic obstacles
27
t=2 t=4 t=6
t=8 t=10 t=12
But is this what we want??
Solution: Don’t use independence
28
t=2 t=4 t=6
t=8 t=10 t=12
If we directly guessed the best explanation, we can come up with this!
Solution: Don’t use independence
28
t=2 t=4 t=6
t=8 t=10 t=12
If we directly guessed the best explanation, we can come up with this!
What is wrong with independence? If I know there is ONE obstacle and if a cell gets a MISS
and neighbor gets a HIT, then cell must be FREE (P=1).
Dynamic obstacle mapping in general
29
“Map building with mobile robots in dynamic environments” D. Hähnel, R. Triebel, W. Burgard, and S. Thrun. 2003
“Occupancy Grid Models for Robot Mapping in Changing Environments” D. Meyer-Delius, M. Beinhofer and W. Burgard 2003