Undelayed Initialization in Bearing-Only SLAM

Undelayed InitializationUndelayed Initializationin in

Bearing-Only SLAMBearing-Only SLAM

Undelayed InitializationUndelayed Initializationin in

Bearing-Only SLAMBearing-Only SLAM

Joan Solà, André Monin, Michel Devy and Thomas Lemaire

LAAS-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

» An efficient undelayed real-time solution:

• The Federated Information Sharing (FIS) algorithm

4

The problem: Landmark Initialization


• The naïve way

?

Te

tnow

tbefore tnow

?

5



• Consider uncertainties

tnow

tbefore tnow

Te

The 3D pointThe 3D pointis insideis inside

?

6



• The Happy and Unhappy cases

Happy

Not so Happy

Unhappy

7



• 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 functionstbefore tnow

Remember previous pose!

8



• 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

Gaussianity TEST needed!

9



• 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

???

10

The KEY IdeaThe KEY Idea

QuickTime™ and aTIFF (LZW) decompressor

are needed to see this picture.






are needed to see this picture.??

Member selection is easy and safe

Last memberis easily incorporated

UNDELAYEDinitialization

DELAYEDINITIALIZATION

Initialapproximation

is easy

<Davison><Bailey>

[Lemaire]

[Kwok]

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

[Peach]

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 handled 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


15

I move and make a secondobservation

Members are distinguishable


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 ⋅λ


17

I prune unlikely members

Which is a trivial and conservative decision

€

C <0.001

number _ of _ members


18

With UNDELAYED methodsI can perform a map update


19

I keep on going…


20

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

21

DELAYED and UNDELAYED methods


Happy

Not so Happy

Unhappy

DELAYED

DELAYED

UNDELAYED

UNDELAYED

UNDELAYED

22



• A naïve algorithm

• A consistent algorithm

• The Batch Update algorithmDELA

YED

UN

DELA

YED

• The multi-map algorithm

• The Federated Information Sharing algorithm

23



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



OFF-LINE METHOD

UN

DELA

YED

24



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 Information Sharing update

3. When only one member is left:• Nothing to do



UN

DELA

YED

25

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

{y, R1 }{y, R2 }

{y, RN }

… …

Information Sharing :

Federated Coefficient i :

Likelihood Privilege :

UN

DELA

YED

26

The FIS algorithmand the Unhappy case


QuickTime™ et undécompresseur Cinepak

sont requis pour visionner cette image.

UNDELAYED

27



UNDELAYED 1 B&W image / 7 cm

512 x 378 pix, 90º HFOV

1 pix noise

28



Side viewTop view

UNDELAYED

29

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

•We can use it in both DELAYED and UNDELAYED methods

In conclusionIn conclusion

•UNDELAYED methods allow us to initialize landmarks in the direction of motion

•Federated Information Sharing permits a Real Time implementation

Thank You!Thank You!Thank You!Thank You!

Documents

Undelayed Initialization in Bearing-Only SLAM