Localisation using the Appearance of Prior Structure Alexander D. Stewart New College Supervisor: Professor Paul Newman Mobile Robotics Group Department of Engineering Science University of Oxford October 2014 Alexander D. Stewart Doctor of Philosophy New College October 2014 Localisation using the Appearance of Prior Structure Abstract Accurate and robust localisation is a fundamental aspect of any autonomous mobile robot. However, if these are to become widespread, it must also be available at low-cost. In this thesis, we develop a new approach to localisation using monocular cameras by leveraging a coloured 3D pointcloud prior of the environment, captured previously by a survey vehicle. We make no assumptions about the external condi- tions during the robot’s traversal relative to those experienced by the survey vehicle, nor do we make any assumptions about their relative sensor configurations. Our method uses no extracted image features. Instead, it explicitly optimises for the pose which harmonises the information, in a Shannon sense, about the ap- pearance of the scene from the captured images conditioned on the pose, with that of the prior. We use as our objective the Normalised Information Distance (NID), a true metric for information, and demonstrate as a consequence the robustness of our localisation formulation to illumination changes, occlusions and colourspace transformations. We present how, by construction of the joint distribution of the appearance of the scene from the prior and the live imagery, the gradients of the NID can be computed and how these can be used to efficiently solve our formulation using Quasi-Newton methods. In order to reliably identify any localisation failures, we present a new classifier using the local shape of the NID about the candidate pose and demonstrate the performance gains of the complete system from its use. Finally, we detail the de- velopment of a real-time capable implementation of our approach using commodity GPUs and demonstrate that it outperforms a high-grade, commercial GPS-aided INS on 57km of driving in central Oxford, over a range of different conditions, times of day and year. ii Statement of Authorship This thesis is submitted to the Department of Engineering Science, University of Oxford, in fulfilment of the requirements for the degree of Doctor of Philosophy. This thesis is entirely my own work, and except where otherwise stated, describes my own research. Alex Stewart, New College Funding The work described in this thesis was funded by the Engineering and Physical Sci- ences Research Council (EPSRC). Acknowledgements Although this thesis bears my name, it would not have been possible without the support and encouragement of a great many people. Foremost amongst them is my advisorPaulNewman. Paul’sseeminglyboundlessenergy, optimismandenthusiasm for robotics and life in general have made the last four years a fantastic adventure. To have been a part of the Mobile Robotics Group (MRG) has been a privilege. Paul, now aided by Ingmar, has built a truly amazing place to do robotics, filled with brilliant and friendly people and a vast array of robots & resources. Despite having grown almost threefold since I started, MRG has maintained the same great atmosphere, outlook and focus, which is a testament to Paul and Ingmar’s leadership and the attitudes of all of the members of the group, past and present. I wish to particularly thank, in no particular order: Winston Churchill, Geoff Hester, Will Maddern, Colin ‘Bear’ McManus, Alastair Harrison and Dan Withers for everything from developing the infrastructure on which the group, and thus this thesis, is built; to debugging recalcitrant robots in horrible weather at 2am, collecting apparently nuisance-free 24 hour datasets and lots of cycling. Last, but by no means least, I wish to thank my family and friends for all of their love, support and encouragement over the years. This thesis is dedicated to my parents, for their unconditional love and support, and for teaching me the wonderful things that come from hard work and the pursuit of knowledge. Contents 1 Introduction 2 1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.2 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 1.3 Structure of the Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . 6 1.4 Publications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 2 Preliminaries 7 2.1 Transformations in SE(3) . . . . . . . . . . . . . . . . . . . . . . . . . 7 2.1.1 Euler Angle Parameterisation . . . . . . . . . . . . . . . . . . 8 2.1.2 Lie Algebra Parameterisation . . . . . . . . . . . . . . . . . . 10 2.2 Cubic B-Splines . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.2.1 Cardinal Cubic B-Splines . . . . . . . . . . . . . . . . . . . . . 12 2.2.2 Cumulative form Cardinal Cubic B-Splines . . . . . . . . . . . 15 2.2.3 Cubic B-Spline Image Interpolation . . . . . . . . . . . . . . . 15 2.3 BFGS Optimisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 2.3.1 Wolfe Line Search . . . . . . . . . . . . . . . . . . . . . . . . . 17 2.3.2 BFGS Method . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 3 Localisation using the Appearance of Prior Structure 23 3.1 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 3.1.1 Assumptions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 ii CONTENTS 3.1.2 Optimisation Objective . . . . . . . . . . . . . . . . . . . . . . 27 3.1.3 Properties of f . . . . . . . . . . . . . . . . . . . . . . . 29 distance 3.2 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30 3.3 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 4 Information Metrics 38 4.1 Shannon Information . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 4.1.1 Entropy Fundamentals . . . . . . . . . . . . . . . . . . . . . . 38 4.1.2 Information Quantities . . . . . . . . . . . . . . . . . . . . . . 40 4.1.3 Entropy Equivalence . . . . . . . . . . . . . . . . . . . . . . . 42 4.1.4 Metric Spaces for Entropy Measures . . . . . . . . . . . . . . . 44 4.2 The Variation of Information . . . . . . . . . . . . . . . . . . . . . . . 45 4.2.1 Properties of the Variation of Information . . . . . . . . . . . 45 4.2.2 Equivalence Problem . . . . . . . . . . . . . . . . . . . . . . . 48 4.3 Normalised Information Distance . . . . . . . . . . . . . . . . . . . . 49 4.3.1 Normalised Information Distance Variants . . . . . . . . . . . 50 4.3.2 Properties of the Normalised Information Distance . . . . . . 50 4.3.3 Importance of Normalisation . . . . . . . . . . . . . . . . . . . 52 4.3.4 Relationship to Normalised Mutual Information . . . . . . . . 53 4.4 Application of the NID to LAPS . . . . . . . . . . . . . . . . . . . . . 54 4.4.1 Choice of Colourspace Dimensionality . . . . . . . . . . . . . . 54 4.4.2 Combining NID from Multiple Cameras . . . . . . . . . . . . 56 4.4.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57 4.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 5 Formulation of Optimisation 66 5.1 Derivatives of NID . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70 5.1.1 Gradient of NID . . . . . . . . . . . . . . . . . . . . . . . . . 70 iii CONTENTS 5.1.2 Hessian of NID . . . . . . . . . . . . . . . . . . . . . . . . . . 75 5.2 Differentiable form of Discrete PDFs . . . . . . . . . . . . . . . . . . 78 5.3 Joint Appearance Distribution . . . . . . . . . . . . . . . . . . . . . . 80 5.3.1 Spatial Weighting . . . . . . . . . . . . . . . . . . . . . . . . . 81 5.3.2 Intensity Weighting . . . . . . . . . . . . . . . . . . . . . . . . 86 5.3.3 Point Projection Jacobians . . . . . . . . . . . . . . . . . . . . 91 5.3.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93 5.3.5 Choice of Weighting Method . . . . . . . . . . . . . . . . . . . 97 5.4 Optimising NID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100 5.4.1 Levenberg-Marquardt . . . . . . . . . . . . . . . . . . . . . . . 101 5.4.2 Newton’s Method . . . . . . . . . . . . . . . . . . . . . . . . . 102 5.4.3 Quasi-Newton . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 5.4.4 Sampling Methods . . . . . . . . . . . . . . . . . . . . . . . . 104 5.4.5 Choice of Optimisation Method . . . . . . . . . . . . . . . . . 105 5.5 Point Subset Selection . . . . . . . . . . . . . . . . . . . . . . . . . . 106 5.6 Point Importance Weighting . . . . . . . . . . . . . . . . . . . . . . . 108 5.7 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114 6 Building the Scene Prior 115 6.1 Scene Prior Pipeline . . . . . . . . . . . . . . . . . . . . . . . . . . . 115 6.2 Experimental Platform . . . . . . . . . . . . . . . . . . . . . . . . . . 115 6.2.1 Sensor Calibration . . . . . . . . . . . . . . . . . . . . . . . . 120 6.3 Construction of Scene Priors . . . . . . . . . . . . . . . . . . . . . . . 121 6.4 Segmentation of Dynamic Obstacles . . . . . . . . . . . . . . . . . . . 126 6.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127 7 Implementation 128 7.1 System Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128 iv CONTENTS 7.2 Calculation of NID & Derivatives on GPUs . . . . . . . . . . . . . . . 132 7.2.1 One Point per Pixel Subsampling of . . . . . . . . . . . . . 135 S 7.2.2 Building Joint Appearance Distributions on a GPU . . . . . . 138 7.3 Optimising NID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141 7.3.1 Convergence Results . . . . . . . . . . . . . . . . . . . . . . . 145 7.4 Identifying Localisation Failures . . . . . . . . . . . . . . . . . . . . . 150 7.4.1 Classification Dataset . . . . . . . . . . . . . . . . . . . . . . . 150 7.4.2 Classification Feature Space . . . . . . . . . . . . . . . . . . . 152 7.4.3 SVM Classification of Optimiser Solution . . . . . . . . . . . . 155 7.5 Fusing LAPS with Odometry . . . . . . . . . . . . . . . . . . . . . . 163 7.5.1 Spline Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . 166 7.5.2 Fusing LAPS with Odometry using Spline Fusion . . . . . . . 172 7.6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 175 8 Localisation using LAPS 176 8.1 Oxford Datasets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 176 8.2 Evaluation Metrics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181 8.2.1 Optimised Ground Truth . . . . . . . . . . . . . . . . . . . . . 182 8.2.2 LAPS Initialisation and Restarts . . . . . . . . . . . . . . . . 184 8.2.3 Explanation of Figures . . . . . . . . . . . . . . . . . . . . . . 185 8.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 188 8.3.1 Summary Results . . . . . . . . . . . . . . . . . . . . . . . . . 189 8.3.2 Traversal: Oxford A1 Prior: Oxford A1 . . . . . . . . . . . 194 → 8.3.3 Traversal: Oxford A2 Prior: Oxford A1 . . . . . . . . . . . 197 → 8.3.4 Traversal: Oxford B2 Prior: Oxford B1 . . . . . . . . . . . 200 → 8.3.5 Traversal: Oxford B1 Prior: Oxford B2 . . . . . . . . . . . 203 → 8.3.6 Traversal: Oxford B3 Prior: Oxford B1 . . . . . . . . . . . 206 → v CONTENTS 8.3.7 Traversal: Oxford B1 Prior: Oxford B3 . . . . . . . . . . . 209 → 8.3.8 Traversal: Oxford B3 Prior: Oxford B3 . . . . . . . . . . . 212 → 8.3.9 Traversal: Oxford A1 Prior: Oxford B1 . . . . . . . . . . . 215 → 8.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 218 8.4.1 LAPS Localiser Performance . . . . . . . . . . . . . . . . . . . 218 8.4.2 Solution Classifier Performance . . . . . . . . . . . . . . . . . 220 8.4.3 Failure Cases . . . . . . . . . . . . . . . . . . . . . . . . . . . 220 8.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 224 9 Conclusions 225 9.1 Summary of Contributions . . . . . . . . . . . . . . . . . . . . . . . . 225 9.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 226 9.3 Concluding Remark . . . . . . . . . . . . . . . . . . . . . . . . . . . . 230 A Levenberg-Marquardt Equivalence to Gradient Descent for a Single Residual 241 1 Chapter 1 Introduction 1.1 Motivation The guiding aim of this thesis is to contribute to the development of a robust, low-cost localisation system for autonomous mobile robots operating outdoors. Lo- calisation is a fundamental aspect of any autonomous mobile robot. Without an accurate estimate of where it is, a robot cannot successfully navigate in its envi- ronment and is thus by definition not mobile, at least not in any controlled sense. Furthermore, without an accurate localisation system a robot cannot index into any prior information available about the environment, whether captured by itself or by others, greatly increasing the online complexity of tasks and so limiting the practical capabilities of the robot. Whilst localisation is a fundamental task, it is not an easy one; particularly outdoors at large-scales. Outdoors, illumination is uncontrolled and highly variable. Appearance changes at all temporal scales, from dynamic shadows, the day-night cycle and local weather conditions through to longer term seasonal and structural changes, for example the construction of a new building, or the destruction of an old one. Operating at large scales necessitates a robust solution. In order that the robot 2