Probabilistic Algorithms for Mobile Robot Mapping

Presented by: Noha Quan Ravi

2

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) By: Sebastian Thrun et. al.

Robot Mapping… •  Robot Mapping aims is to build a

map of the physical environment of the robot

•  Maps are utilized by mobile robots

for their operations. •  Mapping: generating models of

robot environments from sensor data.

•  Wide variety of sensors are used, with different characteristics (e.g. sonar, laser, IR, cameras, GPS)

3

•  Probabilistic methods have recently proved very effective.

The Nursebot Initiative Serviceroboter

Museum Tour-Guide Robots Roomba – Autonomic

robotic vacuum cleaners

Robot Mapping Applications

video

5

Problem q  Building an Online 3D Indoor Robot map. •  Challenges: 1.  Measurement Noise. 2.  High dimensionality of the mapped

entities.(2D Vs. 3D). 3. Dynamic environment. 4.  Data association (correspondence) problem. 5.  Robotic Localization.

(SLAM: Simultaneous Localization & Mapping)

6

Related Work

7

Ø  Focused on building maps in 2D from sensor measurements.

-  Line extraction from a single scan.

Ø  3D system models proposed by Roth* doesn’t consider uncertainty of individual measurements when generating the 3D model.

* G. Roth and R. Wibowo, “A fast algorithm for making mesh-models from multiple-view range data,” in Proceedings of the DND/CSA Robotics and Knowledge Based Systems Workshop, 1995.

Methodology •  Propose an algorithm “Online EM” for 3D models for indoor

mappings that works in real time.

•  Measurements are captured using laser range finder and panoramic camera.

•  Evaluation of the proposed algorithm via real-time

implementation in several different buildings.

•  Assumptions: Ø  prior knowledge : the shape of basic building elements. Ø  Robot pose is given.

8

Agenda

9

Generative Probability Model

10

3D Surface Normal

Offset bet. Surface & origin

Specify size & orient. of the planar surface

! j

jβOffset

Each of the rectangular flat surface is represented by nine parameters in three groups:

>=< jjjj γβαθ ,,

11

Generative Probability Model: World Model •  Two types of surfaces:

}...,,{ 21 Jθθθθ =

… 1θ 2θ Jθ

Non-flat region

Flat Surfaces

Generative Probability Model: Measurements

•  Measurements are captured from a laser range finder.

12

3 Rzi ∈•  All measurements are 3D point cloud:

}{ izZ =

iziz

iz

jθ jθ

jθ

•  Distance between any measurement zi and the boundary of the surface.

d(zi, θi) = (αi.zi - βj) d(zi, θi) = Euclidean dist. Between zi and boundary of the surface

13

Generative Probability Model: Correspondence Variables

1θ2θ

JθNon-flat region iJc2icci1 *ic

ijc…

iz

otherstoscorrespondntmeasusreme

c jij

θ 0 1

⎩⎨⎧

=

11

* =+∑=

J

jiji cc

Measurement Model

14

Probabilistic generative model of the measurements given the world.

P( zi | Ci, θ )

Agenda

15

Error Distribution

16

A. For flat surfaces (Normal distribution)

B. For non-flat surfaces (Uniform distribution)

2

2 ),(21

221),1|( σ

θ

πσθ

jizd

iji eczp−

==

2max

2

2log

21

2max

maxmax

211

0

,0,/1

),1|(

πσ

πσ

θ

z

iiji

ez

otherwisezzifz

czp

−=

≤≤

⎩⎨⎧

==

The Log-Likelihood Function

17

•  Sensor Model: Conditional probability

•  Sensor Model: Joint probability

]),(

2log[

21

2

2

2

2max2

*

21),|( σ

θπσ

πσθ

jij iji

zdczc

ii eCzp ∑=

+−

]),(

2log[

21

2

2

2

2max2

*

2)1(1)|,( σ

θπσ

πσθ

jij iji

zdczc

ii eJ

Czp ∑

+=

+−

The Expectation of the log likelihood

18

•  EM: take the log likelihood (take the log of the above equation)

•  Assuming independence in measurement noise:

∏ ∑

+=

+−

i

zdczc jij iji

eJ

CZp

]),(

2log[

21

2

2

2

2max2

*

2)1(1

)|,(

σθ

πσ

πσ

θ

),(

],|[21

2log],|[

21

2)1(1log

],|)|,([log

2

2

2max

2

*2

⎥⎥⎦

⎤∑−

⎢⎢⎣

⎡−

+

=

∑

σθ

θ

πσθ

πσ

θθ

jij iij

iii

C

zdzcE

zzcEJ

ZCZpE

Agenda

19

EM Algorithm: offline

•  A popular method for optimization •  Uses hill climbing in likelihood space. •  Used for problems with latent variables.

•  Input: measurement , the 3D points •  Output:

A map which maximize the expectation log likelihood

over correspondences •  EM is an iterative process. Each iteration

includes two steps: E-step and M-step.

iz

C

1θ 2θ

JθNon-flat region

... ]1[ +nθ >< +++ ]1[]1[]1[ ,, nnn γβα

EM Algorithm: offline

21

A. The E-Step

-  Initialize θ [0] = A random map -  Calculate the expectations of the unknown

correspondences for the current map θ [n] .

∑−−

−

+

=

=

==

k

zdz

z

ni

ni

nii

niij

nii

ni

ki

ee

e

zpcpczp

zcpzcEe

2

2

2

2max

2

2max

)|(21

2log

21

2log

21

][

][*

][*

][][*

][*

)|()|(),|(

),|( ],|[

σθ

πσ

πσ

θθθ

θθ ∑

−−

−

+

=

=

==

k

zdz

zd

ni

nij

niji

niij

niij

nij

ki

ji

ee

e

zpcpczp

zcpzcEe

2

2

2

2max

2

2

)|(21

2log

21

)|(21

][

][][

][][][

)|()|(),|(

),|( ],|[

σθ

πσ

σθ

θθθ

θθ

22

EM Algorithm: offline (cont.) B. The M-Step

•  Goal: Maximize the expectation of the log likelihood where the expectation is taken over all the correspondences C.

),(

],|[21

2log],|[

21

2)1(1log

],|)|,([log

2

2

2max

2

*2

⎥⎥⎦

⎤∑−

⎢⎢⎣

⎡−

+

=

∑

σθ

θ

πσθ

πσ

θθ

jij iij

iii

C

zdzcE

zzcEJ

ZCZpE

The quadratic optimization problem is solved via Lagrange multipliers.

Agenda

23

Online EM

•  Offline EM Limitations •  1. Require all the measurements as input •  2. Access every measurement and every

model per iteration •  3. Computation grows as dataset grows

•  Solution: Online EM

24

izZ

jθ ),(2 jizd θ

Online EM

•  Key idea: •  Each time interval, constant number of new

range measurements arrive

•  Advantages: •  1. Acquire compact 3-D models in real time •  2. Limited number of measurements as input •  3. Constant time per iteration

25

Online EM (Online E-Step)

•  Constant number of measurements •  1.New arrival measurements •  2. Old measurements

–  At the boundary of two surface models –  Unexplained by any existing surface models

•  Counter •  1. Record E-step calculation times •  2. Freeze when achieve a threshold

26

Online EM (Online M-Step)

•  Goal: •  Re-estimate the parameters of flat surface model

in constant time

•  Control: •  The number of flat surfaces •  The number of measurements

27

Online EM (Online M-Step)

•  Control the number of flat surfaces : •  Active flat surface model only

•  Definition of “active”: •  A surface is active at time i if the E-step

executed at that time changed the ML correspondence to or away from the surface for any of the updated measurement.

28

Online EM (Model Selection)

q Add new models Ø New measurements not “explained” by existing surface

models Ø  “explained”: likelihood > threshold Ø  not “explained”: seed for new surface q Remove models Ø  The surface model is not supported by sufficient

measurements

29

Agenda

30

Experimental Results

31

•  Two stages

–  Offline EM algorithm. 168,120 range measurements 3,270 camera images, extracted 3,220,950 pixels Two minutes

–  Real time online version of our approach •  Different buildings •  Three examples in paper (One presented here)

Stage 1: Offline EM

32

Map without the texture superimposed.

33

Map with Texture vs. EM algorithm

Non-planar surfaces

34

Non-planer region was not mapped to any of the surfaces (!)

Number of surfaces - J

35

Every 20 steps surfaces are restarted. 500 iterations J settles down to 8.76+/- 1.46 (95.5+-0.006% ) 20 minutes for 2000 iterations.

Stage 2: Real-Time Implementation

36

Stage 2: Real-Time Implementation

37

More scans…

38

Agenda

39

Conclusion - Main Contributions

40

•  Convert EM from offline to online

•  Compact & accurate computations.

•  3D mapping instead of 2D mapping

•  Handles surfaces in arbitrary orientations.

41

Conclusion - Critical Analysis

•  Models are visually realistic but not geometrically accurate.

•  Not best in occluded/unstructured environments.

•  Test data has hallways with no furniture/occlusions.

•  Starts with Assumption that surfaces as flat.

•  Quality of images is not good enough for eye-in-hand. just suffices robot movement.

Future Work •  Can be used for representing more complex

models. •  Outdoor mapping •  Use EM to perform SLAM.

42

Questions ?

43