Truncated Signed Distance Fields Applied To Robotics Örebro Studies in Technology 76 Daniel Ricão Canelhas Truncated Signed Distance Fields Applied To Robotics Cover image: Daniel Ricão Canelhas © Daniel Ricão Canelhas, 2017 Title: Truncated Signed Distance Fields Applied To Robotics Publisher: Örebro University, 2017 www.publications.oru.se Printer: Örebro University/Repro 09/2017 ISSN 1650-8580 ISBN 978-91-7529-209-0 Abstract This thesis is concerned with topics related to dense mapping of large scale three-dimensional spaces. In particular, the motivating scenario of this work is one in which a mobile robot with limited computational resources explores an unknown environment using a depth-camera. To this end, low-level topics such as sensor noise, map representation, interpolation, bit-rates, compression are investigated, and their impacts on more complex tasks, such as feature detection and description, camera-tracking, and mapping are evaluated thoroughly. A central idea of this thesis is the use of truncated signed distance fields (TSDF) as a map representation and a comprehensive yet accessible treatise on this subject is the first major contribution of this dissertation. The TSDF is a voxel-based representation of 3D space that enables dense mapping with high surface quality and robustness to sensor noise, making it a good candidate for use in grasping, manipulation and collision avoidance scenarios. The second main contribution of this thesis deals with the way in which information can be efficiently encoded in TSDF maps. The redundant way in which voxels represent continuous surfaces and empty space is one of the main impediments to applying TSDF representations to large-scale mapping. This thesis proposes two algorithms for enabling large-scale 3D tracking and mapping: a fast on-the-fly compression method based on unsupervised learning, and a parallel algorithm for lifting a sparse scene-graph representation from the dense 3D map. The third major contribution of this work consists of thorough evaluations of the impacts of low-level choices on higher-level tasks. Examples of these are the relationships between gradient estimation methods and feature detector repeatability, voxel bit-rate, interpolation strategy and compression ratio on camera tracking performance. Each evaluation thus leads to a better understand- ing of the trade-offs involved, which translate to direct recommendations for future applications, depending on their particular resource constraints. Keywords: 3D mapping, pose estimation, feature detection, shape description, compression, unsupervised learning i Acknowledgements Writing a doctoral dissertation is, in spite of the evidence to the contrary (e.g. this document itself and a superabundance of others like it) not an easy task. That is the nature of survivorship bias: we cannot pile up the dissertations that didn’t make it and measure how tall a stack they make. Under the course of the past few years, I have often felt that my dissertation was close to ending up in the invisible pile of forgotten efforts. So if you, dear reader, happen to be a graduate student reading this and feeling like you are struggling beyond your means, remember: you are not alone. I urge you to reach out. Seek help. That being said, I could only have made it this far because of the unwavering support of those around me, and to them I owe an enormous debt of gratitude. As there are many people and organizations I wish to thank for reasons that range from the intimately personal to the mundanely professional, I prefer to omit the reasons why and simply state their names. I am confident that they will know why, and that is ultimately what matters to me. There are undoubtedly many others whose presence and participation during this period in my life has made the journey easier and much more enjoyable and although I cannot thank them all individually in this small space, I am grateful to them just the same. So, in no particular order but with some distinctions nonetheless, a most sincere Thank You: iii Contents 1 Introduction 1 1.1 Background ............................. 1 1.2 Contributions ............................ 3 1.3 Outline ............................... 4 1.4 List of Publications ......................... 5 1.5 Symbols and Notation ....................... 6 2 Truncated Signed Distance Fields 7 2.1 Distance Fields: Intuition ...................... 7 2.2 Truncated Signed Distance Field (TSDF) ............. 10 2.3 Visualization of TSDFs ....................... 16 2.3.1 Direct methods ....................... 16 2.3.2 Surface Extraction ..................... 18 2.4 Interpolation and Gradient Estimation .............. 19 2.4.1 Mathematical Preliminaries ................ 20 2.4.2 Trilinear interpolation ................... 23 2.4.3 Prismatic interpolation ................... 24 2.4.4 Pyramid interpolation ................... 26 2.4.5 Tetrahedral interpolation .................. 27 2.4.6 Nearest Neighbor (winner takes all) ............ 29 2.4.7 Flooring ........................... 30 2.5 Gradients .............................. 30 2.5.1 Central Differences ..................... 30 2.5.2 Forward and Backward Differences ............ 31 2.6 Drawbacks of TSDF Mapping and Work-arounds ........ 31 2.6.1 Memory ........................... 31 2.6.2 Sharp Edges ......................... 32 2.6.3 Corners in General ..................... 33 2.6.4 Surfaces vs. Truncation Distance ............. 33 2.7 Relationship to Occupancy ..................... 34 v vi CONTENTS 3 Registration of a Depth Image to a TSDF 37 3.1 Representing Motion ........................ 38 3.2 Registration ............................. 41 3.3 Deriving a Registration Algorithm ................. 45 3.3.1 In relation to ICP ...................... 45 3.3.2 In Relation to Lucas-Kanade ................ 47 3.4 Solution ............................... 50 3.4.1 Limitations ......................... 53 3.5 Results ................................ 54 3.6 Discussion .............................. 62 3.6.1 Handling Deformations .................. 63 3.6.2 Thoughts on Surface Orientation ............. 65 4 Feature Detection and Description 67 4.1 Noise Filtering of Depth ...................... 69 4.1.1 Bilateral Filter ........................ 71 4.1.2 Total Variation - L1 Filter ................. 72 4.1.3 TSDF for depth image denoising .............. 72 4.2 Features on Noise-Filtered Depth ................. 72 4.2.1 NARF feature detector ................... 73 4.2.2 NARF feature descriptor .................. 74 4.2.3 Kernel Descriptors ..................... 74 4.2.4 Fast Point Feature Histogram Descriptors ........ 74 4.2.5 Evaluation Methodology .................. 75 4.2.6 Feature Detectors ...................... 76 4.2.7 Feature Descriptors ..................... 77 4.2.8 Results ............................ 78 4.2.9 Discussion .......................... 81 4.3 3D Feature Detection ........................ 83 4.3.1 Harris Corners ....................... 84 4.3.2 Derivatives ......................... 85 4.3.3 Integral Invariant Features ................. 87 4.3.4 Evaluation Methodology .................. 89 4.3.5 Experimental Results .................... 91 4.4 Discussion .............................. 95 5 Compression 97 5.1 Managing memory complexity - Related Work .......... 103 5.1.1 General Purpose Compression ............... 103 5.1.2 Space Partitioning ..................... 103 5.1.3 Hashing ........................... 104 5.1.4 Moving Volumes ...................... 104 5.1.5 Dictionary Learning .................... 104 5.2 Unsupervised learning for TSDF compression .......... 105 CONTENTS vii 5.2.1 Principal Component Analysis (PCA) ........... 105 5.2.2 Artificial Neural Network ................. 106 5.2.3 Methodology ........................ 107 5.2.4 Experimental Results .................... 110 5.3 Discussion .............................. 116 6 Minimalistic Representations from TSDF 119 6.1 SSSG - Construction ........................ 121 6.2 Geometric Place Recognition Using SSSG ............. 126 6.2.1 Related work - 3D-NDT Histogram Matching ...... 127 6.2.2 Random Sample Consensus Matching of SSSG ...... 128 6.2.3 Methodology ........................ 131 6.2.4 Experimental Results .................... 133 6.3 Discussion .............................. 139 6.3.1 Improvements and Future work .............. 140 7 Conclusion 141 7.1 A Practical Guide to TSDF Mapping ............... 141 7.2 Large-Scale Tracking and Mapping ................ 142 7.3 TSDFs and Shape-based Features ................. 142 7.4 Future Work ............................. 143 7.5 Closing Remarks .......................... 144 Appendix A Code listings 147 A.1 C++ code for packing multiple variables into a single byte .... 147 References 149 List of Figures 2.1 Sensor measurements and distance transform ........... 8 2.2 Mean of distance transforms .................... 8 2.3 Mean of signed distance transforms ................ 9 2.4 Projective signed distance and truncation ............. 11 2.5 Truncated signed distance and weights .............. 12 2.6 Reconstruction using projective TSDF compared to ground truth 14 2.7 TSDF surface convergence given multiple noisy measurements . 15 2.8 Relationship between truncation distance, voxel-size and variance 15 2.9 Visualization of TSDFs using different rendering techniques . 17 2.10 Marching Cubes vs. Marching Tetrahedrons - comparison . 18 2.11 Linear interpolation in one dimension ............... 20 2.12 Bilinear interpolation .......................