Switchable Constraints for Robust Simultaneous Localization and Mapping and Satellite-Based Localization

Series Editor’s Foreword
Preface
Contents
Symbols and Notation
	General Mathematical Notation
	Roman Letters
	Greek Letters
	Special Abbreviations in Mathematical Expressions
1 Introduction
	References
2 Simultaneous Localization and Mapping
	2.1 S, L, and M—The Parts of SLAM
		2.1.1 M for Mapping
		2.1.2 L for Localization
		2.1.3 S for Simultaneous
	2.2 Graph Representations for SLAM
		2.2.1 Dynamic Bayesian Networks
		2.2.2 Factor Graphs
		2.2.3 Markov Random Fields
	2.3 SLAM as a Nonlinear Least Squares Optimization Problem
		2.3.1 The Pose Graph SLAM Problem
		2.3.2 Deriving a Nonlinear Least Squares Formulation
		2.3.3 An Intuitive Analogy for the Least Squares Optimization
		2.3.4 Optimization-Based SLAM—A Literature Review
	2.4 Summary
	References
3 Least Squares Optimization
	3.1 Introduction
		3.1.1 A Taxonomy of Optimization Problems
		3.1.2 Least Squares Optimization Problems
	3.2 Linear Least Squares Problems
		3.2.1 Solving Linear Least Squares Problems
		3.2.2 Examples for Linear Least Squares Problems
	3.3 Nonlinear Least Squares Problems
		3.3.1 Gradient Descent
		3.3.2 Newton's Method
		3.3.3 Gauss-Newton
		3.3.4 Levenberg-Marquardt
		3.3.5 Summary
	3.4 Weighted Nonlinear Least Squares Problems
	3.5 Least Squares Optimization for SLAM
		3.5.1 A Sandbox Example
		3.5.2 Why Optimization-Based SLAM Is Efficient
	3.6 Least Squares Optimization in the Presence of Outliers
		3.6.1 Sample Consensus Methods for Outlier Rejection
		3.6.2 Robust Cost Functions
	3.7 Summary
	References
4 Motivation—When Optimization Fails
	4.1 Data Association Errors and Their Effects On SLAM
	4.2 Current Approaches for Outlier Mitigation and Avoidance
		4.2.1 Outlier Avoidance on the Front-End Side
		4.2.2 Outlier Mitigation on the Back-End Side
	4.3 Summary
	References
5 A Robust Back-End for SLAM
	5.1 The Robustified Formulation for Pose Graph SLAM
		5.1.1 First Steps Towards a Mathematical Formulation
		5.1.2 Introducing the Switch Variables and Finding a Suitable Switch Function
		5.1.3 Introducing the Switch Priors
		5.1.4 Putting It All Together
	5.2 Discussion
		5.2.1 The Influence of sij on the Information Matrix Λij
		5.2.2 Establishing the Connection to the Maximum a Posteriori Solution
		5.2.3 The Influence of the Additional Variables and Constraints on The Problem Size
		5.2.4 The Influence of the Additional Variables and Constraints on The Sparse Structure of the Problem
		5.2.5 The Influence of the Additional Variables and Constraints on The Problem's Convergence Properties
	5.3 Summary and a First Working Example
	References
6 Evaluation
	6.1 Error Metrics for SLAM
		6.1.1 The Root-Mean-Square Error (RMSE)
		6.1.2 The Relative Pose Error Metric
		6.1.3 Precision-Recall Statistics
	6.2 Datasets for the Evaluation
	6.3 General Methodology
		6.3.1 Policies for Adding Outlier Loop Closure Constraints
		6.3.2 Loop Closure Displacement
		6.3.3 The Switch Function
		6.3.4 Used Framework and Implementation
	6.4 The Influence of  Ξij on the Estimation Results
		6.4.1 Methodology
		6.4.2 Results and Interpretation
	6.5 The Robustness in the Presence of Outliers
		6.5.1 Methodology
		6.5.2 Results and Interpretation
		6.5.3 Discussion of the Failure Cases
	6.6 Runtime and Convergence Behaviour
		6.6.1 Methodology
		6.6.2 Results and Interpretation
	6.7 Performance in the Outlier-Free Case
		6.7.1 Methodology
		6.7.2 Results and Interpretation
	6.8 The Influence of the Switch Function  Ψ
	6.9 Summary of the Evaluation and First Conclusions
	References
7 Applying the Robust Back-End in a Complete SLAM System on A Real-World Dataset
	7.1 The Front-End
		7.1.1 Visual Odometry by Image Profile Matching
		7.1.2 Place Recognition Using BRIEF-Gist
	7.2 Results of the Complete SLAM System on the St. Lucia Dataset
	7.3 Summary
	References
8 Applications Beyond SLAM—Multipath Mitigation in GNSS-Based Localization Problems Using the Robust Back-End
	8.1 GNSS-Based Localization—A Gentle Introduction
		8.1.1 Systematic Errors
		8.1.2 Multipath Errors
	8.2 Multipath Identification and Mitigation—Related Work
	8.3 Modelling the GNSS-Based Localization Problem as a Factor Graph
		8.3.1 The Vehicle State Vertices
		8.3.2 The Pseudorange Factor
		8.3.3 Additional Factors
		8.3.4 Solving for the Maximum a Posteriori Solution
	8.4 Towards a Problem Formulation Robust to Multipath Errors
		8.4.1 The Switched Pseudorange Factor
		8.4.2 The Switch Transition Factor
	8.5 Multipath Mitigation in a Real-World Urban Scenario
		8.5.1 The Chemnitz City Dataset
		8.5.2 Methodology
		8.5.3 Results
	8.6 Interpretation and Summary
	8.7 Outlook
	References
9 An Outlook on Robust Optimization for Sensor Fusion and Calibration
	9.1 Sensor Fusion by Robust Optimization
	9.2 Sensor Calibration by Robust Optimization
	References
10 Conclusions
	10.1 What Has Been Achieved—Contributions of This Thesis
	10.2 Open Questions—An Outlook on Further Work
		10.2.1 Parameters of the Proposed Robust Formulation
		10.2.2 Convergence Behaviour and the Dangers of Local Minima
		10.2.3 Further Applications of the Robust Approach
	References
11 Retrospective
	11.1 Progress in Robust SLAM—From Heuristics and M-Estimators to Certifiable Robust Optimization
	11.2 General Progress in SLAM
	References