Transcript ppt presentation

Undelayed Initialization
in
Bearing-Only SLAM
Joan Solà, André Monin,
Michel Devy and Thomas Lemaire
LAAS-CNRS
Toulouse, France
This is about…
1.
Bearing-Only SLAM (or Single-Camera SLAM)
2.
Landmark Initialization
3.
Efficiency:

4.
Gaussian PDFs
EKF-SLAM is our choice
Dealing with difficult situations:
2
What’s inside
»
The Problem of landmark initialization
»
The Geometric Ray: an efficient
representation of the landmark position’s PDF
»
delayed and Undelayed methods
»
An efficient undelayed real-time solution:
•
The Federated Information Sharing (FIS)
algorithm
3
The problem:
Landmark Initialization
• The naïve way
?
tnow
?
tbefore
tnow
Te
4
The problem:
Landmark Initialization
• Consider uncertainties
?
The 3D point
is inside
tnow
tbefore
tnow
Te
5
The problem:
Landmark Initialization
• The Happy and Unhappy cases
Not so Happy
Happy
Unhappy
6
The problem:
Landmark Initialization
• The Happy case
• I could compute the resulting
Gaussian:
• The mean is close to the
nominal (naïve) solution
Remember
previous pose!
tbefore
tnow
• The covariance is obtained
by transforming robot and
measure uncertainties via
the Jacobians of the
observation functions
7
The problem:
Landmark Initialization
3
• The Not so Happy case
2
1
0
0
1
2
Gaussianity TEST needed!
3
• Computation gets risky:
• A Gaussian does not suit the true PDF:
• The mean is no longer close to the nominal solution
• The covariance is not representative
• But I can still wait for a better situation
8
The problem:
Landmark Initialization
• The Unhappy case
???
• There’s simply nothing to compute!
• And there’s nothing to wait for.
• But it is the case for landmarks that lie close to
the motion direction
9
The KEY Idea
DELAYED
QuickTime™ and a
TIFF (LZW) decompressor
are needed to see this picture.
?
QuickTime™ and a
TIFF (LZW ) decompressor
are needed to see this picture.
INITIALIZATION
Last member
is easily
incorporated
Initial
approximation
is easy
QuickTime™ and a
TIFF (LZW) decompressor
are needed to see this picture.
UNDELAYED
<Davison>
<Bailey>
[Lemaire]
QuickTime™ and a
TIFF (LZW) decompressor
are needed to see this picture.
Member selection is easy and safe
initialization
[Kwok]
10
Defining the Geometric Ray
Fill the space between rmin and rmax
1. With the minimum number of terms
2. Keeping linearization constraints
r4
r3
Define a geometric
series of Gaussians
4
3
 =  i / ri
 = ri / ri-1
[ rmin
[Peach]
rmax ]
xR : camera position
11
The Geometric Ray’s benefits
• From aspect ratio, geometric base and range bounds:


[rmin , rmax]
• The number of terms is logarithmic on rmax / rmin :
Ng = f( log(rmax / rmin)
1
• This leads to very small numbers:
Scenario
rmin
rmax
Ratio
Ng
Indoor
0.5
5
10
3
Outdoor
1
100
100
5
Long Range
1
1000
1000
7
2
• As members are Gaussian, they are easily handled with EKF.
12
How it works
The first observation
determines the Conic Ray
13
How it works
I model the Conic Ray
with the geometric series
3
I can initialize all members now,
and I have an UNDELAYED method.
14
How it works
I move and make a second
observation
Members are distinguishable
15
How it works
I compute likelihoods and
update member’s credibilities
C  C  

z  y  h(x)
Z  HPH'R
1

exp 12 z  Z 1  z'
2 Z
Which means modifying its shape

16
How it works
I prune unlikely members
C
0.001
number _ of _ members

Which is a trivial and conservative decision
17
How it works
With UNDELAYED methods
I can perform a map update
18
How it works
I keep on going…
19
How it works
And one day I will have
just one member left.
This member is already Gaussian!
3
If I initialize it now, I have a DELAYED method.
20
DELAYED
and UNDELAYED
Unhappy
UNDELAYED
methods
Not so Happy
DELAYED
UNDELAYED
Happy
DELAYED
UNDELAYED
21
UNDELAYED
DELAYED
DELAYED
and UNDELAYED methods
• A naïve algorithm
• A consistent algorithm
• The Batch Update algorithm
• The multi-map algorithm
• The Federated Information Sharing
algorithm
22
UNDELAYED
The multi-map algorithm
1. Initialize all Ray members as landmarks in different maps
2. At all subsequent observations:
• Update map credibilities and prune the bad ones
• Perform map updates as in EKF
3. When only one map is left:
• Nothing to do
OFF-LINE METHOD
QuickTime™ and a
TIFF (LZW) decompressor
are needed to see this picture.
QuickTime™ and a
TIFF (LZW) decompressor
are needed to see this picture.
23
UNDELAYED
The Federated Information
Sharing (FIS) algorithm
1. Initialize Ray members as different landmarks in the same map
2. At all subsequent observations:
• Update credibilities and do member pruning
• Perform a Federated Information Sharing update
3. When only one member is left:
• Nothing to do
QuickTime™ and a
TIFF (LZW) decompressor
are needed to see this picture.
QuickTime™ and a
TIFF (LZW) decompressor
are needed to see this picture.
24
• The Federated soft update: Sharing the Information
EKF update with member 1
{y, RN }
EKF update with member N
EKF update with member 2
…
Observation {y, R}
{y, R1 }
{y, R2 }
…
UNDELAYED
The FIS algorithm
Information Sharing :
Likelihood Privilege :
Federated Coefficient i :
25
The FIS algorithm
and the Unhappy case
QuickTime™ et un
décompresseur Cinepak
sont requis pour visionner cette image.
UNDELAYED
26
The FIS algorithm
and the Unhappy case
 1 B&W image / 7 cm
UNDELAYED
 512 x 378 pix, 90º HFOV
 1 pix noise
27
The FIS algorithm
and the Unhappy case
Top view
Side view
UNDELAYED
28
In conclusion
• The Geometric Ray is a very powerful representation
for Bearing-Only SLAM
• We can use it in both DELAYED and UNDELAYED
methods
• UNDELAYED methods allow us to initialize
landmarks in the direction of motion
• Federated Information Sharing permits
a Real Time implementation
29
Thank You!