cs329 probabilistic robotics 3d mapping with emthrun/cs329/notes/3d.pdf© sebastian thrun, cmu, 2000...

1

© sebastian thrun, CMU, 2000 1

CS329 Probabilistic RoboticsToday’s Topic: 3D Mapping with EM

Sebastian ThrunCarnegie Mellon University

www.cs.cmu.edu/~thrun


The Problem

� Forward-pointed laser (2D information)� Upward-pointed laser (3D information)� Panoramic camera


The (Almost) Raw Range Data


3D Mapping

� Observation: World mostly composed of planar surfaces

� Problem: How to generate multi-plane model?


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)


CAD map

Concurrent Mapping and Localization:To be Discussed in Later Cllases

occupancy grid map

2



� Entails a range of problems• Parameter estimation: Each surface presently


measurement by a surface?• Correspondence: What measurement corresponds to




3D Structure Mapping (Raw Data)


3D Texture Mapping (Raw Data)


Fine-Grained Structure

ν Can we do better?


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


Clustering Data with K-Means


EM: Generalized K-Means


EM for Fitting Multi-Planar Models

******


Probabilistic Model

� 3D Model: },,,{ 21 Jθθθθ �=

ℜ×ℜ∈= 3,jjj βαθ

Planar surface in 3D

βαθ −⋅= ijijzz ),dist(

Distance point-surfacesurface

surface normal α

y

x

z

displacement β


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

−==


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


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


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 θθθθ

=+


The E (Expectation) Step

*


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],|[],|[ θθ


The M (Maximization) Step

* *


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


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


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 αλα =



Entails a range of problems• Parameter estimation: Each surface presently


measurement by a surface?• Correspondence: What measurement corresponds to




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 ++=


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


Quality of 3D model

With EMWithout EM

6


Quality of 3D model

With EMWithout EM


Numerical Results

fraction measurements explained by θJ (number of surfaces in θ)


Advantages and Limitations

Increased compactness, accuracy �

� Not real-time � Only flat surfaces


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


Online EM and Model Selection

mostly planar map with Online EMraw data


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

θ

θθθ

θ

θ

≈

+

7


* *


*

cs329 probabilistic robotics 3d mapping with emthrun/cs329/notes/3d.pdf© sebastian thrun, cmu, 2000...

Documents