geometric rays for bearing-only slam joan solà and thomas lemaire laas-cnrs toulouse, france

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!