geometric rays for bearing-only slam joan solà and thomas lemaire laas-cnrs toulouse, france
Post on 22-Dec-2015
219 Views
Preview:
TRANSCRIPT
Geometric Rays Geometric Rays for for
Bearing-Only SLAMBearing-Only SLAM
Geometric Rays Geometric Rays for for
Bearing-Only SLAMBearing-Only SLAM
Joan Solà and Thomas LemaireLAAS-CNRS
Toulouse, France
2
This is about…This is about…
1. Bearing-Only SLAM (or Single-Camera SLAM)
2. Landmark Initialization
3. Efficiency:
Gaussian PDFs
4. Dealing with difficult situations:
EKF-SLAM is our choice
3
What’s insideWhat’s inside
» The Problem of landmark initialization
» The Geometric Ray: an efficient representation of the landmark position’s PDF
» delayed and undelayed methods
» Two efficient real-time solutions:
• The Batch Update delayed initialization
• The Federated Information Sharing (FIS) undelayed initialization
4
The problem: Landmark Initialization
The problem: Landmark Initialization
• The naïve way
?
Te
tnow
tbefore tnow
?
5
The problem: Landmark Initialization
The problem: Landmark Initialization
• Consider uncertainties
tnow
tbefore tnow
Te
The 3D pointThe 3D pointis insideis inside
?
6
The problem: Landmark Initialization
The problem: Landmark Initialization
• The Happy and Unhappy cases
Happy
Not so Happy
Unhappy
7
The problem: Landmark Initialization
The problem: Landmark Initialization
• The Happy case
•I could compute the resulting Gaussian:
•The mean is close to the nominal (naïve) solution
•The covariance is obtained by transforming robot and measure incertitudes via the Jacobians of the observation functions
tbefore tnow
Remember the past!
8
The problem: Landmark Initialization
The problem: Landmark Initialization
• The Not so Happy case
0
1
2
3
0
1
2
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
Gaussiannity TEST needed
9
The problem: Landmark Initialization
The problem: Landmark Initialization
• The Unhappy case
• There’s simply nothing to compute!
• And there’s nothing to wait for.
• But it could be interesting to initialize landmarks that lie close to the axis of travel
???
10
The KEY IdeaThe KEY Idea
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.??
Initialapproximation
is easy
Member selection is easy and safe
Last memberis easily incorporated
UNDELAYEDinitialization
DELAYEDINITIALIZATION
11
Defining the Geometric RayDefining the Geometric Ray
Define a geometric series of Gaussians
xR : camera position
4r4
3r3
= i / ri
= ri / ri-1
[ rmin rmax ]
Fill the space between rmin and rmax
1. With the minimum number of terms
2. Keeping linearization constraints
12
• From aspect ratio, geometric base and range bounds:
• The number of terms is logarithmic on rmax / rmin :
• This leads to very small numbers:
• As members are Gaussian, they are easily manipulable with EKF.
The Geometric Ray’s benefitsThe Geometric Ray’s benefits
Scenario rmin rmax Ratio Ng
Indoor 0.5 5 10 3
Outdoor 1 100 100 5
Long Range 1 1000 1000 7
[rmin , rmax]
Ng = f( log(rmax / rmin)
1
2
13
How it worksHow it works
The first observationdetermines the Conic Ray
14
I model the Conic Raywith the geometric series
I can initialize all members now,and I have an UNDELAYED method.
3
How it worksHow it works
15
I move and make a secondobservation
Members are distinguishable
How it worksHow it works
16
I compute likelihoods andupdate member’s credibilities
Which means modifying its shape€
z = y − h(x)
Z = HPH '+R
λ =1
2π Zexp − 1
2 z ⋅Z−1 ⋅z'{ }
€
C+ = C ⋅λ
How it worksHow it works
17
I prune unlikely members
Which is a trivial and conservative decision
€
C <0.001
number _ of _ members
How it worksHow it works
18
I keep on going…
How it worksHow it works
19
And one day I will havejust one member left.
This member is already Gaussian!
If I initialize it now, I have a DELAYED method.
3
How it worksHow it works
20
DELAYED and UNDELAYED methods
DELAYED and UNDELAYED methods
• A naïve algorithm
• A consistent algorithm
• The Batch Update algorithmDELA
YED
UN
DELA
YED
• The multi-map algorithm
• The Federated Information Sharing algorithm
21
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
A naïve algorithmA naïve algorithm
1. Express the Ray in world frame
2. Use observations to prune members
3. When one member is left:
• Take its current distance to the camera
• Initialize the landmark with the last observation, using the determined distance as a measure
LACK OF CORRELATIONS
€
ˆ x
€
P
DELA
YED
22
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
A consistent algorithmA consistent algorithm
1. Express the Ray in robot frame
2. Store this frame correlated in the map state vector
3. Use observations to prune members
4. When one member is left:• Initialize the landmark with the first observation, using the
determined distance as a measure
• Perform one update with the last observation
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
DELA
YED
23
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
The Batch Update algorithmThe Batch Update algorithm1. Express the Ray in robot frame2. Store this frame correlated in the map state vector 3. At selected subsequent observations:
• Do member pruning.• Store robot’s frame along with associated observations
4. When one member is left:• Initialize it in the map• Make a batch update with all stored information
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
DELA
YED
24
The Batch Update algorithmThe Batch Update algorithmD
ELA
YED
25
The Batch Update algorithmThe Batch Update algorithmD
ELA
YED
www.laas.fr/~tlemaire/publications/lemaireIROS2005.pdfwww.laas.fr/~tlemaire/publications/lemaireIROS2005.pdf
QuickTime™ et undécompresseur
sont requis pour visionner cette image.
QuickTime™ et undécompresseur
sont requis pour visionner cette image.
26
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
The multi-map algorithmThe multi-map algorithm
1. Initialize all Ray members as landmarks in different maps2. 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
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
OFF-LINE METHOD
UN
DELA
YED
27
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
The Federated Information Sharing (FIS) algorithm
The Federated Information Sharing (FIS) algorithm
1. Initialize Ray members as different landmarks in the same map2. At all subsequent observations:
• Update credibilities and do member pruning• Perform a federated soft update
3. When only one member is left:• Nothing to do
QuickTime™ and aTIFF (LZW) decompressor
are needed to see this picture.
UN
DELA
YED
28
The FIS algorithmThe FIS algorithm
• The Federated soft update: Sharing the Information
Observation {y, R}
EKF update with member 1
EKF update with member 2
EKF update with member N
UN
DELA
YED
{y, R1 }{y, R2 }
{y, RN }
… …
Information Sharing :
Federated Coefficient i :
Likelihood Privilege :
29
The FIS algorithmThe FIS algorithm
QuickTime™ et undécompresseur Cinepak
sont requis pour visionner cette image.
www.laas.fr/~jsola/papers/
undelayedBOSLAM.pdf
www.laas.fr/~jsola/papers/
undelayedBOSLAM.pdf
UN
DELA
YED
30
The FIS algorithmand the Unhappy case
The FIS algorithmand the Unhappy case
UN
DELA
YED
QuickTime™ et undécompresseur Cinepak
sont requis pour visionner cette image.
31
The Geometric Ray is a very powerful representation
for Bearing-Only SLAM
In conclusionIn conclusion
We can use it in several existing DELAYED algorithms
And with UNDELAYED methods we can deal with situations
not affordable until now
Thank You!Thank You!
and wellcome to Catalonia!and wellcome to Catalonia!
Thank You!Thank You!
and wellcome to Catalonia!and wellcome to Catalonia!
top related