1. Undelayed Initialization in Bearing-Only SLAM Joan Solà, André Monin, Michel Devy and Thomas Lemaire LAAS-CNRS Toulouse, France

2. 2 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. 3 What’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. 4 The problem: Landmark Initialization The naïve way ? TeTe t now t before t now ?

5. 5 The problem: Landmark Initialization Consider uncertainties t now t before t now TeTe The 3D point is inside ?

6. 6 The problem: Landmark Initialization The Happy and Unhappy cases Happy Not so Happy Unhappy

7. 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 t before t now Remember previous pose!

8. 8 The problem: Landmark Initialization The Not so Happy case 00 11 22 33 00 11 22 3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. 9 The problem: Landmark Initialization 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. 10 The KEY Idea ? ? Member selection is easy and safe Last member is easily incorporated UNDELAYED initialization DELAYED INITIALIZATION Initial approximation is easy [Lemaire] [Kwok]

11. 11 Defining the Geometric Ray Define a geometric series of Gaussians x R : camera position 44 r4r4 33 r3r3  =  i / r i  = r i / r i-1 [ r min r max ] Fill the space between r min and r max 1.With the minimum number of terms 2.Keeping linearization constraints [Peach]

12. 12 From aspect ratio, geometric base and range bounds: The number of terms is logarithmic on r max / r min : This leads to very small numbers: As members are Gaussian, they are easily handled with EKF. The Geometric Ray’s benefits Scenario r min r max Ratio NgNg Indoor0.5510 3 Outdoor1100 5 Long Range11000 7  [r min, r max ] N g = f (  log( r max / r min ) 1 2

13. 13 How it works The first observation determines the Conic Ray

14. 14 I model the Conic Ray with the geometric series I can initialize all members now, and I have an UNDELAYED method. 3 How it works

15. 15 I move and make a second observation Members are distinguishable How it works

16. 16 I compute likelihoods and update member’s credibilities Which means modifying its shape How it works

17. 17 I prune unlikely members Which is a trivial and conservative decision How it works

18. 18 With UNDELAYED methods I can perform a map update How it works

19. 19 I keep on going… How it works

20. 20 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. How it works 3

21. 21 DELAYED and UNDELAYED methods Happy Not so Happy Unhappy DELAYED UNDELAYED

22. 22 DELAYED and UNDELAYED methods A naïve algorithm A consistent algorithm The Batch Update algorithm DELAYED UNDELAYED The multi-map algorithm The Federated Information Sharing algorithm

23. 23 The multi-map algorithm 1.Initialize all Ray members as landmarks in different maps 2.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 UNDELAYED

24. 24 The Federated Information Sharing (FIS) algorithm 1.Initialize Ray members as different landmarks in the same map 2.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 UNDELAYED

25. 25 The 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, R 1 } { y, R 2 } { y, R N } … … Information Sharing : Federated Coefficient  i : Likelihood Privilege : UNDELAYED

26. 26 The FIS algorithm and the Unhappy case UNDELAYED

27. 27 The FIS algorithm and the Unhappy case UNDELAYED  1 B&W image / 7 cm  512 x 378 pix, 90º HFOV  1 pix noise

28. 28 The FIS algorithm and the Unhappy case Side viewTop view UNDELAYED

29. 29 The Geometric Ray is a very powerful representation for Bearing-Only SLAM We can use it in both DELAYED and UNDELAYED methods In conclusion UNDELAYED methods allow us to initialize landmarks in the direction of motion Federated Information Sharing permits a Real Time implementation

30. Thank You!

31. 31 UNDELAYED The FIS algorithm

32. 32 The FIS algorithm and the Unhappy case UNDELAYED > ATRV robot > 1 image / 7 cm @ 512 x 378 pix B&W