Undelayed Initialization in Bearing-Only SLAM

1. Undelayed Initializationin Bearing-OnlySLAM Joan Solà, André Monin, Michel Devy and Thomas Lemaire LAAS-CNRS Toulouse, France

2. EKF-SLAM is our choice This is about… • Bearing-Only SLAM (or Single-Camera SLAM) • Landmark Initialization • Efficiency: • Gaussian PDFs • Dealing with difficult situations:

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. 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 ??? • 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 Idea DELAYEDINITIALIZATION ? <Davison> <Bailey> [Lemaire] do it the easy way Last memberis easilyincorporated 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 Ray’s 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 member’s 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 UNDELAYEDmethodsI 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. DELAYEDandUNDELAYEDmethods Unhappy UNDELAYED Not so Happy DELAYED UNDELAYED Happy DELAYED UNDELAYED

22. The multi-map algorithm • The Federated Information Sharing algorithm UNDELAYED DELAYEDandUNDELAYEDmethods • 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 inthe 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. {y,R1 } {y,R2} … {y,RN } Information Sharing : Likelihood Privilege : Federated Coefficient i : 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 B&W image / 7 cm • 512 x 378 pix, 90º HFOV • 1 pix noise UNDELAYED

28. The FIS algorithmand the Unhappy case Top view Side view UNDELAYED

29. In conclusion • The Geometric Ray is a very powerful representationfor 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!