Creating Highly Accurate 3D Representations of Large-Scale Outdoor Areas Using SLAM Algorithms

218 Pages·2011·16.34 MB·English

Diploma Thesis
in Informatics
Creating Highly Accurate 3D
Representations of Large-Scale
Outdoor Areas Using SLAM
Algorithms
at the Department of
Technical Aspects of Multimodal Systems,
University of Hamburg
submitted by
Jan Christoph Gries
Jan Girlich
June 3, 2011
supervised by
Prof. Dr. Jianwei Zhang
Dr. Kay Fu¨rstenberg

Abstract
Heavy transports are very important undertakings that due to their cost and risk
factors require careful planning. The project 3D-Heavy Transport Route Finder
(3D-HTRF) was developed by Gustav Seeland GmbH and SICK AG, which resulted
in a car equipped with laser range finders and highly accurate, but expensive inertial
navigation systems to build maps of possible transportation routes for quicker
and cheaper scouting of routes. This work takes a look at the accuracy of the
generated maps, as well as suggest and evaluates a method to further improve the
maps. FastSLAM is a method from the field of robotics and originally developed for
localization and mapping. We adopted the FastSLAM algorithm for 3D application
and for using it to improve the match of laser scan data in a post processing step.
The evaluation shows that, while the basic concept works, FastSLAM cannot help
improving the maps of the 3D-HTRF project using the existing hardware setup. The
reasons why are discussed and suggestions on how to overcome the problems faced
are made.
iii

Contents
Abstract iii
1 Motivation 1
1.1 3D Heavy Transport Route Finder (3D-HTRF) . . . . . . . . . . . . 2
1.2 Challenges of 3D-HTRF . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Experimental comparison of position data sources . . . . . . . . . . . 4
1.3.1 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . 4
1.3.2 Experimental setup . . . . . . . . . . . . . . . . . . . . . . . . 4
1.3.3 Conduct of the experiment . . . . . . . . . . . . . . . . . . . . 5
1.3.4 Experimental results . . . . . . . . . . . . . . . . . . . . . . . 5
1.3.5 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.4 Expanding 3D-HTRF to the field of robotics . . . . . . . . . . . . . . 9
1.5 Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.6 Simultaneous Localization and Mapping to improve map accuracy . . 10
1.6.1 Iterative Closest Point (ICP) . . . . . . . . . . . . . . . . . . . 11
1.6.2 Extended Kalman Filter (EKF) . . . . . . . . . . . . . . . . . 13
1.6.3 FastSLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.7 Contribution of this thesis . . . . . . . . . . . . . . . . . . . . . . . . 14
1.7.1 Reducing financial costs of the current system . . . . . . . . . 14
1.7.2 Creating maps of higher accuracy using the 3D-HTRF hardware 14
1.7.3 Code quality vs. runtime . . . . . . . . . . . . . . . . . . . . . 15
1.8 Summary & Overview . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2 Hardware 17
2.1 Car . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.2 Laser Range Finder . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.3 Position data sources . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
2.3.1 Car sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.3.2 Xsens MTi-G . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
2.3.3 Oxford Technical Solutions RT3040 . . . . . . . . . . . . . . . 23
2.4 System costs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3 State of the Art 25
3.1 The problem of Simultaneous Localization And Mapping . . . . . . . 25
3.2 History of SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
v
Contents
3.3 Common sensors used with SLAM . . . . . . . . . . . . . . . . . . . . 27
3.3.1 LIght Detection And Ranging (LIDAR) . . . . . . . . . . . . . 28
3.3.2 Camera . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.3.3 Radar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.3.4 Sonar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.4 Iterative Closest Point . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.5 Analytical approaches . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.5.1 (Discrete) Kalman Filter . . . . . . . . . . . . . . . . . . . . . 30
3.5.2 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . 32
3.5.3 Further Kalman Filter . . . . . . . . . . . . . . . . . . . . . . 33
3.6 Numerical Approaches . . . . . . . . . . . . . . . . . . . . . . . . . . 34
3.7 FastSLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.8 Related work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.9 Other SLAM related research topics . . . . . . . . . . . . . . . . . . . 36
3.9.1 Loop-Closing . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
3.9.2 Relaxation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
3.9.3 Kidnapped Robot Problem . . . . . . . . . . . . . . . . . . . . 37
3.9.4 Global and local localization . . . . . . . . . . . . . . . . . . . 37
3.10 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
4 Theoretical background 39
4.1 Laser Range Finder (LRF) . . . . . . . . . . . . . . . . . . . . . . . . 39
4.1.1 Infrared light . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
4.1.2 Time of flight . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
4.1.3 Frequency phase-shift . . . . . . . . . . . . . . . . . . . . . . . 41
4.1.4 Echo pulse width . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.2 Inertial Navigation System (INS) . . . . . . . . . . . . . . . . . . . . 43
4.2.1 Inertial Measurement Unit (IMU) . . . . . . . . . . . . . . . . 43
4.2.2 Global Positioning System (GPS) . . . . . . . . . . . . . . . . 43
4.3 Quality of a map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.4 Calculation complexity . . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.5 Relaxation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
4.6 Landmarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
4.7 FastSLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
4.7.1 Particle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
4.7.2 Predicted particle pose . . . . . . . . . . . . . . . . . . . . . . 52
4.7.3 Observation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
4.7.4 Landmark associations . . . . . . . . . . . . . . . . . . . . . . 53
4.7.5 Likelihood . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
4.7.6 Extended Kalman filter . . . . . . . . . . . . . . . . . . . . . . 55
4.7.7 Particle Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
4.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
vi
Contents
5 Implementation 61
5.1 Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
5.1.1 World Geodetic System 1984 (WGS84) . . . . . . . . . . . . . 61
5.2 Program layout . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
5.2.1 AppBase configuration . . . . . . . . . . . . . . . . . . . . . . 64
5.2.2 AppBase data types . . . . . . . . . . . . . . . . . . . . . . . 64
5.2.3 Object File Format (OFF) . . . . . . . . . . . . . . . . . . . . 65
5.2.4 OFF file viewer . . . . . . . . . . . . . . . . . . . . . . . . . . 65
5.3 Preprocessing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
5.3.1 Street border cutter . . . . . . . . . . . . . . . . . . . . . . . . 67
5.3.2 Street surface marking detector . . . . . . . . . . . . . . . . . 67
5.3.3 Echo pulse width cutter . . . . . . . . . . . . . . . . . . . . . 68
5.3.4 Clustering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
5.3.5 Slicing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
5.4 FastSLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
5.4.1 Particle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
5.4.2 Predicted particle pose . . . . . . . . . . . . . . . . . . . . . . 73
5.4.3 Likelihood table . . . . . . . . . . . . . . . . . . . . . . . . . . 74
5.4.4 Associate Observations with Landmarks . . . . . . . . . . . . 76
5.4.5 Kalman Filter for landmark update . . . . . . . . . . . . . . . 77
5.4.6 Pseudo random number generator . . . . . . . . . . . . . . . . 80
5.5 Relaxation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
5.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84
6 Discussion 85
6.1 Design decisions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
6.1.1 FastSLAM 1.0 vs. FastSLAM 2.0 . . . . . . . . . . . . . . . . 85
6.1.2 Slicing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
6.1.3 Edge and corner detection . . . . . . . . . . . . . . . . . . . . 86
6.1.4 Landmark detection . . . . . . . . . . . . . . . . . . . . . . . 87
6.1.5 Likelihoods . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
6.1.6 Resampling . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
6.1.7 Modeling of errors and probabilities . . . . . . . . . . . . . . . 89
6.1.8 Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . 89
6.1.9 Data structures . . . . . . . . . . . . . . . . . . . . . . . . . . 89
6.1.10 Data output . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
6.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
6.2.1 Comparison of ICP to FastSLAM . . . . . . . . . . . . . . . . 92
6.2.2 Replacing an INS . . . . . . . . . . . . . . . . . . . . . . . . . 92
6.2.3 Using the Xsens MTi-G instead of the Oxford Technical Solu-
tions RT3040 . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
6.2.4 Improving maps built from Oxford Technical Solutions RT3040
data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96
vii
Contents
6.2.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96
6.3 Statistics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
6.3.1 Runtime . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
6.3.2 Memory usage . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
6.3.3 Updated Landmarks to Observations ratio . . . . . . . . . . . 100
6.4 Relaxation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
6.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
7 Outlook & Conclusion 107
7.1 Hardware Suggestions . . . . . . . . . . . . . . . . . . . . . . . . . . 107
7.1.1 LRF mounting positions . . . . . . . . . . . . . . . . . . . . . 107
7.1.2 Accuracy of timing . . . . . . . . . . . . . . . . . . . . . . . . 108
7.1.3 Accurate determination of model errors . . . . . . . . . . . . . 109
7.2 Improving FastSLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . 109
7.2.1 Landmark detection . . . . . . . . . . . . . . . . . . . . . . . 109
7.2.2 Reducing calculation complexity and memory usage . . . . . . 111
7.2.3 Probabilistic extensions . . . . . . . . . . . . . . . . . . . . . . 112
7.2.4 Output . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113
7.2.5 Relaxation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113
7.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
Acknowledgment 117
Eidesstattliche Erkl¨arung Jan Girlich 119
Eidesstattliche Erkl¨arung Jan Gries 121
Aufteilung der Gruppenarbeit 123
Bibliography 131
A FastSLAM 1.0 pseudo code 133
B AppBase configuration 135
C Preprocessing workers 137
C.1 Street border cutter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
C.2 Street surface marking detection . . . . . . . . . . . . . . . . . . . . . 144
C.3 Echo pulse width cutter . . . . . . . . . . . . . . . . . . . . . . . . . 148
C.4 Coordinate conversion . . . . . . . . . . . . . . . . . . . . . . . . . . 150
C.5 Slicing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151
D FastSLAM 159
D.1 SLAMWorker . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
viii
Contents
D.2 Landmark . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 170
D.3 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . 171
D.4 Particle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 175
D.5 Likelihood table . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 182
E Relaxation 193
F OFF viewer 197
F.1 ibeo3DVisioFileReader.cpp . . . . . . . . . . . . . . . . . . . . . . . . 197
F.2 ScanPointArray.cpp . . . . . . . . . . . . . . . . . . . . . . . . . . . . 200
ix