Loop detection and extended target tracking using laser data

Transcription

1 Licentiate seminar 1(39) Loop detection and extended target tracking using laser data Karl Granström Division of Automatic Control Department of Electrical Engineering Linköping University Linköping, Sweden

2 Errata 2(39) Paper A "I accept the paper for publication subject to your addressing all the points raised in the review."

3 Automatic control 3(39) Automatic control can be defined as making a system behave the way that you want it to. Example: Automatic Cruise Control (ACC)

4 Automatic control 3(39) Automatic control can be defined as making a system behave the way that you want it to. Example: Automatic Cruise Control (ACC) Must know the state of the system, e.g. what is the current speed?

5 Autonomous vehicles / robots 4(39) Here focus on autonomous vehicles / robots. In the context of robots, knowing the state of the system includes knowing where the robot is, 1. Must be able to recognise previously visited places, i.e. loop detection.

6 Autonomous vehicles / robots 4(39) Here focus on autonomous vehicles / robots. In the context of robots, knowing the state of the system includes knowing where the robot is, 1. Must be able to recognise previously visited places, i.e. loop detection. knowing where other objects are. 2. Must be able to follow objects that are moving, i.e. (extended) target tracking.

7 Autonomous vehicles / robots 4(39) Here focus on autonomous vehicles / robots. In the context of robots, knowing the state of the system includes knowing where the robot is, 1. Must be able to recognise previously visited places, i.e. loop detection. knowing where other objects are. 2. Must be able to follow objects that are moving, i.e. (extended) target tracking. A fundamental requirement is that the robot must be able to sense the world here we have used data from laser range sensors.

8 Problem formulation and contributions 5(39) In this thesis the following two problems are addressed 1. design of a method that can compare pairs of laser data, such that the method may be used to detect loop closure in SLAM. Feature description of laser data AdaBoost used to learn classifier Evaluated using several experiments

9 Problem formulation and contributions 5(39) In this thesis the following two problems are addressed 1. design of a method that can compare pairs of laser data, such that the method may be used to detect loop closure in SLAM. Feature description of laser data AdaBoost used to learn classifier Evaluated using several experiments 2. design of a method for target tracking in scenarios where each target possibly gives rise to more than one measurement per time step. Implementation of Gaussian mixture PHD-filter for extended targets Simple method to partition measurements Evaluation using simulations and experiments

10 Presentation overview 6(39) 1. Automatic control 2. Autonomous vehicles / robots 3. Problem formulation and contributions 4. Laser data 5. Loop detection (L.D.) 6. Extended target tracking (E.T.T.) 7. Future work

11 Laser data motivation 7(39) Laser sensors are common Used frequently in robotics for at least a decade. Receiving increasing attention from e.g. the automotive industry.

12 Laser data basic 2D functionality 8(39) Measures distance to nearest object at certain angles y [m] x [m] Data is called point clouds.

13 Laser data basic 2D functionality 8(39) Measures distance to nearest object at certain angles y [m] y [m] x [m] Data is called point clouds x [m]

14 Laser data data examples 9(39) 2D laservpone.avi 3D laserhannover.avi

15 L.D. Simultaneous localisation and mapping? 1(39) SLAM Map the environment, localise robot in map. Y [m] X [m] State vector is history of poses. Each pose associated to point cloud describing the location.

16 L.D. Simultaneous localisation and mapping? 1(39) SLAM Map the environment, localise robot in map. Y [m] X [m] State vector is history of poses. Each pose associated to point cloud describing the location.

17 L.D. Simultaneous localisation and mapping? 1(39) SLAM Map the environment, localise robot in map. Y [m] X [m] State vector is history of poses. Each pose associated to point cloud describing the location.

18 L.D. problem definition 11(39) Loop closure detection place recognition. Pairwise comparison of data, here point clouds, p k = {p k i }N i=1, pk i R 3 Z [m] Z [m] Y [m] 2 2 X [m] 2 2 Y [m] 2 2 X [m] 2 Are p k and p l from the same location? Yes

19 L.D. problem definition 11(39) Loop closure detection place recognition. Pairwise comparison of data, here point clouds, p k = {p k i }N i=1, pk i R 3 Z [m] Z [m] Y [m] 2 2 X [m] 2 2 Y [m] 2 2 X [m] 2 Are p k and p l from the same location? Yes

20 L.D. problem definition 11(39) Loop closure detection place recognition. Pairwise comparison of data, here point clouds, p k = {p k i }N i=1, pk i R [m] [m] [m] [m] Are p k and p l from the same location? Yes

21 L.D. problem definition 11(39) Loop closure detection place recognition. Pairwise comparison of data, here point clouds, p k = {p k i }N i=1, pk i R [m] [m] [m] [m] Are p k and p l from the same location? Yes

22 L.D. motivation 12(39) Loop closure/place recognition is an important and difficult problem, especially in SLAM. Need a method thas is robust against misclassification, invariant to rotation and computationally inexpensive.

23 L.D. our approach 13(39) Learning phase Point cloud data sets p k = {p k i }N i=1, pk i RD Classification phase (part of SLAM) Point cloud pair: p k, p l Find training pairs: p i1, p i2, y i Extract features: F i1,i2, y i Extract features: F k,l Strong classifier: c (F k,l ) IF No match Next point cloud pair IF Match Use AdaBoost to learn a strong classifier. Point cloud registration Output strong classifier: c (F k,l ) esdf measurement update

24 L.D. feature motivation 14(39) Point clouds are described with features: Meaningful statistics describing shape etc Compact description of point cloud, n f N Easy comparison of p k and p l. Two types of features used, all invariant to rotation.

25 L.D. feature examples 15(39) Type 1: geometric and statistic properites. f 1 area in 2D, volume in 3D f 3 average range... Comparison: f i f i. Same place similar value small f i f i.

26 L.D. feature examples 15(39) Type 1: geometric and statistic properites. f 1 area in 2D, volume in 3D f 3 average range... Comparison: f i f i. Same place similar value small f i f i. Type 2: range histograms. f j bin size b j [.1m, 3m] Comparison: Cross correlation of f j :s. Same place similar f j High cross correlation.

27 L.D. AdaBoost 16(39) AdaBoost is used to learn a 2-class classifier. Iterative learning. Combination of simple, weak, classifiers f f 1 Initialise weights

28 L.D. AdaBoost 16(39) AdaBoost is used to learn a 2-class classifier. Iterative learning. Combination of simple, weak, classifiers. 1.9 c 1 = (f 2 <.33) f f 1 Minimise error Increase weights

29 L.D. AdaBoost 16(39) AdaBoost is used to learn a 2-class classifier. Iterative learning. Combination of simple, weak, classifiers. 1.9 c 1 = (f 2 <.33) f f 1 Minimise error Increase weights

30 L.D. AdaBoost 16(39) AdaBoost is used to learn a 2-class classifier. Iterative learning. Combination of simple, weak, classifiers c 1 = (f 2 <.33) c 2 = (f 1 >.53).7.6 f f 1 Minimise error Increase weights

31 L.D. AdaBoost 16(39) AdaBoost is used to learn a 2-class classifier. Iterative learning. Combination of simple, weak, classifiers c 1 = (f 2 <.33) c 2 = (f 1 >.53).7.6 f f 1 Minimise error Increase weights

32 L.D. AdaBoost 16(39) AdaBoost is used to learn a 2-class classifier. Iterative learning. Combination of simple, weak, classifiers c 1 = (f 2 <.33) c 2 = (f 1 >.53) c 3 = (f 1 >.84) f f 1 Minimise error Increase weights

33 L.D. AdaBoost 16(39) AdaBoost is used to learn a 2-class classifier. Iterative learning. Combination of simple, weak, classifiers c 1 = (f 2 <.33) c 2 = (f 1 >.53) c 3 = (f 1 >.84) f f 1 Minimise error Increase weights

34 L.D. AdaBoost 16(39) AdaBoost is used to learn a 2-class classifier. Iterative learning. Combination of simple, weak, classifiers c 1 = (f 2 <.33) c 2 = (f 1 >.53) c 3 = (f 1 >.84) Likelihood: f c = t α t c t t α t Initialise weights f 1

35 L.D. AdaBoost 16(39) AdaBoost is used to learn a 2-class classifier. Iterative learning. Combination of simple, weak, classifiers c 1 = (f 2 <.33) c 2 = (f 1 >.53) c 3 = (f 1 >.84) Likelihood: f c = t α t c t t α t Decision regions: c τ =.5 Initialise weights f 1

36 L.D. experiments 17(39) 1. Execution time 2. Number of weak classifiers needed 3. Most informative features 4. Receiver Operating Characteristic (ROC) 5. Comparison of 2D and 3D performance 6. Dependence to translation 7. Dynamic objects 8. Repetitive structures in the environment 9. Loop closure detection in SLAM

37 L.D. experiments 17(39) 1. Execution time 2. Number of weak classifiers needed 3. Most informative features 4. Receiver Operating Characteristic (ROC) 5. Comparison of 2D and 3D performance 6. Dependence to translation 7. Dynamic objects 8. Repetitive structures in the environment 9. Loop closure detection in SLAM

38 L.D. ROC / translation dependence 18(39) ROC: detection/false alarm trade-off 1.95 Detection at % false alarm. Outdoor 2D: 1m: 66% detection 2m: 34% detection 3m: 18% detection Outdoor 3D: 3m: 63% detection Indoor 3D: 1m: 53% detection Detection Rate Detection Rate False Alarm Rate hann2 AASS False Alarm Rate 1m 2m 3m

39 L.D. 3D SLAM experiment, data 19(39) c (F k,l ) trained on outdoor data. r max = 3m. N 17. SLAM experiment on indoor data. r max = 15m. N 11. Does c (F k,l ) work in SLAM experiments? Does c (F k,l ) generalise well between environments?

40 L.D. 3D SLAM experiment, results 2(39) ~5% detection, no false alarms, good environment generalisation. Z [m] ESDF DR Y [m] Y [m] X [m] X [m]

41 L.D. comparison 21(39) Comparison with loop closure detection methods for point clouds. 2D: Higher detection at low false alarm.

42 L.D. comparison 21(39) Comparison with loop closure detection methods for point clouds. 2D: Higher detection at low false alarm. 3D outdoor: 2 nd highest detection Faster than related work. Rotation invariant without assumptions.

43 L.D. comparison 21(39) Comparison with loop closure detection methods for point clouds. 2D: Higher detection at low false alarm. 3D outdoor: 2 nd highest detection Faster than related work. Rotation invariant without assumptions. 3D indoor: Lower detection Not enough training data from the same location.

44 L.D. comparison 21(39) Comparison with loop closure detection methods for point clouds. 2D: Higher detection at low false alarm. 3D outdoor: 2 nd highest detection Faster than related work. Rotation invariant without assumptions. 3D indoor: Lower detection Not enough training data from the same location. 2D and 3D: More sensitive to translation.

45 L.D. comparison 21(39) Comparison with loop closure detection methods for point clouds. 2D: Higher detection at low false alarm. 3D outdoor: 2 nd highest detection Faster than related work. Rotation invariant without assumptions. 3D indoor: Lower detection Not enough training data from the same location. 2D and 3D: More sensitive to translation. Much work using images difficult to compare different sensors.

46 L.D. conclusions 22(39) A machine learning approach for the loop closure detection problem using point clouds. > 4 rotation invariant features. Loop closure detected from arbitrary direction. Competitive detection for low false alarm (%). Fast to compute. Method generalises well between environments. SLAM experiments shows the method works in real problems.

47 E.T.T. Target tracking? 23(39) Target tracking Keep track of location of targets Unknown number, not always detected Noise and clutter Difficult data association

48 E.T.T. Target tracking? 23(39) Target tracking Keep track of location of targets Unknown number, not always detected Noise and clutter Difficult data association Early RADAR-airplane-tracking Targets behave as points

49 E.T.T. Target tracking? 23(39) Target tracking Keep track of location of targets Unknown number, not always detected Noise and clutter Difficult data association Early RADAR-airplane-tracking Targets behave as points Modern sensors no longer points Multiple measurements Definition: Extended targets are targets that potentially give rise to more than one measurement per time step.

50 E.T.T. motivation 24(39) Point target assumption is often not valid, e.g....laser sensors...automotive radar...camera images Need method that handles multiple measurements per target.

51 E.T.T. RFS 25(39) Approach: Use random finite sets, RFS. RFS Y = {y (i)} N y i=1 y (i) are random vectors, common assumption y (i) R n N y < is random, common assumption N y POIS (β) { No order, e.g. y (1), y (2)} = {y (2), y (1)}

52 E.T.T. setup 26(39) RFS of targets X k = {x (i) k }N x,k i=1 x (i) k+1 = F kx (i) k + G k w (i) k, w(i) k N (, Q k ). RFS of measurements Z k = {z (j) k }N z,k j=1 z (j) k = H k x (i) k + e (j) k, e(j) k N (, R k ). Goal: Compute target set estimate ˆX k using measurement sets Z k.

53 E.T.T. PHD filter 27(39) Bayes recursion computationally intractable [Mahler (27)].

54 E.T.T. PHD filter 27(39) Bayes recursion computationally intractable [Mahler (27)]. Approximation: propagate first order moment D k k (x Z (k)), called PHD-intensity. Prediction Correction {}}{{}}{... D k k (x Z (k) ) D k+1 k (x Z (k) ) D k+1 k+1 (x Z (k+1) )...

55 E.T.T. PHD filter 27(39) Bayes recursion computationally intractable [Mahler (27)]. Approximation: propagate first order moment D k k (x Z (k)), called PHD-intensity. Prediction Correction {}}{{}}{... D k k (x Z (k) ) D k+1 k (x Z (k) ) D k+1 k+1 (x Z (k+1) )... Analogous to α-β-filter, which propagates first order moment of random variable (i.e. mean vector).

56 E.T.T. PHD example 28(39) D k k (x).4.2 PHD-intensity is sum of Gaussians y [m] x [m] D k k (x Z (k)) = J k k j=1 ( ) w (j) k k N x m (j) k k, P(j) k k PHD-intensity

57 E.T.T. PHD example 28(39) D k+1 k (x) x PHD-intensity is sum of Gaussians y [m] x [m] D k k (x Z (k)) = J k k j=1 ( ) w (j) k k N x m (j) k k, P(j) k k Predicted intensity

58 E.T.T. PHD example 28(39) D k+1 k (x) x y [m] x [m] Cluttered set of measurements 4 PHD-intensity is sum of Gaussians D k k (x Z (k)) = J k k j=1 ( ) w (j) k k N x m (j) k k, P(j) k k

59 E.T.T. PHD example 28(39) D k+1 k+1 (x) PHD-intensity is sum of Gaussians y [m] x [m] D k k (x Z (k)) = J k k j=1 ( ) w (j) k k N x m (j) k k, P(j) k k Corrected intensity

60 E.T.T. measurement pseudo likelihood 29(39) Implementationt of prediction shown by [Vo and Ma, 26]. D k k 1 (x Z) is predicted PHD-intensity. Corrected PHD-intensity D k k (x Z) = L Zk (x) D k k 1 (x Z), where measurement pseudo-likelihood is given by ( L Zk (x) =1 1 e γ(x)) p D (x) + [Mahler, 29] e γ(x) γ (x) p D (x) ω p W d p Z k W p W z W φ z (x) λ k c k (z).

61 E.T.T. measurement partitioning 3(39) In each time step Z k must be partitioned. A partition p is a division of Z k into cells W. Important since more than one measurement can stem from the same target.

62 E.T.T. measurement partitioning example 31(39) Partition the measurement set Z k = { } z (1) k, z (2) k, z (3) k 1.5 Z 1.5 p p 2 y 1.5 z (2) k z (1) k y 1.5 W 1 1 y 1.5 W 2 1 z (3) k x x W x 1.5 p p p 5 y 1.5 W 3 2 W 3 1 y 1.5 W 4 2 W 4 1 y 1.5 W 5 2 W 5 3 W x x x

63 E.T.T. measurement partitioning method 32(39) Measurements belong to same cell W if distance is small. Partions p i where cells contain measurements < d i m apart.

64 E.T.T. measurement partitioning method 32(39) Measurements belong to same cell W if distance is small. Partions p i where cells contain measurements < d i m apart. Let {d m i } N z,k i=1 be set of measurement to measurement distances. Good partitions for d i corresponding to d min d m i < d max

65 E.T.T. measurement partitioning method 32(39) Measurements belong to same cell W if distance is small. Partions p i where cells contain measurements < d i m apart. Let {d m i } N z,k i=1 be set of measurement to measurement distances. Good partitions for d i corresponding to d min d m i < d max Use knowledge about scenario to determine d min and d max

66 E.T.T. measurement partitioning method example33(39) Y [m] Measurements X [m] Y [m] d = X [m] Y [m] d = X [m] d = d = d = Y [m] 2 Y [m] 2 Y [m] X [m] X [m] X [m]

67 E.T.T. simulation measurements 34(39) 1 True vs measured 5 X [m] Time 1 True vs measured 5 Y [m] Time True target track crossing at time k = 56. New target birth and target spawned at time k = 66. Evaluation against standard GM-PHD using OSPA metric.

68 E.T.T. simulation results 35(39) True Ext. PHD PHD OSPA, c=6, p= Ext. PHD PHD Time [s] Cardinality Time [s] OSPA

69 E.T.T. experiment 36(39) 2D laser range scanner Two human targets One target occluded by the other Variable probability of detection to handle occlusion targettracking.avi

70 E.T.T. conclusions 37(39) Implementation of GM-PHD-filter for extended targets. Simple method for measurement set partitioning. Variable p D to handle occlusion. The suggested implementation handles......unknown number of targets....noisy and cluttered measurement sets.

71 Future work 38(39) Loop detection: Decrease sensitivity to translation

72 Future work 38(39) Loop detection: Decrease sensitivity to translation Extended target tracking: Estimate target shape and size Cardinalised PHD-filter robust estimate of number of targets Compare to other extended target tracking solutions

73 Future work 38(39) Loop detection: Decrease sensitivity to translation Extended target tracking: Estimate target shape and size Cardinalised PHD-filter robust estimate of number of targets Compare to other extended target tracking solutions Further extensions Environment labeling of data Separation of background environment and dynamic targets Semi-supervised learning instead of supervised learning

74 The End 39(39) Thank you for listening!