Geometric Rays for BearingOnly SLAM

1
Geometric Rays for Bearing-Only SLAM

• Joan Solà and Thomas Lemaire
• LAAS-CNRS
• Toulouse, France

2
This is about

• Bearing-Only SLAM (or Single-Camera SLAM)
• Landmark Initialization
• Efficiency
• Gaussian PDFs
• Dealing with difficult situations

3
Whats inside

• The Problem of landmark initialization
• The Geometric Ray an efficient representation of
  the landmark positions 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 naïve way

?
tnow
?
tbefore
tnow
Te
5
The problem Landmark Initialization

• Consider uncertainties

?
The 3D pointis inside
tnow
tbefore
tnow
Te
6
The problem Landmark Initialization

• The Happy and Unhappy cases

Not so Happy
Happy
Unhappy
7
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

Remember the past!
tbefore
tnow
8
The problem Landmark Initialization
3?
2?

• The Not so Happy case

1?
0?
0?
1?
2?
Gaussiannity 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

9
The problem Landmark Initialization

• The Unhappy case

???

• Theres simply nothing to compute!
• And theres nothing to wait for.
• But it could be interesting to initialize
  landmarks that lie close to the axis of travel

10
The KEY Idea
DELAYEDINITIALIZATION
?
do it the easy way
Last memberis easily incorporated
Initialapproximation is easy
UNDELAYEDinitialization
Member selection is easy and safe
11
Defining the Geometric Ray

• Fill the space between rmin and rmax
• With the minimum number of terms
• Keeping linearization constraints

• Define a geometric series of Gaussians

?4
r4
?3
r3
? ?i / ri
? ri / ri-1
rmin
rmax
xR camera position
12
The Geometric Rays benefits

• 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.

rmin , rmax
???????
?????
Ng f(???? log(rmax / rmin)
1
2
13
How it works
The first observationdetermines the Conic Ray
14
How it works
I model the Conic Raywith the geometric series
I can initialize all members now,and I have an
UNDELAYED method.
3
15
How it works
I move and make a secondobservation
Members are distinguishable
16
How it works
I compute likelihoods andupdate members
credibilities
Which means modifying its shape
17
How it works
I prune unlikely members
Which is a trivial and conservative decision
18
How it works
I keep on going
19
How it works
And one day I will have just one member left.
3
This member is already Gaussian! If I initialize
it now, I have a DELAYED method.
20
DELAYED and UNDELAYED methods

• A naïve algorithm
• A consistent algorithm
• The Batch Update algorithm

DELAYED
21
A naïve algorithm

• Express the Ray in world frame
• Use observations to prune members
• 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

DELAYED
LACK OF CORRELATIONS
22
A consistent algorithm

• Express the Ray in robot frame
• Store this frame correlated in the map state
  vector
• Use observations to prune members
• 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

DELAYED
23
The Batch Update algorithm

• Express the Ray in robot frame
• Store this frame correlated in the map state
  vector
• At selected subsequent observations
• Do member pruning.
• Store robots frame along with associated
  observations
• When one member is left
• Initialize it in the map
• Make a batch update with all stored information

DELAYED
24
The Batch Update algorithm
DELAYED
25
The Batch Update algorithm
DELAYED
www.laas.fr/tlemaire/publications/lemaireIROS2005
.pdf
26
The multi-map algorithm

• Initialize all Ray members as landmarks in
  different maps
• At all subsequent observations
• Update map credibilities and prune the bad ones
• Perform map updates as in EKF
• When only one map is left
• Nothing to do

UNDELAYED
OFF-LINE METHOD
27
The Federated Information Sharing (FIS) algorithm

• Initialize Ray members as different landmarks in
  the same map
• At all subsequent observations
• Update credibilities and do member pruning
• Perform a federated soft update
• When only one member is left
• Nothing to do

UNDELAYED
28
The FIS algorithm

• The Federated soft update Sharing the Information

UNDELAYED
EKF update with member 1
EKF update with member 2
Observation y, R

EKF update with member N
29
The FIS algorithm
UNDELAYED
www.laas.fr/jsola/papers/undelayedBOSLAM.pdf
30
The FIS algorithmand the Unhappy case
UNDELAYED
31
In conclusion

• The Geometric Ray is a very powerful
  representation for Bearing-Only SLAM

We can use it in several existing DELAYED
algorithms
And with UNDELAYED methods we can deal with
situations not affordable until now
32
Thank You!and wellcome to Catalonia!
33
Conic Rays for Bearing-Only SLAM

• I want to give you some concepts
• That I consider valuable for Bearing-Only SLAM
• specially for landmark initialization
• Im in the EKF-SLAM framework.

34
The problem Landmark Initialization

• The key questions (and answers)
• How can you solve each of these cases?
• Well I just wait until I get the Happy case!
• How do you know in which case you are?
• I define a criteria and then
• And what about the Unhappy case?
• Theres nothing to do
• Is this everything you can say??
• Excuse me?
• Cant you find anything else?

35
Why such a hurryfor a Gaussian?

• Multi-hypothesis

• Test of Gaussiannity

• Wait for baseline

?

• Store several old poses

• Particle Filters

36
Modifying the Conic Ray
I prune unlikely members
Which is a trivial and conservative decision
37
Modifying the Conic Ray
With UNDELAYED methodsI can perform an update
38
Modifying the Conic Ray
I keep on going
39
Modifying the Conic Ray
And one day I will have just one member left.
This member is already Gaussian! If I initialize
it now, I have a DELAYED method.
40
What can I do with the Ray?

• The naïve algorithm Keep it in world frame,
  uncorrelated.
• The improved algorithm Keep it in robot frame,
  and insert this frame into the map.
• The batch update algorithm Keep it in robot
  frame, and insert this and subsequent frames into
  the map.
• The multi-map algorithm Build an hypothetic EKF
  map with each member.
• The FIS algorithm Store the whole ray in one
  map, fully correlated.

DELAYED
UNDELAYED
41
The Batch Update algorithm

• Issues on stored robot poses
• Computational load fixed number of stored
  poses
• Optimality keep only the most important
  poses, considering associated rays and associated
  ray observations
• Issues on visual features and landmarks
• Define more rays from visual features than
  landmarks are needed
• Some features are bad or lost select the best
  ones to initialize
• Experiments
• Images acquired with our ATRV
• Feature matching uses an on-the-shelf algorithm

DELAYED
42
The Federated Information Sharing (FIS) algorithm

• It is a strong approximation of the multi-map
  method
• where all ray members are correlated in a single
  map core
• A map update using the wrong member can lead to
  divergence
• Updates using all members will lead to
  inconsistency
• FIS performs soft updates with all members to
  overcome the risks above

UNDELAYED
singlecore
multicore
43
And my advice

• When EKF is over, dont rush for particles

consider Gaussian sums before!
44
Why such a hurry for aGaussian?
I like thoserays instead
45
(No Transcript)
46
The ideause the Conic Rays!

• What do I want to do?
• Define and model the landmarks PDF at the first
  observation
• Keep modifying its shape with new observations
• Find a criteria-free method to conclude that we
  got the Happy case
• Avoid any calculation to resolve this Happy case
• And manage even the Unhappy case!

47
The ideafrom Rays to Rugby balls

• And How?
• Approximating the ray with a minimal series of
  Gaussian members,
• Updating members likelihoods with measurements,
  
• And pruning unlikely members,
• sooner or later, only one member will be left,
  and Ill be over!