Intelligent Robotics

64-424 Intelligent Robotics 64-424 Intelligent Robotics http://tams.informatik.uni-hamburg.de/ lectures/2013ws/vorlesung/ir Jianwei Zhang / Eugen Richter Faculty of Mathematics, Informatics and Natural Sciences Technical Aspects of Multimodal Systems Winter semester 2013/2014 Jianwei Zhang / Eugen Richter 1

Outline 64-424 Intelligent Robotics 1. Sensor fundamentals 2. Rotation and motion 3. Force and pressure 4. Frame transformations 5. Distance measurement 6. Scan data processing 7. Recursive state estimation 8. Vision systems 9. Fuzzy logic Jianwei Zhang / Eugen Richter 2

6Scandataprocessing Outline 64-424 Intelligent Robotics 1. Sensor fundamentals 2. Rotation and motion 3. Force and pressure 4. Frame transformations 5. Distance measurement 6. Scan data processing Scan data filtering Feature extraction Applications 7. Recursive state estimation 8. Vision systems 9. Fuzzy logic Jianwei Zhang / Eugen Richter 300

6Scandataprocessing Processing of distance measurements 64-424 Intelligent Robotics There are certain procedures for measuring using laser measurement systems I Extraction of line segments I Extraction of corners I Classification of scan points I Scan filtering: I Smoothing I Data reduction Jianwei Zhang / Eugen Richter 301

6Scandataprocessing Scan 64-424 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 absolute coordinates apple apple xi x = y i y + apple cos sin sin cos apple ri cos i r i sin i Jianwei Zhang / Eugen Richter 302

6Scandataprocessing Scan (cont.) 64-424 Intelligent Robotics Jianwei Zhang / Eugen Richter 303

6.1 Scan data processing - Scan data filtering 64-424 Intelligent Robotics Scan data filtering I General issues: big amount of data, unwanted scan points I Therefore, filtering of scan data is necessary I Popular scan data filters are: I I I I Median filter Reduction filter Angle reduction filter Line filter Jianwei Zhang / Eugen Richter 304

6.1.1 Scan data processing - Scan data filtering - Median filter 64-424 Intelligent Robotics Median filter I The median filter recognizes outliers and replaces them with a 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 wsize defines window size (Number of points in the median filter) I Big values equal strong smoothing I Disadvantage of the median filter: Corners are rounded Jianwei Zhang / Eugen Richter 305

6.1.1 Scan data processing - Scan data filtering - Median filter 64-424 Intelligent Robotics Median filter (cont.) Algorithm Median filter Eingabe Scan s, window size wsize Ausgabe 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 Jianwei Zhang / Eugen Richter 306

6.1.1 Scan data processing - Scan data filtering - Median filter 64-424 Intelligent Robotics Median filter (cont.) Jianwei Zhang / Eugen Richter 307

6.1.2 Scan data processing - Scan data filtering - Reduction filter 64-424 Intelligent Robotics Reduction filter I The reduction filter reduces point clusters (clouds) to one point I A point cluster is specified through a radius r I The first point (starting point) of a scan starts a 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 Jianwei Zhang / Eugen Richter 308

6.1.2 Scan data processing - Scan data filtering - Reduction filter 64-424 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 +1 Jianwei Zhang / Eugen Richter 309

6.1.2 Scan data processing - Scan data filtering - Reduction filter 64-424 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 Jianwei Zhang / Eugen Richter 310

6.1.2 Scan data processing - Scan data filtering - Reduction filter 64-424 Intelligent Robotics Reduction filter (cont.) Jianwei Zhang / Eugen Richter 311

6.1.2 Scan data processing - Scan data filtering - Reduction filter 64-424 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 postprocessing 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 Jianwei Zhang / Eugen Richter 312

6.1.3 Scan data processing - Scan data filtering - Angle reduction filter 64-424 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 to evenly reduce scans with a high angle resolution I Apart from other applications this can be used for merging several scans Jianwei Zhang / Eugen Richter 313

6.1.3 Scan data processing - Scan data filtering - Angle reduction filter 64-424 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 +1 Jianwei Zhang / Eugen Richter 314

6.1.3 Scan data processing - Scan data filtering - Angle reduction filter 64-424 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 Jianwei Zhang / Eugen Richter 315

6.1.3 Scan data processing - Scan data filtering - Angle reduction filter 64-424 Intelligent Robotics Angle reduction filter (cont.) Jianwei Zhang / Eugen Richter 316

6.1.4 Scan data processing - Scan data filtering - Line filter 64-424 Intelligent Robotics Line filter I The line filter uses the line extraction algorithm, which is 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) on average I The filter is used, if further processing requires polygonal environments Jianwei Zhang / Eugen Richter 317

6.1.4 Scan data processing - Scan data filtering - Line filter 64-424 Intelligent Robotics Line filter (cont.) Jianwei Zhang / Eugen Richter 318

6.2 Scan data processing - Feature extraction 64-424 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 maximum distance of two consecutive points for group building Jianwei Zhang / Eugen Richter 319

6.2.1 Scan data processing - Feature extraction - Lines 64-424 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 Jianwei Zhang / Eugen Richter 320

6.2.1 Scan data processing - Feature extraction - Lines 64-424 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 Jianwei Zhang / Eugen Richter 321

6.2.1 Scan data processing - Feature extraction - Lines 64-424 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 Jianwei Zhang / Eugen Richter 322

6.2.1 Scan data processing - Feature extraction - Lines 64-424 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) Jianwei Zhang / Eugen Richter 323

6.2.1 Scan data processing - Feature extraction - Lines 64-424 Intelligent Robotics Lines (cont.) Jianwei Zhang / Eugen Richter 324

6.2.1 Scan data processing - Feature extraction - Lines 64-424 Intelligent Robotics Lines (cont.) Jianwei Zhang / Eugen Richter 325

6.2.2 Scan data processing - Feature extraction - Corners 64-424 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 Jianwei Zhang / Eugen Richter 326

6.2.2 Scan data processing - Feature extraction - Corners 64-424 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 Jianwei Zhang / Eugen Richter 327

6.2.2 Scan data processing - Feature extraction - Corners 64-424 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 Jianwei Zhang / Eugen Richter 328

6.2.2 Scan data processing - Feature extraction - Corners 64-424 Intelligent Robotics Corners (cont.) Jianwei Zhang / Eugen Richter 329

6.3.1 Scan data processing - Applications - Scan-Matching 64-424 Intelligent Robotics Scan-Matching I In mobile robotics, laser scans are frequently used to determine the position 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 Jianwei Zhang / Eugen Richter 330

6.3.1 Scan data processing - Applications - Scan-Matching 64-424 Intelligent Robotics Procedure by Cox I One of the first proposals for overlapping scan data and an a priori line model was made by Cox (1990, 1991) I The proposed algorithm assigns a line of the model to each of the scan points I The result of the assignment allows to determine location and orientation relative to the line model I The procedure requires a rough estimate of the measurement position (e.g from odometry data) Jianwei Zhang / Eugen Richter 331

6.3.1 Scan data processing - Applications - Scan-Matching 64-424 Intelligent Robotics Procedure by Cox (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 Jianwei Zhang / Eugen Richter 332

6.3.1 Scan data processing - Applications - Scan-Matching 64-424 Intelligent Robotics Procedure by Cox (cont.) 6. Repeat steps 2-5 until the procedure converges The result produced by the algorithm is (ˆx, ŷ, ˆ ) T 7. Calculate the error covariance matrix P match Jianwei Zhang / Eugen Richter 333

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Merging line segments Jianwei Zhang / Eugen Richter 334

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Merging line segments (cont.) Jianwei Zhang / Eugen Richter 335

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Merging line segments (cont.) Jianwei Zhang / Eugen Richter 336

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Merging line segments (cont.) Jianwei Zhang / Eugen Richter 337

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Merging line segments (cont.) Jianwei Zhang / Eugen Richter 338

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Hough-Transformation 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 to 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 Jianwei Zhang / Eugen Richter 339

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Straight line recognition I Parameter: 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 Jianwei Zhang / Eugen Richter 340

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Straight line recognition (cont.) y r θ x Jianwei Zhang / Eugen Richter 341

6.3.2 Scan data processing - Applications - Merging line segments 64-424 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 Jianwei Zhang / Eugen Richter 342

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Straight line recognition (cont.) Jianwei Zhang / Eugen Richter 343

6.3.2 Scan data processing - Applications - Merging line segments 64-424 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 Jianwei Zhang / Eugen Richter 344

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Agglomerative Clustering 1. All elements are single clusters 2. Clusters which are closest to each other are merged 3. Repeat 2. 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 Jianwei Zhang / Eugen Richter 345

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Typical distance functions Following functions are frequently used to determine the distance between two clusters A and B: I Single Linkage Clustering: Minimum distance between two elements min {d(a, b)} a2a,b2b I Complete Linkage Clustering: Maximum distance between two elements max {d(a, b)} a2a,b2b I Average Linkage Clustering: Average distance between all elements 1 A B X a2a,b2b d(a, b) Jianwei Zhang / Eugen Richter 346

6.3.2 Scan data processing - Applications - Merging line segments 64-424 Intelligent Robotics Typical distance functions (cont.) I Average Group Linkage: Average distance of all elements in the union of A and B 1 X d(x, y) C x,y2c,c=a[b I Centroid Method: Distance between the average values d( x, ȳ) I Ward s Method: Increase of the variance during unification of A and B von A und B d( x, ȳ) 1/ A + 1/ B I and various other functions... Jianwei Zhang / Eugen Richter 347

6.3.3 Scan data processing - Applications - People-tracking using laser measurement systems 64-424 Intelligent Robotics People-tracking using laser measurement systems I Stationary laser measurement systems can be used for tracking of people I 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 Jianwei Zhang / Eugen Richter 348

6.3.3 Scan data processing - Applications - People-tracking using laser measurement systems 64-424 Intelligent Robotics Background subtraction I First, a background model is created: I For each angle and a certain timespan, 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 can be aggregated into groups, similar to line extraction Jianwei Zhang / Eugen Richter 349

6.3.3 Scan data processing - Applications - People-tracking using laser measurement systems 64-424 Intelligent Robotics Background subtraction (cont.) I If the background changes, the procedure needs to be re-initialised 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 Jianwei Zhang / Eugen Richter 350

6.3.3 Scan data processing - Applications - People-tracking using laser measurement systems 64-424 Intelligent Robotics Clustering of foreground clusters I Clustering is done similar to line extraction I In practice, laser measurement systems are frequently mounted near the ground I Therefore, the legs of persons are mostly visible I Foreground clusters whose diameter is larger than 20-30cm, can be ignored for person detection I Using Hough transformation it would be possible to check for (semi-)circles as well I Depending on the distance to the laser scanner, the foreground clusters have varying amounts of measurement points Jianwei Zhang / Eugen Richter 351

6.3.3 Scan data processing - Applications - People-tracking using laser measurement systems 64-424 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 Jianwei Zhang / Eugen Richter 352

6.3.3 Scan data processing - Applications - People-tracking using laser measurement systems 64-424 Intelligent Robotics Tracking (cont.) Jianwei Zhang / Eugen Richter 353

6.3.3 Scan data processing - Applications - People-tracking using laser measurement systems 64-424 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, probabilites regarding turning direction can be determined I Thus, predictions about where a person is going can be made Jianwei Zhang / Eugen Richter 354

6.3.3 Scan data processing - Applications - People-tracking using laser measurement systems 64-424 Intelligent Robotics Learning of a movement graph (cont.) Jianwei Zhang / Eugen Richter 355