1
© sebastian thrun, CMU, 2000 1
CS329 Probabilistic RoboticsToday’s Topic: 3D Mapping with EM
Sebastian ThrunCarnegie Mellon University
www.cs.cmu.edu/~thrun
© sebastian thrun, CMU, 2000 2
The Problem
� Forward-pointed laser (2D information)� Upward-pointed laser (3D information)� Panoramic camera
© sebastian thrun, CMU, 2000 3
The (Almost) Raw Range Data
© sebastian thrun, CMU, 2000 4
3D Mapping
� Observation: World mostly composed of planar surfaces
� Problem: How to generate multi-plane model?
© sebastian thrun, CMU, 2000 5
The 3D Multi-Plane Mapping Problem
� Entails a range of problems• Parameter estimation: Each surface presently
possesses 6 parameters (x, y, z, θ, φ, ψ)• Outlier identification: Shall we explain every single
measurement by a planar surface?• Correspondence: What measurement corresponds to
what surface?• Model selection: How many surfaces should there be?
• Robot localization: Noise in robot motion has a systematic effect on multiple measurements (this spoils any independence assumption)
© sebastian thrun, CMU, 2000 6
CAD map
Concurrent Mapping and Localization:To be Discussed in Later Cllases
occupancy grid map
2
© sebastian thrun, CMU, 2000 7
The 3D Multi-Plane Mapping Problem
� Entails a range of problems• Parameter estimation: Each surface presently
possesses 6 parameters (x, y, z, θ, φ, ψ)• Outlier identification: Shall we explain every single
measurement by a surface?• Correspondence: What measurement corresponds to
what surface?• Model selection: How many surfaces should there be?
• Robot localization: Noise in robot motion has a systematic effect on multiple measurements (this spoils any independence assumption)
© sebastian thrun, CMU, 2000 8
3D Structure Mapping (Raw Data)
© sebastian thrun, CMU, 2000 9
3D Texture Mapping (Raw Data)
© sebastian thrun, CMU, 2000 10
Fine-Grained Structure
ν Can we do better?
© sebastian thrun, CMU, 2000 11
Fine-Grained Structure (Wall data):Scatter plot of surface normals
By W. Burgard and D. Hähnel© sebastian thrun, CMU, 2000 12
The Remaining Problem
Mostly Multi-Planar modelData
3
© sebastian thrun, CMU, 2000 13
Clustering Data with K-Means
© sebastian thrun, CMU, 2000 14
EM: Generalized K-Means
© sebastian thrun, CMU, 2000 15
EM for Fitting Multi-Planar Models
******
© sebastian thrun, CMU, 2000 16
Probabilistic Model
� 3D Model: },,,{ 21 Jθθθθ �=
ℜ×ℜ∈= 3,jjj βαθ
Planar surface in 3D
βαθ −⋅= ijijzz ),dist(
Distance point-surfacesurface
surface normal α
y
x
z
displacement β
© sebastian thrun, CMU, 2000 17
Measurement Model
� Case 1: Measurement zi caused by plane θj
2
2)(
2
1
22
1)|( σ
βα
πσθ
jij z
jiezp
−⋅−
=
ν Case 2: zi caused by non-planar object
2
2max
2ln
2
1
2max
*2
11)|( πσ
πσθ
z
ie
zzp
−==
© sebastian thrun, CMU, 2000 18
Measurement Model
−⋅+− ∑
= =
J
j
jijj
zc
zc
Ji eccczp 12
2
2
2max
*)(
2ln
21
2*12
1),,,,|(
σβα
πσ
πσθ �
correspondence variables C:
}
∑=
=+
∈J
jj
j
cc
cc
1*
*
1
}1,0{,
∏=
−⋅+− ∑
=⇒=
1
)(
2ln
21
2
12
2
2
2max
*
2
1),|(
i
zc
zc
J
j
jijiji
eCZpσ
βα
πσ
πσθ
4
© sebastian thrun, CMU, 2000 19
Expected Log-Likelihood Function
∏=
−⋅+− ∑
= =
1
)(
2ln
21
2
12
2
2
2max
*
2
1),|(
i
zc
zc
J
j
jijiji
eCZpσ
βαπσ
πσθ
[ ] ∑
−⋅−
−
+
=i
jijij
ic
zZcE
zZcE
J
ZCZpE
2
2
2
2max
*
2
)(],|[
2
1
2ln],|[
2
1
2)1(
1ln
,|)|,(ln
σβα
θ
πσθ
πσ
θθ
…after some simple math
© sebastian thrun, CMU, 2000 20
The EM Algorithm
� Define:
� Initialization: Pick random θ
� E-Step: Calculate expectations in
� M-Step: Generate New Map
[ ] ∑
−⋅−
−
+
==i
jijij
ic
zZcE
zZcE
J
ZCZpEQ
2
2
2
2max
*
2
)''(],|[
2
1
2ln],|[
2
1
2)1(
1ln
,|)'|,(ln)|'(
σβα
θ
πσθ
πσ
θθθθ
)|( ][]1[ nnQ θθ +
)|(argmax ][]1[ nn Q θθθθ
=+
© sebastian thrun, CMU, 2000 21
The E (Expectation) Step
*
© sebastian thrun, CMU, 2000 22
E-Step
� Calculate expectations with fixed model θ :
(normalize so that )
2
2
2
2max
)(
21
][
2ln
21
][*
],|[
],|[
σβα
πσ
θ
θjij z
nij
zn
i
eZcE
eZcE
−⋅−
−
∝
∝
∑=
=+J
j
nij
ni ZcEZcE
1
][][* 1],|[],|[ θθ
© sebastian thrun, CMU, 2000 23
The M (Maximization) Step
* *
© sebastian thrun, CMU, 2000 24
M-Step
� Maximize
1'' =⋅ jj ααsubject to
ν Is equivalent to minimizing
∑∑=−⋅
i
J
jjij
nij zZcE
1
2][
)''](,|[ βαθ 1'' =⋅ jj ααsubject to
[ ] ∑
−⋅−
−
+
==i
jijij
ic
zZcE
zZcE
J
ZCZpEQ
2
2
2
2max
*
2
)''(],|[
2
1
2ln],|[
2
1
2)1(
1ln
,|)'|,(ln)|'(
σβα
θ
πσθ
πσ
θθθθ
5
© sebastian thrun, CMU, 2000 25
M-Step: Solve via Lagrange MultipliersNotice: the notation has been simplified
∑∑=
→−⋅i
J
jjijij zcE
1
2 min)]([ βα 1: =⋅∀ jjj αα
∑∑ ∑= =
⋅+−⋅=i
J
j
J
jjjjjijij zcEL
1 1
2)]([ ααλβα
ν Define
0)]([!
=−⋅=∂∂
jijijj
zcEL
βαβ
0)]([!
=−−⋅=∂∂
jjijijijj
zzcEL
αλβαα
1=⋅ jj αα
ν And observe that
© sebastian thrun, CMU, 2000 26
M-Step: Solve via Lagrange Multipliers� Solve for β :
� Substitute back:
� Is of the linear form:
� Solution: two Eigenvectors of A with smallest Eigenvalues.
∑∑ ⋅
=⇒
kjkj
kkjkj
j cE
zcE
α
αβ
][
][0)]([
!
=−⋅=∂∂
jijijj
zcEL
βαβ
0)][
][]([
!
=−⋅
−⋅=∂∂⇒
∑∑
jji
kjkj
kkjkj
ijijj
zcE
zcEzcE
Lαλ
α
αα
α
0)]([!
=−−⋅=∂∂
jjijijijj
zzcEL
αλβαα
jjjA αλα =
© sebastian thrun, CMU, 2000 27
The 3D Multi-Plane Mapping Problem
Entails a range of problems• Parameter estimation: Each surface presently
possesses 6 parameters (x, y, z, θ, φ, ψ)• Outlier identification: Shall we explain every single
measurement by a surface?• Correspondence: What measurement corresponds to
what surface?• Model selection: How many surfaces should there be?
• Robot localization: Noise in robot motion has a systematic effect on multiple measurements (this spoils any independence assumption)
© sebastian thrun, CMU, 2000 28
Choosing the “Right” Number of Planes: AIC
J=2 J=3 J=5J=0 J=1 J=4
increased data likelihood
increased prior probability
)(log)|(log)|(log JpJdpconstdJp ++=
© sebastian thrun, CMU, 2000 29
Model Selection
Approximately every 20 iterations of EM: Start new surfaces
• Near any set of collinear measurements Terminate unsupported surfaces
• If not supported by enough measurements• If density of measurements too low• If two planes are too close to each other
© sebastian thrun, CMU, 2000 30
Quality of 3D model
With EMWithout EM
6
© sebastian thrun, CMU, 2000 31
Quality of 3D model
With EMWithout EM
© sebastian thrun, CMU, 2000 32
Numerical Results
fraction measurements explained by θJ (number of surfaces in θ)
© sebastian thrun, CMU, 2000 33
Advantages and Limitations
Increased compactness, accuracy �
� Not real-time � Only flat surfaces
© sebastian thrun, CMU, 2000 34
Towards Online EM
� Each Measurement is only considered in ≤ N E-steps
� When doing so, each measurement only compared to ≤ K surfaces
� Each surface calculated from ≤ M measurements� New surfaces only started from recent
measurements, discarded if not growing fast
� …with small N, K, and M
© sebastian thrun, CMU, 2000 35
Online EM and Model Selection
mostly planar map with Online EMraw data
© sebastian thrun, CMU, 2000 36
Relation to Bayes Filters??
� Maximizes posterior likelihood
instead of computing posterior incrementally
� What does this imply?
)),|(()|( 1..1..1 ttt zzpfzp −= θθ
)|(argmax
)(log],|)|,([logargmax
..1
..1..1..1..1
t
tttc
zp
pzczpEt
θ
θθθ
θ
θ
≈
+