Undelayed Initialization in BearingOnly SLAM

1
Undelayed Initializationin Bearing-Only SLAM

• Joan Solà, André Monin, Michel Devy 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
• An efficient undelayed real-time solution
• The Federated Information Sharing (FIS) algorithm

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 uncertainties via the Jacobians of
  the observation functions

Remember previous pose!
tbefore
tnow
8
The problem Landmark Initialization
3?
2?

• The Not so Happy case

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

9
The problem Landmark Initialization

• The Unhappy case

???

• Theres simply nothing to compute!
• And theres nothing to wait for.
• But it is the case for landmarks that lie close
  to the motion direction

10
The KEY Idea
DELAYEDINITIALIZATION
?
ltDavisongt ltBaileygt Lemaire
do it the easy way
Last memberis easily incorporated
Initialapproximation is easy
UNDELAYEDinitialization
Member selection is easy and safe
Kwok
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
Peach
? 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 handled
  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
With UNDELAYED methodsI can perform a map update
19
How it works
I keep on going
20
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.
21
DELAYED and UNDELAYED methods
Unhappy
UNDELAYED
Not so Happy
DELAYED
UNDELAYED
Happy
DELAYED
UNDELAYED
22
DELAYED and UNDELAYED methods

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

DELAYED
23
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
24
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 Information Sharing update
• When only one member is left
• Nothing to do

UNDELAYED
25
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
26
The FIS algorithmand the Unhappy case
UNDELAYED
27
The FIS algorithmand the Unhappy case

• 1 BW image / 7 cm
• 512 x 378 pix, 90º HFOV
• 1 pix noise

UNDELAYED
28
The FIS algorithmand the Unhappy case
Side view
Top view
UNDELAYED
29
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

30
Thank You!
31
The FIS algorithm
UNDELAYED
32
The FIS algorithmand the Unhappy case
UNDELAYED
gt ATRV robot gt 1 image / 7 cm _at_ 512 x 378 pix BW