1. Laser Scan Matching in Polar Coordinates with Application to SLAM
Albert Diosi
Lindsay Kleeman
ARC Centre for Perceptive and Intelligent Machines in Complex Environments
Dept. of Electrical and Computer Systems Engineering
Monash University, Australia

2. Overview New 2D laser scan match method presented.
Polar Scan Matching (PSM) – point to point scan match approach in polar coords.
Avoids searching for point correspondences by simply matching bearing.
Faster than Iterated Closest Point (ICP) method.
SLAM implementation demonstrated with PSM.

3. Previous Work
Scan matching can be categorized by the association method:
Feature to feature,
eg line segments [Gutman PhD 2000] or range extrema [Lingemann et al IROS 2004]
Point to feature.
Eg [Cox IEEE R&A 1991]
Point to point.
Least dependent on environment and chosen here.

4. Point to Point Literature
Iterative Closest Point (ICP)
[Besl and McKay IEEE PAMI 1992]
Iterative Matching Range Point (IMRP)
[Lu and Milios JIRS 1997]
Iterative Dual Correpondence (IDC)
Uses IMRP for rotation and ICP for translation in each iteration [Lu and Milios JIRS 1997].
One iteration required if associations known.
Requires iterations in practice since correct associations are initially unknown.

5. Associations are the Key!
IDC needs to associate each point in the new scan with one in the reference scan.
Each point can require all points in other scan to be checked => O(n2) computation.
In practice some search restrictions can reduce this computation.
Polar Scan Matching (PSM) in this paper just needs to check the same angle of a transformed scan.

6. Polar Scan Matching PSM has these steps: Scan Preprocessing
Scan Projection
Translation Estimation
Orientation Estimation
Error Covariance Estimation

7. PSM Preprocessing
Points not suitable for matching are removed by median filtering:
Chair and people’s legs
Mixed pixels caused by range discontinuities
Maximum range measurements (ie no object) removed by thresholding

8. Segmentation of objects
Interpolation between distinct objects avoided
Possibility of tracking and deleting moving objects between successive scans.
New segment when consecutive difference > 20 cm
Same segment when 3 measurements on nearly same polar line.

9. Raw Scan Example

10. Preprocessed Scan

11. Scan Projection Transform current scan into reference scan frame.
Current scan transformed
To reference frame
Reference scan origin

12. Scan Projection (cont’d)
Occlusion handled by over-writing further ranges.
Points are occluded by nearer scan points.
Ranges at reference scan positions are calculated by linear interpolation in polar coords. Shown as
Ranges are tagged invisible if in opposite order

13. Translation Estimation
Aim is to find new that minimises
Association of ranges trivial – just use the bearing.
Linearized regression used over a few iterations – see paper for details.
Sigmoid weight that favours small errors [Dudek2000]
reference
current projected

14. Orientation Estimation
Change of orientation is simply a shift in polar coords.
The shift with min absolute average range error chosen.
Parabolic interpolation applied to find sub angle sample estimate using 3 samples centred on min.

15. Error behaviour – simulated data
Scan initial mismatch

16. Error Estimation
If correct associations are known, error covariance can be estimated from the linear regression analysis.
In practice this covariance is optimistic since inevitable errors of association and moving objects cause larger errors.
Heuristic diagonal covariance used based on average range residual except in “corridors”.
Corridors are identified by the variance of orientations of lines segments < threshold.
Non-diagonal covariance used for corridors.

17. Corridor Drift
Corridor environments tend to cause biased matches:

18. Ground Truth Experiments
Various positions and orientations of a laser template were marked on a sheet.
Measurements from known relative positions of the sheet were taken of different environments using Sick LMS 200
Displacements of 80 cm and up to 27°

19. Ground truth experiments

20. Ground truth matches: PSM

21. Ground truth matches: ICP

22. PSM Error Behaviour X error o Y error Angle error +cm
deg
msec
Iteration number
X error o
Y error
Angle error +
Note corridor scene fails to converge

23. ICP Error behaviour X error o Y error Angle error +cm
deg
msec
Iteration number
X error o
Y error
Angle error +
Note corridor scene fails to converge

24. Regions of Convergence
o = no convergence
o = incorrect convergence point
+ = true shift point + +
PSM
ICP

25. Areas of Convergence Averaged over 10 scenes with initial 12º offset
Convergence defined as within 10 cm and 2º of true position
PSM converged from 2.99 m2
ICP converged from 2.80 m2

26. SLAM Implementation

27. SLAM
EKF based SLAM implemented similar to [Bosse,Newman,Leonard &Teller IJRR2004]
Laser scans are used as landmarks.
New scans stored every 1 m of robot and SLAM updated every 1 m or 15º
Scan matching acts as a landmark measurement.
Odometry used over short distances.

28. Polar Scan Matching SLAM
10 meters

29. Iterative Closest Point (ICP) SLAM
10 meters

30. Polar Scan Matching SLAM Video (2 min)
Average scan match time during SLAM on a Celeron 900 MHz laptop was:
ICP: 65 matches successful at 12.6 msec each.
PSM: 100 matches successful at 3.1 msec each.

31. Conclusions and Future Work
Presented a new scan match method that works in native Polar coordinates called Polar Scan Matching – code will be made available on www – see paper.
Removal of occluding points simply done based on further range.
PSM ~4 times faster than ICP.
PSM SLAM successfully demonstrated.

32. Future Work
Comparison of PSM with IDC and other scan matching approaches.
Tracking and removal of moving segments to improve robustness.
Funding of the ARC Centre for Perceptive and Intelligent Machines in Complex Environments is acknowledged.