Lecture 22: Graph-SLAM (2)aplicaciones.cimat.mx/.../files/clase22-eng.pdf · Lecture 22: Graph-SLAM (2) Dr. J.B. Hayet CENTRO DE INVESTIGACION EN MATEM ATICAS Abril 2014, J.B. Hayet

Lecture 22: Graph-SLAM (2)

Dr. J.B. Hayet

CENTRO DE INVESTIGACION EN MATEMATICAS

Abril 2014

,J.B. Hayet Probabilistic robotics, Abril 2014 1 / 35

Outline

1 Data association in Graph-SLAM

2 Improvements and examples


Full SLAM

In that case the state vector is X0:t , that combines all positionsin the trajectory and the positions of the map elements,

X0:t =

R0

R1

. . .Rt

M

y Xt =

(Rt

M

).

The posterior we want to estimate is p(X0:t |Z1:t ,U1:t ,C1:t).


Graph-SLAM

[From Probabilistic Robotics, MIT Press]


Graph-SLAM



Graph-SLAM



Graph-SLAM: general algorithm

1 Inicializar µ.2 Repeat until convergence:

(ξ,Ω) ← Linearize()(ξ, Ω) ← Reduce()(µ,Σ0:t) ← Resolve()

3 Returns µ y ΣR0:tR0:t .


Data association in Graph-SLAM

Outline





Data association

In EKF-SLAM, for data association: check for all i(observations), which map element j among all of the mapbest corresponds (or if you need to create a new one);

In Graph-SLAM, the idea is to start with uniquecorrespondences (i.e. initially as many landmarks asobservations) and to merge the ones that correspond toidentical physical elements, two by two. Hence, at thecore of this strategy is to model the distribution of mj −mk , forany (j , k), to test the hypothesis that the 2 landmarks are thesame.



Data association

It is a batch problem, hence one can take advantage of somespecificities:

one can consider the observations in any order, and even anysub-set of the observations at each step;

the computation of correspondences and the one of the map aremixed; to choose among two associations and merge them willlead to different maps, different posterior correspondences. . .

a priori associations could be un-done.



Data association

For any pair of characteristics j and k in the map, we can estimatethe joint distribution of mj and mk :

p(mj ,mk |Z1:t ,U1:t ,C1:t) =∫Rt...∫R1p(mj ,mk |R1:t ,Z1:t ,U1:t ,C1:t)p(R1:t |Z1:t ,U1:t ,C1:t)dR1...dRt .

As we saw it, this distribution is Gaussian, as a marginal of the jointdistribution (ξ,Ω).



Data association

Let τ(j) and τ(k) be the set of poses at which landmark j or k areseen

τ(j , k) = τ(j) ∪ τ(k),

Now let us remind that the output of the algorithm is the mean µover trajectories and map elements.



Data association

With a Gaussian of joint state vector (x , y), represented in theform of information matrix/vector,

Ω =

(Ωxx Ωxy

Ωyx Ωyy

)y ξ =

(ξxξy

),

the marginal over x can be written through:

Ωxx = Ωxx − ΩxyΩ−1yy Ωyx ,

and

ξx = ξx − ΩxyΩ−1yy ξy .



Data association

Let (ξmjmk, Ωmjmk

) be the parameters of the marginal over thelandmarks positions j and k , we can deduce from themarginalization formula:

Ωmjmk= Ωmjmk ,mjmk

− Ωmjmk ,Rτ (j ,k)ΣRτ (j ,k)Rτ (j ,k)ΩRτ (j ,k),mjmk,

where ΣRτ (j ,k)Rτ (j ,k) is the sub-matrix of the covariance matrix overtrajectories ΣR0:tR0:t (output of the algorithm).



Data association

Rτ(j,k)τ(j,k)

ΩRτ(j,k)Rτ(j,k)

j

k

Ωmjmk

Ωmjmk,Rτ(j,k)



Data association

Finally, the joint distribution over the positions of the landmarks jand k has the following parameters:



and

ξmjmk= Ωmjmk ,mjmk

µmjmk

= Ωmjmk ,mjmkΩ−1

mjmk ,mjmk(ξmjmk

+ ΩmjmkRτ(j,k)µτ(j ,k))



Data association

We deduce the expression of the joint distribution :

p(mj ,mk |Z1:t ,U1:t ,C1:t) ∝ e− 1

2

mj

mk

T

Ωmjmk

mj

mk

+

mj

mk

T

ξmjmk

.

Now, we can examine the distribution of ∆j ,k = mj −mk , that wecan link easily to the joint one:(

mj −mk

mj + mk

)=

(1 −11 1

)(mj

mk

)= J

(mj

mk

).



Data association

From the formula,

(mj −mk

mj + mk

)has also a Gaussian distribution,

mean:

(µmj− µmk

µmj+ µmk

)= J

(µmj

µmk

)covariance JΣmjmk

JT

In particular, the marginal of mj −mk has covariance:(1−1

)T

Σmjmk

(1−1

).



Data association

Its inverse (check it!):

Ω∆jk=

1

4

(1−1

)T

Σ−1mjmk

(1−1

).

and in terms of what we have already computed:

Ω∆jk=

1

4

(1−1

)T

Ωmjmk

(1−1

).



Data association

In the classic form of moments :

p(∆jk |Z1:t ,U1:t ,C1:t) =1√

det(2πΩ−1∆jk

)e−

12

(∆−µ∆)T Ω∆jk(∆−µ∆)

and the test to be done is:

p(∆jk = 0|Z1:t ,U1:t ,C1:t),

which can be written:

1√det(2πΩ−1

∆jk)e−

12µT∆Ω∆jk

µ∆ .



Data association: modified algorithm

Correspondence test j , k , given Ω, ξ (joint distribution), µ,Σ0:t :



Ω∆jk= 1

4

(1−1

)T

Ωmjmk

(1−1

),

µ∆ = µj − µk ,

πjk = 1√det(2πΩ−1

∆jk)e−

12µT∆Ω∆jk

µ∆ ,

Si πjk > τ , returns true, otherwise returns false.




Initialize correspondences c1:t in a unique way1 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-estimatetrajectories and map with the modified correspondences:∀i/c it = k , c it = j .




Once the correspondences vector is corrected, restart everythingwith the modified c1:t :

1 (ξ,Ω) ← Linearize()

2 (ξ, Ω) ← Reduce()

3 (µ,Σ0:t) ← Resolve()



Data association: limits

Greedy algorithm, because again the problem is combinatorial(explore all possible associations)

By starting by unique correspondences, we then have to checkmerges two by two, this is not efficient.

It does not take into account the fact that at some t, onelandmark can only be seen through one observation,not two,

i 6= j ⇒ c it 6= c jt ,

with the algorithm above, we could have cases of associationsfor the same time t. But it is simple to handle this constraint.




In the exploration of data associations, the order counts: bystarting with one fusion, we may lose the track of the goodassociations set.

Negative information is not considered (absence of a landmarkthat should have been seen);

We do not use the possibility to remove a correspondence. . .

There are techniques dedicated to these problems.




This algorithm is essentially a theoretical base; many extensionscould be proposed:

merge features since the beginning, when it is very likely thatthey correspond to the same element;

use sub-maps built with other techniques locally efficient(EKF, occupancy grids. . . ) then use Graph-SLAM betweensub-maps.



Graph-SLAM and other methods

Graph-SLAM has a lot in common with the technique ofEstrada 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-matchingtechniques, use generally graph-based representations tomanage the loop closure problem. . .


Improvements and examples

Outline





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.



























Optimization-based variants

An improvement point resides in the optimization; what we couldfind is the map/trajectory maximizing the posterior, i.e.minimizing:

l0:t = cste. + RT0 Ω0R0+

12

t∑τ=1

(Rτ − g(Rτ−1|Uτ ))TΣ−1R (Rτ − g(Rτ−1|Uτ ))+

12

t∑τ=1

oτ∑i=1

(Z iτ − h(Xτ ,C

iτ ))TΣ−1

M (Z iτ − h(Xτ ,C

iτ ))

.

The basic algorithm linearizes g and h. . . but it has the advantage ofestimating the covariance on trajectories.




Instead of sequences linearizations/reductions/optimizations, thisproblem can be handled by classical optimization methods todetermine the MAP (maximum a posteriori) :

gradient descent;

Levenberg-Marquardt;

conjugated gradient. . .

Most of the literature is based on this type of approach.




They allow to handle much larger dimensions (examples upto 108 features. . . );

They are faster in general than the original algorithm;

They are sensible to local minima. . .

They just output the mode (MAP).










Documents

Lecture 22: Graph-SLAM (2)aplicaciones.cimat.mx/.../files/clase22-eng.pdf · Lecture 22: Graph-SLAM (2) Dr. J.B. Hayet CENTRO DE INVESTIGACION EN MATEM ATICAS Abril 2014, J.B. Hayet