Summer School on SLAM August 2002, Stockholm

Data Association in SLAM Juan D. Tardós University of Zaragoza, Spain [email protected] http://www.cps.unizar.es/~jdtardos/

University of Zaragoza - J.D. Tardós, J. Neira

1

Zaragoza, Aragón

University of Zaragoza - J.D. Tardós, J. Neira

2

UZ – Robotics, Vision and Real Time Group 10 Faculty members ~10 PhD and Master students Vision Laser

Sonar

Wheel Encoders Triton Otilio University of Zaragoza - J.D. Tardós, J. Neira

3

UZ: SLAM

University of Zaragoza - J.D. Tardós, J. Neira

4

UZ: Computer Vision and 3D Model Building

University of Zaragoza - J.D. Tardós, J. Neira

5

UZ: Robot Navigation

University of Zaragoza - J.D. Tardós, J. Neira

6

Data Association in SLAM 1. Introduction 2. Feature extraction: Laser, Sonar,… 3. Data association in continuous SLAM • • • •

Nearest Neighbor .vs. Joint Compatibility SLAM with Laser and Sonar Map joining The loop closing problem

4. Robot relocation and map matching • • •

Geometric Constraints A linear time algorithm Application to multi-robot mapping

5. Conclusion University of Zaragoza - J.D. Tardós, J. Neira

7

The SLAM problem 14 12 10 8 6 4 2 0 -2 -4 -4

University of Zaragoza - J.D. Tardós, J. Neira

-2

0

2

4

6

8

10

12

14

8

Data Association • Given an environment map • And a set of sensor observations • Associate observations with map elements

University of Zaragoza - J.D. Tardós, J. Neira

9

Importance of Data Association • Measurement y is used to improve estimation of x: xˆ k = xˆ k −1 + K k (− hk −1 )

EKF update:

Pk = Pk −1 − K k Hk −1Pk −1

(

T

)

T −1

K k = Pk −1 HTk −1 Hij P H ij + Gij S G ij

• If the association of Ei with feature Fj is….. correct: error: x − xˆ covariance: P

Consistency University of Zaragoza - J.D. Tardós, J. Neira

spurious: x − xˆ P

Divergence! 10

Difficulties: clutter • Influence of the type, density, precision and robustness of features considered: 10000

Laser scanner:

8000

• Small amount of features (n) • Small amount of measurements (m) • Low spuriousness

6000 4000 2000 0 -2000 -6000 -4000 -2000

0

2000 4000 6000

Low clutter University of Zaragoza - J.D. Tardós, J. Neira

11

Difficulties: clutter

Vertical Edge Monocular vision:

10000 8000 6000 4000 2000 0

• • • •

Many features (n large) Many measurements (m large) no depth information higher spuriousness

-2000 -6000 -4000 -2000

University of Zaragoza - J.D. Tardós, J. Neira

0

2000 4000 6000

High clutter 12

Difficulties: imprecision • Both the sensor and the vehicle introduce imprecision 10000

10000

8000

8000

6000

6000

4000

4000

2000

2000

0

0

-2000 -6000 -4000 -2000

0

2000 4000 6000

-2000 -6000 -4000 -2000

Vertical Edge Trinocular vision: variable depth precision good angular precision

0

2000 4000 6000

Robot imprecision: introduces CORRELATED error

University of Zaragoza - J.D. Tardós, J. Neira

13

Search in configuration space 100

….

50

0

-50

-100 -50

0

50

University of Zaragoza - J.D. Tardós, J. Neira

100

150

200 14

Approaches to Data Association •

Search in configuration space: find robot location with maximal data to map overlapping – Can be done with raw data – Or with features » Speed of convergence?

•

Search in correspondence space: find a consistent correspondence hypothesis and compute robot location 1. Extract features from data » If sparse data, move and build a local map 2. Feature-based map (points, lines, trees, …) 3. Search for data feature to map feature correspondences » Exponential number of solutions?

University of Zaragoza - J.D. Tardós, J. Neira

15

2. Feature extraction: Laser Split and merge: 1. Recursive Split:

2000

1. Obtain the line passing by the two extreme points 2. Obtain the point more distant to the line 3. If distance > error_max, split and repeat with the left and right sub-scan

1000

0

-1000

-2000

-3000

2. Merge: -4000

-14000

-12000

-10000

1. If two consecutive segments are close enough, obtain the common line and the more distant point 2. If distance n : – Randomly select a subset S1 of n data points and compute the model M1 – Determine the consensus set S1* of points is P compatible with M1 (within some error tolerance) – If #(S1*) > t, use S1* to compute (maybe using least squares) a new model M1* – If #(S1*) < t, randomly select another subset S2 and repeat – If, after some predetermined number of trials there is no consensus set with t points, return with failure

University of Zaragoza - J.D. Tardós, J. Neira

20

RANSAC

S&M TLS RANSAC RANSAC+TLS

University of Zaragoza - J.D. Tardós, J. Neira

21

Feature extraction: Sonar • Very sparse and noisy data 10 Laser scan Sonar returns

8

6

4

2

0

−2

−4 −4

−2

0

2

University of Zaragoza - J.D. Tardós, J. Neira

4

6

8

10

22

Sonar data, several steps S onar data

2

Line?

1.5 1

Line?

yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy

0.5

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

0 y

x

-0.5 -1

Corner?

-1.5 -2 -2.5 -2

• •

-1

0

1

2

3

You need to move the robot You need to delay decisions

University of Zaragoza - J.D. Tardós, J. Neira

23

Sonar data, several steps S onar data

2

Wall!

1.5

People!

1 yyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyyy

0.5

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

0 x

-0.5

Corner!

y

-1 -1.5 -2 -2.5 -2

•

-1

0

1

2

3

You need a good sensor model

University of Zaragoza - J.D. Tardós, J. Neira

24

Sonar Model for Lines

ρ Sj

Possible Line Apparent Sonar Return

ρ Bi

θSj k

Hough Voting for i in 1..n_positions for j in 1..n_sensors Compute xBiSj for θSjk in -β/2..β/2 step δ Compute θBjk ρBjk Vote(θBjk, ρBjk) end end end

Sj

β

Bi θBi k

Bi ySj

Bi xSj

University of Zaragoza - J.D. Tardós, J. Neira

25

Hough Transform for Lines Hough Table for Lines 50 150 45 100

40

2

35

50 ) g e d( at e h T

30 0

25 20

-50

15

1 -100

10 5

-150 0

1

2

3

Ro (m) University of Zaragoza - J.D. Tardós, J. Neira

4

5

0

26

Sonar Model for Points Possible Point

ρ Sj

Apparent Sonar Return θSj k

Sj

Bi

ρ

Hough Voting for i in 1..n_positions for j in 1..n_sensors Compute xBiSj for θSjk in -β/2..β/2 step δ Compute θBjk ρBjk Vote(θBjk, ρBjk) end end end

β

Bi θBi k

Bi ySj

Bi xSj

University of Zaragoza - J.D. Tardós, J. Neira

27

Hough Transform for Points Hough Table for Points 50 150 45 100

2

40 35

50 ) g e d( at e h T

30 0

25 20

-50

15 -100

10 1

-150

5 0

0

1

2

3

4

5

Ro (m)

University of Zaragoza - J.D. Tardós, J. Neira

28

Looking for Local Maxima • Winner-takes-all strategy Hough Table for Lines

Hough Table for Points 50

50

150

150 45

100

40

2

35

50 ) g e d( at e h T

45 100

) g e d( at e h T

25 20

-50

35 30

0

25 20

-50

15

1 -100

15 -100

10 5

-150 0

40

50

30 0

2

1

2

3

4

1

-150

0

5

10

0

1

5

2

Ro (m)

3

4

5

0

Ro (m)

University of Zaragoza - J.D. Tardós, J. Neira

29

Perceptual Grouping with Hough 4 3 2 1

2 2 yyyyyyyyyyyyyyyyyyyyyyyyy xxxxxxxxxxxxxxxxxxxxxxxx y x

0 -1

1

1

-2 -3 -4 -5

-4

-3

-2

-1

0

University of Zaragoza - J.D. Tardós, J. Neira

1

2

3

4

5 30

4. Data Association • n map features:

•

Interpretation tree (Grimson et al. 87):

m sensor measurements: ...... ......

......

......

• Goal: obtain a hypothesis that associates each observation Ei with a feature Fji

...... ...... • Non matched observations: possible hypotheses University of Zaragoza - J.D. Tardós, J. Neira

31

Use constraints to prune the tree • Map:

F1 F2 F6

......

F3 F4

X ...... X

F5

......

• Observations: E2 E1

E4

......

E3 E5

......

X X ......

• Constraints: – Feature location (needs an estimation of robot location) – Geometric relations: angles, distances,… (location independent) University of Zaragoza - J.D. Tardós, J. Neira

32

Individual Compatibility • Measurement equation for observation Ei and feature Fj

• Ei and Fj are compatible if:

d = length(zi)

• Nearest Neighbour (NN) rule: – associate Ei with the feature Fj having smaller Mahalanobis distance Dij

University of Zaragoza - J.D. Tardós, J. Neira

33

Nearest Neighbor

University of Zaragoza - J.D. Tardós, J. Neira

Greedy algorithm: O(mn) ...... ......

...... ......

NN: for i = 1 to m -- feature Ei D2min = Mahalanobis2(Ei, F1) nearest = 1 for j = 2 to n Dij2 = Mahalanobis2(Ei, Fj) if Dij2 < D2min then nearest = j D2min = Dij2 fi rof if D2min m -- leaf node? if pairings(H) > pairings(Best) -- did better? Best = H fi else for j = 1 to n if individual_compatibility(Ei, Fj) and then joint_compatibility(H, Ei, Fj) JCBB([H j], i + 1) –- pairing (Ei, Fj) accepted fi rof if pairings(H) + m – i >= pairings(Best) –- can do better? JCBB([H 0], i + 1) -- star node: Ei not paired fi fi

University of Zaragoza - J.D. Tardós, J. Neira

38

SLAM with laser segments 14 12 10 8 6 4 2 0 -2 -4 -4

-2

0

2

4

6

8

University of Zaragoza - J.D. Tardós, J. Neira

10

12

14

39

SLAM with laser segments t=1061.899s 12

10

8

6

4

2

y x

0 x

y

−2 −2

0

2

4

University of Zaragoza - J.D. Tardós, J. Neira

6

8

10

12 40

SLAM with Sonar

University of Zaragoza - J.D. Tardós, J. Neira

41

Perceptual Grouping with Hough: Lines and Points

University of Zaragoza - J.D. Tardós, J. Neira

42

Map Joining: local maps • Environment information related to a set of elements:

F0 = Robot

• represented by a map: Fn

xBFn x B F2

B

xFB1

F1

xFB0

F2

F0

University of Zaragoza - J.D. Tardós, J. Neira

43

Local map building • Periodically, the robot starts a new map, relative to its current location:

• EKF approximates the conditional mean:

3

t= 33.700s 2.5 2 1.5 1

P2

y

x

S4

• Given measurements:

0.5

x

y

y

B1 0

R1

y

x

x -0.5 y -1

P1

S3 x

-1.5 -2 -2

University of Zaragoza - J.D. Tardós, J. Neira

y

x

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

3

3.5

44

Local map building • Second map:

t= 48.492s

3 2.5

• No information is shared:

2 1.5

S2

1 x

Maps are uncorrelated

0.5

y

y

B2 0

x

x y

-0.5

• Common reference:

R2

y

S1 x

-1 -1.5 -2 -2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

University of Zaragoza - J.D. Tardós, J. Neira

3

3.5

45

Map Joining • Given: 1. Two statistically independent stochastic maps 2. A common reference

xAA1

A1

xAAi

Bj

Ai B B xBj

A

xAA0

A0

xAA2

A2

University of Zaragoza - J.D. Tardós, J. Neira

xBB1 B xBB0 1

xBB2

B2

B0

46

Map Joining • Conveys the information of the two maps into a single fully consistent stochastic map:

Bj

A1

A

xBA2 xBA

B2 B

A0

xBA1

A B21

xBA0

B0

University of Zaragoza - J.D. Tardós, J. Neira

47

Change the base of map B to Bj • New state vector:

• New covariance matrix:

University of Zaragoza - J.D. Tardós, J. Neira

48

Map Joining • New state vector: • New covariance matrix:

University of Zaragoza - J.D. Tardós, J. Neira

49

Local map sequencing 3

t= 33.700s

2.5 2

2

1.5

1.5

1

y P2 x S4 x y y B1 y R1 x

0.5 0

x

P1

-1.5

x -1.5

y -0.5

0

0.5

1

1.5

2

2.5

3

3.5

t= 48.492s

-2 -2

S1 x

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

3

3.5

t= 48.492s

3 2.5

2

2

1.5

1.5

1

y y

0.5

P2 x x

1

S4

B1

0

x

y

x

S6 y

P1

-1.5

-1

-0.5

0

0.5

1

2

S4 y R2

y

x

x

y -1

S5 1.5

x

-0.5 x

x

S3

y

x

0

x

y

y

P1

y

P2x B1

y

0.5 y R2

-0.5

-2 -2

y

-1.5

2.5

-1.5

x

x

-1

x

3

-1

y R2

B2 y

-0.5

S3

-1

x y

0

y -1

S2

1 0.5

-0.5

-2 -2

t= 48.492s

3 2.5

2.5

-1.5 3

3.5

University of Zaragoza - J.D. Tardós, J. Neira

-2 -2

P1

S3

-1.5

x

y

x -1

-0.5

0

0.5

1

1.5

2

2.5

3

3.5

50

Loop closing: Joint Compatibility!

2

1

x

y

x 0

y

-1

-2 0

1

2

3

4

6

5

University of Zaragoza - J.D. Tardós, J. Neira

t=1061.650s

51

Sonar .vs. Laser

12

12

10

10

8

8

6

6

4

4

2

2

y

y x

0

x

0 x

x

y

y −2 −2

0

t=1061.899s

2

4

6

8

10

12

−2 −2

0

2

4

6

8

12

10

•Very robust map building with sonar •Local maps: 60x faster than standard SLAM University of Zaragoza - J.D. Tardós, J. Neira

52

4. Robot Relocation Previously built map (A)

Local map (B)

30 8 25

6

x

4

20

y

2 0

15

-2 10

-10

-5

0

5

0

Where is the robot? -10

-5

0

5

10

15

20

University of Zaragoza - J.D. Tardós, J. Neira

53

No robot estimation: use geometric constraints • Location independent constraints – Unary: depend on a single matching (size, color,…) » Tree diameter » Wall length: the observed length should be less than the length in the map (if map is complete) – Binary: depend on two matchings (angles, distances) » Distances between points » Angles between lines » “Distances” between segments – N-ary: more complex, little pruning

University of Zaragoza - J.D. Tardós, J. Neira

54

Binary Constraints for Points • Distance between two map points and the two matching observed points should coincide:

– If using a probabilistic model, 95% error bound:

– Or use an empiric value

University of Zaragoza - J.D. Tardós, J. Neira

55

Binary Constraints for Segments • Angle constraint: • Distance constraint (verifies overlapping) – Let [d, D] be the interval of possible distances between any two points of two segments: 1

2 d23

d13 3

d24

d14

1

2

X d1

d3

d4

d2

3 4

D = max(d13, d23, d14, d24) University of Zaragoza - J.D. Tardós, J. Neira

4

d = min(d13, d23, d14, d24, d1, d2, d3, d4) 56

.

Binary Constraints for Segments • Component constraint: – Let [c, C] be the interval of possible longitudinal or lateral component any point of one segment relative to the center of the other segment:

x cy

x cX

y Cy

CX y

University of Zaragoza - J.D. Tardós, J. Neira

57

Location independent constraints are not a sufficient condition

• You need to verify global consistency

University of Zaragoza - J.D. Tardós, J. Neira

58

Geometric Constraints Branch and Bound GCBB(H, i): -- find pairings for feature Ei if i > m -- leaf node? if pairings(H) > pairings(Best) and then -- did better? is_consistent(H) then –- globaly consistent? Best = H fi else for j = 1 to n if unary(Ei, Fj)and then binary(H, Ei, Fj) –- test new with all pairings in H GCBB([H j], i + 1) –- pairing (Ei, Fj) accepted fi rof if pairings(H) + m – i > pairings(Best) –- can do better? GCBB([H 0], i + 1) -- star node: Ei not paired fi fi

University of Zaragoza - J.D. Tardós, J. Neira

59

Exploiting locality 0

10

20

30

40

50

60

70 0

10

20

30 40 nz = 755

50

60

70

• Limit search in the global map to subsets of covisible features • Locality makes GCBB linear with the global map size University of Zaragoza - J.D. Tardós, J. Neira

60

Information Matrix .vs. Covisibility 1 0.9 50 0.8 0.7

100

0.6 150

0.5 0.4

200 0.3 0.2

250

0.1 300 50

100

150

200

250

300

0

• Sydney park dataset (Nebot et al.)

University of Zaragoza - J.D. Tardós, J. Neira

61

Robot relocation example 30

25

20

8 6

15

x

4

y

2

x

10

y

0 5

-2 0

5

10

15 0 -5

0

5

10

15

20

25

100% success rate for n=30 local maps University of Zaragoza - J.D. Tardós, J. Neira

62

Multirobot map building y

robot 2

x

robot 1 x

y

Match, join and fuse to one full stochastic map University of Zaragoza - J.D. Tardós, J. Neira

63

Multirobot map building

40

• Randomly select a feature in one map • Try to associate its covisible features in the other map using y y GCBB x 40

35

35

30

30

25

25

20

20

15

15

10

10

5

5

0

x

0 -10

-5

0

5

10

15

20

25

-10

-5

0

5

10

15

20

25

Unsuccessful tries University of Zaragoza - J.D. Tardós, J. Neira

64

Multirobot map building • Final map

y

x

30

35

25

30

20

25

15

20

10

y x robot 2

• Successful try 40

5

15

robot 0 1

x

y

10 -5 5 -10 0 -15

-10

-5

0

5

10

15

20

25

-10

0

10

20

University of Zaragoza - J.D. Tardós, J. Neira

30

65

Multirobot map building

Final map

University of Zaragoza - J.D. Tardós, J. Neira

66

5. Open Issues in SLAM • Data association • Big environments – Big loop closing – Computational efficiency (constant time?) – New estimation techniques

• Outdoor & complex environments – Feature extraction and modeling – Data association

• SLAM with vision • 3D SLAM

University of Zaragoza - J.D. Tardós, J. Neira

67

Recommended Readings • Grimson, W. E. L. : "Object Recognition by Computer: The Role of Geometric Constraints", The MIT Press, Cambridge, Mass., 1990 • José A. Castellanos and Juan D. Tardós, Mobile Robot Localization and Map Building: A Multisensor Fusion Approach, Kluwer Ac. Pub., Boston, 1999 • J. Neira and J.D. Tardós: “Data Association in Stochastic Mapping Using the Joint Compatibility Test”, IEEE Trans. Robotics and Automation, vol. 17, no. 6, pp. 890-897, Dec 2001. • P.M. Newman, J.J. Leonard, J. Neira and J.D. Tardós: “Explore and Return: Experimental Validation of Real Time Concurrent Mapping and Localization”. IEEE Int. Conf. Robotics and Automation, May, 2002. • J.D. Tardós, J. Neira, P.M. Newman and J.J. Leonard: “Robust Mapping and Localization in Indoor Environments using Sonar Data”, Int. J. Robotics Research, 2002 (to appear)

University of Zaragoza - J.D. Tardós, J. Neira

68