Processing of distance measurement data

Transcription

1 7Scanprocessing Outline Intelligent Robotics 1. Introduction 2. Fundamentals 3. Rotation / Motion 4. Force / Pressure 5. Frame transformations 6. Distance 7. Scan processing Scan data filtering Feature extraction Applications Literature 294

2 7Scanprocessing Processing of distance measurement data Intelligent Robotics There are several procedures for processing/using distance measurement data I Scan data filtering: I I Smoothing Data reduction I Extraction of line segments I Extraction of corners I Classification of scan points 295

3 7Scanprocessing Scan conversion Intelligent Robotics I A scan is a set of measurement values n o m i =( i, r i ) T i = 0...n 1 specified in polar coordinates ( i, r i ) T I For a given measuring position l =(x, y, ) T a scan point m i =( i, r i ) T can be converted to cartesian coordinates apple apple xi x = y i y + apple cos sin sin cos apple ri cos i r i sin i 296

4 7Scanprocessing Scan conversion (cont.) Intelligent Robotics 297

5 7.1 Scan processing - Scan data filtering Intelligent Robotics Scan data filtering I General issues: Big amount of data, noise/outliers, etc. I Therefore, filtering of scan data is useful/necessary I Popular scan data filters are: I I I I Median filter Reduction filter Angle reduction filter Line filter 298

6 7.1.1 Scan processing - Scan data filtering - Median filter Intelligent Robotics Median filter I The median filter recognizes outliers and replaces them with a more suitable measurement I Around each scan point, a window containing measurements before and after the point is placed I The scan point is replaced by a point having the same measurement angle but the median distance within the filter window I Window size (wsize) is the main parameter of the median filter I Big window sizes lead to a strong smoothing e ect I Disadvantage: Corners are rounded 299

7 7.1.1 Scan processing - Scan data filtering - Median filter Intelligent Robotics Median filter (cont.) Algorithm Median filter Input Scan s, window size wsize Output Scan s 0 for i := 0 to numpoints(s)-1 do p := n-th-scanpoint(s,i) for j := 0 to wsize do k := (i + j - wsize/2) mod numpoints(s) p k := n-th-scanpoint(s,k) d(j) := distance-value(p k ) endfor d median := median(d) n-th-scanpoint(s 0,i) :=(angle-value(p),d median ) endfor return s 0 300

8 7.1.1 Scan processing - Scan data filtering - Median filter Intelligent Robotics Median filter (cont.) 301

9 7.1.2 Scan processing - Scan data filtering - Reduction filter Intelligent Robotics Reduction filter I The reduction filter reduces point clusters (point clouds) to a single point I A point cluster is specified through a radius r from the starting point I The first point (starting point) of a scan starts the first cluster I All subsequent points at a distance d < 2 r are added to the cluster I A new cluster is started at the first point with a bigger distance I Each cluster is replaced by the center of gravity of the corresponding points 302

10 7.1.2 Scan processing - Scan data filtering - Reduction filter Intelligent Robotics Reduction filter (cont.) Algorithm Reduction filter Input Scan s, radius r Output Scan s 0 j := 0 p 0 := n-th-scanpoint(s,0) p sum := p 0 n := 0 for i := 1 to numpoints(s)-1 do p := n-th-scanpoint(s,i) if distance(p 0,p) < 2r then p sum := p sum + p n := n

11 7.1.2 Scan processing - Scan data filtering - Reduction filter Intelligent Robotics Reduction filter (cont.) else n-th-scanpoint(s 0,j) :=p sum/n j := j +1 p 0 := p p sum := p 0 n := 1 endif endfor n-th-scanpoint(s 0,j) :=p sum/n j := j +1 numpoints(s 0 ):=j return s 0 304

12 7.1.2 Scan processing - Scan data filtering - Reduction filter Intelligent Robotics Reduction filter (cont.) 305

13 7.1.2 Scan processing - Scan data filtering - Reduction filter Intelligent Robotics Reduction filter (cont.) I For n points the reduction filter algorithm has a time complexity of O(n) I Advantages of the reduction filter: I Reduction of the number of scan points without significant information loss I This leads to shorter duration of scan post-processing I The result is a more uniform distribution of the points I Disadvantages of the reduction filter: I Feature extraction is not as easy any more I Possibly too few points for a feature (e.g. corner) I Better option: Feature extraction before the reduction filter 306

14 7.1.3 Scan processing - Scan data filtering - Angle reduction filter Intelligent Robotics Angle reduction filter I The angle reduction filter resembles the reduction filter I Scan points having a similar measurement angle are grouped and replaced by the point with the median distance I The function median-dist(q, n) yields this point in the following algorithm I The time complexity is O(n), havingn as the number of scan points I The angle reduction filter is used for an even reduction of scans with a high angular resolution 307

15 7.1.3 Scan processing - Scan data filtering - Angle reduction filter Intelligent Robotics Angle reduction filter (cont.) Algorithm Angle reduction filter Input Scan s, angle Output Scan s 0 j := 0 q(0) := n-th-scanpoint(s,0) n := 1 for i := 1 to numpoints(s)-1 do p := n-th-scanpoint(s,i) if abs(angle-value(p) - angle-value(q(0))) < then q(n) :=p n := n

16 7.1.3 Scan processing - Scan data filtering - Angle reduction filter Intelligent Robotics Angle reduction filter (cont.) else n-th-scanpoint(s 0,j) := median-dist(q,n) j := j +1 q(0) := p n := 1 endif endfor n-th-scanpoint(s 0,j) := median-dist(q,n) j := j +1 numpoints(s 0 ):=j return s 0 309

17 7.1.3 Scan processing - Scan data filtering - Angle reduction filter Intelligent Robotics Angle reduction filter (cont.) 310

18 7.1.4 Scan processing - Scan data filtering - Line filter Intelligent Robotics Line filter I The line filter uses the line extraction algorithm (which will be introduced later) I The algorithm removes scan points that cannot be assigned to aline I Time complexity equals line extraction complexity O(n log n) I The line filter is used in applications, where further processing requires polygonal environments 311

19 7.1.4 Scan processing - Scan data filtering - Line filter Intelligent Robotics Line filter (cont.) 312

20 7.2 Scan processing - Feature extraction Intelligent Robotics Feature extraction I Extraction of features instead of low-level processing of complete scans I Common features: lines, corners I In the following algorithm, maxdist is the parameter specifying the maximum distance of two consecutive points for the formation of a group of points 313

21 7.2.1 Scan processing - Feature extraction - Lines Intelligent Robotics Lines Algorithm Line extraction Input Scan s, parameter maxdist Output Set of lines l l := empty start := 0 for i:=1 to numpoints(s)-1 do p 1 := n-th-scanpoint(s,i-1) p 2 := n-th-scanpoint(s,i) if distance(p 1,p 2) > maxdist then l := l [ split(s,start,i-1) start := i endif endfor l := l [ split(s,start,numpoints(s)-1) return l 314

22 7.2.1 Scan processing - Feature extraction - Lines Intelligent Robotics Lines (cont.) Algorithm split(s,start,end) Input Scan points, specified through s, start and end Parameters minpointsonline, maxsigma Output Set of lines l l := empty line := make-line(s,start,end) if numpoints(line) minpointsonline then if (line) < maxsigma then l := l [ {line} else p start := n-th-scanpoint(s,start) p end := n-th-scanpoint(s,end) i split := start d := 0 315

23 7.2.1 Scan processing - Feature extraction - Lines Intelligent Robotics Lines (cont.) for i := start+1 to end-1 do p := n-th-scanpoint(s,i) if distance-to-line(p,p start,p end ) > d then i split := i d := distance-to-line(p,p start,p end ) endif endfor l := l [ split(s,start,i split ) l := l [ split(s,i split,end) endif endif return l 316

24 7.2.1 Scan processing - Feature extraction - Lines Intelligent Robotics Lines (cont.) I Initially a regression line is fitted to the points I If the deviation (line) is too big, the set of points is divided and the function split is applied to the new sets I The dividing point is the point with the biggest distance to the straight line through start and end I minpointsonline and maxsigma control number of lines and their quality I Line extraction is a typical divide and conquer algorithm I Time complexity similar to Quicksort: O(n 2 ) in the worst case, O(n log n) on average (n: Number of scan points) 317

25 7.2.1 Scan processing - Feature extraction - Lines Intelligent Robotics Lines (cont.) 318

26 7.2.1 Scan processing - Feature extraction - Lines Intelligent Robotics Lines (cont.) 319

27 7.2.2 Scan processing - Feature extraction - Corners Intelligent Robotics Corners I Similar to line extraction algorithm I Only the split-function needs to be replaced I Consecutive lines are checked for intersection I Time complexity equal to that of the line extraction algorithm Algorithm split(s,start,end) Input Scan points specified through s, start and end Parameters minpointscorner, maxsigma Output Set of corners e 320

28 7.2.2 Scan processing - Feature extraction - Corners Intelligent Robotics Corners (cont.) e := empty if (end start) 2 minpointscorner then p start := n-th-scanpoint(s,start) p end := n-th-scanpoint(s,end) i split := start d := 0 for i := start+1 to end-1 do p := n-th-scanpoint(s,i) if distance-to-line(p,p start,p end ) > d then i split := i d := distance-to-line(p,p start,p end ) endif endfor 321

29 7.2.2 Scan processing - Feature extraction - Corners Intelligent Robotics Corners (cont.) if (i split - start) minpointscorner and (end - i split ) minpointscorner then line 1 := make-line(s,i split - minpointscorner,i split ) line 2 := make-line(s,i split, i split + minpointscorner) if (line 1) < maxsigma and (line 2) < maxsigma then e := e [ {make-corner(line 1,line 2)} endif endif e := e [ split(s,start,i split ) e := e [ split(s,i split,end) endif return e 322

30 7.2.2 Scan processing - Feature extraction - Corners Intelligent Robotics Corners (cont.) 323

31 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Scan matching I In mobile robotics, laser scans are frequently used to determine the location (position and orientation) of a robot on a map I For that purpose the scan is initially transformed to a set of lines I The a priori available map is searched for an overlap with the measured/extracted positioning of the lines I This procedure is called scan matching I It can also be applied directly to the distance measurement values 324

32 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Scan matching (cont.) I One of the many approaches to the problem of mobile robot localization is based on matching scan data with an a priori known line model of the environment I This approach assigns a line of the model to each of the scan points I The result of the assignment allows to determine position and orientation relative to the line model I The procedure requires a rough estimate of the initial measurement location (e.g from odometry data) 325

33 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Scan matching (cont.) 1. Let (ˆx, ŷ, ˆ ) T =(s x, s y, s ) T,where(s x, s y, s ) T is the initial position estimate of the measurement scan based on the odometry 2. Translate and rotate scan into position (ˆx, ŷ, ˆ ) T 3. For each scan point, determine the model line (also: target line) closest to that point 4. Calculate the transformation ˆb =( x, y, ) T,whichminimizesthe sum of the squared distances between the scan points and the respective target line 5. Let (ˆx, ŷ, ˆ ) T =(ˆx, ŷ, ˆ ) T +( x, y, ) T 6. Repeat steps 2-5 until the procedure converges ) The result produced by the algorithm is (ˆx, ŷ, ˆ ) T 326

34 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Scan processing pipeline Scan data: Acquisition 327

35 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Scan processing pipeline (cont.) Scan data: Conversion 328

36 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Scan processing pipeline (cont.) Scan data: Filtering 329

37 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Scan processing pipeline (cont.) Scan data: Feature extraction 330

38 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Scan processing pipeline (cont.) Scan data: Matching 331

39 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Hough transform I Procedure for recognition of I Straight lines I Circles I Arbitrary other geometric figures I Frequently applied to gradient images in image processing I Points in the image are mapped onto a parameter space I Suitable parameters: I Straight line: Slope and y-intercept I Circle: Radius and center I Point in the image space corresponds to a figure in parameter space I Searched figure is located at the clusters in parameter space 332

40 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Straight line recognition I Parameters: Slope and y-intercept I Disadvantage: Straight lines having an infinite slope can not be mapped I Better: Straight line in Hessian normal form r = x cos( )+y sin( ) with I : Angle between x-axis and normal of the straight line I r: Distance between origin and straight line 333

41 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Straight line recognition (cont.) y r θ x 334

42 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Straight line recognition (cont.) 335

43 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Straight line recognition (cont.) I All extracted/recognized line segments can be formulated in Hessian normal form I For each line segment a -r-point is mapped onto the parameter space I For clusters the center of gravity is determined I Parameters of the center of gravity describe a straight line, on which the line segments lie I Center of gravity can be found through hierarchical clustering 336

44 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Hierarchical clustering Two basic procedures: I Agglomerative clustering I Widely used in practice I Step by step, single elements are grouped together I Divisive clustering I A large group of elements is divided into smaller clusters 337

45 7.3.1 Scan processing - Applications - Scan-Matching Intelligent Robotics Agglomerative clustering 1. All elements are single clusters 2. Clusters which are closest to each other are merged 3. Repeat the second step until: I All clusters exceed a certain distance to each other or I A minimum amount of clusters is reached I A distance function d is required for the distance between two clusters. A simple example of such a function is: I Single Linkage Clustering: Minimum distance between two elements min {d(a, b)} a2a,b2b 338

46 7.3.2 Scan processing - Applications - People-tracking using scan data Intelligent Robotics People-tracking using scan data Stationary laser rangefinders can be used for tracking of people. Common approaches operate using the following three steps: 1. Separation of the measurement data in foreground and background through background subtraction 2. Grouping within the foreground data 3. Tracking of the groups in subsequent scans 339

47 7.3.2 Scan processing - Applications - People-tracking using scan data Intelligent Robotics Background subtraction I First, a background model is created: I For each angle and a certain time span, a histogram of the distance measurements is determined I The histogram for each angle is given through a distribution function (usually: Gaussian distribution using average value µ and standard deviation ) I Using the background model, the scans are filtered during operation: I Distance measurements smaller than µ n are classified as foreground I The foreground measurements are aggregated into groups, similar to line extraction 340

48 7.3.2 Scan processing - Applications - People-tracking using scan data Intelligent Robotics Background subtraction (cont.) I If the background changes, the procedure needs to be re-initialized I This can be avoided by using a gliding background I Objects classified as foreground, which are not moving, are added to the background model after a certain amount of time I Background subtraction is frequently used for object tracking in image processing as well 341

49 7.3.2 Scan processing - Applications - People-tracking using scan data Intelligent Robotics Clustering of foreground clusters I Clustering is done similar to line extraction I In practice, laser rangefinders are frequently mounted close to the ground I Therefore, people s legs are the most visible features I Simple approach: Foreground clusters whose diameter is smaller or larger than 20-30cm, can be ignored for person detection I Using Hough transformation it would be possible to check for (semi-)circles as a representation of a leg I Depending on the distance to the laser scanner, the foreground clusters have varying amounts of measurement points 342

50 7.3.2 Scan processing - Applications - People-tracking using scan data Intelligent Robotics Tracking I Due to occlusion it is not always possible to see both legs I Usually one leg is standing while the other one is moving I Di culty: Which pair of legs belongs together? I In literature, complex motion models are introduced I Tracking in consecutive scans is done using Kalman filters, particle filters or similar approaches 343

51 7.3.2 Scan processing - Applications - People-tracking using scan data Intelligent Robotics Tracking (cont.) 344

52 7.3.2 Scan processing - Applications - People-tracking using scan data Intelligent Robotics Learning of a movement graph I If movement is tracked over a longer period of time in a larger area, it is possible to determine probabilities of walking into certain areas I From this probability distribution, a graph can be produced which is showing the paths that people are walking on I At the nodes of this graph, probabilities regarding turning direction can be determined I Thus, predictions about where a person is going can be made 345

53 7.3.2 Scan processing - Applications - People-tracking using scan data Intelligent Robotics Learning of a movement graph (cont.) 346

54 7.4 Scan processing - Literature Intelligent Robotics Literature list [1] Andrea Censi. An icp variant using a point-to-line metric. In Robotics and Automation, ICRA IEEE International Conference on, pages 19 25, May [2] Richard O. Duda and Peter E. Hart. Use of the hough transformation to detect lines and curves in pictures. Communications of the ACM, 15(1):11 15, January

55 7.4 Scan processing - Literature Intelligent Robotics Literature list (cont.) [3] Katsuyuki Nakamura, Huijing Zhao, Ryosuke Shibasaki, Kiyoshi Sakamoto, Tomowo Ooga, and Naoki Suzukawa. Tracking pedestrian by using multiple laser range scanners. In Proceedings of the XXth ISPRS Congress, pages , July [4] V. Nguyen, A. Martinelli, N. Tomatis, and R. Siegwart. A comparison of line extraction algorithms using 2d laser rangefinder for indoor mobile robotics. In Intelligent Robots and Systems, (IROS 2005) IEEE/RSJ International Conference on, pages , August