Lecture 22: Graph-SLAM (2)

Transcription

1 Lecture 22: Graph-SLAM (2) Dr. J.B. Hayet CENTRO DE INVESTIGACIÓN EN MATEMÁTICAS Abril 2014 J.B. Hayet Probabilistic robotics Abril / 35

2 Outline 1 Data association in Graph-SLAM 2 Improvements and examples J.B. Hayet Probabilistic robotics Abril / 35

3 Full SLAM In that case the state vector is X 0:t that combines all positions in the trajectory and the positions of the map elements X 0:t = R 0 R 1... R t M y X t = ( Rt M ). The posterior we want to estimate is p(x 0:t Z 1:t U 1:t C 1:t ). J.B. Hayet Probabilistic robotics Abril / 35

4 Graph-SLAM [From Probabilistic Robotics MIT Press] J.B. Hayet Probabilistic robotics Abril / 35

5 Graph-SLAM [From Probabilistic Robotics MIT Press] J.B. Hayet Probabilistic robotics Abril / 35

6 Graph-SLAM [From Probabilistic Robotics MIT Press] J.B. Hayet Probabilistic robotics Abril / 35

7 Graph-SLAM: general algorithm 1 Inicializar µ. 2 Repeat until convergence: (ξ Ω) Linearize() ( ξ Ω) Reduce() (µ Σ 0:t ) Resolve() 3 Returns µ y Σ R0:t R 0:t. J.B. Hayet Probabilistic robotics Abril / 35

8 Outline Data association in Graph-SLAM 1 Data association in Graph-SLAM 2 Improvements and examples J.B. Hayet Probabilistic robotics Abril / 35

9 Data association in Graph-SLAM Data association In EKF-SLAM for data association: check for all i (observations) which map element j among all of the map best corresponds (or if you need to create a new one); In Graph-SLAM the idea is to start with unique correspondences (i.e. initially as many landmarks as observations) and to merge the ones that correspond to identical physical elements two by two. Hence at the core of this strategy is to model the distribution of m j m k for any (j k) to test the hypothesis that the 2 landmarks are the same. J.B. Hayet Probabilistic robotics Abril / 35

10 Data association in Graph-SLAM Data association It is a batch problem hence one can take advantage of some specificities: one can consider the observations in any order and even any sub-set of the observations at each step; the computation of correspondences and the one of the map are mixed; to choose among two associations and merge them will lead to different maps different posterior correspondences... a priori associations could be un-done. J.B. Hayet Probabilistic robotics Abril / 35

11 Data association in Graph-SLAM Data association For any pair of characteristics j and k in the map we can estimate the joint distribution of m j and m k : p(m j m k Z 1:t U 1:t C 1:t ) = R t... R 1 p(m j m k R 1:t Z 1:t U 1:t C 1:t )p(r 1:t Z 1:t U 1:t C 1:t )dr 1...dR t. As we saw it this distribution is Gaussian as a marginal of the joint distribution (ξ Ω). J.B. Hayet Probabilistic robotics Abril / 35

12 Data association in Graph-SLAM Data association Let τ(j) and τ(k) be the set of poses at which landmark j or k are seen τ(j k) = τ(j) τ(k) Now let us remind that the output of the algorithm is the mean µ over trajectories and map elements. J.B. Hayet Probabilistic robotics Abril / 35

13 Data association in Graph-SLAM Data association With a Gaussian of joint state vector (x y) represented in the form of information matrix/vector ( ) Ωxx Ω Ω = xy y ξ = Ω yx Ω yy the marginal over x can be written through: and ( ξx Ω xx = Ω xx Ω xy Ω 1 yy Ω yx ξ x = ξ x Ω xy Ω 1 yy ξ y. ξ y ) J.B. Hayet Probabilistic robotics Abril / 35

14 Data association in Graph-SLAM Data association Let ( ξ mj m k Ω mj m k ) be the parameters of the marginal over the landmarks positions j and k we can deduce from the marginalization formula: Ω mj m k = Ω mj m k m j m k Ω mj m k R τ (jk)σ Rτ (jk)r τ (jk)ω Rτ (jk)m j m k where Σ Rτ (jk)r τ (jk) is the sub-matrix of the covariance matrix over trajectories Σ R0:t R 0:t (output of the algorithm). J.B. Hayet Probabilistic robotics Abril / 35

15 Data association in Graph-SLAM Data association Ω Rτ(jk) R τ(jk) Ω mj m k R τ(jk) R τ(jk)τ(jk) Ω mj m k j k J.B. Hayet Probabilistic robotics Abril / 35

16 Data association in Graph-SLAM Data association Finally the joint distribution over the positions of the landmarks j and k has the following parameters: and Ω mj m k = Ω mj m k m j m k Ω mj m k R τ (jk)σ Rτ (jk)r τ (jk)ω Rτ (jk)m j m k ξ mj m k = Ωmj m k m j m k µ mj mk = Ω mj m k m j m k Ω 1 m j m k m j m k (ξ mj m k + Ω mj m k R τ(jk) µ τ(jk) ) J.B. Hayet Probabilistic robotics Abril / 35

17 Data association in Graph-SLAM Data association We deduce the expression of the joint distribution : p(m j m k Z 1:t U 1:t C 1:t ) e 1 2 m j m k T Ω mj m k m j m k m + j m k T ξ mj m k. Now we can examine the distribution of jk = m j m k that we can link easily to the joint one: ( ) ( ) ( ) ( ) mj m k 1 1 mj mj = = J. m j + m k 1 1 m k m k J.B. Hayet Probabilistic robotics Abril / 35

18 Data association in Graph-SLAM Data association ( ) mj m From the formula k has also a Gaussian distribution m j + m k ( ) ( ) µmj µ mean: mk µmj = J µ mj + µ mk µ mk covariance JΣ mj m k J T In particular the marginal of m j m k has covariance: ( 1 1 ) T Σ mj mk ( 1 1 ). J.B. Hayet Probabilistic robotics Abril / 35

19 Data association in Graph-SLAM Data association Its inverse (check it!): Ω jk = 1 4 ( 1 1 ) T Σ 1 m j m k ( and in terms of what we have already computed: Ω jk = 1 4 ( 1 1 ) T Ωmj mk 1 1 ( 1 1 ). ). J.B. Hayet Probabilistic robotics Abril / 35

20 Data association in Graph-SLAM Data association In the classic form of moments : p( jk Z 1:t U 1:t C 1:t ) = 1 det(2πω 1 jk ) e 1 2 ( µ ) T Ω jk ( µ ) and the test to be done is: which can be written: p( jk = 0 Z 1:t U 1:t C 1:t ) 1 e 1 2 µt Ω jk µ. det(2πω 1 jk ) J.B. Hayet Probabilistic robotics Abril / 35

21 Data association in Graph-SLAM Data association: modified algorithm Correspondence test j k given Ω ξ (joint distribution) µ Σ 0:t : Ω mj m k = Ω mj m k m j m k Ω mj m k R τ (jk)σ Rτ (jk)r τ (jk)ω Rτ (jk)m j m k ( ) T ( ) 1 1 Ω jk = 1 4 Ω 1 mj mk 1 µ = µ j µ k 1 π jk = e 1 2 µt Ω jk µ det(2πω 1 ) jk Si π jk > τ returns true otherwise returns false. J.B. Hayet Probabilistic robotics Abril / 35

22 Data association in Graph-SLAM Data association: modified algorithm Initialize correspondences c 1:t in a unique way 1 Inicializar µ. 2 (ξ Ω) Linearize() 3 ( ξ Ω) Reduce() 4 (µ Σ 0:t ) Resolve() 5 repeat until there is no more mergeable pairs j k: if TestCorrespondence(j k ξ Ω µ Σ 0:t ) re-estimate trajectories and map with the modified correspondences: i/c i t = k c i t = j. J.B. Hayet Probabilistic robotics Abril / 35

23 Data association in Graph-SLAM Data association: modified algorithm Once the correspondences vector is corrected restart everything with the modified c 1:t : 1 (ξ Ω) Linearize() 2 ( ξ Ω) Reduce() 3 (µ Σ 0:t ) Resolve() J.B. Hayet Probabilistic robotics Abril / 35

24 Data association in Graph-SLAM Data association: limits Greedy algorithm because again the problem is combinatorial (explore all possible associations) By starting by unique correspondences we then have to check merges two by two this is not efficient. It does not take into account the fact that at some t one landmark can only be seen through one observation not two i j c i t c j t with the algorithm above we could have cases of associations for the same time t. But it is simple to handle this constraint. J.B. Hayet Probabilistic robotics Abril / 35

25 Data association in Graph-SLAM Data association: limits In the exploration of data associations the order counts: by starting with one fusion we may lose the track of the good associations set. Negative information is not considered (absence of a landmark that should have been seen); We do not use the possibility to remove a correspondence... There are techniques dedicated to these problems. J.B. Hayet Probabilistic robotics Abril / 35

26 Data association in Graph-SLAM Data association: limits This algorithm is essentially a theoretical base; many extensions could be proposed: merge features since the beginning when it is very likely that they correspond to the same element; use sub-maps built with other techniques locally efficient (EKF occupancy grids... ) then use Graph-SLAM between sub-maps. J.B. Hayet Probabilistic robotics Abril / 35

27 Data association in Graph-SLAM Graph-SLAM and other methods Graph-SLAM has a lot in common with the technique of Estrada et al. (cf. lecture 20) to stick sub-maps together except that here one handles information matrices In the same way in the most advanced scan-matching techniques use generally graph-based representations to manage the loop closure problem... J.B. Hayet Probabilistic robotics Abril / 35

28 Outline Improvements and examples 1 Data association in Graph-SLAM 2 Improvements and examples J.B. Hayet Probabilistic robotics Abril / 35

29 Improvements and examples Example: mine exploration Example from the book: mapping of abandoned mines... Network of corridors hundreds of meters long... Algorithm: 1 construction of an occupancy grid with scan matching (see previous lecture) 2 division of the map in sub-maps of 5m 3 construction of a graph where each node corresponds to a sub-map (pose node in Graph-SLAM) 4 association test for identical data (map correlation) and re-estimation of the map. J.B. Hayet Probabilistic robotics Abril / 35

30 Improvements and examples Example: mine exploration [From Probabilistic Robotics MIT Press] J.B. Hayet Probabilistic robotics Abril / 35

31 Improvements and examples Example: mine exploration [From Probabilistic Robotics MIT Press] J.B. Hayet Probabilistic robotics Abril / 35

32 Improvements and examples Example: mine exploration [From Probabilistic Robotics MIT Press] J.B. Hayet Probabilistic robotics Abril / 35

33 Improvements and examples Example: mine exploration [From Probabilistic Robotics MIT Press] J.B. Hayet Probabilistic robotics Abril / 35

34 Improvements and examples Example: mine exploration [From Probabilistic Robotics MIT Press] J.B. Hayet Probabilistic robotics Abril / 35

35 Improvements and examples Example: mine exploration [From Probabilistic Robotics MIT Press] J.B. Hayet Probabilistic robotics Abril / 35

36 Improvements and examples Optimization-based variants An improvement point resides in the optimization; what we could find is the map/trajectory maximizing the posterior i.e. minimizing: l 0:t = cste. + R0 T Ω 0 R 0 + t 1 (R 2 τ g(r τ 1 U τ )) T Σ 1 R (R τ g(r τ 1 U τ ))+ τ=1 t o τ (Zτ i h(x τ Cτ)) i T Σ 1 M (Z τ i h(x τ Cτ)) i 1 2 τ=1 i=1. The basic algorithm linearizes g and h... but it has the advantage of estimating the covariance on trajectories. J.B. Hayet Probabilistic robotics Abril / 35

37 Improvements and examples Optimization-based variants Instead of sequences linearizations/reductions/optimizations this problem can be handled by classical optimization methods to determine the MAP (maximum a posteriori) : gradient descent; Levenberg-Marquardt; conjugated gradient... Most of the literature is based on this type of approach. J.B. Hayet Probabilistic robotics Abril / 35

38 Improvements and examples Optimization-based variants They allow to handle much larger dimensions (examples up to 10 8 features... ); They are faster in general than the original algorithm; They are sensible to local minima... They just output the mode (MAP). J.B. Hayet Probabilistic robotics Abril / 35

39 Improvements and examples Optimization-based variants [From Probabilistic Robotics MIT Press] J.B. Hayet Probabilistic robotics Abril / 35

40 Improvements and examples Optimization-based variants [From Probabilistic Robotics MIT Press] J.B. Hayet Probabilistic robotics Abril / 35