1. Rolf Lakaemper Temple University, Philadelphia,PA,USA Towards a Segment Based Mapping System

2. Part 1.1 : FFSPart 1.2: FFS + Virtual ScansPart 2: Segments

3. Rolf Lakaemper, Nagesh Adluru Temple University, Philadelphia,PA,USA (ECMR 2007, Freiburg) PART 1.1Force Field Based n-ScanAlignment

4. The FFS (Force Field Simulation) algorithm tries to solve the problem of robot mapping under the challenging conditions of: • (Extremely) poor pre-alignment • lack of landmarks and • minimal overlap between scans

5. Motivation: • FFS was designed to solve mapping problems in typical rescue scenarios to enable the deployment of autonomous robots for search and rescue tasks.

6. Currently these robots are remotely controlled. The mandatory connection between the operator and the vehicle, as well as the limited field of view cause problems. Robots deployed after the 9/11 attack. Interviews with the rescue teams revealed heavy limitations in the usability.

7. To simulate scenarios like these, a Rescue Robot Exercise was held 2009 in DISASTER CITY, Texas

9. The following slide shows 4 example scans of a typical data set from a disaster arena set up and scanned by NIST, 2006 • The entire data set consists of 64 scans, taken from 16 different positions • no odometry or additional sensor data is provided • No scan order is given (simulation of 16 different robots taking 4 scans each)

10. (NIST disaster data set, 4 scans out of 64)

11. The scans are superimposed on top of each other (but not merged) to gain a global map. Scans 1..n G L O B A L M A P

12. A shape based algorithm resulted in a rough estimation of the robots’ poses for an initial global map

13. Results first: • FFS iteratively translates/rotates the single scans to align them to a more visually consistent global map.

14. FFS problem statement: • A global map is a 3n-dimensional vector defining the poses (x,y,a) of the n robots (e.g. in the NIST example we assume 64 independent robots). • Given a scalar fitness target function P on the 3n dimensional space M of global maps, find vector m in M which minimizes P (P defines the quality of alignment).

15. FFS is motivated by physics: each scan is seen as a rigid body, consisting of masses (the laser reflection points). • A global map is then a system of rigid bodies. Forces between the masses of the different scans create a force field. • FFS simulates the movement of the bodies in this force field. • FFS is a gradient descent approach. It minimizes the force field’s potential P, which resembles the visual fitness.

16. The basic principle: corresponding to the forces, scans are iteratively translated/rotated. • By laws of physics, such a system converges towards a (local) minimum of its potential

17. The following questions must be answered: • How to design the forces, such that the overlying potential describes an appropriate fitness function for alignment • How to robustly design the system with respect to local minima • How to overcome the O(n2) complexity of a brute force approach (=pair wise computation of all forces between all n scan points)

18. How to design the forces, such that the overlying potential describes an appropriate fitness function for alignment

19. Design of the Force Function • Goal: design force between 2 data points (laser reflection points) • A correspondence V between points p1, p2 is defined as a vector: • C(p1,p2) describes the strength of correspondence:

20. Strength of correspondence • Gauss function of distance • Mass (or weight) of points • Direction of points

21. Two points p1,p2 correspond strongly, if… • Their euclidean distance is small The influence of distance is steered by the σt parameter of the Gauss function. • Their mass (perceptual weight) is high • The underlying linear structures are parallel

22. Underlying linear structures: • The data is modeled using line segments • The direction of a point is the direction of the closest line segment • Points on parallel segments correspond strongly

23. The force acting on each point is the sum of correspondences • The overlying potential is defined by • with

24. FFS is a gradient descent approach to minimize this potential. • FFS computes forces between points, and the resulting forces on the rigid bodies (scans). • The resulting acceleration for translation, inertia and torque for rotation are computed. (These are the gradient vectors!) • The resulting transformation is performed, using a dynamic step width dT.

25. A simple example: Left: initial configuration, showing forces in each data point Middle: after translation Right: after 15 iterations

26. FFS computes the transformations for all scans simultaneously. • FFS does not take into account any time order of the scans, it is therefore applicable to multi robot mapping • FFS can easily be changed to an online, sequential, (single robot) mapping process: each new scan is just added to the previous set of scans. To reduce computational time, the previous global map can be fixed if necessary.

27. The following questions must be answered: • How to design the forces such that the overlying potential describes an appropriate fitness function for alignment • How to robustly design the system with respect to local minima • How to overcome the O(n^2) complexity of a brute force approach (=pair wise computation of all forces between scan points)

28. FFS contains 2 steering parameters: • σT, defining the influence of distance in the force computation • dT, the step width of the gradient descent in each iteration.

29. Both parameters are exponentially decreasing during the iterative process. • This simulates a cooling process, similar to cooling processes known from strategies like simulated annealing etc. • Since σT defines the radius of influence of data points, the process changes from global influence (large σT) to local influence (small σT). • Decreasing dT allows the system to jump out of local minima in the beginning (large dT), while allowing precise adjustment at the end (small dT)

30. Using this cooling strategy, experiments showed a very robust behavior of FFS with respect to local minima. • Typically, the potential function is not monotonically decreasing, indicating escapes from possible local minima.

31. How to overcome the O(n2) complexity of a brute force implementation

32. The definition of the correspondence function leads to a complexity of O(n2), n=number of data points if implemented straightforwardly. • To reduce the complexity, different techniques can be used, taking advantage of 2 properties of the correspondence function

33. For each data point, only its local neighborhood (determined by σt) must be examined. Techniques to reduce complexity: • KD-trees • Bounding box trees Current implementation: bounding box tree collision on the underlying line segments. Reduces complexity to an expected O(m), m = number of line segments.

34. Some numbers: • NIST 2006 data set contains 21420 data points, can be modeled by 332 line segments (average: 65 points per segment) • Average number of colliding segments per iteration: 1500 • Computations: 65x65x1500 = 6,337,500 (compared to 214202=460,000,000)

35. Second complexity reduction step: Data points have to be evaluated with certain accuracy only. Approximating the force field suggests 2 techniques to reduce the complexity: • Sub sampling the linear segments, to lower the number of points (and to achieve a uniform point density!) • Fast Gauss Transform (FGT, Greengard and Strain, 1991), leading to O(n) with a constant factor depending on precision. Current implementation: resampling.

36. Some numbers: • Resampling reduces the average number of data points per segment to 7 (compared to 65). The resulting number of computations is therefore 7x7x1500=73,500. • Note: resampling also reduces the space complexity: only the line segments must be stored.

37. Experiments and Results • 1.NIST 06 Data Set

40. Experiments and Results • 2. Apartment Data Set • http://staff.science.uva.nl/~zivkovic/FS2HSC/dataset.html • 2000 scans taken in an apartment of size 16x8m • We used every 10th scan • Pre alignment based on shape

41. Initial configuration shows error (encircled) due to incorrect loop closing and is very imprecise (blurry features)

42. A large step width dT blurs the map in the first iteration steps to escape local minima

43. Iteration 50

44. Final (Iteration 150): FFS has contracted the edges and has realigned the entire map to fix the loop closing error

45. (Movie of FFS)

46. Rolf Lakaemper, Nagesh Adluru Temple University, Philadelphia,PA,USA IROS 2008 Part 1.2Improving Sparse Laser Scan Alignment with Virtual Scans

47. Problem: too little information for FFS + = + =

48. Ambiguous information

49. Solution: • augment laser data with hypotheses about existing objects (expected structures) : add hypotheses to the data set as if they were real data. • We call the hypothetical data Virtual Scans.

50. Example 1 + =