Create Presentation
Download Presentation

Download Presentation

Towards a Segment Based Mapping System

Download Presentation
## Towards a Segment Based Mapping System

- - - - - - - - - - - - - - - - - - - - - - - - - - - E N D - - - - - - - - - - - - - - - - - - - - - - - - - - -

**Rolf Lakaemper**Temple University, Philadelphia,PA,USA Towards a Segment Based Mapping System**Sorry... since I met some of you before the talk already, I**changed the topic to...**Rolf Lakaemper, Nagesh Adluru**Temple University, Philadelphia,PA,USA (ECMR 2007, Freiburg) PART 1.1Force Field Based n-ScanAlignment**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**Motivation:**• FFS was designed to solve mapping problems in typical rescue scenarios to enable the deployment of autonomous robots for search and rescue tasks.**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.**To simulate scenarios like these, a Rescue Robot Exercise**was held 2009 in DISASTER CITY, Texas**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)**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**A shape based algorithm resulted in a rough estimation of**the robots’ poses for an initial global map**Results first:**• FFS iteratively translates/rotates the single scans to align them to a more visually consistent global map.**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).**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.**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**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)**How to design the forces, such that the overlying potential**describes an appropriate fitness function for alignment**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:**Strength of correspondence**• Gauss function of distance • Mass (or weight) of points • Direction of points**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**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**The force acting on each point is the sum of correspondences**• The overlying potential is defined by • with**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.**A simple example:**Left: initial configuration, showing forces in each data point Middle: after translation Right: after 15 iterations**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.**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)**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.**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)**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.**How to overcome the O(n2) complexity of a brute force**implementation**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**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.**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)**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.**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.**Experiments and Results**• 1.NIST 06 Data Set**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**Initial configuration shows error (encircled) due to**incorrect loop closing and is very imprecise (blurry features)**A large step width dT blurs the map in the first iteration**steps to escape local minima**Final (Iteration 150): FFS has contracted the edges and has**realigned the entire map to fix the loop closing error**Rolf Lakaemper, Nagesh Adluru**Temple University, Philadelphia,PA,USA IROS 2008 Part 1.2Improving Sparse Laser Scan Alignment with Virtual Scans