robot maptalk
TRANSCRIPT
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