SLAM technology in autonomous driving and robotics: from theory to practice (Gao Xiang)

\"SLAM Technology in Autonomous Driving and Robotics: From Theory to Practice\" [2] discusses the classic Kalman filter and modern pre-integration and graph optimization theory. The book is logically complete and self-consistent, and the content is easy to understand. It can be used as a textbook in the field of autonomous driving and robot positioning, and is suitable for students, teachers, and researchers who are interested in this field. Part I Basic Mathematics 1 Chapter 1 Autonomous Driving 3 1.1 Autonomous Driving Technology 3 1.1.1 Autonomous Driving Capabilities and Classification 3 1.1.2 Typical L4 Services 6 1.2 Positioning and Mapping in Autonomous Driving 10 1.2.1 Why L4 Autonomous Driving Needs Positioning and Mapping 10 1.2.2 Contents and Production of High-Precision Maps 12 1.3 Order of Introduction 14 Chapter 2 Review of Basic Mathematics 17 2.1 Geometry 19 2.1.1 Coordinate System 19 2.1.2 Lie Groups and Lie Algebras 26 2.1.3 BCH Linear Approximation on SO(3) 27 2.2 Kinematics 27 2.2.1 Kinematics from the Perspective of Lie Groups 28 2.2.2 Kinematics from the Perspective of Quaternions 29 2.2.3 Conversion between Quaternion Lie Algebra and Rotation Vector 30 2.2.4 Other Kinematic Expressions 32 2.2.5 2.2.1 The IMU Measurement Model 51 3.2.2 The IMU Measurement Model 51 3.2.3 The IMU Measurement Model 53 3.2.4 The IMU Measurement Model 54 3.2.5 The IMU Measurement Model 55 3.2.6 The IMU Measurement Model 55 3.2.7 The IMU Measurement Model 56 3.2.8 The IMU Measurement Model 56 3.2.9 The IMU Measurement Model 57 3.3 The IMU Measurement Model 58 3.4 The IMU Measurement Model 59 3.5 The IMU Measurement Model 60 3.3 Satellite Navigation 61 3.3.1 GNSS Classification and Suppliers 61 3.3.2 Practical RTK Installation and Data Reception 63 3.3.3 Common World Coordinate Systems 64 3.3.4 Display of RTK Readings 66 3.4 Integrated Navigation Using Error State Kalman Filter 72 3.4.1 ESKF Mathematical Derivation 72 3.4.2 Discrete Time ESKF Equations of Motion 77 3.4.3 ESKF Motion Process 78 3.4.4 ESKF Update Process 79 3.4.5 ESKF Error State Post-Processing 80 3.5 Integrated Navigation Using ESKF 82 3.5.1 ESKF Implementation 82 3.5.2 Prediction Process 83 3.5.3 RTK Observation Process 84 3.5.4 ESKF System Initialization 87 3.5.5 Running ESKF 90 3.5.6 Velocity Observables 95 3.6 Chapter Summary 98 Exercises 98 Chapter 4 Pre-integration Mathematics 99 4.1 Pre-integration Mathematics of IMU States 101 4.1.1 Definition of Pre-integration 101 4.1.2 Pre-integration Measurement Model 103 4.1.3 Pre-integration Noise Model 106 4.1.4 Bias Update 109 4.1.5 Pre-integration Model Reduced to Graph Optimization 112 4.1.6 Pre-integration Jacobian Matrix 113 4.1.7 Summary 115 4.2 Practice: Pre-integration Program Implementation 116 4.2.1 Implementing Pre-integration Class 116 4.2.2 Graph Optimized Vertices of Pre-integration 120 4.2.3 Graph Optimized Edges of Pre-integration Scheme 121 4.2.4 Implementing GINS Based on Pre-integration and Graph Optimization 126 4.3 Chapter Summary 133 Exercises 133 Part II LiDAR Positioning and Mapping 135 Chapter 5 Basic Point Cloud Processing 137 5.1 LiDAR Sensor and Point Cloud Mathematical Model 139 5.1.1 LiDAR Sensor Mathematical Model 139 5.1.2 Point Cloud Representation 141 5.1.3 Packet Representation 143 5.1.4 Top View and Distance Map 144 5.1.5 Other Representations 148 5.2 Nearest Neighbor Problem 148 5.2.1 Brute Force Nearest Neighbor Method 149 5.2.2 Grid and Voxel Method 152 5.2.3 Binary Tree and Kd Tree 160 5.2.4 Quadtree and Octree 172 5.2.5 Other Tree Methods 179 5.2.6 Summary 180 5.3 Fitting Problem 181 5.3.1 Plane Fitting 181 5.3.2 5.3.3 Line Fitting 185 5.3.4 Line Fitting 187 5.4 Chapter Summary 189 Exercises 190 Chapter 6 2D SLAM 191 6.1 Basic Principles of 2D SLAM 193 6.2 Scan Matching Algorithm 195 6.2.1 Point-to-Point Scan Matching 195 6.2.2 Point-to-Point ICP Implementation (Gauss-Newton Method) 199 6.2.3 Point-to-Line Scan Matching Algorithm 203 6.2.4 Point-to-Line ICP Implementation (Gauss-Newton Method) 204 6.2.5 Likelihood Field Method 207 6.2.6 Likelihood Field Method Implementation (Gauss-Newton Method) 209 6.2.7 Likelihood Field Method Implementation (g2o) 212 6.3 Occupancy Grid Map 215 6.3.1 Principle of Occupancy Grid Map 215 6.3.2 Map Generation Based on Bresenham Algorithm 216 6.3.3 Map Generation Based on Template 218 6.4 Submap 223 6.4.1 Principle of Submap 223 6.4.2 Implementation of Submap 224 6.5 Loop Detection and Loop Closure 228 6.5.1 Multi-resolution Loop Detection 229 6.5.2 Loop Correction Based on Submap 233 6.5.3 Discussion 238 6.6 Summary of this Chapter 241 Exercises 241 Chapter 7 3D SLAM 243 7.1 Working Principle of Multi-line LiDAR 245 7.1.1 Mechanical Rotating LiDAR 245 7.1.2 Solid-state LiDAR 246 7.2 Scan Matching of Multi-line LiDAR 248 7.2.1 Point-to-point ICP 248 7.2.2 Point-to-Line and Point-to-Plane ICP 254 7.2.3 NDT Method 258 7.2.4 Comparison of Various Registration Methods in This Section with PCL Built-in Methods 265 7.3 Direct LiDAR Odometry 267 7.3.1 Using NDT to Construct LiDAR Odometry 267 7.3.2 Incremental NDT Odometry 273 7.4 Feature-Based LiDAR Odometry 280 7.4.1 Feature Extraction 280 7.4.2 Feature Extraction Based on LiDAR Lines 280 7.4.3 Implementation of Feature Extraction 281 7.4.4 Implementation of Feature-Based LiDAR Odometry 286 7.5 Loosely Coupled LIO System 293 7.5.1 Coordinate System Description 293 7.5.2 Motion and Observation Equations of Loosely Coupled LIO System 294 7.5.3 Data Preparation of Loosely Coupled LIO System 294 7.5.4 301 7.5.5 Registration of Loosely Coupled LIO System 306 7.6 Chapter Summary 308 Exercises 308 Part III Application Examples 309 Chapter 8 Tightly Coupled LIO System 311 8.1 Principles and Advantages of Tight Coupling 313 8.2 IEKF-Based LIO System 313 8.2.1 IEKF State Variables and Equations of Motion 313 8.2.2 Iteration Process in the Observation Equation 315 8.2.3 Equivalent Processing of High-Dimensional Observations 317 8.3 Implementation of IEKF-Based LIO System 319 8.4 Pre-integration-Based LIO System 321 8.4.1 Principle of Pre-integration LIO System 321 8.4.2 Code Implementation 325 8.5 Chapter Summary 328 Exercises 328 Chapter 9 Offline Map Construction for Autonomous Driving Vehicles 329 9.1 Point Cloud Mapping Process 331 9.2 Front-end implementation 332 9.3 Backend Pose Graph Optimization and Outlier Detection 337 9.4 Loop Detection 339 9.5 Map Export 345 9.6 Chapter Summary 347 Exercises 348 Chapter 10 Real-Time Positioning System for Autonomous Driving Vehicles 351 10.1 Design of Point Cloud Fusion Positioning 353 10.2 Algorithm Implementation 354 10.2.1 RTK Initial Search 354 10.2.2 Peripheral Test Code 358 10.3 Chapter Summary 360 Exercises 361 References 363