Large-Scale Simultaneous Localization and Mapping 9811919712, 9789811919718

Citation preview

Cognitive Intelligence and Robotics

Janusz Będkowski

Large-Scale Simultaneous Localization and Mapping

Cognitive Intelligence and Robotics Series Editors Amit Konar, ETCE Department, Jadavpur University, Kolkata, India Witold Pedrycz, Department of Electrical and Computer Engineering, University of Alberta, Edmonton, AB, Canada

Cognitive Intelligence refers to the natural intelligence of humans and animals, it is considered that the brain performs intelligent activities. While establishing a hard boundary that distinguishes intelligent activities from others remains controversial, most common behaviors and activities of living organisms that cannot be fully synthesized using artificial means are regarded as intelligent. Thus the acts of sensing and perception, understanding the environment, and voluntary control of muscles, which can be performed by lower-level mammals, are indeed intelligent. Besides the above, advanced mammals can perform more sophisticated cognitive tasks, including logical reasoning, learning, recognition, and complex planning and coordination, none of which can yet be realized artificially to the level of a baby, and thus are regarded as cognitively intelligent. This book series covers two important aspects of brain science. First, it attempts to uncover the mystery behind the biological basis of cognition, with a special emphasis on the decoding of stimulated brain signals or images. Topics in this area include the neural basis of sensory perception, motor control, sensory-motor coordination, and understanding the biological basis of higher-level cognition, including memory, learning, reasoning, and complex planning. The second objective of the series is to publish consolidated research on brain-inspired models of learning, perception, memory, and coordination, including results that can be realized on robots, enabling them to mimic the cognitive activities performed by living creatures. These braininspired models of machine intelligence complement the behavioral counterparts studied in traditional artificial intelligence. The series publishes textbooks, monographs, and contributed volumes.

More information about this series at https://link.springer.com/bookseries/15488

Janusz B˛edkowski

Large-Scale Simultaneous Localization and Mapping

Janusz B˛edkowski Institute of Fundamental Technological Research Polish Academy of Sciences Warsaw, Poland

ISSN 2520-1956 ISSN 2520-1964 (electronic) Cognitive Intelligence and Robotics ISBN 978-981-19-1971-8 ISBN 978-981-19-1972-5 (eBook) https://doi.org/10.1007/978-981-19-1972-5 © The Editor(s) (if applicable) and The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 This work is subject to copyright. All rights are solely and exclusively licensed by the Publisher, whether the whole or part of the material is concerned, specifically the rights of translation, reprinting, reuse of illustrations, recitation, broadcasting, reproduction on microfilms or in any other physical way, and transmission or information storage and retrieval, electronic adaptation, computer software, or by similar or dissimilar methodology now known or hereafter developed. The use of general descriptive names, registered names, trademarks, service marks, etc. in this publication does not imply, even in the absence of a specific statement, that such names are exempt from the relevant protective laws and regulations and therefore free for general use. The publisher, the authors and the editors are safe to assume that the advice and information in this book are believed to be true and accurate at the date of publication. Neither the publisher nor the authors or the editors give a warranty, expressed or implied, with respect to the material contained herein or for any errors or omissions that may have been made. The publisher remains neutral with regard to jurisdictional claims in published maps and institutional affiliations. This Springer imprint is published by the registered company Springer Nature Singapore Pte Ltd. The registered company address is: 152 Beach Road, #21-01/04 Gateway East, Singapore 189721, Singapore

I dedicate this book for my Wife Tresya.

Preface

This book is written for engineers and researchers who would like to increase the knowledge in area of mobile mapping systems. Therefore, the flow of the derived information is divided into sub problems corresponding to certain mobile mapping data and related observation equations. The proposed methodology is not fulfilling all Simultaneous Localization and Mapping aspects evident in the literature. It is based on the experience within the context of the pragmatic and realistic applications. Thus, it can be supportive information for those who are familiar with SLAM and would like to have broader overview in the subject. The novelty is a complete and interdisciplinary methodology for large scale mobile mapping applications. The contribution is a set of programming examples available as supportive complementary material for this book. All observation equations are implemented and for each the programming example is provided. The programming examples are simple C++ implementations that can be elaborated by students or engineers, therefore the experience in coding is not mandatory. Moreover, since the implementation does not require many additional external programming libraries, it can be easily integrated with any mobile mapping framework. The purpose of this book is to collect all necessary observation equations and solvers to build computational system capable providing large scale maps. Crucial aspects related with multi-domain (air, ground, underground, indoor, outdoor) localization mapping are collected based on pragmatic engineering and research problems. The aim was to collect methods well known in photogrammetry, mobile robotics and machine vision in one book to minimize an effort and technological gap for beginners and other researchers looking for algorithmic solutions. The main contribution is a set of observation equations that are crucial for building mobile mapping systems. Thus, for most of observations relevant examples and sample programming codes in Python and C++ are provided. Due to volume of data and possible applications this book concerns the so called large scale mobile mapping methodology. The main focus was to collect and fulfill the gap between widely used methods to build the system capable collecting sensors’ observations and transforms them into globally consistent map. Furthermore, this map can be used for localization purpose. This book concerns the concepts and methods known mainly from three domains such as vii

viii

Preface

mobile robotics, photogrammetry and machine vision. These domains introduce the methodology for mobile mapping. It is based on computing the absolute pose of the measurement instruments based on its raw information typically transformed into the feature space. There are similarities and differences in these methodologies, thus different approaches are evident for solving the same problem of building globally consistent map. This book helps in choosing mobile mapping approach for certain application. This book is organized into four parts and a complementary open-source project [1] containing Python and C++ examples. Therefore, each observation equation is discussed and supported by relevant example showing how to implement it. Warsaw, Poland March 2022

Janusz B˛edkowski, D.Sc., Ph.D.

Acknowledgements

It is challenging to acknowledge all projects and experiments yielding writing this book. Most important experiments were performed during European Land Robotic Trial: ELROB and the European Robotics Hackathon: ENRICH. Morover, four projects had important impact into book: POIR.01.01.01-00-0206/17 “Design and development of the autonomous mobile platform moving in production environment”. PBS3/B3/0/2014 “Research of new scanning unit and its modification for mobile robot mapping applications”. POIR.01.01.01-00-0494/20 “Development and verification of the automatic location and 3D visualization of the selected objects in urban environment technology together with people flow modeling”. H2020 ESMERA, “MANDALA”, No 780265. I am indebted to prof. Piotr Skrzypczy´nski and prof. Bartosz Mitka for their comments which enabled me to correct errors and clarify the presentation. I would like to thank the staff at Springer, in particular Nick Zhu, Sivananth S. Siva Chandran for help and support. I would like to thank dr. Frank E. Schneider and prof. Andreas Nüchter for giving me an opportunity to perform experiments in their laboratories. Finally, I would like to thank dr. Karol Majek, Artur Adamek, Michał Pełka, Jakub Ratajczak, Piotr Kowalski, Bła˙zej Kubiak, dr. Hubert Nowak and Krzysztof Miksa for long substantive discussions.

ix

Contents

Part I

Introduction

1

Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.1 Novelty and Contribution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 Terminology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.1 Basic Terms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3 3 8 8 10

2

Mobile Mapping Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1 Commercial Measurement Instruments . . . . . . . . . . . . . . . . . . . . . . 2.2 Mobile Mapping Data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.1 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.2 Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.3 Light Detection and Ranging . . . . . . . . . . . . . . . . . . . . . . . 2.2.4 Mono-, Stereo-, Spherical and Depth Cameras . . . . . . . . 2.3 Ground Truth Data Sources . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3.1 Large-scale Data Sets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3.2 Long-Term Data Sets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3.3 Terrestrial Laser Scanning Data . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

13 13 19 19 20 21 21 22 22 23 24 24

Part II 3

Methodology

Coordinate Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2.1 Tait–Bryan and Euler Angles . . . . . . . . . . . . . . . . . . . . . . . 3.2.2 Euler–Rodrigues Formula . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2.3 Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.3 Cartesian Coordinate System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4 Spherical Coordinate System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

31 31 32 32 34 36 38 38

xi

xii

Contents

3.5

Geographic Coordinate System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.5.1 Gauss–Krüger Geographic Coordinate System and UTM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

39 39 40

4

Simultaneous Localization and Mapping . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Weighted Non-linear Least Squares Optimization . . . . . . . . . . . . . 4.2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2.2 Solvers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2.3 Robust Least Squares . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2.4 Manifolds . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Probabilistic Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.4 Graphical Representation with Factor Graphs . . . . . . . . . . . . . . . . . 4.5 Graph SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.5.1 Pose Graph SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.6 Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

41 41 41 42 57 61 72 83 85 87 90 91 93

5

Trajectory Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1 Motion Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2 Kalman Fiter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.2.1 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3 Particle Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.1 Variable of Interest . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.2 Prediction Stage (Motion Model) . . . . . . . . . . . . . . . . . . . 5.3.3 Use of Semantic Data in the Particle Filter . . . . . . . . . . . . 5.3.4 Update Stage and Resampling . . . . . . . . . . . . . . . . . . . . . . 5.4 Structure from Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4.1 Pinhole Camera Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4.2 Essential and Fundamental Matrices . . . . . . . . . . . . . . . . . 5.4.3 Homography Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4.4 Bundle of Rays Intersection (Triangulation) . . . . . . . . . . 5.4.5 Bundle Adjustment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.5 Lidar Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

95 95 95 98 99 100 101 101 101 103 104 105 107 108 110 111 115

6

Errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.1 Uncertainty, Accuracy and Precision . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Evaluation Metrics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.3 Mean and Covariance of the Point Cloud . . . . . . . . . . . . . . . . . . . . 6.4 Black-Box Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.5 Error Propagation Law . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.6 Closed-Form Estimate of Covariance . . . . . . . . . . . . . . . . . . . . . . . . 6.7 Global Positioning System Accuracy Assessment . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

119 119 119 122 123 123 125 134 136

Contents

xiii

Part III Observation Equations 7

Camera Metrics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.1 Pinhole Camera Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.1.1 Pinhole Camera Reprojection Error . . . . . . . . . . . . . . . . . . 7.1.2 Intrinsic Calibration of Perspective Camera . . . . . . . . . . . 7.1.3 Calculating Pinhole Camera External Orientation Using Plücker Lines . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.2 Metric Camera Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.2.1 Colinearity Observation Equation . . . . . . . . . . . . . . . . . . . 7.2.2 Coplanarity Observation Equation . . . . . . . . . . . . . . . . . . . 7.3 Equirectangular Camera Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7.3.1 Colinearity Observation Equation . . . . . . . . . . . . . . . . . . . 7.3.2 Coplanarity Observation Equation . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

141 141 141 144

8

LiDAR Metrics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.1 Point to Point Metrics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.1.1 Point to Point . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.1.2 Point to Point—Source to Target . . . . . . . . . . . . . . . . . . . . 8.1.3 Point to Point—Source to Landmark . . . . . . . . . . . . . . . . . 8.1.4 Point to Point with Scale . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.1.5 Semantic Point to Point . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.1.6 Point to Projection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.2 Point to Feature Metrics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.2.1 Point to Line . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.2.2 Point to Plane . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.2.3 Distance Point to Plane . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.2.4 Point to Surface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.3 Feature to Feature Metrics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.3.1 Line to Line . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.3.2 Plane to Plane . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.4 Reflection Observation Equation . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.5 Normal Distributions Transform . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.6 Local Geometric Features—Surfels . . . . . . . . . . . . . . . . . . . . . . . . . 8.7 Nearest Observations Search . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.7.1 Anisotropic Nearest Neighborhood Search . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

171 171 171 175 180 183 187 190 196 196 199 202 204 206 206 211 215 218 221 222 224 226

9

Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.1 Norm of the Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.2 Fixed Optimized Parameter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.3 Anisotropic Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.4 Linear and Square Functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

231 231 232 232 236

148 153 153 157 161 161 165 169

xiv

Contents

9.5

Relative Pose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.5.1 Variant (1) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.5.2 Variant (2) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Smoothness . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Georeference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.7.1 Case 1: Georeferenced Position (tx , t y , tz ) . . . . . . . . . . . . 9.7.2 Case 2: Georeferenced Pose [R, t] . . . . . . . . . . . . . . . . . . . 9.7.3 Case 3: Georeferenced Primitive (LiDAR Metric) . . . . . 9.7.4 Case 4: Georeferenced Primitive (Metric Camera) . . . . . Distance to Circle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

240 240 249 252 257 258 260 261 262 264

10 Metrics’ Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.2 Rectangular Object with Unknown Width/Height . . . . . . . . . . . . . 10.3 Sheaf of Planes Intersection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10.4 Surface Reconstruction from LiDAR Point Cloud . . . . . . . . . . . . . 10.5 Multi-measurement Instrument System . . . . . . . . . . . . . . . . . . . . . . 10.5.1 Multi-sensor Extrinsic Calibration . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

267 267 267 270 272 273 274 275

9.6 9.7

9.8

Part IV Applications 11 Building Large-Scale SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.1 Normal Equation for BA . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Trajectory Centric Versus Map-Centric Approach . . . . . . . . . . . . . 11.3 Continuous Trajectory SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.4 Design Matrix—Programming Example . . . . . . . . . . . . . . . . . . . . . 11.5 Loop Closing and Change Detection . . . . . . . . . . . . . . . . . . . . . . . . 11.5.1 Processing Video Information . . . . . . . . . . . . . . . . . . . . . . 11.5.2 Processing LiDAR Information . . . . . . . . . . . . . . . . . . . . . References . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

279 279 280 281 282 283 284 285 286

12 Final Map Qualitative and Quantitative Evaluation . . . . . . . . . . . . . . 12.1 Measurement Instrument . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.2 TLS Matching . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.3 Mobile Backpack Mapping System Qualitative Evaluation . . . . . 12.4 Improving State-of-the-Art Visual SLAM . . . . . . . . . . . . . . . . . . . . Reference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

291 291 291 295 303 304

Appendix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 305

Acronyms

3-DOF 6-DOF 6DSLAM AHRS AI ATE BA BRIEF BRISK CCS CORS CPU DLT DNN DOF EGG EKF ENU EOP FAST FOG FOV FPFH GCP GCS GLOH GN GNSS GPS GPU GUI

Three Degree of Freedom Six Degree of Freedom Six Degree of Freedom Simultaneous Localisation and Mapping Attitude and Heading Reference System Artifical Intelligence Absolute Trajectory Error Bundle Adjustment Binary Robust Independent Elementary Feature Binary Robust Invariant Scalable Keypoint Cartesian Coordinate System Concentric Ring Signature Central Processing Unit Direct Linear Transform Deep Neural Network Degree of Freedom Elliptic Gabriel Graph Extended Particle Filter East-North-Up External Orientation Parameters Features from Accelerated Segment Test Fiber-Optic Gyroscope Field Of View Fast Point Feature Histogram Ground Control Point Geographic Coordinate System Gradient Location and Orientation Histogram Gauss-Newton Global Navigation Satellite System Global Positioning System General Purpose Graphics Processing Unit Graphical User Interface xv

xvi

ICP IMU INS KF KLT LALE LBS LG LiDAR LM LO LRGC LT LTP MEMS ML MMS MSE MSER NARF NDT NED NNS NPP ODE ORB PCG PDF PF PFH PGSLAM PPF RANSAC RIFT RMS RMSE ROI ROS RPE RTK SD SE(3) SFC SFM SIFT

Acronyms

Iterative Closest Point Inertial Measurement Unit Inertial Navigation system Kalman Filter Kanade-Lucas-Tomasi Low-Altitude Long-Endurance Location-Based Services Lie Group Light Detection and Ranging Levenberg-Marquardt LiDAR Odometry Local Registration-Global Correlation Lie Theory Local Tangent Plane Coordinates Micro-Electro-Mechanical-System Machine Learning Mobile Mapping System Mean Squared Error Maximally Stable Extremal Region Normal Aligned Radial Feature Normal Distributions Transform North-East-Down Nearest Neighbourhood Search Nuclear Power Plant Ordinary Differential Equation Oriented FAST and Rotated BRIEF Preconditioned Conjugate Gradient Probability Distribution Function Particle Filter Point Feature Histogram Pose Graph Simultaneous Localisation and Mapping Point Pair Feature Random Sample Consensus Rotation Invariant Feature Transform Root Mean Square Root Mean Square Error Region Of Interest Robot Operating System Relative Pose Error Real Time Kinematic Standard Deviation Special Euclidean Group Space Filling Curve Structure From Motion Scale Invariant Feature Transform

Acronyms

SLAM SLERP SO(3) SSR SURF SVD TLS UAV UGV USV UTM UUV UV VFH VIO VO

xvii

Simultaneous Localisation and Mapping Spherical Linear Interpolation Special Orthogonal Group Sum Squared Residuals Speeded Up Robust Feature Singular Value Decomposition Terrestial Laser Scanning Unmanned Aerial Vehicle Unmanned Ground Vehicle Unmanned Surface Vehicle Universal Transverse Mercator Unmanned Underwater Vehicle Unmanned Vehicle Viewpoint Feature Histogram Visual Inertial Odometry Visual Odometry

Symbols β β  r y A

State vector Vector of increments Model function Residuum Target value Design matrix

r

J 

J



  R W w g H ≈ diag λ ρ ϒ





Jacobian of the objective function Jacobian of the model function Transpose Information matrix Covariance matrix Rotation matrix Weight matrix Weight Gradient vector Hessian matrix Approximation Diagonal of the matrix Damping factor Robust function Influence function First derivative Second derivative

xviii

Acronyms

e

J M T X E TX M TE M G ◦ V X˙ m τ ⊕  AdX Ad X SE(3) SO(3) so(3) × [R, t] R t Rx Ry Rz Tr N (x; μ, )

Jacobian of the predictive model Manifold Tangent space Point on manifold M Identity point of manifold M Tangent at the point X Tangent at the identity point E of manifold M (Lie algebra) Group Composition operation Set of actions Velocity of point X in tangent space TX M Lie algebra Vector element of the Lie algebra Oplus operator Ominus operator Adjoint Adjoint matrix Special Euclidean Group, 4 × 4 matrices Special Orthogonal Group, 3 × 3 matrices Lie algebra of SO(3) Operator of skew symmetric matrix, cross product Pose ∈ SE(3) Rotation ∈ SO(3) Translation ∈ R3 Vector ∈ R3 of X axis of the pose [R, t] Vector ∈ R3 of Y axis of the pose [R, t] Vector ∈ R3 of Z axis of the pose [R, t] Trace Normal distribution over vectors x, with mean value μ and a covariance matrix 

Part I

Introduction

Chapter 1

Introduction

1.1 Novelty and Contribution In this book, crucial aspects related to multi-domain (air, ground, underground, indoor and outdoor) localization and mapping are collected based on the pragmatic engineering and research problems. The aim was to collect methods well known in photogrammetry, mobile robotics and machine vision in one place to minimize an effort and technological gap for beginners and other researchers looking for Simultaneous Localization and Mapping methodology and instructions for the implementation. The main contribution is a set of observation equations that is crucial for building mobile mapping systems. Thus, for most of the observations relevant examples and sample programming codes in Python and C++ are provided. The core concept is a pose expressed as (1.1) and shown in Fig. 1.1 incorporating a right-handed Cartesian coordinate system. It is 4 × 4 matrix ∈ SE(3), where R is rotation matrix ∈ SO(3) and t ∈ R3 is a translation vector. R x ∈ R3 is a vector of X-axis, R y ∈ R3 is the vector of Y-axis and R z ∈ R3 is the vector of Z-axis of the pose. Right-handed Cartesian coordinate system determines R z = R x × R y . Most of the observation equations elaborated in this book provide formulas to manipulate poses in SE(3). Generally, finding the optimal configuration of the poses is an essential problem for SLAM, therefore the optimal map can be reconstructed. ⎡ Rx r11 R t ⎢ = ⎢r21 01×3 1 ⎣r31 0





R y Rz t ⎤ r12 r13 t1 r22 r23 t2 ⎥ ⎥ r32 r33 t3 ⎦ 0 0 1

(1.1)

Due to the volume of data and possible applications, this book concerns the socalled large-scale mobile mapping methodology. The main focus was to collect and fulfill the gap between widely used methods to build a system capable of collecting

© The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_1

3

4

1 Introduction

Fig. 1.1 Visualization of the pose expressed as 4 × 4 matrix ∈ SE(3), where R is rotation matrix ∈ SO(3) and t ∈ R3 is a translation vector. Right-handed Cartesian coordinate system determines R z = R x × R y

sensors’ observations and transform them into a globally consistent map. Therefore, this map can be used for localization purposes. Obviously, the term large-scale corresponds to applications where the volume of data that is related to the total length of the trajectories is significant and requires high computational resources. This topic is relevant looking at recent developments in mobile robotics such as search and rescue applications where reconstructing 3D scenes is one of the goals of the autonomous machine and autonomous cars that will use predefined maps for localization purposes [2, 3]. Obviously, it is evident that autonomous cars can collect data and contribute to global map updates, thus we cope with large-scale problem that is recently addressed by many researchers. The term SLAM [4, 5] corresponds to the so-called “chicken and egg dilemma”—what was first the chicken or the egg? Therefore, we should have a proper map representation that is compatible with observations derived from sensors to localize within this map, and we need accurate localization to build the map. The core concept is the pose that represents a position and an orientation for a given time. The set of consecutive poses gives the trajectory. Attaching measurements to the trajectory using so-called relative pose gives an opportunity to reconstruct the map of raw measurements, e.g. point cloud in the case of using LiDAR technology. Obviously, the calibration pose has to be also considered to have a proper relative pose between the trajectory pose and the sensors’ origin. Having in mind an accurate trajectory, accurate calibration and accurate measurements, it is possible to reconstruct an accurate map. In practice, there are many factors affecting those accuracies, therefore reducing related errors yield reaching the desired precision—so-called ground truth that is almost impossible looking at the complexity of the problem. In the literature [6], the SLAM problem is divided into a front-end and a back-end. The front-end is typically responsible for an initial trajectory generation and the back-end is responsible for a loop closing. Loop is the situation when the measurement instrument is visiting the same location as visited some time ago assuming a continuous trajectory between these two time intervals. The back-end is typically solved using the so-called graph-based technique that minimizes the error between an observed and a desired relative pose between the trajectory/trajectories nodes. Due to the large-scale aspect of SLAM, it can be mentioned that recent research shows an interest in cloud-based calculations of the back-end [4, 7, 8], thus a fully scalable implementation can be reached.

1.1 Novelty and Contribution

5

This book concerns the concepts and methods known mainly from three domains such as mobile robotics [9], photogrammetry and machine vision. These domains introduce a methodology for mobile mapping. It is based on computing the absolute pose of the measurement instruments based on its raw information typically transformed into a feature space. There are similarities and differences in these methodologies, thus different approaches are evident for solving the same problem of building a globally consistent map. Mobile robotics is more focused on an autonomous behavior [10, 11] of the mobile robot rather than an absolute accuracy as a target of photogrammetry, the therefore SLAM problem is more evident in many mobile robotic frameworks [12]. Machine vision provides computationally efficient techniques for camera pose estimation, thus there are commonly used by the robotics community. All domains use a similar sensor setup such as monocular, equirectangular, RGB-D, stereo cameras, IMU, GPS, LiDAR and odometers. The term graph-based optimization is common in mobile robotics. The main difference between mobile mapping approaches in these domains is related to observations’ equations that are used for optimization purposes. An important difference is related to real-time calculations crucial for mobile robotics. For the sake of fulfilling the gap between these domains that was also addressed in [13], this book elaborates the observations’ equations that are widely used in practical applications looking from air, ground and underground mobile mapping systems. The problem of fusing these observations is the main focus of the proposed methodology, therefore the solution for merging, e.g. camera, IMU, GPS and LiDAR observations is discussed. This book is organized into four parts and a complementary open-source project [1] containing Python and C++ examples. Therefore, each observation equation is discussed and supported by a relevant example showing how to implement it. Part I: this introduces the scope of this book and the motivation behind this contribution. Sect. 1.2 discusses terminology and basic terms related to the mobile mapping methodology. Mobile mapping data and mobile mapping systems are introduced in Chap. 2. It also shows the accessible data sources that can be used as a benchmark for algorithms. Part II: the rotations and coordinate systems are discussed in Chap. 3. Thus, Tait–Bryan and Euler angles, Euler–Rodrigues formula and quaternions as commonly used rotation representations are elaborated. Furthermore, Cartesian, Spherical and Geographic coordinate systems are discussed. The SLAM problem is discussed in Chap. 4. The Newton, Gauss–Newton and Levenberg–Marquardt methods being the core of the Weighted Non-Linear Least Square Optimization are discussed in Sect. 4.2. The probabilistic formulation for the SLAM problem and its graphical representation with Factor Graphs is elaborated for the purpose of introducing a mobile mapping approach in mobile robotics. Some aspects of Lie theory [14] and the approach for the mobile mapping systems are given, therefore it is considered as a concise methodology for defining the mobile mapping system optimization problems. Trajectory estimation as a fundamental aspect of mobile mapping system is divided into several approaches in Chap. 5 including the importance of the motion model and description of the algorithms such as KF, PF, SFM and LO. An important aspect of the error estimation including the closed-form estimate of the covariance is discussed in Chap. 6. Part III: Chaps. 7 and 8 elaborate observation equations for

6

1 Introduction

cameras and LiDAR. The pinhole camera model is discussed. The colinearity and coplanarity observation equations for metric and equirectangular cameras are given. The LiDAR metrics are provided including a point-to-point, a point-to-feature and a feature-to-feature observation equations. The NDT and surfels organizing the local geometric features conclude the chapter. The nearest neighborhood search approach concludes the chapter. Chapter 9 discusses other observation equations organized as a set of constraints useful and sometimes even mandatory for building the mobile mapping optimization problems such as relative pose, smoothness and georeference. Metrics’ fusion is elaborated in Chap. 10 for solving some examples of pragmatic problems such as (1) finding the location of a rectangular object with unknown dimensions in the point cloud reconstructed by SFM, (2) sheaf of planes intersection, (3) surface reconstruction from LiDAR point cloud and (4) multi-measurement instrument system. Part IV: the aspects of the building large-scale SLAM optimization problems are discussed in Chap. 11. The loop closing and change detection based on camera and LiDAR data are elaborated in this chapter. Chapter 12 demonstrates some experiments conducted based on the proposed methodology, thus the final map qualitative and quantitative evaluation is shown. Appendix A provides guidance on the complementary open-source project [1] containing Python and C++ examples. This book is dedicated to the engineers and the researchers who would like to increase their knowledge in the area of mobile mapping systems. Therefore, the flow of the derived information is divided into sub-problems corresponding to a certain mobile mapping data and the related observations’ equations. The proposed methodology is not fulfilling all SLAM aspects evident in the literature, but it is based on the experience within the context of the pragmatic and realistic applications. Thus, it can be supportive information for those who are familiar with the SLAM and would like to have a broader overview of the subject. The novelty is a complete and interdisciplinary methodology for the large-scale mobile mapping applications. The contribution is the set of the programming examples available as the supportive complementary material for this book [1]. Example GUI of the program for 2D PGSLAM is shown in Fig. 1.2 demonstrating the state before the optimization and in Fig. 1.3 demonstrating the state after the optimization. Moreover, all the observation equations elaborated in this book are implemented and for each, the programming example is provided. The programming examples are simple C++ implementations that can be elaborated by students and engineers, therefore the experience in coding is not mandatory. Moreover, since the implementation does not require many additional external programming libraries, it can be easily integrated with any mobile mapping framework. Finally, the purpose of this book is to collect all the necessary observation equations and solvers to build a computational system capable of providing maps as in Fig. 1.4.

1.1 Novelty and Contribution Fig. 1.2 Example program for PGSLAM. State before optimization

Fig. 1.3 Example program for PGSLAM. State after optimization

Fig. 1.4 Example map provided by software built based on the methodology elaborated in this book

7

8

1 Introduction

1.2 Terminology Starting from the SLAM problem, the trajectory, sensors’ readings and the map are the terms commonly used in mobile robotics. The trajectory can be expressed as consecutive 6-DOF poses or as a continuous trajectory [15]. Collecting consistent 3D LiDAR data using a moving mobile mapping system is often difficult looking at a scanning time. Thus, the precision of collected data is related to a motion. For this reason, the trajectory of the sensor during the scan must be taken into account when constructing, e.g. point clouds. To address this issue, many researchers use the so-called stop-scan technique, therefore they stop the moving platform and take stationary scans [16, 17]. In contrary, recent research advances address a continuoustime mapping [18, 19]. The continuous-time mapping relates to a new term of a time calibration method [20]. It is introducing a continuous-time simultaneous localization and mapping approach for mobile robotics. Contrary to mobile robotics, the mobile mapping systems used in geodesy use the synchronized sensors’ readings, therefore the problem of time calibration is not concerned. Sensors’ readings are crucial for map building looking from all domains such as mobile robotics, photogrammetry and machine vision. Sensor is called a measurement instrument in photogrammetry.

1.2.1 Basic Terms In mobile robotics, machine vision and photogrammetry, there are common basic terms related to localization and mapping, sensors, methods, etc. Therefore, the relevant ones according to the methodology described in this book are explained below. • Perception [21] is related to the gathering, identification and interpretation of the data derived from the measurement instrument in order to represent and understand the presented information or environment. • Localization [21] is the ability to retrieve current pose relative to global coordinate systems or to a reference map. • Cognition [21] concerns the ability of, e.g. the robot to decide how to act to achieve its goals. It is related to the process of acquiring knowledge and understanding through experience and the senses. • Locomotion [21, 22] is the ability to move in a certain terrain, e.g. legged robots [23]. • Measurement [24] is the quantitative numerical information of the attributes of an object or event, which can be used to compare with other objects or events. • Measurement instrument is a device that acquires a quantitative numerical information (physical quantity) of the attributes of an object or event. • Calibration is a process of retrieving measurement instrument parameters to reach a known accuracy.

1.2 Terminology

9

• Intrinsic calibration parameters are parameters of the measurement instrument related to its mechanical, optical design and functionality. • Extrinsic calibration parameters are poses relative to the reference coordinate system. • Boresight [25] is the difference in the rotations of the sensor (e.g. camera) rotational axes and the rotational axes of the IMU. • Lever-arm [26, 27] is the relative position of the, e.g. INS and other sensors. • Observation is an acquisition of an information from a primary source. It can be done, e.g. using the measurement instrument or calculated based on other facts. • Real measurement [28] is the quantitative numerical information derived from the measurement instrument. This term is used in Graph SLAM. • Expected measurement [28] is the unknown quantitative numerical information (such as from equivalent measurement instrument) derived from calculations. • Real observation [28] is a real measurement. • Expected observation [28] is an expected measurement. • Observation equation is a mathematical formula relating residual, target value and model function. • Model function models expected measurements to retrieve a target value based on an optimization process. • SLAM is the ability to build the map and localize in this map assuming simultaneous execution. • Weighted Non-Linear Least Square Optimization [29] is an optimization technique to solve the non-linear system of the observation equations assuming weights related to the impact of a certain equation and a correlation between unknowns. • Tie point [29] is a ground point existing in more than one photograph as its image coordinates. • Control point [29] is the ground point existing in more than one photograph as its image coordinates and georeference information. • Terrestrial Close-Range Imagery is close-range terrestrial digital photogrammetry with the primary products such as high-resolution 3D photographic surface models. • Aerial Imagery is aerial photogrammetry with the primary products such as highresolution 3D photographic ground surface models. • Bundle Adjustment [29] is the problem of simultaneously refining the 3D coordinates describing the scene geometry, the parameters of the relative motion and the optical characteristics of the cameras. The basic term, mostly elaborated within the context of the observations’ equations, is the pose expressed as camera←world [R, t]cw or world←camera [R, t]wc transformation matrix that satisfies condition (1.2).   −1    Rwc t wc Rcw t cw Rwc −Rwc t wc = = 01×3 1 01×3 1 01×3 1



(1.2)

10

1 Introduction

The world← camera [R, t]wc matrix transforms local coordinates to a global. The camera←world [R, t]cw matrix transforms the global coordinates to the local. Navigation and localization are the two most important tasks for mobile robots [30]. The main goal for the mobile robot is to move (navigate) in a certain environment, therefore we distinguish several types of UV. Unmanned means there is no human on board. There are two main scenarios: indoor and outdoor. The robotics community addresses those two scenarios due to the fact of different challenges. For example, UV in the indoor scenario has to cope with so-called GNSS, GPS denied environments. Outdoor scenario is impacted by change of the environmental conditions, and it is related to so-called long-term localization [31] and long-term navigation [32]. The term indoor–outdoor transition [33] sometimes called indoor– outdoor switching corresponds to the scenarios when UV changes location between an indoor-like to an outdoor-like environment. The indoor environments have a rich set of challenges related to, e.g. long-narrow paths, surfaces with limited texture information, thus localization algorithms are affected by so-called perceptual aliasing confusion [34], therefore ML methods are usually addressed to solve this problem. Instead of the indoor–outdoor UV, we distinguish UAV which flies over the area, and UGV that traverses the arena due to wheels, trucks or other sophisticated mechanical designs such as mechanical-legs. USV moves onto water and UUV can dive under water. The evaluation of the navigation and localization capabilities refers to the quantitative evaluation of the quality of an estimated trajectory [35].

References 1. J. Bedkowski, Observation equations (2021). https://github.com/JanuszBedkowski/ observation_equations 2. Sung-Hyuck, J.G.-I. Im, J.-H. Im, Extended line map-based precise vehicle localization using 3D LIDAR. Sensors 18(10:3179) (2018) 3. C. Badue, R. Guidolini, R.V. Carneiro, P. Azevedo, V.B. Cardoso, A. Forechi, L. Jesus, R. Berriel, T.M. Paixao, F. Mutz, L. de Paula Veronese, T. Oliveira-Santos, A.F. De Souza, Selfdriving cars: a survey. Expert Syst. Appl. 165, 113816 (2021) 4. J. Leonard, H. Durrant-Whyte, Simultaneous map building and localization for an autonomous mobile robot, in Proceedings IROS ’91:IEEE/RSJ International Workshop on Intelligent Robots and Systems ’91, vol. 3, pp. 1442–1447 (1991) 5. P. Skrzypczy´nski, Simultaneous localization and mapping: a feature-based probabilistic approach. Int. J. Appl. Math. Comput. Sci. 19(4), 575–588 (2009) 6. R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige, W. Burgard, G2o: a general framework for graph optimization, in 2011 IEEE International Conference on Robotics and Automation, pp. 3607–3613 (2011) 7. K. Ayush, N.K. Agarwal, Real time visual slam using cloud computing, in 2013 Fourth International Conference on Computing, Communications and Networking Technologies (ICCCNT), pp. 1–7 (2013) 8. H. Zhang, C. Huang, Y. Liu, A novel RGB-D SLAM algorithm based on cloud robotics. Sensors 19(23:5288) (2019) 9. P. Skrzypczy´nski, Planning positioning actions of a mobile robot cooperating with distributed sensors, in ed. by M. Kurzynski, E. Puchala, M. Wozniak, A. Zolnierek, Computer Recognition Systems, Proceedings of the 4th International Conference on Computer Recognition

References

10. 11. 12. 13.

14. 15. 16.

17. 18. 19. 20.

21. 22. 23.

24. 25. 26. 27.

28. 29. 30. 31. 32.

11

Systems, CORES 05, 22–25 May 2005, Rydzyna Castle, Poland, volume 30 of Advances in Soft Computing (Springer, 2005), pp. 427–434 A. Ratajczak, J. Karpi´nska, K. Tcho´n, Task-priority motion planning of underactuated systems: an endogenous configuration space approach. Robotica 28(6), 943–943 (2010) M. Michalek, K. Kozlowski, Feedback control framework for car-like robots using the unicycle controllers. Robotica 30(4), 517–535 (2012) C. Zieli´nski, T. Winiarski, Motion Generation in the MRROC++ Robot Programming Framework. Int. J. Rob. Res. 29(4), 386–413 (2010) P. Agarwal, W. Burgard, C. Stachniss, Survey of geodetic mapping methods: geodetic approaches to mapping and the relationship to graph-based SLAM. Robot. Autom. Mag. IEEE 21, 63–80, 09 (2014) J. Sola, J. Deray, D. Atchuthan, A micro lie theory for state estimation in robotics (2020) M. Bosse, R. Zlot, Continuous 3D scan-matching with a spinning 2D laser, in ICRA (IEEE, 2009), pp. 4312–4319 A. Nuchter, K. Lingemann, J. Hertzberg, H. Surmann, 6D slam with approximate data association, in Proceedings of the 12th International Conference on Advanced Robotics, 2005, ICAR ’05, pp. 242–249 (2005) D. Silver, D. Ferguson, A. Morris, S. Thayer, Topological exploration of subterranean environments. J. Field Robot. 23(6–7), 395–415 (2006) L. Kaul, R. Zlot, M. Bosse, Continuous-time three-dimensional mapping for micro aerial vehicles with a passively actuated rotating laser scanner. J. Field Robot. 33(1), 103–132 (2016) R. Zlot, M. Bosse, Efficient large-scale three-dimensional mobile mapping for underground mines. J. Field Robot. 31(5), 758–779 (2014) S. Du, H.A. Lauterbach, X. Li, G.G. Demisse, D. Borrmann, A. Nuchter, Curvefusion—a method for combining estimated trajectories with applications to slam and time-calibration. Sensors 20(23), 6918 (2020) R. Siegwart, I.R. Nourbakhsh, Introduction to Autonomous Mobile Robots (Bradford Company, USA, 2004) R. Siegwart, P. Lamon, T. Estier, M. Lauria, R. Piguet, Innovative design for wheeled locomotion in rough terrain. Robot. Auton. Syst. 40(2–3), 151–162 (2002) M. Nowicki, D. Belter, A. Kostusiak, P. Cizek, J. Faigl, P. Skrzypczy´nski, An experimental study on feature-based SLAM for multi-legged robots with RGB-D sensors. Ind. Robot 44(4), 428–441 (2017) R.R. Macdonald, Measurement, design and analysis: an integrated approach, by E.J. Pedhazur, I.P. Schmelkin. Brit. J. Math. Stat. Psychol. 45(1), 163–163 (1992) P. Gabrlik, Boresight calibration of a multi-sensor system for UAS photogrammetry. 2018 ELEKTRO, pp. 1–6 (2018) M. Leslar, J.G. Wang, B. Hu, Boresight and lever arm calibration of a mobile terrestrial lidar system. Geomatica 70(2), 97–112 (2016) K.-W. Chiang, M.-L. Tsai, N. El-Sheimy, A. Habib, C.-H. Chu, New calibration method using low cost mems imus to verify the performance of UAV-borne MMS payloads. Sensors (Basel, Switzerland) 15, 6560–6585 (2015) G. Grisetti, R. Kuemmerle, C. Stachniss, W. Burgard, A tutorial on graph-based SLAM. Intell. Transp. Syst. Mag. IEEE 2(4), 31–43 (2010) K. Kraus, I.A. Harley, S. Kyle, Photogrammetry: Geometry from Images and Laser Scans (De Gruyter, Berlin, Boston, 18 Oct. 2011) P. Skrzypczy´nski, Uncertainty models of the vision sensors in mobile robot positioning. Int. J. Appl. Math. Comput. Sci. 15(1), 73–88 (2005) G. Kim, B. Park, A. Kim, 1-day learning, 1-year localization: long-term lidar localization using scan context image. IEEE Robot. Autom. Lett. 4(2), 1948–1955 (2019) W. Churchill, P. Newman, Experience-based navigation for long-term localisation. Int. J. Rob. Res. 32(14), 1645–1661 (2013)

12

1 Introduction

33. Y. Zhu, H. Luo, Q. Wang, F. Zhao, B. Ning, Q. Ke, C. Zhang, Curvefusion—a method for combining estimated trajectories with applications to slam and time-calibration. Sensors 4(19), 786 (2019) 34. P.A. Crook, G. Hayes, Learning in a state of confusion: perceptual aliasing in grid world navigation, in In Towards Intelligent Mobile Robots 2003 (TIMR 2003), 4th British Conference on (Mobile) Robotics (2003) 35. Z. Zhang, D. Scaramuzza, A tutorial on quantitative trajectory evaluation for visual (-inertial) odometry, pp. 7244–7251 (2019)

Chapter 2

Mobile Mapping Systems

2.1 Commercial Measurement Instruments MMSs are composed of proprioceptive, exteroceptive and interoceptive sensors. Proprioceptive sensors measure the internal state of the system in the environment such as rotary position, linear position, velocity, accelerations and temperature. Exteroceptive sensors measure parameters external to the system such as pressure, forces and torques, vision, proximity and active ranging. Vision sensors include monocular, stereo/multiple cameras, equirectangular/spherical cameras and structured lighting. Active ranging systems are laser line scanners, LiDAR, RADAR, and SONAR. Interoceptive sensors measure electrical properties (voltage, current), temperature, battery charge state, stress/strain and sound. All the above-mentioned sensors are typical electronics that synchronize all inputs with GPS receivers, thus all raw data can be transformed into global reference systems. There is a need to cope with GPS denied environments, thus recent developments show the progress of mobile mapping technologies uses also SLAM algorithms. Figures 2.1, 2.2, 2.3 and 2.4 show a ZEB family of devices introduced in [1]. The ZEB family of mobile mapping devices uses the advantage of rotated LiDAR to perceive full 360 ◦ C distance measurements. Further developments introduce an equirectangular camera that can augment metric information with spherical images. Many mobile mapping applications incorporate an equirectangular camera Ladybug5/5+ to perceive synchronized 360 spherical images as shown in Figs. 2.5, 2.6, 2.7, 2.8, 2.9 and 2.10. High-end MMS RIEGL VMX-450 and RiCOPTER with VUX-1UAV ALS are shown in Figs. 2.11 and 2.12 and the evaluation of their accuracy is published in [2, 3]. Figures 2.13 and 2.14 show TomTom’s Mobile Mapping Van and map representation for autonomous cars. Figures 2.15 and 2.16 show Leica’s hybrid sensor solution [4] that features active

© The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_2

13

14 Fig. 2.1 Handheld laser scanning system ZEB Go

Fig. 2.2 Handheld laser scanning system ZEB Horizon

Fig. 2.3 Handheld laser scanning system ZEB Pano

2 Mobile Mapping Systems

2.1 Commercial Measurement Instruments Fig. 2.4 Georeferenced laser scanning system ZEB Locate

Fig. 2.5 MMS from Viametris

Fig. 2.6 MMS from Maverick

15

16

2 Mobile Mapping Systems

Fig. 2.7 MMS from Topcon

Fig. 2.8 MMS backpack from Leica

laser and passive image sensors on the same platform. Such approaches are rapidly entering the airborne market of topographic and urban mapping improving the quality of geospatial products.

2.1 Commercial Measurement Instruments Fig. 2.9 MMS backpack from GreenValley International

Fig. 2.10 MMS backpack from Viametris

17

18 Fig. 2.11 MMS RIEGL VMX-450

Fig. 2.12 MMS RiCOPTER with VUX-1UAV ALS

Fig. 2.13 MMS by TomTom

Fig. 2.14 Map representation for autonomous cars by TomTom

2 Mobile Mapping Systems

2.2 Mobile Mapping Data

19

Fig. 2.15 MMS—The Leica CityMapper hybrid sensor by Leica

Fig. 2.16 3D colored map generated by Leica CityMapper [4]

2.2 Mobile Mapping Data 2.2.1 IMU IMU is an electronic device composed mainly of accelerometers and gyroscopes to measure the acceleration and rotation of the moving vehicle. These devices can be used in many applications such as biomedical applications [5] including measuring tibial accelerations in human health monitoring [6], mobile robotics for determination of motion parameters [7], mobile mapping system for 3D road asset inventory [8] and UAV applications [9] such as 3D state estimation [10]. IMU equipped with magnetometers (AHRS) can provide absolute heading information that comes from the Earth’s gravitational field. The difference between AHRS and IMU is that AHRS includes the embedded attitude data solution unit and heading information, and IMU only provides sensor data that are affected by incremental drift. IMU is the heart of many mobile mapping systems. It provides necessary information for an initial trajectory estimation. In UAV applications, IMU helps in navigation tasks. The choice of IMU for certain applications is the compromise between the cost of the device and the application requirements. Low-cost IMU is based on MEMS technology, where

20

2 Mobile Mapping Systems

Fig. 2.17 3D map reconstructed using ClearPath Jackal mobile robot odometry and LiDAR

Fig. 2.18 3D map reconstructed using tracking camera Intel RealSense T265 VO and LiDAR

all sensors are embedded into highly integrated electronics [11]. High-precision IMU are more expensive, and they can use FOG technology for providing accurate data [12]. More information concerning the MEMS and FOG comparison can be found in [13–16].

2.2.2 Odometry Odometry [17] is the relative incremental localization ability [18] of the moving platform equipped with encoders mounted on the wheels or tracking cameras VO. Relative localization methods retrieve the trajectory based on previous measurements, thus an incremental error is evident. In mobile robotics, the identification of odometer parameters [19] and calibration [20] is a common issue. Odometry can be built based on incremental and absolute encoders that measure the rotation of the wheel. Such measurement can be recalculated to the pose increment, thus some problems related to the limited resolution and slipperiness of the moving platform can affect the resulting accuracy. In recent years, an alternative odometry technique based on VO [21–23] appeared. The widely used implementation that provides the VO is the ORB-SLAM3 [24]—entire visual SLAM framework including VO for initial trajectory estimation. It was an inspiration for other researchers to implement other solutions such as OpenVSLAM [25] that addresses the usage of the equirectangular camera. The advantage of encoder-based odometry over VO is less computational complexity to calculate the trajectory. The major issue with VO is the scale factor in the case of a monocular camera setup. For this reason, stereo cameras are used. The comparison between encoder-based odometry and VO provided by tracking stereo camera Intel RealSense T265 is shown in Figs. 2.17 and 2.18. It can be observed that VO has less rotational errors, but the problem with the scale factor is still evident even when the stereo camera is used.

2.2 Mobile Mapping Data

21

2.2.3 Light Detection and Ranging LiDAR in general is the method for determining ranges (variable distance) by targeting an object with a laser and measuring the time for the reflected light returned to the receiver. This measurement instrument is mainly used for 3D digitization of the environment. There are many types of LiDAR applications such as TLS that uses highly accurate measurement instrument that works in so-called stop-scan fashion looking from a mobile robotics point of view. It means, that robot stops in certain goals to acquire highly accurate 3D measurements. Another family of applications are mobile mapping systems composed of highly accurate planar LiDARs, or 3D multi-beam lasers such as Velodyne. Recently, the development of solid-state LiDARs shows the potential reduction of such devices. Typical data produced by LiDAR is measured in distance and reflectance, thus white reflective objects such as lines or signs can be identified easily. An important aspect of the multi-beam LiDAR is the intrinsic calibration [26–28]. Such calibration is crucial in mobile mapping applications where the measurement instrument is heavily occupied and works assuming diverse environmental conditions.

2.2.4 Mono-, Stereo-, Spherical and Depth Cameras Cameras are the main source of deriving color information in mobile mapping system applications. There are different types of these measurement instruments mainly monocular, stereo-, equirectangular (spherical) and depth cameras. In aerial photogrammetry, the so-called metric camera is used and it is characterized by a stable, known and repeatable interior orientation, defined by fiducial marks and carried out either in the laboratory, on the job or by self-calibration [29]. Metric camera has calibrated radial distortion, thus every calibrated camera by a test field could consequently be considered as a metric camera. Consequently, it can be stated that photographs taken with a metric camera can be used for precision measurements [30]. The comparisons of 3D analysis between photogrammetry and Computer Vision are given in [31], and different approaches for epipolar resampling of the image sequence are discussed in [32]. It can be seen that in photogrammetry, the colinearity equation (Eq. 7.50) is less compact than colinearity equation used in Computer Vision (Eq. 7.5). Moreover, in photogrammetry the pose is represented as [R, t]wc (world←camera transformation) and in computer vision as [R, t]cw (camera←world transformation) where [R, t]wc = [R, t]−1 cw . Stereo cameras are combined from mono cameras with known intrinsic and extrinsic calibration parameters, thus a depth can be retrieved using epipolar geometry [33]. Such measurement instrument provides data for VO that do not suffer from the scale problem. The spherical cameras [34] (called equirectangular) are typically composed of many mono cameras to obtain a 360 ◦ C field of view. Another family of cameras are measurement

22

2 Mobile Mapping Systems

instruments using the so-called structural light to retrieve 3D measurements. The comparison of depth cameras and other sensors for 3D mobile mapping is discussed in [35].

2.3 Ground Truth Data Sources Ground truth data sources relate mobile mapping data with georeference acquired by, e.g. GNSS. The performance of GNSS is assessed using four criteria: Accuracy: the difference between measured and real positions; Integrity: a system with a capacity to provide a threshold of confidence and, in the event of an anomaly in the positioning data; Continuity: a system with the ability to function without interruption; Availability: the percentage of time a signal fulfills the above accuracy, integrity and continuity criteria.

2.3.1 Large-scale Data Sets In recent research, many open-source large data sets appeared since mobile mapping systems are more and more affordable. The complex urban data set [36] provides LiDAR data and stereo images with various position sensors targeting a highly complex urban environment. It captures features in urban environments (e.g. metropolitan areas, complex buildings and residential areas). The data of 2D and 3D typical types of LiDAR are provided. Raw sensor data for vehicle navigation and development tools are presented in an ROS file format. The Multi-Vehicle Stereo Event Dataset [37] is a collection of data designed for the development of novel 3D perception algorithms for event-based cameras. Stereo event data is collected from car, motorbike, hexacopter and handheld data, and fused with LiDAR, IMU, motion capture and GPS to provide ground truth pose and depth images. In addition, images from a standard stereo frame-based camera pair for comparison with traditional techniques are provided. Dataset Cityscapes [38] is comprised of a large, diverse set of stereo video sequences recorded in streets from 50 different cities. 5000 of these images have high-quality pixel-level annotations; 20000 additional images have coarse annotations to enable methods that leverage large volumes of weakly labeled data. The authors claim that this data set exceeds previous attempts in terms of data set size, annotation richness, scene variability and complexity. The accompanying empirical study provides an in-depth analysis of the data set characteristics, as well as a performance evaluation of several state-of-the-art approaches based on the presented benchmark. This data set relates to the previous work published in [39]. Data set [40] collects data from the AtlantikSolar UAV that is a small-sized, handlaunchable, LALE, solar-powered UAV optimized for large-scale aerial mapping and inspection applications.

2.3 Ground Truth Data Sources

23

The Oxford RobotCar Dataset [41] contains over 100 repetitions of a consistent route through Oxford, United Kingdom, captured for over a year. The data set captures many different combinations of weather, traffic and pedestrians, along with longer term changes such as construction and roadworks. Additionally, the authors provide RTK Ground Truth [42]. The Málaga Stereo and Laser Urban Data Set [43] was gathered entirely in urban scenarios with a car equipped with a stereo camera (Bumblebee2) and five LiDARs. One distinctive feature of the data set is the existence of high-resolution stereo images grabbed at a high rate (20fps) during a 36.8 km trajectory, turning the data set into a suitable benchmark for a variety of computer vision techniques. The KITTI-360 data set [44] collects data from autonomous driving platform Annieway to develop novel challenging real-world computer vision benchmarks. The tasks of interest are stereo, optical flow, visual odometry, 3D object detection and 3D tracking. For this purpose, the authors equipped a standard station wagon with two high-resolution color and grayscale video cameras. Accurate ground truth is provided by a Velodyne LiDAR and a GPS. The data sets are captured by driving around the mid-size city of Karlsruhe, in rural areas and on highways. Up to 15 cars and 30 pedestrians are visible per image. Besides providing all data in raw format, the authors extract benchmarks for each task. For each of the benchmarks, the authors also provide evaluation metrics. The authors provide several use cases: processing raw data [45], performing road benchmark [46] and stereo benchmark [47]. The Kagaru Airborne Stereo data set [21] is a vision data set gathered from a radiocontrolled aircraft flown at Kagaru, Queensland, Australia, on August 31, 2010. The data consists of visual data from a pair of downward-facing cameras, translation and orientation information as a ground truth from an XSens Mti-g INS/GPS and additional information from a Haico HI-206 USB GPS. The data set traverses over farmland and includes views of grass, an air strip, roads, trees, ponds, parked aircraft and buildings.

2.3.2 Long-Term Data Sets Long-term data sets collect multi-session, long-term data. The purpose is to address the impact of multi-season, different weather and other disturbances on localization algorithms. The KAIST multi-spectral data set [48] covers urban to residential regions for autonomous systems. This data set provides different perspectives of the world captured in coarse time slots (day and night) in addition to fine time slots (sunrise, morning, afternoon, sunset, night and dawn). For the all-day perception of autonomous systems, the authors propose the use of a different spectral sensor, i.e. a thermal imaging camera. Toward this goal, they develop a multi-sensor platform, which supports the use of a co-aligned RGB/Thermal camera, RGB stereo, 3D LiDAR and inertial sensors GPS/IMU and a related calibration technique. This data set provides a wide range of visual perception tasks including object detection, drivable region

24

2 Mobile Mapping Systems

detection, localization, image enhancement, depth estimation and colorization using a single/multi-spectral approach. The Visual-Inertial Canoe Data set [49] collects data from a canoe along the Sangamon River in Illinois. The canoe was equipped with a stereo camera, an IMU and a GPS device, which provide visual data suitable for stereo or monocular applications, inertial measurements and position data for ground truth. A canoe trip was recorded up and down the river for 44 min covering 2.7 km round trip. The data set adds to those previously recorded in unstructured environments and is unique in that it is recorded on a river, which provides its own set of challenges and constraints. University of Michigan North Campus Long-Term (NCLT) Vision and LiDAR Dataset [50] consists of omnidirectional (equirectangular) imagery, 3D LiDAR, planar LiDAR, GPS and proprioceptive sensors for odometry collected using a Segway robot. The data set was collected to facilitate research focusing on long-term autonomous operation in changing environments. The data set is comprised of 27 sessions spaced approximately biweekly for 15 months. The sessions repeatedly explore the campus, both indoors and outdoors, on varying trajectories, and at different times of the day across all four seasons. This allows the data set to capture many challenging elements including moving obstacles (e.g. pedestrians, bicyclists and cars), changing lighting, varying viewpoints, seasonal and weather changes (e.g. falling leaves and snow) and long-term structural changes caused by construction projects. The authors also provide ground truth poses for all sessions in a single frame of reference. Alderley Day/Night Dataset [51] consists of vehicle data for vision-based place recognition with manually annotated ground truth frame correspondences. The data set was captured in two different conditions for the same route: one on a sunny day and one during a rainy night. Both data sets were taken in the suburb of Alderley, Queensland.

2.3.3 Terrestrial Laser Scanning Data Terrestrial Laser Scanning Data are widely disseminated by many vendors and organizations [52, 53]. An interesting data set [54] relates to 3D woody structure of large tropical trees. Many interesting data sets acquired by mobile robots equipped with accurate terrestrial laser scanning systems are available publicly [55].

References 1. M. Bosse, R. Zlot, P. Flick, Zebedee: design of a spring-mounted 3-D range sensor with application to mobile mapping. IEEE Trans. Robot. 28(5), 1104–1119 (2012) 2. I. Toschi, P. Rodríguez-Gonzálvez, F. Remondino, S. Minto, S. Orlandini, A. Fuller, Accuracy evaluation of a mobile mapping system with advanced statistical methods. ISPRS— International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, vol. XL-5/W4, pp. 245–253 (2015)

References

25

3. B. Brede, A. Lau, H.M. Bartholomeus, L. Kooistra, Comparing riegl ricopter UAV lidar derived canopy height and DBH with terrestrial lidar. Sensors 17(10) (2017) 4. I. Toschi, F. Remondino, R. Rothe, K. Klimek, Combining airborne oblique camera and LIDAR sensors: investigation and new perspectives. ISPRS—Int. Arch. Photogramm. Remote Sens. Spatial Inf. Sci. 621, 437–444 (2018). (September) 5. J. Lourenço, L. Martins, R. Almeida, C. Quaresma, P. Vieira, Low cost inertial measurement unit for motion capture in biomedical applications, in ed. by L.M. Camarinha-Matos, A.J. Falcão, N. Vafaei, S. Najdi, Technological Innovation for Cyber-Physical Systems, Cham (Springer International Publishing, 2016), pp. 151–158 6. C.D. Johnson, J. Outerleys, A.S. Tenforde, I.S. Davis, A comparison of attachment methods of skin mounted inertial measurement units on tibial accelerations. J. Biomech. 113, 110118 (2020) 7. P. Dabek, M. Trojnacki, Comparison of configurations of inertial measurement units for determination of motion parameters of mobile robots—part 1: theoretical considerations, in ed. by R. Szewczyk, C. Zieli´nski, M. Kaliczy´nska, Automation 2018, Cham (Springer International Publishing, 2018), pp. 528–536 8. N. Sairam, S. Nagarajan, S. Ornitz, Development of mobile mapping system for 3D road asset inventory. Sensors 16(3) (2016) 9. C. Perez-D’Arpino, D. Vigouroux, W. Medina-Melendez, L. Fermin, R.R. Torrealba, J.C. Grieco, G. Fernandez-Lopez, Development of a low cost inertial measurement unit for UAV applications with kalman filter based attitude determination, in 2011 IEEE Conference on Technologies for Practical Robot Applications, pp. 178–183 (2011) 10. H. Du, W. Wang, C. Xu, R. Xiao, C. Sun, Real-time onboard 3D state estimation of an unmanned aerial vehicle in multi-environments using multi-sensor data fusion. Sensors 20(3) (2020) 11. W. Zhao, Y. Cheng, S. Zhao, X. Hu, Y. Rong, J. Duan, J. Chen, Navigation grade mems IMU for a satellite. Micromachines 12(2) (2021) 12. Y.N. Korkishko, V.A. Fedorov, V.E. Prilutskiy, V.G. Ponomarev, I.V. Fedorov, S.M. Kostritskii, I.V. Morev, D.V. Obuhovich, S.V. Prilutskiy, A.I. Zuev, V.K. Varnakov, High-precision inertial measurement unit imu-5000, in 2018 IEEE International Symposium on Inertial Sensors and Systems (INERTIAL), pp. 1–4 (2018) 13. O. Deppe, G. Dorner, S. Konig, T. Martin, S. Voigt, S. Zimmermann, Mems and fog technologies for tactical and navigation grade inertial sensors-recent improvements and comparison. Sensors 17(3) (2017) 14. D. Gautam, A. Lucieer, Z. Malenovsky, C. Watson, Comparison of mems-based and fogbased IMUS to determine sensor pose on an unmanned aircraft system. J. Surv. Eng. 143(4), 04017009, 110118 (2017) 15. D. Chulkovs, E. Grabs, A. Ipatovs, Comparison of mems and fog gyroscopes for daily use in camera stabilizing systems, in 2020 24th International Conference Electronics, pp. 1–4 (2020) 16. C. Goodall, S. Carmichael, Trusted Positioning, and B Scannell, The Battle Between Mems and Fogs for Precision Guidance (2013) 17. Mordechai Ben-Ari and Francesco Mondada. Robotic Motion and Odometry (Springer International Publishing, Cham, 2018), pp. 63–93 18. D. Davidek, J. Klecka, K. Horak, P. Novacek, Odometer module for mobile robot with position error estimation. IFAC-PapersOnLine 49(25), 346–351 (2016). 14th IFAC Conference on Programmable Devices and Embedded Systems PDES 2016 19. C.U. Dogruer, Online identification of odometer parameters of a mobile robot, in ed. by J.G. de la Puerta, I.G. Ferreira, P.G. Bringas, F. Klett, A. Abraham, A.C.P.L.F. de Carvalho, Á. Herrero, B. Baruque, H. Quintián, E. Corchado, International Joint Conference SOCO’14CISIS’14-ICEUTE’14, Cham. (Springer International Publishing, 2014), pp. 195–206 20. Y. Tu, H. Min, Calibration method of Mecanum wheeled mobile robot odometer, in 2019 Chinese Automation Congress (CAC), pp. 3014–3019 (2019) 21. M. Warren, D. McKinnon, H. He, A. Glover, M. Shiel, B. Upcroft, Large scale monocular vision-only mapping from a fixed-wing sUAS, in International Conference on Field and Service Robotics, Matsushima, Japan (2012)

26

2 Mobile Mapping Systems

22. Marhaban M. H. Saripan M. I. Aqel, M. O., N.B. Ismail, Review of visual odometry: types, approaches, challenges, and applications, in SpringerPlus, vol. 5 (2016) 23. K.L. Lim, T. Bräunl, A review of visual odometry methods and its applications for autonomous driving (2020) 24. C. Campos, R. Elvira, J.J. Gomez Rodriguez, J.M.M. Montiel, J.D. Tardos, Orb-slam3: an accurate open-source library for visual, visual-inertial and multi-map slam (2020) 25. S. Sumikura, M. Shibuya, K. Sakurada, OpenVSLAM: a versatile visual SLAM framework, in Proceedings of the 27th ACM International Conference on Multimedia, MM ’19, New York, NY, USA (ACM, 2019), pp. 2292–2295 26. C.-Y. Chen, H.-J. Chien, P.-S. Huang, W.-B. Hong, C.-F. Chen, Intrinsic parameters calibration for multi-beam lidar using the levenberg-marquardt algorithm, in Proceedings of the 27th Conference on Image and Vision Computing New Zealand, IVCNZ ’12, New York, NY, USA (Association for Computing Machinery, 2012), pp. 19–24 27. R. Bergelt, O. Khan, W. Hardt, Improving the intrinsic calibration of a velodyne lidar sensor, 11 (2017) 28. J. Levinson, S. Thrun, Unsupervised Calibration for Multi-beam Lasers (Springer, Berlin, Heidelberg, 2014), pp. 179–193 29. E. Oniga, M. Diac, Metric and non-metric cameras calibration for the improvement of real-time monitoring process results. Environ. Eng. Manag. J. 12(4), 719–726, 110118 (2013) 30. O.R. Kolbl, Metric or non-metric cameras. Photogramm. Eng. Remote Sens. 42(1), 103–113, 110118 (1976) 31. C. Zhang, W. Yao, The comparisons of 3D analysis between photogrammetry and computer vision. Int. Arch. Photogramm. Remote Sens. Spat. Inf. Sci. 37, 33–36 (2008) 32. J.-I. Kim, T. Kim, Comparison of computer vision and photogrammetric approaches for epipolar resampling of image sequence. Sensors 16(3) (2016) 33. R. Hartley, A. Zisserman, Multiple View Geometry in Computer Vision, 2nd edn. (Cambridge University Press, USA, 2003) 34. L. Barazzetti, M. Previtali, M. Scaioni, Procedures for condition mapping using 360 images. ISPRS Int. J. Geo-Inf. 9(1) (2020) 35. T. Kornuta, M. Stefanczyk, Porownanie metod akwizycji obrazow rgb-d na potrzeby rejestracji trojwymiarowych modeli obiektow. Prace Naukowe Politechniki Warszawskiej. Elektronika, z. 195, t. 2, 357–366 (2016) 36. J. Jeong, Y. Cho, Y.-S. Shin, H. Roh, A. Kim, Complex urban dataset with multi-level sensors from highly diverse urban environments. Int. J. Robot. Res. 643–657 (2019) 37. A.Z. Zhu, D. Thakur, T. Özaslan, B. Pfrommer, V. Kumar, K. Daniilidis, The multi vehicle stereo event camera dataset: an event camera dataset for 3D perception (2018). arxiv:1801.10202 38. M. Cordts, M. Omran, S. Ramos, T. Rehfeld, M. Enzweiler, R. Benenson, U. Franke, S. Roth, B. Schiele, The cityscapes dataset for semantic urban scene understanding, in Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR), June 2016 39. M. Cordts, M. Omran, S. Ramos, T. Scharwächter, M. Enzweiler, R. Benenson, U. Franke, S. Roth, B. Schiele, The cityscapes dataset, in CVPR Workshop on The Future of Datasets in Vision (2015) 40. P. Oettershagen, T. Stastny, T. Mantel, A. Melzer, K. Rudin, P. Gohl, G. Agamennoni, K. Alexis, R. Siegwart, Long-endurance sensing and mapping using a hand-launchable solarpowered UAV, in ed. by D.S. Wettergreen, T.D. Barfoot, Field and Service Robotics : Results of the 10th International Conference, vol. 113 (Springer, Berlin, 2016), pp. 441–454. 10th International Conference on Field and Service Robotics (FSR 2015); Conference Location: Toronto, Canada; Conference Date: June 24–26, 2015 41. W. Maddern, G. Pascoe, C. Linegar, P. Newman, 1 Year, 1000km: the Oxford RobotCar dataset. Int. J. Robot. Res. (IJRR) 36(1), 3–15, 110118 (2017) 42. W. Maddern, G. Pascoe, M. Gadd, D. Barnes, B. Yeomans, P. Newman, Real-time kinematic ground truth for the oxford robotcar dataset (2020). arXiv: 2002.10152 43. J.-L. Blanco, F.-A. Moreno, J. Gonzalez-Jimenez, The malaga urban dataset: high-rate stereo and lidars in a realistic urban scenario. Int. J. Robot. Res. 33(2), 207–214, 110118 (2014)

References

27

44. A. Geiger, P. Lenz, R. Urtasun, Are we ready for autonomous driving? The kitti vision benchmark suite, in Conference on Computer Vision and Pattern Recognition (CVPR) (2012) 45. A. Geiger, P. Lenz, C. Stiller, R. Urtasun, Vision meets robotics: the kitti dataset. Int. J. Robot. Res. (IJRR) (2013) 46. J. Fritsch, T. Kuehnl, A. Geiger, A new performance measure and evaluation benchmark for road detection algorithms, in International Conference on Intelligent Transportation Systems (ITSC) (2013) 47. M. Menze, A. Geiger, Object scene flow for autonomous vehicles, in Conference on Computer Vision and Pattern Recognition (CVPR) (2015) 48. S. Hwang, K. Park, J. Shin, Y. Kyounghwan An, in ed. by S. Kweon, Y. Choi, N. Kim, Kaist multi-spectral day/night dataset for autonomous and assisted driving (2018) 49. M. Miller, S.-J. Chung, S. Hutchinson, The visual-inertial canoe dataset. Int. J. Robot. Res. 37(1), 13–20, 110118 (2018) 50. N. Carlevaris-Bianco, A.K. Ushani, R.M. Eustice, University of Michigan north campus longterm vision and lidar dataset. Int. J. Robot. Res. 35(9), 1023–1035, 110118 (2015) 51. M. Milford, W. Scheirer, E. Vig, A. Glover, O. Baumann, J. Mattingley, D. Cox, Conditioninvariant, top-down visual place recognition, in 2014 IEEE International Conference on Robotics and Automation (ICRA), pp. 5571–5577 (2014) 52. UNAVCO. Terrestrial Laser Scanning (TLS) Data (2021) 53. Washington State Department of Transportation. 3D Terrestrial Laser Scanning (2021) 54. J.G. de Tanago, A. Lau, H. Bartholomeus, M. Herold, V. Avitabile, P. Raumonen, C. Martius, R.C. Goodman, M. Disney, S. Manuri, A. Burt, K. Calders, Estimation of above-ground biomass of large tropical trees with terrestrial lidar. Methods Ecol. Evol. 9(2), 223–234 (2018) 55. A. Nuchter, Robotic 3D Scan Repository (2021)

Part II

Methodology

Chapter 3

Coordinate Systems

3.1 Introduction Large-scale surveying and mapping relates to the shape of the Earth and spatial relations between objects located at close proximity to the surface. Thus, it is evident that global and local coordinate systems are useful for calculations. To describe the position in the global reference system (global geocentric Terrestrial system), the coordinates are defined with respect to the center of the Earth. It is much easier to describe spatial relations between objects using local reference system. 3D Cartesian coordinates are not very convenient for describing positions on the surface of the earth. It is actually more convenient to project the curved surface of the earth on a flat plane, it is related to the so called map projections. Usually, the local coordinate system is with the Y-axis pointing in the north direction, the Z-axis in the up direction, and the X-axis completing the pair, and therefore, pointing in the east direction. This type of system is referred to as a topocentric coordinate system. For the coordinates, it is common to use the capital letters ENU instead of X, Y, Z [1] and it is called LTP. The alternative way of expressing z coordinate as a positive number (convenient for airplanes) is NED. All observations’ equations described in this book are expressed in the right-handed local 3D Cartesian coordinate system, therefore, it is important to keep in mind the transformation function from local to global coordinate system looking from GPS data used for georeferencing [2]. Rigid transformation in SE(3) can be separated into two parts: translation and rigid rotation. There are plenty of ways to express rotations [3, 4] such as using Tait– Bryan and Euler angles, quaternions [5], Rodriguez [6–8] and e.g. Cayley formula [9] or Givens Rotations [10]. Common angle representation is yaw, pitch roll shown in Fig. 3.1, where yaw is a rotation via Z-axis, pitch is the rotation via Y-axis and roll is the rotation via X-axis. Further information how to construct transformation matrices can be found in [11–13]. The information how to compute derivatives for rotations can be found in [14, 15]. This book focuses on applications, and therefore, functions transforming rigid rotation into parameters’ space that are used in the optimization process will be derived. © The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_3

31

32

3 Coordinate Systems

z

Fig. 3.1 Common angle representation as yaw, pitch and roll. Yaw is the rotation via Z-axis, pitch is the rotation via Y-axis and roll is the rotation via X-axis

Yaw

Pitch Roll

y

x

3.2 Rotations In this section, parametrization of rigid transformation in SO(3) into Tait–Bryan, Rodrigues and quaternion are discussed. For each parameter, a listing of code in Python is given. These representations of the transformation matrix can be used to formulate SLAM optimization problem.

3.2.1 Tait–Bryan and Euler Angles In 3D space, rotations via X,Y,Z axes are given as ⎡ ⎤ 1 0 0 Rx (ω) = ⎣0 cos(ω) − sin(ω)⎦ 0 sin(ω) cos(ω) ⎤ cos(ϕ) 0 sin(ϕ) 1 0 ⎦ R y (ϕ) = ⎣ 0 − sin(ϕ) 0 cos(ϕ)

(3.1)

⎡

⎤ cos(κ) − sin(κ) 0 Rz (κ) = ⎣ sin(κ) cos(κ) 0⎦ 0 0 1

(3.2)

⎡

(3.3)

There are two different conventions for the definition of the rotation axes—proper Euler angles (R z -R x -R z , R x -R y -R x , R y -R z -R y , R z -R y -R z , R x -R z -R x , R y -R x R y ) and Tait–Bryan angles (R x -R y -R z , R y -R z -R x , R z -R x -R y , R x -R z -R y , R z R y -R x , R y -R x -R z ). In this book rigid transformation in SE(3) is calculated using Tait–Bryan convention (R x -R y -R z )—Eq. 3.4.

3.2 Rotations

33

R(ω, ϕ, κ) = Rx (ω)R y (ϕ)R z (κ) ⎡ ⎤ c(ϕ)c(κ) −c(ϕ)s(κ) s(ϕ) = ⎣c(ω)s(κ) + s(ω)s(ϕ)c(κ) c(ω)c(κ) − s(ω)s(ϕ)s(κ) −s(ω)c(ϕ)⎦ (3.4) s(ω)s(κ) − c(ω)s(ϕ)c(κ) s(ω)c(κ) + c(ω)s(ϕ)s(κ) c(ω)c(ϕ)

where s is sin and c is cos functions. The sample code in Python on how to construct transformation matrix ∈ SE(3) is given in Listing 3.1. 1

from s y m p y i m p o r t *

2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22

def rotX ( a ) : r e t u r n M a t r i x ([[1 , 0 , 0 , 0] , [0 , cos ( a ) , - sin ( a ) , 0] , [0 , sin ( a ) , cos ( a ) , 0] , [0 , 0 , 0 , 1]]) def rotY ( a ) : r e t u r n M a t r i x ([[ cos ( a ) , 0 , sin ( a ) , 0] , [0 , 1 , 0 , 0] , [ - sin ( a ) , 0 , cos ( a ) , 0] , [0 , 0 , 0 , 1]]) def rotZ ( a ) : r e t u r n M a t r i x ([[ cos ( a ) , - sin ( a ) , 0 , 0] , [ sin ( a ) , cos ( a ) , 0 , 0] , [0 , 0 , 1 , 0] , [0 , 0 , 0 , 1]]) def t r a n s X Y Z ( tx , ty , tz ) : r e t u r n M a t r i x ([[1 , 0 , 0 , tx ] , [0 , 1 , 0 , ty ] , [0 , 0 , 1 , tz ] , [0 , 0 , 0 , 1]])

23 24 25

def m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) : r e t u r n t r a n s X Y Z ( tx , ty , tz ) * rotX ( om ) * rotY ( fi ) * rotZ ( ka )

26 27 28

def m a t r i x 4 4 F r o m T a i t B r y a n Z Y X ( tx , ty , tz , om , fi , ka ) : r e t u r n t r a n s X Y Z ( tx , ty , tz ) * rotZ ( ka ) * rotY ( fi ) * rotX ( om )

29 30 31 32 33 34 35

def t a i t B r y a n F r o m M a t r i x 4 4 C a s e 1 ( m ) : o m f i k a = M a t r i x ([0 , 0 , 0]) o m f i k a [0] = a t a n 2 ( - m [1 ,2] , m [2 ,2]) # om o m f i k a [1] = a s i n ( m [0 ,2]) # fi o m f i k a [2] = a t a n 2 ( - m [0 ,1] , m [0 ,0]) # ka return omfika

36 37 38 39 40 41 42 43

def t a i t B r y a n F r o m M a t r i x 4 4 C a s e 2 ( m ) : o m f i k a = M a t r i x ([0 , 0 , 0]) o m f i k a [0] = - a t a n 2 ( m [1 ,0] , m [1 ,1]) # om o m f i k a [1] = - pi / 2.0 # fi o m f i k a [2] = 0 # ka return omfika

34 44 45 46 47 48 49

3 Coordinate Systems

def t a i t B r y a n F r o m M a t r i x 4 4 C a s e 3 ( m ) : o m f i k a = M a t r i x ([0 , 0 , 0]) o m f i k a [0] = a t a n 2 ( m [1 ,0] , m [1 ,1]) # om o m f i k a [1] = pi / 2.0 # fi o m f i k a [2] = 0 # ka return omfika Listing 3.1 Python example of constructing transformation matrix ∈ SE(3) based on Tait–Bryan angle convention

3.2.2 Euler–Rodrigues Formula Euler–Rodrigues formula was introduced in [16] and it was elaborated independently in [17]. A Rodrigues vector presents a way for geometrically constructing a rotation matrix using a rotation angle θ around the axis u = (u x , u y , u z ) [18, 19]. Equation 3.5 shows Rodrigues formula used in computer vision. R = cos θ I + sin θ [u]× + (1 − cos θ ) uu

(3.5)

⎡

⎤ 0 −u z u y [u]× = ⎣ u z 0 −u x ⎦ −u y u x 0

where:

(3.6)

is the cross product (skew-symmetric) matrix. For the Rodrigues parameters (u x , u y , u z , θ ), the rotation matrix R ∈ SO(3) is given in Eq. 3.7 ⎡

⎤ u 2x (1 − cθ ) + cθ u x u y (1 − cθ ) − u z sθ u x u z (1 − cθ ) + u y sθ R = ⎣u x u y (1 − cθ ) + u z sθ u 2y (1 − cθ ) + cθ −u x sθ + u y u z (1 − cθ )⎦ u x u z (1 − cθ ) − u y sθ u x sθ + u y u z (1 − cθ ) u 2z (1 − cθ ) + cθ (3.7) where sθ is sin(θ ) and cθ is cos(θ ) function. The rotation vector s = θ u is compact representation of the rotation. To retrieve θ from s or R, Eq. (3.8) can be used.  θ = s = arccos

Tr(R) − 1 2

 (3.8)

Consequently, retrieving u from s can be done based on Eq. 3.9. u=

s s = s θ

(3.9)

The sample code in Python on how to construct transformation matrix ∈ SE(3) from position parameters ( px , p y , pz ) and Rodrigues parameters (sx , s y , sz ) is given in Listing 3.2.

3.2 Rotations 1

35

from s y m p y i m p o r t *

2 3 4 5 6 7 8 9 10 11 12

13

14 15 16 17 18

def m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) : norm = sqrt ( ( sx ) *( sx ) + ( sy ) *( sy ) + ( sz ) *( sz ) ) ux = sx / norm uy = sy / norm uz = sz / norm theta = norm c = cos ( t h e t a ) s = sin ( t h e t a ) c1 = 1. - c rrt = M a t r i x ([[ ux * ux , ux * uy , ux * uz ] , [ ux * uy , uy * uy , uy * uz ] , [ ux * uz , uy * uz , uz * uz ]]) r_x = M a t r i x ([[0 , - uz , uy ] , [ uz , 0 , - ux ] , [ - uy , ux , 0]]) c_eye = M a t r i x ([[ c , 0 , 0] , [0 , c ,0] , [0 , 0 , c ]]) c 1 _ r r t = c1 * rrt s _ r _ x = s * r_x R = c_eye + c1_rrt + s_r_x r e t u r n M a t r i x ([[ R [0 ,0] , R [0 ,1] , R [0 ,2] , tx ] ,[ R [1 ,0] , R [1 ,1] , R [1 ,2] , ty ] ,[ R [2 ,0] , R [2 ,1] , R [2 ,2] , tz ] ,[0 ,0 ,0 ,1]])

19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36

def r o d r i g u e s F r o m M a t r i x 4 4 ( m ) : r o d r i g u e s = M a t r i x ([0 , 0 , 0]) r_x = m [2 ,1] - m [1 ,2] r_y = m [0 ,2] - m [2 ,0] r_z = m [1 ,0] - m [0 ,1] s = s q r t (( r_x * r_x + r_y * r_y + r_z * r_z ) * 0 . 2 5 ) c = ( m [0 , 0] + m [1 , 1] + m [2 , 2] - 1) *0.5 theta = acos ( c ) vth = 1 / ( 2 * s ) vth = vth * t h e t a r_x = r_x * vth r_y = r_y * vth r_z = r_z * vth r o d r i g u e s [0] = r_x r o d r i g u e s [1] = r_y r o d r i g u e s [2] = r_z return rodrigues Listing 3.2 Python example of constructing transformation matrix ∈ SE(3) based on translation vector and Rodriguez formula

Finally, compact representation of the R ∈ SO(3) for the Rodrigues parameters (sx , s y , sz ) of rotation is given in Eq. 3.10. ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ 4⎢ ⎢ ⎢ ⎣



 sx2 +s y2 +sz2 sx2 1.0−cos

+ cos

sx2 +s y2 +sz2

 sx s y 1.0−cos sx2 +s y2 +sz2 sx2 +s y2 +sz2

 sx2 +s y2 +sz2 sx sz 1.0−cos sx2 +s y2 +sz2



sx2 + s y2 + sz2

+ −



sz sin sx2 +s y2 +sz2

sx2 +s y2 +sz2 

sx2 +s y2 +sz2 s y sin

sx2 +s y2 +sz2





 sx2 +s y2 +sz2 sx s y 1.0−cos sx2 +s y2 +sz2

 s y2 1.0−cos sx2 +s y2 +sz2 sx2 +s y2 +sz2 

sx sin sx2 +s y2 +sz2

sx2 +s y2 +sz2

+

−



sx2 +s y2 +sz2 sz sin

sx2 +s y2 +sz2



+ cos sx2 + s y2 + sz2

 sx2 +s y2 +sz2 s y sz 1.0−cos sx2 +s y2 +sz2

 ⎤

sx2 +s y2 +sz2 s y sin

+ ⎥ sx2 +s y2 +sz2 sx2 +s y2 +sz2 ⎥ 

 ⎥

⎥ sx sin sx2 +s y2 +sz2 s y sz 1.0−cos sx2 +s y2 +sz2 ⎥

− + 2 2 2 ⎥ s +s +s x y z sx2 +s y2 +sz2 ⎥

 ⎥

sz2 1.0−cos sx2 +s y2 +sz2 ⎦ 2 + s2 + s2 s + cos 2 2 2 x y z sx +s y +sz

 sx2 +s y2 +sz2 sx sz 1.0−cos



(3.10)

36

3 Coordinate Systems

3.2.3 Quaternions Unit quaternions provide a convenient mathematical notation for representing spatial rotations of elements in 3D space. The quaternion is given in (3.11) that satisfies (3.12). (3.11) q = q0 + iq1 + jq2 + kq3 i 2 = j 2 = k 2 = −1 i j = k, ji = −k jk = i, k j = −i ki = j, ik = − j

(3.12)

Quaternions can be represented as an ordered pair (3.13) q = [s, v] = [s, xi + y j + zk]; s, x, y, z ∈ R

(3.13)

where s is the real scalar ∈ R and v is the vector ∈ i jk of the imaginary space. A quaternion product is given in Eq. (3.14). q a · q b = [sa sb − v a v b , sa v a + sb v b + v a × v b ]

(3.14)

A sum of quaternions is in Eq. (3.15). qa + qb = [sa , va ] + [sb , vb ] = sa + sb + (xa + xb )i + (ya + yb ) j + (z a + z b )k (3.15) The length of the quaternion is given in (3.16) with the property expressed as (3.17). ||q a || =

sa2 + xa2 + ya2 + z a2

||q a q b || = ||q a ||||q b ||

(3.16) (3.17)

A unit quaternion (||q a || = 1) stays unit when multiplied by another unit quaternion. A conjugate is a negation of the vector part of a quaternion (3.18). q a∗ = [sa , −v a ] = (sa , −xa i, −ya j, −z a k)

(3.18)

Inverting a quaternion is given in (3.19). q a−1 =

q a∗ ||q a ||

(3.19)

A geometrical interpretation of an unit quaternion q(q0 , q1 , q2 , q3 ), ||q|| = 1 which represents a general rotation is given in Eq. 3.20:

3.2 Rotations

37

q0 = cos(ω/2) q1 = X sin(ω/2) q2 = Y sin(ω/2) q3 = Z sin(ω/2)

(3.20)

Where (X, Y, Z) is the unit length axis of rotation in 3D space and ω is the angle of the unit rotation about the axis in radians. For the unit quaternion q(q0 , q1 , q2 , q3 ), ||q|| = 1, the rotation matrix R ∈ SO(3) is given as ⎡

⎤ 1 − 2q22 − 2q32 2q1 q2 + 2q0 q3 2q1 q3 − 2q0 q2 R = ⎣ 2q1 q2 − 2q0 q3 1 − 2q12 − 2q32 2q2 q3 + 2q0 q1 ⎦ 2q1 q3 + 2q0 q2 2q2 q3 − 2q0 q1 1 − 2q12 − 2q22

(3.21)

The sample code in Python on how to construct rotation matrix from quaternion and vice versa is given in Listing 3.1. 1

from sympy i m p o r t *

2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22

def m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) : q11 = q1 * q1 ; q22 = q2 * q2 ; q33 = q3 * q3 ; q03 = q0 * q3 ; q13 = q1 * q3 ; q23 = q2 * q3 ; q02 = q0 * q2 ; q12 = q1 * q2 ; q01 = q0 * q1 ; mat0 = 1 - 2 * ( q22 + q33 ) ; mat1 = 2.0*( q12 + q03 ) ; mat2 = 2.0*( q13 - q02 ) ; mat4 = 2.0*( q12 - q03 ) ; mat5 = 1 - 2 * ( q11 + q33 ) ; mat6 = 2.0*( q23 + q01 ) ; mat8 = 2.0*( q13 + q02 ) ; mat9 = 2.0*( q23 - q01 ) ; mat10 = 1 - 2 * ( q11 + q22 ) ; r e t u r n M a t r i x ([[ mat0 , mat4 , mat8 , tx ] ,[ mat1 , mat5 , mat9 , ty ] ,[ mat2 , mat6 , mat10 , tz ] ,[0 ,0 ,0 ,1]])

23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38

def q u a t e r n i o n F r o m M a t r i x 4 4 ( m ) : q = M a t r i x ([1 , 0 , 0 , 0]) T = 1 + m [0 ,0] + m [1 ,1] + m [2 ,2] S = sqrt ( T ) * 2 X = ( m [1 ,2] - m [2 ,1] ) / S Y = ( m [2 ,0] - m [0 ,2] ) / S Z = ( m [0 ,1] - m [1 ,0] ) / S W = 0.25 * S q [0] = W q [1] = -X q [2] = -Y q [3] = -Z sq = sqrt ( q [0]* q [0] + q [1]* q [1] + q [2]* q [2] + q [3]* q [3]) q [0] = q [0]/ sq q [1] = q [1]/ sq

38 39 40 41

3 Coordinate Systems q [2] = q [2]/ sq q [3] = q [3]/ sq return q

Listing 3.3 Python example of constructing transformation matrix ∈ SE(3) based on translation vector and quaternion

3.2.3.1

Quaternion Interpolation

SLERP is an example for the application of quaternions [20]. In general, it is an interpolation of a point movement along an arc given by Eq. (3.22). For t = 0, the interpolation is the starting point of the arc as q 0 . For t = 1, the interpolation is the ending point of the arc as q 1 . t SLERP(q 0 , q 1 , t) = (q 1 q −1 0 ) q0

(3.22)

3.3 Cartesian Coordinate System It uniquely determines the position of points and other geometric elements in 3D space. Cartesian coordinate system (CCS) consists of coordinate axes oriented perpendicularly to each other, known as Cartesian coordinates. There are two conventions: right-handed coordinate system and left-handed coordinate system. In this book, the right-handed coordinate system is considered, thus it satisfies the right-hand rule. This rule determines the orientation of the cross product u × v. The right-hand rule states that the orientation of the vectors’ cross product is determined by placing u and v tail-to-tail, flattening the right hand, extending it in the direction of u and then curling the fingers in the direction that the angle v makes with u. The thumb then points in the direction of u × v.

3.4 Spherical Coordinate System A spherical coordinate system is a coordinate system for 3D space. The position of a point is specified by three numbers (r, θ, ϕ) (Fig. 3.2), where r the radial distance of that point from a fixed origin (0, 0, 0), θ is the polar angle measured from a fixed zenith direction, ϕ is the azimuthal angle of its orthogonal projection on the reference plane that passes through the origin and is orthogonal to the zenith, measured from a fixed reference direction on that plane. The spherical coordinates (r, θ, ϕ) of a point x, y, z can be obtained from (3.23). ⎤ ⎡ ⎤ ⎡ 2 x + y2 + z2 r z ⎥ ⎣θ ⎦ = ⎢ (3.23) ⎣arccos √x 2 +y 2 +z 2 ⎦ y ϕ arctan( ) x

3.5 Geographic Coordinate System

39 Z

Fig. 3.2 Spherical coordinates

(r, θ, φ) r θ

φ

y

X

3.5 Geographic Coordinate System A geographic coordinate system (GCS) uses a 3D spherical surface to define locations on the Earth. A GCS includes an angular unit of measure, a prime meridian and a datum (based on a spheroid). The spheroid defines the size and shape of the earth model, while the datum connects the spheroid to the earth’s surface. A point is referenced by its longitude and latitude values. Longitude and latitude are angles measured from the earth’s center to a point on the earth’s surface. The angles often are measured in degrees (or in grads). A GCS enables geographic datasets to use common locations for integration.

3.5.1 Gauss–Krüger Geographic Coordinate System and UTM Gauss–Krüger Geographic coordinate system is named after Carl Friedrich Gauss and Johann Heinrich Louis Krüger. It is composed of the transverse Mercator projections (map projections) used in narrow zones. The Gauss–Krüger coordinates is a pair of ‘Rechtswert’ (East) and ‘Hochwert’ (North) coordinates. UTM is a map projection system for assigning coordinates to locations on the surface of the Earth. It ignores altitude and treats the Earth as a perfect ellipsoid. It consists of 60 zones, each 6degrees of longitude in width [21]. Within each zone, coordinates are measured as northings and eastings in meters. The northing values are measured from zero at the equator in a northerly direction. More information can be found in [22]. This projection is useful to construct the SLAM system having the origin in the center of projections and using Cartesian coordinate system instead of the Spherical coordinate system.

40

3 Coordinate Systems

References 1. M. Hotine, Geodetic Coordinate Systems (Springer, Berlin, 1991), pp. 65–89 2. George P. Gerdan, Rodney E. Deakin, Transforming cartesian coordinates x, y, z to geographical coordinates. Aust. Surv. 44(1), 55–63 (1999) 3. J. Pujol, Hamilton, rodrigues, gauss, quaternions, and rotations: a historical reassessment. Commun. Math. Anal. 13(2), 1–14 (2012) 4. F.S. Grassia, Practical parameterization of rotations using the exponential map. J. Graph. Tools 3(3), 29–48 (1998) 5. M. Jolde¸s, J.-M. Muller, Algorithms for manipulating quaternions in floating-point arithmetic, in ARITH-2020 - IEEE 27th Symposium on Computer Arithmetic, Proceedings of ARITH-2020, IEEE 27th Symposium on Computer Arithmetic, Portland, United States (2020). IEEE, pp. 1–8 6. Jian S. Dai, Euler-Rodrigues formula variations, quaternion conjugation and intrinsic connections. Mech. Mach. Theory 92, 144–152 (2015) 7. K.K. Liang, Efficient conversion from rotating matrix to rotation axis and angle by extending rodrigues’ formula (2018) 8. G. Terzakis, M. Lourakis, D. Ait-Boudaoud, Modified rodrigues parameters: an efficient representation of orientation in 3d vision and graphics. J. Math. Imaging Vis. 60(3), 422–442 (2018) 9. Siddika Ozkaldi, Halit Gundogan, Cayley formula, euler parameters and rotations in 3dimensional lorentzian space. Adv. Appl. Clifford Algebr. 20(2), 367–377 (2010) 10. N. Demmel, C. Sommer, D. Cremers, V. Usenko, Square root bundle adjustment for largescale reconstruction, in IEEE Conference on Computer Vision and Pattern Recognition (CVPR) (2021) 11. James Diebel, Representing attitude: Euler angles, unit quaternions, and rotation vectors. Matrix 58(15–16), 1–35 (2006) 12. J.-L. Blanco, A tutorial on SE(3) transformation parameterizations and on-manifold optimization. Technical Report, University of Malaga (2010) 13. X. Gao, T. Zhang, Y. Liu, Q. Yan, 14 lectures on visual SLAM: from theory to practice. Technical Report (2017) 14. G. Gallego, A. Yezzi, A compact formula for the derivative of a 3-d rotation in exponential coordinates. J. Math. Imaging Vis. 51(3), 378–384 (2014) 15. J. Sola, J. Deray, D. Atchuthan, A micro lie theory for state estimation in robotics (2020) 16. L. Euler, Nova methodus motum corporum rigidorum determinandi. Novi Commentari Acad. Imp. Petrop. 20(1775), 208–238 (1775) 17. O. Rodrigues, Des lois géométriques qui régissent les déplacements d’un systéme solide dans l’espace, et de la variation des coordonnees provenant de ces déplacements consideres indépendamment des causes qui peuvent les produire. J. Math 5(1840), 380–440 (1840) 18. J.S. Dai, Geometrical Foundations and Screw Algebra for Mechanisms and Robotics (Translated From Screw Algebra and Kinematic Approaches for Mechanisms and Robotics, 2016) (Higher Education Press, Beijing, 2014). ISBN 9787040334838 19. K.H. Hunt, J. Davidson, Robots and Screw Theory, Applications of Kinematics and Statics to Robotics (Oxford University Press, New York, 2004) 20. K. Shoemake, Animating rotation with quaternion curves. SIGGRAPH Comput. Graph. 19(3), 245–254 (1985) 21. M. Rudnicki, T.H. Meyer, Methods to convert local sampling coordinates into geographic information system/global positioning systems (GIS/GPS)-compatible coordinate systems. Northern J. Appl. For. 24(3), 233–238 (2007) 22. J.P. Snyder, Map projections - a working manual (1987)

Chapter 4

Simultaneous Localization and Mapping

4.1 Introduction The term SLAM [1] corresponds to the so-called “chicken and egg dilemma”. Therefore, it is necessary to have a proper map representation that is compatible with all the observations derived from the sensors to localize the vehicle within the map, and accurate localization is needed to build this map simultaneously. Obviously, it relates to the performance and the robustness of the approach. Looking at autonomous mobile robots, the accuracy is less important than the consistency of the mapping and localization results. The core concept is the pose that represents a position and an orientation at a given time. A set of consecutive poses produces a trajectory that reconstructs a map based on raw measurements, e.g. the point cloud in the case of using LiDAR. The calibration parameters have to be also considered to assure a proper transformation from the trajectory pose to the origin of the sensor. This book concerns the concepts and methods known from mobile robotics and geodesy domains. These domains introduce a methodology for map building based on computing the absolute pose of the measurement instruments assuming raw information is typically transformed into a feature space [2]. It is addressed how to fill the gap between these domains, which is also discussed in [3].

4.2 Weighted Non-linear Least Squares Optimization This section concerns a Weighted Non-Linear Least Squares method a special case of a Generalized Least Squares, known, e.g. in photogrammetry [4] and LiDAR data matching [5]. The core component is the observation equation that models an information derived from the mobile mapping measurement instruments and the other possible information sources. This model can be used for calculating an optimal trajectory and intrinsic and extrinsic parameters of the measurement instruments.

© The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_4

41

42

4 Simultaneous Localization and Mapping

Thus, all observations equations elaborated in this book can be used for building a large-scale optimization system using this method.

4.2.1 Introduction An observation Eq. (4.1) is composed of a target value yi , a model function  [β] (x i ) and its residual ri defined as a difference between the target value and the value of the model function for x i and a state vector β ri =  r esidual

yi 

target value

−  [β] (x i )   

(4.1)

model f unction

where β is the vector of n adjustable parameters by the optimization process (Weighted Non-Linear Least Squares Optimization). This method finds the optimal n parameter values (β) by minimizing the objective function being a sum of C squared residuals (4.2). SS R =

C 

ri2 =

i=1

C   i=1



yi −  [β] (x i ) 

2

(4.2)



objective f unction

therefore, the optimization problem is defined as (4.3) ∗

β = min β

C  

2 yi −  [β] (x i )

(4.3)

i=1

where there are C correspondences between target values and values of the model function. The optimal solution (minimum value of SSR) occurs when the gradient is zero. Model function  [β] (x i ) contains n parameters of β; thus, there are n gradient Eq. (4.4) derived from (4.2). C C   ∂ [β] (x i ) ∂ SS R ∂ri =2 ri = −2 ri = 0 ( j = 1, . . . , n) ∂β j ∂β j ∂β j i=1 i=1

(4.4)

To solve such an optimization problem, we can use in some cases the so-called closed-form solution [6, 7]; therefore, the solution is obtained in a single step. Such a practical application is ATE evaluation metric calculation that will be discussed in Sect. 6.2. ATE is commonly used for the trajectory accuracy evaluation based on known ground truth data obtained typically with an accurate GPS. The Horn method

4.2 Weighted Non-linear Least Squares Optimization

43

Fig. 4.1 Calculation of the transformation matrix SE(3) between the trajectory (blue line) and corresponding GPS measurements (red dots) with the closed-form Horn method [6, 7]. The correspondences between the poses and exact GPS measurements are known

finds transformation matrix SE(3) in a single step due to known correspondences between two point clouds. In that sense, the trajectory and the corresponding GPS data are the input for the Horn method as shown in Fig. 4.1 where also the result of the registration is shown. In other cases, the optimization problem is solved iteratively; thus, (4.5) β ≈ β k+1 = β k + β where β is a vector of increments, β k is a current vector of adjustable parameters and β k+1 is an updated vector. It is evident that an initial guess for β k=1 in first iteration has to be taken into an account. Moreover, SLAM problem is non-linear due to rotations; therefore, it can be linearized by approximation to a first-order Taylor polynomial expansion about β k (4.6)  [β] (x i ) ≈  [β k ] (x i ) +

n n    ∂ [β k ] (x i )   β j − β kj =  [β k ] (x i ) + J i j β j ∂β j j j

(4.6) From observation Eq. (4.1), we can derive (4.7) ri = yi −  [β] (x i ) ⇒ r

r  ∂ri ∂ [β] (x i ) = Jij = − = − J i j , i ∈ 1 . . . C, j ∈ 1 . . . n ∂β j ∂β j (4.7) 

where J is Jacobian—the partial derivative of the objective function and J is Jacobian of the model function, and they are given by Eq. (4.8) and change over optimization iterations. Jacobian is sometimes called design matrix A [4, 8].

44

4 Simultaneous Localization and Mapping ∂r1 ⎤ . . . ∂β n ∂r2 ⎥ . . . ∂β n ⎥ r ∂r3 ⎥ . . . J= ∂βn ⎥ ⎥= ⎥ ⎢ .. ⎣· · · · · · · · · . · · ·⎦ ∂rC ∂rC ∂rC C . . . ∂r ∂β1 ∂β2 ∂β3 ∂βn ⎡ ∂ (x ) ∂ (x ) ∂ (x ) [β] 1 [β] 1 [β] 1 ... ∂β2 ∂β3 1 ⎢ ∂ ∂β (x ) ∂ (x ) ∂ [β] 2 [β] 2 [β] (x 2 ) ⎢ ∂β ... ⎢ ∂ 1(x ) ∂ ∂β2(x ) ∂ ∂β3(x )  [β] 3 [β] 3 ⎢ [β] 3 ... − J = − ⎢ ∂β1 ∂β2 ∂β3 ⎢ .. ⎢ . ··· ··· ⎣ ··· ∂ [β] (x C ) ∂ [β] (x C ) ∂ [β] (x C ) ... ∂β1 ∂β2 ∂β3

⎡ ∂r1

∂β1 ⎢ ∂r2 ⎢ ∂β1 ⎢ ∂r3 ⎢ ∂β1 ⎢

∂r1 ∂β2 ∂r2 ∂β2 ∂r3 ∂β2

∂r1 ∂β3 ∂r2 ∂β3 ∂r3 ∂β3

⎤

(4.8)

∂ [β] (x 1 ) ∂βn ∂ [β] (x 2 ) ⎥ ⎥ ∂βn ⎥ ∂ [β] (x 3 ) ⎥ ⎥ ∂βn

···

∂ [β] (x C ) ∂βn

⎥ ⎥ ⎦

The residuals for kth iteration are given in (4.9) yi = yi −  [β k ] (x i )

(4.9)

Therefore, Eq. (4.1) can be rewritten as (4.10)     ri = yi −  [β k ] (x i ) +  [β k ] (x i ) −  [β] (x i )

(4.10)

Taking into the account Eqs. (4.9), (4.10) becomes (4.11).   ri = yi +  [β k ] (x i ) −  [β] (x i )

(4.11)

From Eqs. (4.6) and (4.11), we obtain Eq. (4.12). ⎛

⎞ n    J i j β j ⎠ ≈ yi − J i j β j ri ≈ yi + ⎝ [β k ] (x i ) −  [β k ] (x i ) − j

j=1

(4.12) Combining (4.7) with gradient Eq. (4.4), we obtain (4.13). C C    ∂ SS R ∂ri =2 ri = −2 J i j ri = 0 ( j = 1, . . . , n) ∂β j ∂β j i=1 i=1

Including (4.12) into (4.13) gives (4.14).

(4.13)

4.2 Weighted Non-linear Least Squares Optimization

45

  C n     ∂ SS R = −2 J i j yi − J ik βk = ∂β j i=1 k=1 −2

C C n      J i j yi + 2 Jij J ik βk = i=1

−2

i=1

(4.14)

k=1

C C  n      J i j yi + 2 J i j J ik βk = 0 ( j = 1, . . . , n) i=1

i=1 k=1

Finally, we obtain n linear equations (4.15). n C  C      J i j J ik βk = J i j yi ( j = 1, . . . , n) i=1 k=1

(4.15)

i=1 

Furthermore, (4.15) can be rewritten in matrix form (4.16) incorporating Jacobian J of the model function , desired update of β and residuals  y.  



J  Jβ = J   y

(4.16)

r

For the Jacobian J of the objective function SS R from Eq. (4.2), the first-order Taylor polynomial expansion of the model function about β k is given in Eq. (4.17) due to r



J = − J from Eq. (4.7).  [β] (x i ) ≈  [β k ] (x i ) −

n 

r

J i j β j

(4.17)

j

Therefore, Eq. (4.12) becomes (4.18). ri ≈ yi +

n  k=1

Similarly to (4.14), we obtain (4.19).

r

J ik βk

(4.18)

46

4 Simultaneous Localization and Mapping

  C C n    r r ∂ SS R ∂ri =2 ri = 2 J i j yi + J ik βk = ∂β j ∂β j i=1 i=1 k=1 2

C 

r

J i j yi + 2

C 

i=1

2

C 

r

J i j yi + 2

i=1

r

Jij

i=1

C  n 

r

n 

r

J ik βk =

(4.19)

k=1

r

J i j J ik βk = 0 ( j = 1, . . . , n)

i=1 k=1

Again we obtain n linear equations (4.20) which can be rewritten in matrix notation (4.21). n C  C   r r r J i j J ik βk = − J i j yi ( j = 1, . . . , n) (4.20) i=1 k=1

i=1 r

r

r

J  Jβ = − J   y

(4.21)

Formulas (4.16) and (4.21) are defining equations of the Gauss–Newton algorithm given in Sect. 4.2.2.2. By introducing weights w for observations, Eq. (4.2) becomes (4.22) C  wi ri2 (4.22) SS R = i=1

Therefore, we obtain (4.23) 





J  W Jβ = J  W  y

(4.23)

where W is a diagonal weight matrix. An important remark is that a number of the observation equations C should be more than or equal to n adjustable parameters β of model function. More information concerning observations and Least Squares method can be found in [9, 10]. Assuming that observational errors are uncorrelated, the weight matrix W is diagonal; otherwise, it is evident to incorporate, e.g. the covariance matrix [11] that will correlate unknowns during the optimization step. Such a technique could help in, e.g. following the optimization updates assuming the motion model as the desired direction of the movement. It is explained in Sect. 6.2. The covariance matrix is also used in SLAM probabilistic formulation discussed in Sect. 4.3. Therefore, to build optimization problem assuming correlation of the observational errors, the weight matrix W is rotated what is shown in Eq. (4.24).

4.2 Weighted Non-linear Least Squares Optimization 

47





J  RW R Jβ = J  RW R  y 





RW R =

⇒

= −1

J   Jβ = J   y ⇒ 



(4.24)



J   −1 Jβ = J   −1  y where R is a rotation matrix, W is the diagonal weight matrix,  is the information matrix and  is the covariance matrix. The information matrix is an inverse of the covariance matrix; thus,  =  −1 .

4.2.1.1

Other Notation

In other notation (e.g. in photogrammetry [4]), the optimization problem is formulated in Eq. (4.25) (4.25) A R P R Ax = A R P R b where A is a design matrix—Jacobian of the model function, P is weight matrix and b is residual part. In this case, the optimization problem can be solved using Gauss–Newton’s method described in Sect. 4.2.2.2. A similar approach can be found in work on continuous 3D scan matching [12] where a Cauchy function is applied to cope with outliers. The robust least squares approach is discussed in Sect. 4.2.3. To solve the single iteration as x = ( A R P R A)−1 ( A R P R b)

(4.26)

the sparse Cholesky factorization[13] can be used in case where ( A R P R A) is invertible.

4.2.1.2

Data Fusion

An exact definition of a data fusion is not straight forward, and it depends on the specific domain. For example, in the geospatial domain, it is equivalent to data integration. According to the methodology elaborated in this book, the data fusion is performed when all of the observation equations describing certain optimization systems can be integrated/fused into a single Jacobian. This approach covers the probabilistic approach known in mobile robotics described in Sect. 4.3. We start p p p with an example SLAM system composed of three poses p1 = (x1 , y1 , θ1 ), p2 = p p p p p p 1 2 l l l l 3 (x2 , y2 , θ2 ), p = (x3 , y3 , θ3 ); two landmarks l = (x1 , y1 ), l = (x2 , y2 ); trajectory composed of two relative poses (r elative1 , r elative2 ) and an information that first pose should be fixed during the optimization process. First landmark is observed from the first and the second pose, and the second landmark is observed from the third pose. It is shown in Fig. 4.2. The state vector β of unknown variables x is composed

48

4 Simultaneous Localization and Mapping

p

p

p

p

p

p

Fig. 4.2 SLAM system composed of three poses p1 = (x1 , y1 , θ1 ), p2 = (x2 , y2 , θ2 ), p3 = p p p (x3 , y3 , θ3 ); two landmarks l 1 = (x1l , y1l ), l 2 = (x2l , y2l ); trajectory composed of two relative poses (r elative1 , r elative2 ) and the information that the first pose should be fixed during the optimization process. First landmark is observed from the first and the second pose, and the second landmark is observed from the third pose

of 13 elements and it is given in Eq. 4.27. ⎛

⎞

⎜ p p p p p p p p p⎟ β = ⎝ x1l , y1l , x2l , y2l , x1 , y1 , θ1 , x2 , y2 , θ2 , x3 , y3 , θ3 ⎠                pose1

landmar k1 landmar k2

pose2

(4.27)

pose3

To add the information that the first pose should be fixed during the optimization process, the instance of the observation equation given as (4.28) is considered 1, f i xed

r1

1, f i xed

= y1

p

p

p

1, f i xed

−  1[β] (x1 , y1 , θ1 ) = y1

f i xed

−  1[β] (x 11 )

(4.28)

f i xed

where r 1 is vector of residuals, y1 is the vector of target values and  1[β] (x 1 ) is the model function. For this SLAM system, we can compose two instances of the observation equations with model function  2 , vector of target values yr elative and the vector of residuals r r elative relating three poses into the trajectory as (4.29). p

p

p

p

p

p

elative elative elative = y2,r −  2[β] (x1 , y1 , θ1 , x2 , y2 , θ2 ) = y2,r −  2[β] (x 21 ) r 2,r 1 1 1 p p p p p p 2,r elative 2,r elative 2,r elative 2 r2 = y2 −  [β] (x2 , y2 , θ2 , x3 , y3 , θ3 ) = y2 −  2[β] (x 22 ) (4.29) We can add to this system three instances of observation equations relating three poses and two landmarks p ⇔ l as (4.30).

4.2 Weighted Non-linear Least Squares Optimization 3, p⇔l

3, p⇔l

p

p

49 p

3, p⇔l

r1 = y1 −  3[β] (x1l , y1l , x1 , y1 , θ1 ) = y1 −  3[β] (x 31 ) 3, p⇔l 3, p⇔l p p p 3, p⇔l = y2 −  3[β] (x1l , y1l , x2 , y2 , θ2 ) = y2 −  3[β] (x 32 ) r2 3, p⇔l 3, p⇔l p p p 3, p⇔l 3 l l = y3 −  [β] (x2 , y2 , x3 , y3 , θ3 ) = y3 −  3[β] (x 33 ) r3

(4.30)

The SLAM system for this particular example is defined as (4.31) ⎛

⎞

⎜ ⎟ 2 3   ⎜ 1  2  3    ⎟ 1 1 2 2 2 2 3 3 2⎟ y y y β ∗ = min ⎜ −  (x ) + −  (x ) + −  (x ) 1 [β] 1 i [β] i i [β] i ⎟= β ⎜   i=1 ⎝ ⎠ i=1       block of ‘fixed’ block of ‘relative’ C K    K

min β

block of ‘ p⇔l’



 k 2

yik −  k[β] x i

k=1 i=1

(4.31) where there are K blocks of the observations equations of the same type, and in each block, we have C K instances of these observation equations. Equation (4.31) is the core concept for building the SLAM systems. It can be easily modified for a robust approach that is discussed in Sect. 4.2.3, for LT approach discussed in Sect. 4.2.4 and for probabilistic approach that is discussed in Sect. 4.3. Finally, in this book, the data fusion term is related to the optimization system that can be defined, e.g. as (4.31) or its modification and where the observation equations relate the unknowns; thus, single Jacobian contains all the partial derivatives necessary for solving the r

optimization problem. For this particular optimization problem, the Jacobian J of the objective function is given in (4.32).

r

J=

4.2.1.3

 1[β] (x 11 )  2[β] (x 21 )  2[β] (x 22 )  3[β] (x 31 )  3[β] (x 32 )  3[β] (x 33 )

l 1 l 2 p1 p2 p3 ⎤ ⎡ J13 ⎥ ⎢ J23 J24 ⎥ ⎢ ⎢ J34 J35 ⎥ ⎥ ⎢ ⎥ ⎢ J41 J43 ⎥ ⎢ ⎦ ⎣ J51 J54 J62 J65

(4.32)

One-Dimensional Example

Figures 4.3 and 4.4 demonstrate model function and the visualization of the optimization problem for observation Eq. (4.33) with offset y = 1.0 (black line), where r is residual, 0 is the target value and y −  [x] (x) = 1.0 − (0.1x 3 + 0.5x 2 ) is the model function.

50

4 Simultaneous Localization and Mapping

Fig. 4.3 Visualization of the model function of the Eq. (4.33). Red: 0.1x 3 + 0.5x 2 ; black: offset

Fig. 4.4 Visualization of the optimization problem for observation Eq. (4.34) (red color); offset (black line)

r =  r esidual

0 

target value

− (y −  [x] (x)) =    model f unction

0 

target value

  − 1.0 − (0.1x 3 + 0.5x 2 )    model f unction

(4.33) The optimization problem is defined in Eq. (4.34).  2 min 0 − (1.0 − (0.1x 3 + 0.5x 2 )) x

(4.34)

Figures 4.5 and 4.6 demonstrate model function and the visualization of the optimization problem for observation Eq. (4.35) for different offset equals 2.3 (black line). (4.35) r = 0 − (y −  [x] (x)) = 0 − (2.3 − (0.1x 3 + 0.5x 2 ) The optimization problem is defined in Eq. (4.36). 2  min 0 − (2.3 − (0.1x 3 + 0.5x 2 )) x

(4.36)

For both cases, the same  function is used with different y offsets. It is non-linear and there is only one adjustable parameter by the optimization process and only one observation equation; thus, C equals n. Therefore, the non-linear least squares optimization method finds the optimal solution as an intersection of the black line

4.2 Weighted Non-linear Least Squares Optimization

51

Fig. 4.5 Visualization of the model function for the observation Eq. (4.35). Red: 0.1x 3 + 0.5x 2 ; black: offset

Fig. 4.6 Visualization of the optimization problem for the observation Eq. (4.36) (red color); offset (black line)

and the red curve. The Jacobian of the objective function has only one element and it is given in (4.37). r   (4.37) J = 0.3x 2 + x It can be seen that there are three possible solutions for (4.33) and only one for (4.35), and they are determined by intersections of the red curve with the offset. This example shows that in such a case with a multiple solutions, the optimization process relies on an initial guess what is shown in Figs. 4.7, 4.8 and 4.9. The second use case shows that a local minimum could occur; therefore, the optimization process searches for the solution with many iterations around this potential result. It is shown in Figs. 4.11 and 4.10. The Python code for this example is given in Listing 4.1.

52 Fig. 4.7 First solution. Blue dot—solution; green line—incremental steps of optimization process

Fig. 4.8 Second solution. Blue dot—solution; green line—incremental steps of optimization process

Fig. 4.9 Third solution. Blue dot—solution; green line—incremental steps of optimization process

Fig. 4.10 Small number of iterations due to satisfactory initial guess. Blue dot—solution; green line—incremental steps of the optimization process

4 Simultaneous Localization and Mapping

4.2 Weighted Non-linear Least Squares Optimization

53

Fig. 4.11 Many iterations around the potential result due to a bad initial guess. Blue dot—solution; green line—incremental steps of the optimization process

1

from s y m p y i m p o r t *

2 3 4

x , y = s y m b o l s ( ’x y ’) all_symbols = [x]

5 6 7 8 9 10

psi = 0 . 1 * ( x ) *( x ) *( x ) + 0 . 5 * ( x ) *( x ) target_value = 0 m o d e l _ f u n c t i o n = y - psi o b s _ e q = M a t r i x ([ t a r g e t _ v a l u e - m o d e l _ f u n c t i o n ]) . vec () obs_eq_jacobian = obs_eq . jacobian ( all_symbols ) r

Listing 4.1 Generating Jacobian J of the objective function for one dimensional example

4.2.1.4

Two-Dimensional Example

Figure 4.12 demonstrates model function (2D Gaussian function) and Fig. 4.13 shows the visualization of the optimization problem for observation Eq. (4.38) with offset z = 1.0 (black plane).  r =  r esidual

0 

target value

− (z −  [x,y] (x, y)) =    model f unction

0 

target value

2

2

(y−2) − (x−1) 50 − 50

− 1.0 − e  

 

model f unction

(4.38) (x, y) = where r is the residual, 0 is the target value and z −  [x,y]   2 (y−2)2 − (x−1) − 50 1.0 − e 50 is the model function. The optimization problem is defined in Eq. (4.39).   2 (y−2)2 (x−1)2 min 0 − 1.0 − e− 50 − 50 (4.39) x,y

54

4 Simultaneous Localization and Mapping

Fig. 4.12 Green to red (x−1)2

color: e− 50 − plane: z offset

(y−2)2 50

; black

Fig. 4.13 Visualization of observation Eq. (4.38). In this example, one optimal solution exists

Fig. 4.14 Green to red (x−1)2

color: e− 50 − plane: z offset

(y−2)2 50

; black

Figures 4.14 and 4.15 demonstrate model function (2D Gaussian function) and the visualization of the optimization problem for observation Eq. (4.40) with offset z equals 0.5 (black plane).     (y−2)2 (x−1)2 r = 0 − z −  [x,y] (x, y) = 0 − 0.5 − e− 50 − 50

(4.40)

The optimization problem is defined in Eq. (4.41).   2 2 2 − (x−1) − (y−2) 50 50 min 0 − 0.5 − e x,y

(4.41) r

For both observation Eqs. (4.38) and (4.40), the Jacobian of the objective function J has two elements and it is given in (4.42).

4.2 Weighted Non-linear Least Squares Optimization

55

Fig. 4.15 Visualization of the optimization problem (green to red color)—infinite solutions

Fig. 4.16 Visualization of an unique solution for observation Eq. (4.38). Blue dot—solution; blue line—incremental steps of optimization process

Fig. 4.17 Visualization of example solution for observation Eq. (4.40). Blue dot—solution; blue line—incremental steps of optimization process

r

J=

 1 25

−

x 25



2

e

2

(y−2) − (x−1) 50 − 50

2 25

−

(y−2) y  − (x−1) e 50 − 50 25 2

2

 (4.42)

It can be seen in Fig. 4.16 that there is one optimal solution for (4.38) and infinite solutions for (4.40) in Figs. 4.17, 4.18, 4.19 and 4.20. Moreover, model function generates 2D optimization problem; therefore, C = 1 and it is less than n = 2. In such a case, it is impossible to incorporate non-linear least squares optimization without introducing another observation equations such as (4.43) (fixed optimized parameter discussed in Sect. 9.2), assuming very small weight w x x (x, y) = 0,  [x,y] (x, y) = x k r x = xmk −  [x,y] y

y

r y = ymk −  [β] (x, y) = 0,  [β] (x, y) = y k

(4.43)

where x k , y k are, respectively, x,y values of the model function for current k iteration of the optimization process and xmk = x k , ymk = y k are the target values. Thus, r

Jacobian J of the objective function is given in (4.44)

56

4 Simultaneous Localization and Mapping

Fig. 4.18 Visualization of example solution for observation Eq. (4.40). Blue dot—solution; blue line—incremental steps of optimization process

Fig. 4.19 Visualization of example solution for observation Eq. (4.40). Blue dot—solution; blue line—incremental steps of optimization process

Fig. 4.20 Visualization of example solution for observation Eq. (4.40). Blue dot—solution; blue line—incremental steps of optimization process

⎡ r

J =⎣

1 25

−

x 25



(x−1)2 50

e− −1 0

2

− (y−2) 50

2 25

−

(y−2) y  − (x−1) e 50 − 50 25

and example weight matrix W is given in (4.45). ⎤ ⎡ 1 0 0 W = ⎣0 10−9 0 ⎦ 0 0 10−9

2

0 −1

2

⎤ ⎦

(4.44)

(4.45)

4.2 Weighted Non-linear Least Squares Optimization

57

Figure 4.16 shows an unique solution for combined observation Eqs. (4.38) and (4.43) into the single optimization problem. Figures 4.17, 4.18, 4.19 and 4.20 show examples of different, not unique solutions for combined observation Eqs. (4.40) and (4.43) into the single optimization problem. To reduce the number of potential solutions, the additional constraints (observation equations) have to be incorporated and it will be elaborated in Chap. 9. The Python code for this example is given in Listing 4.2. 1

from s y m p y i m p o r t *

2 3 4 5 6

x ,y , z = s y m b o l s ( ’ x y z ’) a l l _ s y m b o l s = [x , y ] ro_x =5 ro_y =5

7 8

psi = exp ( -((( x -1) *( x -1) ) /(2* ro_x * ro_x ) + (( y -2) *( y -2) ) /(2* ro_y * ro_y ) ) )

9 10 11 12 13

target_value = 0 m o d e l _ f u n c t i o n = z - psi o b s _ e q = M a t r i x ([ t a r g e t _ v a l u e - m o d e l _ f u n c t i o n ]) . vec () obs_eq_jacobian = obs_eq . jacobian ( all_symbols ) r

Listing 4.2 Generating Jacobian J of the objective function for two dimensional example

4.2.2 Solvers This section provides all necessary information concerning solving the system of non-linear equations. First, Newton’s method is elaborated. Then, Gauss–Newton’s and Levenberg–Marquardt’s methods are described.

4.2.2.1

Newton’s Method

We start with quadratic Taylor series of the function r (β + δβ) [8] given by Eq. (4.46) 1 r (β + δβ) ≈ r (β) + g  δβ + δβ  Hδβ 2    quadratic local model

∂r (β) g≡ ∂β    gradient vector 2

∂ r H≡ (β) ∂β 2    Hessian matrix

(4.46)

58

4 Simultaneous Localization and Mapping

where g is a gradient vector and H is a Hessian matrix. The geometric transformation shown by the Hessian H relates to the gradient change of the function. Considering Hessian as a linear transformation as other matrices, the eigenvectors of the Hessian matrix represent the principal axis of this transformation and the eigenvalues represent the degree of this transformation. Thus, it describes the local curvature of the function of many variables. This curvature gives an information that the given function is more convex or concave. For the eigenvalues that are all positive, it makes the given function more convex. Conversely, if the eigenvalues are all negative, it makes the given function more concave. If some of the eigenvalues are positive, and some are negative, the given function has a saddle shape. In our case, it is assumed that the Hessian is positive definite and its matrix form is given in (4.47). ⎤ ⎡ ∂ 2r 2 ∂ 2r ∂ 2r ∂ 2r r · · · ∂β∂1 ∂β ∂β1 ∂β2 ∂β1 ∂β3 ∂β1 ∂β4 ∂β12 n ⎢ ∂ 2r 2 2 2 r ∂ 2r r ⎥ ⎥ ⎢ ∂β ∂β ∂ r2 ∂β∂ ∂β · · · ∂β∂2 ∂β 2 3 ∂β2 ∂β4 n⎥ ⎢ 22 1 ∂β2 2 2 2 2 ⎢ ∂r r ∂ r ∂ r r ⎥ · · · ∂β∂3 ∂β ⎥ ⎢ ∂β3 ∂β1 ∂β∂3 ∂β ∂β3 ∂β4 ∂β32 2 n⎥ ⎢ 2 ∂ 2r ∂ 2r ∂ 2r ∂ 2r ⎥ (4.47) H =⎢ ∂r ⎢ ∂β4 ∂β1 ∂β4 ∂β2 ∂β4 ∂β3 ∂β42 · · · ∂β4 ∂βn ⎥ ⎥ ⎢ . .. .. .. . . . ⎥ ⎢ . . .. ⎥ ⎢ . . . . ⎣ .. ∂ 2 r ⎦ ∂ 2r ∂ 2r ∂ 2r ∂ 2r . ∂β 2 ∂βn ∂β1 ∂βn ∂β2 ∂βn ∂β3 ∂βn ∂β4 n

Thus, the quadratic local model has a unique global minimum and it can be found ∂r explicitly using linear algebra. Setting ∂β (β + δβ) ≈ Hδβ + g to zero for a stationary point results in the Newton step expressed as (4.48). δβ = −H −1 g

(4.48)

In our case, Newton’s method optimizes iteratively the quadratic objective function of sum squared residuals expressed as Eq. (4.2) via the approximation using Eq. (4.49) β k+1 ≈ β k + δβ = β k − H −1 g

(4.49)

where β is a set of parameters and H and g are a Hessian matrix and a gradient of the function (4.2). The gradient g is calculated with Eq. (4.4). The Hessian matrix H is calculated with Eq. (4.50). H jk = 2

 C   ∂ri ∂ri ∂ 2 ri + ri ∂β j ∂βk ∂β j ∂βk i=1

(4.50)

    2  ∂ri ∂ri   ri   Assuming ri ∂β∂ j ∂β   , the Hessian can be approximated by ignoring the ∂β ∂β k j k second-order derivative terms and it can be expressed as (4.51)

4.2 Weighted Non-linear Least Squares Optimization

H jk ≈ 2

C 

r

59 r

J i j J ik

(4.51)

i=1 r

r

r

∂ri ∂ri and J ik = ∂β are elements of the Jacobian J of the objective funcwhere J i j = ∂β j k tion derived in Eq. (4.7). The matrix notation for the gradient and the approximated Hessian is given in (4.52) r

r

r

g = 2 J r, H ≈ 2 J  J

(4.52)

what gives the foundation for Gauss–Newton’s method discussed in next Sect. 4.2.2.2.

4.2.2.2

Gauss–Newton’s Method

The Gauss–Newton algorithm solves the non-linear least squares optimization problems. It is sufficient for finding the minimum of the sum of squared function values. The advantage of the algorithm is the fact that the second derivatives are not required. Having in mind observation Eq. (4.1) composed of the target value y, the model function  [β] (x) and its residual r, the Gauss–Newton method can be expressed in terms r

of the Jacobian J of the objective function as (4.53)  β

k+1

=β − k

r



r

−1

J J

r

J  r(β k )

(4.53)



and in terms of the Jacobian J of model function as (4.54)  β

k+1

=β + k

  

−1

J J



J  r(β k )

(4.54)

where β is a set of the parameters being optimized.

4.2.2.3

Levenberg–Marquardt’s Method

This method [14] introduces a damping factor λ to Eq. (4.53); thus, it results in (4.55)  β

k+1

=β −

r



r

J J + λI

k

−1

r

J  r(β k )

(4.55)

where I is the identity matrix. Smaller value of λ yields this method similar with Gauss–Newton’s method. The modified Marquardt subroutine [15] for non-linear r

r

least squares replaces I by diag( J  J), and it is given in Eq. (4.56)

60

4 Simultaneous Localization and Mapping

Fig. 4.21 Result of the Gauss–Newton method. Red: objective function; green: consecutive optimization results; red square: starting point; blue square: final optimization result. The Gauss–Newton method finds global minimum of this example function with large steps

 β

k+1

=β − k

r



r

r



r

J J + λdiag( J J)

−1

r

J  r(β k )

(4.56)

Thus, this approach yields scale invariant. Choosing the damping parameter λ is relevant since it contributes to the local convergence of the algorithm. The advantage of this algorithm is that it can converge very slowly close to the optimum. The idea is to check the progress of the optimization during consecutive steps and decide the change of damping parameter λ. The parameter is increased by a small amount for each uphill step and decreased by a large amount for each downhill step. The idea is to slow down the convergence close to the optimum. The comparison between the Gauss–Newton and Levenberg–Marquardt method with large value of λ is shown in Figs. 4.21 and 4.22. It converges to the local minimum since the large value of the damping parameter λ is insufficient for large uphill jumps over the objective function. Obviously, in such a case, the Levenberg–Marquardt algorithm during a larger number of iterations will decrease the value of λ; thus, it will behave as the Gauss–Newton method. The advantage of this method over the Gauss–Newton method is smaller steps while reaching the optimum.

Fig. 4.22 The Levenberg–Marquardt method with large value of λ. Red: objective function; green: consecutive optimization results; red square: starting point; blue square: final optimization result. In this particular example, the Levenberg–Marquardt method finishes the search in local minimum since the convergence is controlled with a large damping factor λ resulting in small uphill jumps

4.2 Weighted Non-linear Least Squares Optimization

61

4.2.3 Robust Least Squares 4.2.3.1

M-Estimators

C ri2 . The solution The Least Squares method minimizes sum of squared residuals i=1 is stable for the situation when all residuals as the differences between the target value and the model function are small. The deviations are called outliers. A robust technique for reducing the negative impact of these outliers on the solution is the so-called M-estimator [16]. It replaces the squared residuals ri2 by the function ρ(r ); thus, the optimization problem becomes (4.57).

β ∗ = min

C 

β

ρ (ri )

(4.57)

i=1

C where β is the vector of optimized parameters and Sum = i=1 ρ (ri ) is the objective function. The robust loss function ρ(r ) is symmetric and positive definite with a unique minimum at zero and it should not introduce any additional local minimums to the objective function. Chosen function should be less increasing than square; thus, it is capable of addressing an issue with outliers. The gradient equation derived from (4.57) is obtained with chain rule and it is given by (4.58).

 ∂ρ(ri ) ∂ri  ∂ri ∂ Sum = = ϒ(ri ) ∂β j ∂r ∂β ∂β i j j i=1 i=1 C

C

(4.58)

i) is called an influence function. It can be seen that when ρ(r ) = where ϒ(ri ) = ∂ρ(r ∂ri 2 r , Eq. (4.58) becomes (4.4). After defining a weight function as (4.59)

w(r ) =

ϒ(r ) r

(4.59)

Eq. (4.58) becomes (4.60)  ∂ Sum ∂ri = w(ri )ri ∂β j ∂β j i=1 C

(4.60)

and it is the gradient equation for optimization problem defined as (4.61) β ∗ = min β

C  i=1

w(rik−1 )ri2

(4.61)

62

4 Simultaneous Localization and Mapping

Table 4.1 M-estimators [16] for optimization problem defined as (4.57); ρ(r )—M-estimators; ϒ(r )—influence function; w(r )—weight Type

ρ(r )

ϒ(r )

w(r )

L1

|r |

sgn(r )

1 |r |

r

1

! r 2 1+ r2

sgn(r )|r |ν−1

! 1 2 1+ r2 ν−2 |r |

r 1+ |rc|

"

r2

L2

2!

L1 − L2

2

Fair Huber

i f |r | < k i f |r | ≥ k

Cauchy Geman–McClure Welsch " Tukey



|r |ν ν c2 [ |rc| − log(1 + |rc| )]

Lp "

2 1 + r2 − 1

"

r2 2

"

r

r2 2

1+r 2

i f |r | ≤ c

1

r (1+( rc )2 )

k |r | 1 (1+( rc )2 )

r (1+r 2 )2

1 (1+r 2 )2

ksgn(r )

k(|x| − k2 ) c2 log(1 + ( r )2 ) c 2

c2 [1 − e−( rc )2 ] 2

i f |r | > c

1 1+ |rc|

r 2

r e−( c ) ⎧ " ⎨ c2 (1 − [1 − ( r )2 ]3 ) r [1 − ( rc )2 ]2 c 6 2 c ⎩ 0

r 2

e−( c ) " [1 − ( rc )2 ]2

6

0

where weight w(rik−1 ) is recomputed after each iteration for the purpose of the use in the next iteration. Commonly used M-estimators [16] for the optimization problem defined as (4.57) are enumerated in Table 4.1 and more are elaborated in [17]. Figure 4.23 shows plots of these M-estimators, Fig. 4.24 shows influence functions ϒ(r ) and Fig. 4.25 shows weights w(r ).

4.2.3.2

Robustified Gauss–Newton Approximation

For the optimization problem considered as (4.62) [18] with robust kernel ρ 1  2  ρ ri (β) 2 i=1 C

β ∗ = min β

(4.62)

solved by Newton’s method described in Sect. 4.2.2.1, the gradient g is given by Eq. (4.63) and approximated Hessian H is given by (4.64) r

g(β) = ρ  J (β)r(β)

(4.63)

r  r H(β) ≈ J (β) ρ  + 2ρ  r(β)r  (β) J(β)

(4.64)

4.2 Weighted Non-linear Least Squares Optimization

63

Fig. 4.23 M-estimators ρ(r )

Fig. 4.24 Influence functions ϒ(r )

r

where J is Jacobian of r(β). For example, robust loss functions ρ, their first derivatives ρ  and second derivatives ρ  are given in Table 4.2 and visualized in Figs. 4.26, 4.27 and 4.28. Robust ρ capable of down-weighting the outliers should satisfy (4.65), and it should behave similar with the squared cost for the small residuals.

64

4 Simultaneous Localization and Mapping

Fig. 4.25 Weights w(r ) Table 4.2 Robust loss function ρ, its first derivative ρ  and second derivative ρ  related to Eqs. (4.62) and (4.66) Type ρ(s) ρ  (s) ρ  (s) TrivialLoss HuberLoss SoftLOneLoss

s "

s s≤1 √ 2 s−1 s >1 √ 2 s+1−2

1 "

1 s≤1 s>1

√1 s

0 "

√1 s+1

−

CauchyLoss

log (s + 1)

1 s+1

−

ArctanLoss

atan (s)

1 s 2 +1

−

TolerantLoss

  a −b log 1 + e− b +  −a+s  b log e b + 1

−a+s b −a+s e b +1

e

1 3

2s 2 1

3

2(s+1) 2 1 (s+1)2 2s

(s 2 +1)2 −a+s

e b  −a+s b e b +1 

e

b e

ρ(0) = 0 ρ  (0) = 1 ρ  (r 2 ) < 1 in the outlier region ρ  (r 2 ) < 0 in the outlier region

s≤1 s>1

0 −

2(−a+s) b −a+s b

−

2

+1

(4.65)

The robustification interval can be scaled with factor a > 0; thus, ρ(s, a) = a 2 ρ( as2 ).  The first derivative is ρ  ( as2 ) and the second derivative is a12 ρ as2 . For the optimization

4.2 Weighted Non-linear Least Squares Optimization

65

Fig. 4.26 Robust loss functions ρ(r 2 )

Fig. 4.27 First derivatives of robust loss functions ρ  (r 2 )

problem considered as (4.66) with robust kernel ρ, weights W and the prediction error e(β) given by Eq. (4.67) 1 ρ (ei (β) W i ei (β)) 2 i=1 C

β ∗ = min β

(4.66)

66

4 Simultaneous Localization and Mapping

Fig. 4.28 Second derivatives of robust loss functions ρ  (r 2 )

e(β)    prediction error

=

eobs −  observation

e(β) 

(4.67)

predictive model

the gradient g is given by Eq. (4.68) and approximated Hessian H is given by (4.69) e

e

where J =

4.2.3.3

∂e ∂β

g(β) = ρ  J (β)W e(β)

(4.68)

e e  H(β) ≈ J (β) ρ  W + 2ρ  (W e(β))(W e(β)) J(β)

(4.69)

is the Jacobian of the predictive model.

Adaptive Robust Loss Function

The proper choice of the best M-estimator for a given problem is not straightforward. Prominent candidates are Huber, Cauchy, Geman–McClure or Welsch functions [16]. Robust kernels define the distribution modeling of the outliers; thus, their choice is problem-specific. In most cases, there is no prior knowledge of the outlier process; therefore, the choice of the kernel is often done empirically. For some approaches such as BA, the choice of M-estimator between iterations is based on the outlier rejection heuristics. It is evident in SLAM that the outlier distribution changes continuously depending on the structure of the environment, dynamic objects in the scene and other environmental factors. For this purpose, a fixed robust kernel chosen a-priori cannot be a generic solution; thus, it cannot deal effectively with all situations. The

4.2 Weighted Non-linear Least Squares Optimization

67

prominent approach—adaptive robust loss function—was introduced in [19]. Further investigation in relation to the implementation for ICP and BA is shown in [20], where the authors proposed the use of a generalized robust kernel family, which is automatically tuned based on the distribution of the residuals and includes the common M-estimator. Barron’s adaptive robust loss function is given in Eq. (4.70) ⎞ ⎛  α2 |α − 2| ⎝ ( rc )2 − 1⎠ ρ(r, α, c) = α |α − 2|

(4.70)

where α ∈ R is a shape parameter that controls the robustness of the loss and c > 0 is a scale parameter that controls the size of the loss quadratic bowl near r = 0. Setting α = −2, the ρ(r, α, c) becomes the Geman−McClur e loss function (4.71).  2 2 rc ρ(r, −2, c) =  2 r +4 c

(4.71)

For α = 0, the ρ(r, α, c) is undefined and it approaches the Cauchy loss in the limit as shown in Eq. (4.72).     1 r 2 lim ρ(r, α, c) = log +1 α→0 2 c

(4.72)

Similarly, for α = 2, the ρ(r, α, c) is undefined and it approaches L2 loss in the limit as shown in Eq. (4.73). 1  r 2 (4.73) lim ρ(r, α, c) = α→2 2 c For α = 1, the loss function ρ(r, α, c) becomes the L1−L2 loss function (4.74). & ρ(r, 1, c) =

1+

 r 2 c

−1

(4.74)

For α approaching the negative infinity, the ρ(r, α, c) becomes the W elsch loss function (4.75). 1 r 2 (4.75) lim ρ(r, α, c) = 1 − e− 2 ( c ) α→−∞

Finally, ρ(r, α, c) Barron’s loss function with its removable singularities at α = 0, α = 2 and its limit at α = −∞, the ϒ(r )—influence function and the corresponding w(r )—weight is given in Table 4.3. Figure 4.29 shows plots of ρ(r, α, c), Fig. 4.30 shows ϒ(r ) and Fig. 4.31 shows w(r ) for different α. Tuning α allows interpolate between range of robust loss functions; thus, the following optimization problem can be defined as (4.76)

68

4 Simultaneous Localization and Mapping

Table 4.3 Barron’s adaptive robust loss function [19]; ρ(r, α, c); ϒ(r )—influence function; w(r )— weight and reproduced loss function; (*) singularity Condition

ρ(r )

α = −∞∗

− ( ) 1−e 2 c

α = −2

  2 rc 2  r 2 c +4

α = 0∗ α=1 α = 2∗

Otherwise

1 r 2

    2 log 21 rc + 1 !  2 1 + rc − 1   1 r 2 2

c

⎞ ⎛  r 2 α 2 |α−2| ⎝ ( c ) ⎠ − 1 α |α−2|

ϒ(r )

w(r )

1 r 2 r e− 2 ( c ) c2   −2 r 2 r c +1 2 4 c

1 r 2 1 e− 2 ( c ) c2   −2 r 2 1 c +1 2 4 c

2r r 2 +2c2

2 r 2 +2c2

c r c2

c 1 c2

 1  r  r 2 + 1 − 2 c 2

r c2

  r 2



Reproduced loss function Welsch Geman– McClure Cauchy

 1  1  r 2 + 1 − 2 c 2



 α −1 2 c + 1 |α−2|

1 c2

L1 − L2 L2

  r 2





 α −1 2 c + 1 |α−2|

Generic

Fig. 4.29 Barrons’s adaptive robust loss function ρ(r )

min β,α

C 

ρ (ri (β) , α)

(4.76)

i=1

where compared with (4.57) there is an addition parameter α to optimize. Thus, an expected solution is in the same time the optimized parameters β and optimal robust loss function. It efficiently addresses the issue with outliers. Authors of [20] commented the fact that optimizing (4.76) can be easily done by choosing α that weights down all residuals to small values without affecting β. For this reason, Barron [19]

4.2 Weighted Non-linear Least Squares Optimization

69

Fig. 4.30 Barrons’s influence function ϒ(r )

Fig. 4.31 Barrons’s weight w(r ) for different α

constructs a probability distribution based on the generalized loss function ρ(r, α, c) as (4.77) 1 e−ρ(r,α,c) P(r, α, c) = (4.77) cZ (α) where Z (α) is a normalization term called a partition function given by (4.78).

70

4 Simultaneous Localization and Mapping

' Z (α) =

∞

e−ρ(r,α,1) dr

(4.78)

−∞

Furthermore, Eq. (4.79) provides the adaptive loss ρa (r, α, c) that is a shifted general loss ρ(r, α, c) by the log partition. ρa (r, α, c) = − log P(r, α, c) = ρ(r, α, c) + log cZ (α)

(4.79)

This shift enables the optimization given in Eq. (4.76) finding a suitable α. The probability distribution P(r, α, c) is only defined for α ≥ 0 since the integral in the partition function Z (α) is unbounded for α < 0. This causes the fact that α < 0 cannot be a solution of (4.79) minimization. It can be seen from Fig. 4.31 that weights w(r, α, c) for α < 0 stronger down-weights the outliers, what is important in scenarios with large number of outliers such as ICP and BA. For this reason, the authors of [20] proposed an extension to the adaptive loss (4.79) by introducing an approximate partition function Z˜ (α) given in Eq. (4.80), truncated loss function (4.81) and optimization of α via alternating minimization. Z˜ (α) =

'

τ

e−ρ(r,α,1) dr

(4.80)

−τ

˜ α, c) = ρ(r, α, c) + log c Z˜ (α) ρ˜a (r, α, c) = − log P(r,

(4.81)

It enables the automatic tuning of α in its complete range, including α < 0. Modified ˜ α, c) obtained by truncating P(r, α, c) at |r | < τ is probability distribution P(r, shown in Fig. 4.32 and truncated robust loss ρ(r, ˜ α, c) is shown in Fig. 4.33. The optimization of α via alternating minimization is composed of two steps: in the first step, the maximum likelihood value for α is calculated, and the second step computes β by taking the optimal α from the previous step. The estimation of α in the first step is done based on observed current residuals as a minimization and the negative log-likelihood given as (4.82). L(α) = =

N N ˜ i (β), α, c) (r, α, c) = − i=1 log P(r i=1 ρ˜a  N ˜ i=1 ρ(ri (β), α, c) + log c Z (α)

(4.82)

thus, the optimization problem is defined as (4.83). The α is a scalar value and it assuming the approximate partition is not possible to derive the derivative ∂ L(α) ∂α function Z˜ (α) analytically; thus, (4.82) is solved using 1D grid search for α ∈< αmin , 2 >. The αmin is chosen empirically as, e.g. αmin = −10 since the difference to the corresponding weights for α = −∞ for large residuals (|r | < τ ) is rather small. (4.83) min L(α) α

4.2 Weighted Non-linear Least Squares Optimization

71

˜ α, c). Modified probability distribution Fig. 4.32 The truncated probability distribution P(r, ˜ α, c) obtained by truncating P(r, α, c) at |r | < τ P(r,

Fig. 4.33 The truncated robust loss ρ˜a (r, α, c)

72

4 Simultaneous Localization and Mapping

Fig. 4.34 Initial state of the simple y = ax + b regression optimization problem—red line. The goal is to fit y = ax + b function to the blue points. Outliers are evident

Example The example is simple y = ax + b regression optimization problem. The observation equation is given in (4.84) and Python code in Listing 4.3. ri =  r esidual

1

yi 

target value

− (axi + b)   

(4.84)

model f unction

from s y m p y i m p o r t *

2 3 4

x , y , a , b = s y m b o l s ( ’x y a b ’ ) a l l _ s y m b o l s = [a , b ]

5 6 7 8 9

target_value = y model_function = a*x + b o b s _ e q = M a t r i x ([ t a r g e t _ v a l u e - m o d e l _ f u n c t i o n ]) . vec () obs_eq_jacobian = obs_eq . jacobian ( all_symbols ) Listing 4.3 Python code for generating observation equation with ‘y’ as the target value and ‘ax+b’ as the model function

The comparison between the adaptive robust loss function, Cauchy and L1−L2 Mestimators and no-robust approach is shown in Figs. 4.34 and 4.35. It is evident that the adaptive robust loss function enables sufficient outliers handling by choosing optimal α capable of their down-weighting.

4.2.4 Manifolds Manifolds are smooth topologic surfaces of the LG where the state representations evolve [21]. Figure 4.36 shows an example of the manifold M as smooth spherical surface and planar vector space TX M tangent at the point X. Relying on the LT, it is possible to construct a rigorous calculus corpus to handle uncertainties, derivatives and integrals with precision and ease. One of the advantages of such an approach

4.2 Weighted Non-linear Least Squares Optimization

73

Fig. 4.35 Optimal state of the simple y = ax + b regression optimization problem. The adaptive robust loss function (red line); Cauchy (green line); L1−L2 (blue line); no-robust approach (black line). It can be observed that the black line converges to outliers, what is not the desired solution

Fig. 4.36 The example of the manifold M as smooth spherical surface and planar vector space TX M tangent at the point X

is the much less computational complexity of the derivatives; thus, the optimization problem can be solved much faster. This can be interpreted as moving in the planar vector space TX M in the area of the tangent at the point X to the manifold M approximates the same operation performed onto the manifold. The second advantage is the rotation representation without singularities. Another advantage is the numerous open-source frameworks supporting the development. It is evident that all observation equations elaborated in this book can be used for constructing the optimization system incorporating LT. A set G with composition operation ‘◦’ is a gr oup (G, ◦). It satisfies the axioms (4.85) for elements X, Y, Z ∈ G.

74

4 Simultaneous Localization and Mapping

Closure under ◦ : X◦Y ∈G Identity E : E◦X=X◦E=X X−1 ◦ X = X ◦ X−1 = E Inverse X−1 : Associativity : (X ◦ Y) ◦ Z = X ◦ (Y ◦ Z)

(4.85)

The well-known gr oups are rotation SO(3) written in (4.86) and rigid motion SE(3) written in (4.87). ) ( S O(3) = R ∈ R3×3 |R R = I, det (R) = 1 , * + R t ∈ R4×4 |R ∈ S O(3), t ∈ R3 S E(3) = M =  0 1

(4.86) (4.87)

Moreover, axioms (4.85) state that the composition of the elements of the manifold remains on the manifold, e.g. for SO(3) multiplication of the rotation matrices gives the rotation matrix. Each element has an inverse in the manifold, e.g. for SO(3) inverse of the rotation matrix is also rotation matrix. A special one of these elements is the identity; thus, a special one of the tangent spaces is the tangent at the identity, which we call the Lie algebra of the Lie group, and e.g. for SO(3), the E is the identity 3 × 3 matrix (4.88). ⎡ ⎤ 100 (4.88) E S O(3) = ⎣0 1 0⎦ 001 The crucial functionality of the LG is the group action as the capability to transform its elements, e.g. rotate, translate and scale. Given LG M and a set V, the action is noted as X · v of X ∈ M and v ∈ V (4.89) and it has to satisfy axioms (4.90). · : M × V → V; (X, v) → X · v

(4.89)

Identity: E·v =v Compatibility: (X ◦ Y) · v = X · (Y · v)

(4.90)

˙ = ∂X in The velocity of the X element moving in LG’s manifold M is given as X ∂t ˙ belongs to the tangent space TX M and it is shown in Fig. 4.37. TX M. This velocity X The smoothness of the manifold determines the absence of edges or spikes. It implies the existence of an unique tangent space at each point of the manifold. Moreover, the structure of such tangent spaces is the same everywhere.

4.2.4.1

Lie Algebra

Lie algebra is the tangent space T of the manifold M at the identity E. It is denoted  as m = TE M. Three important aspects of the Lie algebra are as follows:

4.2 Weighted Non-linear Least Squares Optimization

75

Fig. 4.37 Manifold M as smooth function, vector space TX M tangent at the point X and velocity element ˙ = ∂X in TX M X ∂t

• exponential map, exp : m → M that converts elements of the Lie algebra into elements of the group; • logarithmic map, log : M → m that converts elements of the the group into elements of the Lie algebra; • adjoint operation capable of transforming vectors of the tangent space at X to the tangent space at the identity E. It is worth mentioning that Lie algebras can be defined locally to a tangent point X; thus, it determines establishing local coordinates for TX M. The exponential map and logarithmic map can be written as (4.91). exp : m → M; τ ∧ → X = exp(τ ∧ ) log : M → m; X → τ ∧ = log(X)

(4.91)

where ∧ is ‘hat’ that denotes Lie algebra elements. Thus, velocities are denoted as v ∧ and general elements are denoted as τ ∧ = (vt)∧ = v ∧ t. Closed form of the exponential map can be obtained with Taylor series as (4.92). 1 2 1 3 exp(τ ∧ ) = E + τ ∧ + τ ∧ + τ ∧ + · · · 2 3!

(4.92)

An example is the Lie algebra so(3) of the group 3 × 3 matrices S O(3) denoted as R. The orthogonality condition states that R R = I, where I is an identity matrix. The tangent space can be found by taking the time interval of this constraint as ˙ +R ˙  R = 0. The expression R R ˙ is skew-symmetric matrix noted as [ω]× R R given in (4.93). ⎡ ⎤ ⎡ ⎤ ωx 0 −ωz ω y [ω]× = ⎣ω y ⎦ = ⎣ ωz 0 −ωx ⎦ (4.93) ωz × −ω y ωx 0 ˙ = [ω]× . Thus, the ˙ = [ω]× , and for R = I, we have R Moreover, we obtain R R following statement is evident (4.94). [ω]× ∈ so(3)

(4.94)

76

4 Simultaneous Localization and Mapping

The elements of this vector space can be decomposed into (4.95) [ω]× = ωx E x⎡+ ω y E y⎤+ ωz E z 00 0 E x = ⎣0 0 −1⎦ ⎡0 1 0 ⎤ 0 01 E y = ⎣ 0 0 0⎦ ⎡ −1 0 0⎤ 0 −1 0 E z = ⎣1 0 0 ⎦ 0 0 0

(4.95)

˙ = where ω = (ωx , ω y , ωz ) ∈ R3 is the vector of angular velocities. From R R ˙ = R[ω]× . It is ODE for ω constant with the solution R(t) = [ω]× , we obtain R R0 exp([ω]× t). At the origin R0 = I, the exponential map is given by (4.96). R(t) = exp([ω]× t) ∈ S O(3)

(4.96)

An integrated rotation in angle-axis form is defined in (4.97) 



θ = uθ = ωt ∈ R3

(4.97)

where θ is angle and u is unit axis. Consequently, [θ ]× ∈ so(3) is the rotation expressed in the Lie algebra. Equation (4.96) rewritten as a power series gives (4.98) R = exp([θ]× t) =

 θk k

k!

(θ [u]× )2 (θ [u]× )3 θ [u]× + + + ··· 1! 2! 3! (4.98) ⎤

([u]× )k = I +

⎡ ⎤ ⎡ ux 0 −u z u y where [u]× = ⎣u y ⎦ = ⎣ u z 0 −u x ⎦ is skew-symmetric matrix with propuz × −u y u x 0 erties given by (4.99). [u]0× = I [u]1× = [u]× [u]2× = uu − I (4.99) [u]3× = −[u]× [u]4× = −[u]2× Equation (4.98) can be rewritten assuming (4.99) as (4.100).

4.2 Weighted Non-linear Least Squares Optimization

77

    1 1 2 1 1 1 θ − θ4 + θ6 − . . . R = I + [u]× θ − θ 3 + θ 5 − . . . + [u]2× 4!  6!  3! 2 5!   sin θ

1−cos θ

(4.100) Finally, the closed-form Rodrigues rotation formula becomes (4.101) and it is elaborated more in Sect. 3.2.2. R = I + [u]× sin θ + [u]2× (1 − cos θ )

4.2.4.2

(4.101)

Mappings

The elements τ ∧ of the Lie algebra are characterized by non-trivial structures and they can be expressed as linear combinations of the base elements E i called generator s of m. They are the derivatives of X around the origin. The coordinates of τ as vectors in Rm are essential for the simplified manipulation. For this reason, it is crucial to pass from m to Rm or from Rm to m through two mutually inverse linear maps hat : (·)∧ and vee : (·)∨ (4.102) m τi E i Hat : Rm → m; τ → τ ∧ = i=1 m m ∧ ∧ ∨ Vee : m → R ; τ → (τ ) = i=1 τi ei

(4.102)

where ei is one of the base vector of Rm ; thus, ei∧ = E i . These mappings are shown in Fig. 4.38, where capitalized Exp and Log maps are considered as convenient shortcuts for mapping vector elements τ ∈ Rm of the Lie algebra with elements X ∈ M of the Manifold. Finally, all necessary mapping from/to Manifold to/from Lie algebra are given in (4.103).

Fig. 4.38 Passing from m to Rm or from Rm to m through two mutually inverse linear maps hat : (·)∧ and vee : (·)∨ . The exponential map (exp) and logarithmic map (log) from Eq. (4.91) capable of mapping from manifold to Lie algebra and vice versa. Exp and Log map directly the vector space Rm from/to M

78

4 Simultaneous Localization and Mapping 

Exp : Rm → M; τ → X = E x p(τ ) = exp(τ ∧ ) 

Log : M → Rm ; X → τ = Log(X) = log(X)∨

(4.103)

The implementation of these maps for different Manifolds can be found in [21]. For the SE(3), Lie algebra and vector elements are given in (4.104). +

, + , , + R t θ [θ]× ρ ∧ M = 1×3 , τ= , τ = 0 0 ρ 0 1

4.2.4.3

(4.104)

Plus and Minus Operators

Plus ⊕ and minus  operators allow applying increments to the elements of the Manifold and express them in its tangent vector space by combining one Exp/Log operation with one composition. We distinguish right and left versions due to the non-commutativity of the operands’ composition and express them in the local or global frame. All of these versions are given in Eq. (4.105). 

Y = X ⊕ X τ = X ◦ Exp(X τ ) ∈ M

right⊕ :



right : X τ = Y  X = Log(X−1 ◦ Y) ∈ TX M 

Y = E τ ⊕ X = Exp(E τ ) ◦ X ∈ M

left⊕ : left :

E

(4.105)



τ = Y  X = Log(Y ◦ X−1 ) ∈ TE M

A left superscript (e.g. X τ , E τ ) is added to specify the precise tangent space. Thus, τ belongs to the tangent space at X and it is expressed in the local frame at X. Exp(E τ ) is on the left, and due to E τ ∈ TE M, it is expressed in global frame.

X

4.2.4.4

Adjoint

From (4.105) by comparing right⊕ with left⊕, we obtain (4.106) right⊕ : Y = X ⊕ X τ ⇒ Eτ ⊕ X = X ⊕ Xτ left⊕ : Y = E τ ⊕ X

(4.106)

where X τ is expressed in local frame at X and E τ is expressed in global frame. From the key property of the exponential map expressed as (4.107), (4.103) and (4.105), we obtain (4.108). (4.107) exp(Xτ ∧ X−1 ) = X exp(τ ∧ )X−1 Exp(E τ )X = XExp(X τ ) ∧ ∧ exp( τ ) = X exp(X τ )X−1 = exp(XX τ X−1 ) E ∧ X ∧ −1 τ =X τ X E ∧

(4.108)

4.2 Weighted Non-linear Least Squares Optimization

79

The adjoint of M at X is defined as (4.109) 

AdX : m → m; τ ∧ → AdX (τ ∧ ) = Xτ ∧ X−1

(4.109)

∧

Thus, E τ = AdX (X τ ∧ ) what is the adjoint action of the group on its own Lie algebra. An important tool is an adjoint matrix Ad X (4.110) that maps the tangent vectors E τ and X τ . Ad X : Rm → Rm ; X τ → E τ = Ad X X τ (4.110) The adjoint matrix is used for transforming vectors of the tangent space at X onto vectors of the tangent space at the origin E. The properties of the adjoint matrix are given in (4.111). X ⊕ τ = ( Ad X τ ) ⊕ X (4.111) Ad X−1 = Ad X −1 Ad XY = Ad X Ad Y The adjoint matrix of SE(3) is given in (4.112). , , + R [t]× R R t 6×6 ∈ R , where M = 1×3 ∈ R4×4 = 3×3 R 1 0 0 +

Ad M

4.2.4.5

(4.112)

Derivatives on Lie Groups

The Jacobian matrix for a multivariate function f : Rm → Rn is given as n × m matrix of all partial derivatives (4.113). ⎤ · · · ∂∂xfm1 f (x + h) − f (x)  ⎢ . . ∂ f (x)  ⎥ J= = lim = ⎣ .. . . ... ⎦ ∈ Rn×m h→0 ∂x h ∂ fn · · · ∂∂xfmn ∂ x1 ⎡ ∂ f1

∂ x1

(4.113)

Right Jacobian on Lie groups uses ⊕ and  in its definition (4.114). X

D f (X)  f (X ⊕ τ )  f (X) ∈ Rn×m , τ ∈ TX M = lim τ →0 DX τ

(4.114)

Consequently, the definition of the left Jacobian on Lie groups is given in (4.115). E

D f (X)  f (τ ⊕ X)  f (X) ∈ Rn×m , τ ∈ TE M = lim τ →0 DX τ

(4.115)

The right Jacobian matrix Rn×m linearly maps the local tangent spaces TX M → T f (X) N. In contrary, the left Jacobian matrix Rn×m linearly maps the global tangent spaces TE M → TE N being Lie algebras of M and N. Left and right Jacobians are

80

4 Simultaneous Localization and Mapping

related to adjoints of M and N as (4.116). E

X D f (X) D f (X) Ad X = Ad f (X) DX DX

(4.116)

The chain rule for derivatives of Y = f (X) and Z = g(Y) is given in (4.117). DZ DZ DY = DX DY DX

(4.117)

The following example shows left and right Jacobians matrices in SE(3) for point p transformation via M expressed as (4.118) pg = R p + t

(4.118)

where pg is result point expressed in global frame. We start from right Jacobian for rotation part R of M from (4.104) assuming mapping (4.103); thus, we begin with (4.119). (4.119) R = exp([θ]× ) Right Jacobian

R

D f (R p) DR

is calculated with (4.120).

D f (R p) R exp([θ]× ) p − R p R(I + [θ]× ) p − R p = lim = lim = θ →0 θ →0 DR θ θ R p + R([θ]× ) p − R p R[θ]× p −R[ p]× θ = lim = lim = −R[ p]× lim θ →0 θ→0 θ→0 θ θ θ (4.120) E D f (R p) Consequently, left Jacobian D R for rotation part R of M is given by (4.121). R

E

D f (R p) exp([θ]× )R p − R p (I + ([θ ]× )R p − R p = lim = lim = θ→0 θ→0 DR θ θ R p + ([θ]× )R p − R p [θ ]× (R p) −[R p]× θ lim = lim = lim = −[R p]× θ →0 θ→0 θ→0 θ θ θ (4.121) Calculation of the left and right Jacobians for translation part t of M is straightfort p+t) is given in (4.122). ward. Thus, right Jacobian D f (R Dt t

D f (R p + t) R( p + ρ) + t − R p Rρ + t = lim = lim =R ρ→0 ρ→0 Dt ρ ρ

Finally, left Jacobian E

E

D f (R p+t) Dt

(4.122)

is given in (4.123).

D f (R p + t) Rp + t + ρ − Rp = lim =I ρ→0 Dt ρ

(4.123)

4.2 Weighted Non-linear Least Squares Optimization

81

Fig. 4.39 Initial state. Gray point cloud is the target that green and red point clouds are registered to

 t p+t) R D f (R p) is given by (4.124) and Putting all together, the right Jacobian D f (R Dt DR E  E p+t) D f (R p) left Jacobian D f (R is given by (4.125). Dt DR t

D f (R p+t) Dt

E

D f (R p+t) Dt

R

D f (R p) DR

E

D f (R p) DR

 

  = R −R[ p]× , τ ∈ TX M

(4.124)

  = I −[R p]× , τ ∈ TE M

(4.125)

At this stage, we can state three important facts as follows: • left and right Jacobians for point p transformation characterize low computational complexity; • left Jacobian is used in case of pose M given in global frame; • right Jacobian is used in case of pose M given in local frame. The second and third facts have a significant impact on the final accuracy of the data registration. The qualitative comparison between incorporation of the left and right Jacobians matrices into optimization process is shown in Figs. 4.39, 4.40 and 4.41. Gray point cloud is the target that green and red point clouds are registered to. In this particular example, the green point cloud is close to the origin, and the red point cloud is far from the origin. Due to large angular error and large distance of the red point cloud to the origin, the left Jacobian approach has the problem to reach the satisfactory result as the right Jacobian approach. It is related to the need for local rotation that left Jacobian approach cannot provide for poses placed far from the origin within a small number of optimization iterations. This is one of the prerequisites for many optimization frameworks to use local frames. Obviously, left

Fig. 4.40 Optimal state after 30 iterations with left Jacobian. Gray point cloud is the target that green and red point clouds are registered to

82

4 Simultaneous Localization and Mapping

Fig. 4.41 Optimal state after 30 iterations with right Jacobian. Gray point cloud is the target that green and red point clouds are registered to

and right Jacobian approaches converge to a similar optimal solution, but in this particular case, the left Jacobian requires more of the optimization iterations. 4.2.4.6

Uncertainty in Manifolds and Covariance Propagation

To define covariance matrix on the tangent space at a certain point, it is necessary to ¯ ∈ M in introduce local perturbation. Thus, τ can be perturbed around the point X the tangent vector space TX¯ M with ⊕ and  as in (4.126). ¯ ⊕ τ , τ = X  X, ¯ τ ∈ TX¯ M X=X

(4.126)

Having this perturbation, it is possible to define the covariance matrix on this tangent ¯ through the standard expectation operator E[·] as (4.127). space at X  ¯ ¯  ] ∈ Rm×m  X)  X = E[τ τ  ] = E[(X  X)(X

(4.127)

¯  X ) on the manifold. Thus, it is possible to define Gaussian variables X ∼ N(X; X, To express perturbation in global reference frame what is equivalent with the tangent space at the origin TE M, the ⊕ and  operators are incorporated as in (4.128). ¯ τ = X  X, ¯ τ ∈ TE M X = τ ⊕ X,

(4.128)

Global perturbation E τ ∈ TE M and local perturbation X τ ∈ TX M are related to adjoint as in (4.110). Therefore, the covariance can be transformed from local to global frame using (4.129). E

 X = Ad X X  X Ad X 

(4.129)

Covariances can be propagated through the linearized function f : M → N; X → Y = f (X) with (4.130). Y ≈

Df  Df X ,  Y ∈ Rn×n ,  X ∈ Rm×m DX DX

(4.130)

It is necessary to notice that the above covariances describe the tangent perturbations τ.

4.3 Probabilistic Formulation

83

4.3 Probabilistic Formulation It is assumed that a mobile robot moves in an unknown environment along the consecutive nodes of the trajectory. It consists of a measurement instrument mounted with an unknown calibration—the c. These nodes are described ) ( random variable by the random variables p1:P = p1 , . . . , p P . During motion, a sequence of the odometry measurements u1:T = {u1 , . . . , u T } and the perception measurements o1:O = {o1 , . . . , oO } are acquired. The SLAM problem is formulated as the estimation of the posterior probability of all nodes of the trajectory p1:P , calibration c and the map m of the environment for given all the measurements and the arbitrarily chosen initial pose p0 as in Eq. (4.131). p( p1:P , c, m | o1:O , u1:T , p0 )       x={x 1 ,...,x N }

(4.131)

z={z 1 ,...,z K }

The map m can be represented as spatially located landmarks, by dense representations like occupancy grids and surface maps or by raw sensor measurements (e.g. dense point cloud). A general problem definition assumes that p1:P , c, m are state variables of the stationary system denoted as x = {x 1 , . . . , x N }. The indirect observation of the system is done by a set of sensors or by other sources as a set of measurements denoted as z = {z 1 , . . . , z K }. The measurements are affected by noise; therefore, z is multidimensional vector of random variables. Normal distribution N(x; μ, ) over vectors is called multivariate and is characterized by density function of the form (4.132). − 21

p (x) = det (2π )

* 1  −1 exp − (x − μ)  (x − μ) 2

(4.132)

where μ is the mean vector,  is a covariance matrix and (x − μ)  −1 (x − μ) denotes the squared Mahalanobis distance. The state of the system can be used for predicting the distribution of the measurements. It is impossible to estimate the state of the system for the given measurements affected by the noise. Therefore, we compute a distribution over the potential states of the system given these measurements. Thus, the probability distribution of the state x, given the measurement z, is estimated with (4.133). p (x|z) = p (x 1 , . . . , x N |z 1 , . . . , z K ) = p (x 1:N |z 1:K ) Furthermore, from Bayes’ theorem, we obtain (4.134).

(4.133)

84

4 Simultaneous Localization and Mapping

likeli hood o f measur ements given the states    p (z 1:K |x 1:N ) p (x 1:N ) p (x 1:N |z 1:K ) = p (z 1:K )

(4.134)

where the term p (z 1:K |x 1:N ) is known as likelihood of the measurements given the states. It is conditional distribution and it is related to the states’ changes. The prior p (x 1:N ) models the states before the measurements. The prior is an uniform distribution whose value is a constant px . The p (z 1:K ) does not dependent on the states and it is constant number pz . Therefore, Eq. (4.134) becomes p (x 1:N |z 1:K ) =

K . px p (z 1:K |x 1:N ) ∝ p (z k |x 1:N ) pz k=1

(4.135)

Thus, the distribution over the possible states given the measurements is proportional to the likelihood of the measurement given the states. Assuming that the measurements are affected by a zero mean additive Gaussian noise characterized by an information matrix k =  −1 k , the distribution of the measurement z k given a state x is proportional to N(z k ; zˆ k , −1 k ); thus,      p (z k |x) ∝ exp − z k − zˆ k k z k − zˆ k

(4.136)

where zˆ k = hk (x) is the prediction of the measurement for given state x. p (z k |x) is commonly known as sensor model or obser vation model. It models the probability of performing the observation z k given that the robot is at a known location in the map (the state x is known). From (4.135) and (4.136), the SLAM problem can be formulated as finding x ∗ that x ∗ = max x

K . k=1

p (z k |x 1:N ) = max x

K .

     exp − z k − zˆ k k z k − zˆ k

(4.137)

k=1

After taking the logarithm and removing the minus, Eq. (4.137) becomes x ∗ = max x

K K            − z k − zˆ k k z k − zˆ k = min z k − zˆ k k z k − zˆ k k=1

x

k=1

(4.138) After introducing the error function ek (x) as a difference between the observation z k and the prediction zˆ k ek (x) = z k − zˆ k (4.139) final optimization problem is defined as (4.140).

4.3 Probabilistic Formulation

85

x ∗ = min F (x) = min x

x

K 

ek (x) k ek (x)

(4.140)

k=1

4.4 Graphical Representation with Factor Graphs Many problems in robotics are effectively described by a factor graph [22–26]. Factor graphs are a family of probabilistic graphical models. Other examples are Bayesian networks and Markov random fields, which are well known from the statistical modeling and machine learning literature. A factor graph is a graphical model expressing the joint likelihood of the known measurements with respect to a set of unknown conditional variables. It can be efficiently used for SLAM [27]. To solve a factor graph, it is necessary to find the values of the variables that maximize the joint likelihood of the measurements. Thus, if the noise affecting the sensor data is Gaussian, the optimization problem is defined by Eq. (4.140) discussed in Sect. 4.3. It solves dense and spar se problems. It is discriminated by the connectivity of the factor graph. A dense problem is characterized by many measurements affected by relatively few variables. Sparse problems are characterized by measurements that depend only on a small subset of the variables. An interesting aspect is the fact that Factor Graph can be considered as high conceptual level (modeling level). From this level, we can enter Graph SLAM that is discussed in next Sect. 4.5 and finally into PGSLAM described in Sect. 4.5.1 being a specialized instance of Graph SLAM. The relation between Factor Graph, Graph SLAM and PGSLAM is shown in Fig. 4.42. Factor graph is a bipartite graph F = (U, V, E) [22]. There are two types of nodes: • factors φi ∈ U. • variables x j ∈ V.

Fig. 4.42 The relation between Factor Graph, Graph SLAM and Pose Graph SLAM

86

4 Simultaneous Localization and Mapping

Fig. 4.43 Example Factor Graph that is composed of three robot poses x1 , x2 , x3 and two landmarks l1 , l2 as unknown states; thus, x1 , x2 , x3 , l1 , l2 ∈ V. Robot motion is modeled as follows: x1 → x2 → x3 . There are nine factor functions φi ∈ U, i ∈ 1 . . . 9, shown as a small black node and it is connected to only those state variables it is a function of. Edges ei j ∈ E are between factor nodes and variable nodes

The assumption is that all edges ei j ∈ E are between factor nodes and variable nodes. N(φi ) denotes the set of variable nodes adjacent to a factor φi and the assignment to this set is denoted as X i . The factorization of a global function φ(X ) by the factor graph F is defined by Eq. (4.141). φ(X ) =

.

φi (X i )

(4.141)

i

Solving factor graph is equivalent to SLAM problem formulated in Eqs. (4.137) and (4.138). Figure 4.43 shows simple SLAM example from [22]. It is composed of three robot poses x1 , x2 , x3 and two landmarks l1 , l2 as unknown states; thus, x1 , x2 , x3 , l1 , l2 ∈ V. Robot motion is modeled as follows: x1 → x2 → x3 . There are nine factor functions φi ∈ U, i ∈ 1, . . . , 9, shown as a small black node and it is connected to only those state variables it is a function of. Factors φ4 , φ5 , φ6 give prior knowledge concerning the first pose and landmarks positions. Edges ei j ∈ E are between factor nodes and variable nodes. The factor graph factorization is given in (4.142), and block structure of the sparse Jacobian J for this example is given in  (4.143) and the Hessian H = J  J is given in (4.144). Having Jacobian and Hessian, it is straightforward to construct optimization systems to solve this simple SLAM problem.

4.4 Graphical Representation with Factor Graphs

87

φ(l1 , l2 , x1 , x2 , x3 ) = φ1 (x1 )φ2 (x2 , x1 )φ3 (x3 , x2 ) ×φ4 (l1 )φ5 (l2 ) ×φ6 (x1 ) ×φ7 (x1 , l1 )φ8 (x2 , l1 )φ9 (x3 , l2 ) φ1 φ2 φ3 φ J= 4 φ5 φ6 φ7 φ8 φ9 ⎡

l1 l2 x 1 x 2 x 3 ⎤ J13 ⎢ ⎥ J23 J24 ⎢ ⎥ ⎢ ⎥ J J 34 35 ⎥ ⎢ ⎢ J41 ⎥ ⎢ ⎥ ⎢ ⎥ J 52 ⎢ ⎥ ⎢ ⎥ J 63 ⎢ ⎥ ⎢ J71 ⎥ J 73 ⎢ ⎥ ⎣ J81 ⎦ J84 J92 J95

(4.142)

⎡

H11

H13 H14

(4.143)

⎤

⎢ H22 H25 ⎥ ⎢ ⎥ ⎥ H H H H =⎢ 33 34 ⎢ 31 ⎥ ⎣ H41 H43 H44 H45 ⎦ H52 H54 H55

(4.144)

It is recommended to compare Jacobian from (4.143) with the Jacobian from (4.32). The only difference is related to factors φ4 , φ5 , φ6 that give prior knowledge concerning the first pose and landmarks positions. Such information can be removed from the optimization system. Moreover, each factor is similar to the instance of the observation equation with such a difference that the probabilistic approach is taken into the consideration for constructing the factor graph.

4.5 Graph SLAM Optimization problems in robotics, computer vision and photogrammetry that involve the minimization of a non-linear error function (described in Sect. 4.2) can be represented as a graph [28]. The graph is composed of vertices related to a vector of parameters β = x (in our case x i , x j ) and edges related to constraints (Fig. 4.44). Authors of [28] defined an objective function as Eq. (4.145) and optimization problem in the form of (4.146) F(x) =

 i, j∈C



ei j (x i , x j , z i j ) i j ei j (x i , x j , z i j )    

Fi j

objective f unction



(4.145)

88

4 Simultaneous Localization and Mapping

Fig. 4.44 Graph SLAM where zi j is a mean and i j is the information matrix of a constraint relating the parameters x i and x j

x ∗ = min F(x)

(4.146)

x





where x = (x 1 , . . . , x n ) is the vector of parameters β, z i j is a mean and i j is the information matrix of a constraint relating the parameters x j and x i and ei j (x i , x j , z i j ) is a vector error function that measures how well β parameters satisfy the constraint z i j . Thus, it is 0 when x i and x j satisfy the constraint. To solve (4.146), we assume having a reasonable initial guess of the optimum x˘ . The idea is to approximate the error function ei j by its first-order Taylor expansion around the current initial guess x˘ as in Eq. (4.147).   ei j x˘ i + x i , x˘ j + x j = ei j ( x˘ + x)  ei j + J i j x

(4.147)

de f.

where J i j is the Jacobian of ei j (x) calculated in x˘ and ei j = ei j ( x˘ ). Furthermore, after applying (4.147) to F i j from (4.145), we obtain (4.148) F i j ( x˘ + x) = ei j ( x˘ + x) i j ei j ( x˘ + x)      ei j + J i j x i j ei j + J i j x    = x  J i j i j J i j x + 2 J i j i j ei j x + ei j i j ei j          Hi j

bi j

(4.148)

ci j



= x H i j x + 2bi j x + ci j Therefore, Eq. (4.145) becomes (4.149). F( x˘ + x) 

  

x  H i j x + 2bi j x + ci j (4.149)

i, j∈C 

= x H x + 2b x + c This quadratic form can be minimized by setting derivative (4.150) to zero. ∂ x  H x + 2b x + c = 2H x + 2b = 0 ∂ x

(4.150)

4.5 Graph SLAM

89

Fig. 4.45 2D map reconstructed using ClearPath Jackal mobile robot odometry and LiDAR. Red: trajectory edges

Fig. 4.46 2D map reconstructed using LO. Red: trajectory edges

Thus, the iterative solution x ∗ is derived from solving the linear system given by Eq. (4.151). (4.151) H x ∗ = −b Looking from terminology introduced in Sect. 4.2, it straightforward that the information matrix i j is related to a rotated weight matrix R P R . Function ei j (x i , x j , z i j ) [29] relates to observation equation. Graph SLAM is typically solved using LM algorithm already discussed in Sect. 4.2.2.3 [15, 30] as (H + λI)x ∗ = −b  H i j = J i j i j J i j bi j =

(4.152)

 J i j i j ei j

LM discussed in Sect. 4.2.2.3 is a dumped version of the GN method where λ is a damping factor. It means that we expect larger x for smaller values of λ. Moreover, graph SLAM is solved using Least Squares on a Manifold [31] discussed in Sect. 4.2.4 and for certain problems of characteristic structure of H matrix (for instance, BA) using reduced system formed by taking the Schur complement of this matrix [32]. It is discussed in Chap. 11. More information concerning Graph SLAM can be found in [33–36]. Figures 4.45, 4.46 and 4.47 demonstrate the result of Graph SLAM mapping of the indoor environment. The desired solution is that e(x i , x j , z i j ) equals 0 when x i and x j perfectly match the constraint e. Therefore, assuming naming convention in this book introduced in Sect. 4.2, the graph SLAM constraint can be rewritten into observation Eq. (4.153) and it is related to the relative pose (2) observation equation discussed in Sect. 9.5.2. δ =  r esidual

0 

target value

− e(x i , x j , z i j )    model f unction

(4.153)

90

4 Simultaneous Localization and Mapping

Fig. 4.47 2D map reconstructed using Graph SLAM and LO. Red: trajectory edges; green: loop closure edges

Authors of graph SLAM tutorial [29] encode the indices of the measurement in the indices of the error function as Eq. (4.154) e(x i , x j , z i j ) = z i j − zˆ i j (x i , x j )

(4.154)

where according to [29] e(x i , x j , z i j ) is error function, z i j is named measurement and real observation and zˆ i j (x i , x j ) is named expected measurement and expected observation. Finally, according the notation used in this book, the graph SLAM observation Eq. (4.153) can be rewritten as (4.155) δ =  r esidual

−z i j 

target value

− (−ˆz i j (x i , x j )) = −e(x i , x j , zi j )   

(4.155)

model f unction

and it is related to the relative pose (1) observation equation discussed in Sect. 9.5.1. It can be seen that the target value equals minus real observation (−z i j ) according to notation considered in this book. Similarly, all observation equations elaborated in this book can be rewritten to graph SLAM observation equation.

4.5.1 Pose Graph SLAM PGSLAM is a specialized instance of the Graph SLAM. It is composed mainly from relative pose observation equations described in Sects. 9.5.1 and 9.5.2. In that sense, it is the lightest graph that can be built upon the set of unknown variables—poses. Thus, the solver finds the optimal configuration of all poses in the graph. There is no strict boundary between PGSLAM and Graph SLAM; therefore, it can be assumed that adding different types of observation equations to the PGSLAM makes it closer to Graph SLAM.

4.6 Approach

91

4.6 Approach In this chapter, a bunch of concepts and algorithms were presented to approach SLAM as the least squares estimation problem. This section provides the methodology for putting all together and constructing the solver for a given problem. First, we list the most important concepts: • • • • • • • • • •

parameterization for the state variables; parameterization for the measurements; parameterization for the increments; measurement function, error function and observation equation; solver (Sect. 4.2.2); robustness (Sect. 4.2.3); manifolds (Sect. 4.2.4); probabilistic formulation (Sect. 4.3); graphical representation (Sect. 4.4); graph SLAM (Sect. 4.5).

These concepts are strongly related to each other, but sometimes we can omit some of them to build a simple implementation. For the sake of giving an overview of the proposed bottom-up and top-down methodologies, Fig. 4.48 shows two possible approaches for SLAM. The top-down methodology assumes that all state variables and the measurements are described by the random variables. Moreover, the measurements are affected by a zero mean additive Gaussian noise characterized by the information matrix. The bottom-up approach starts from the observation equations for the given least squares problem. Both approaches result in the optimization problem defined with a system of linear equations. This system can be solved to some extent by any solver described in Sect. 4.2.2. The top-down methodology is the probabilistic approach described in Sect. 4.3 that is well-established approach for SLAM in the mobile robotics domain. There are plenty of the open-source frameworks providing relevant implementation as g2o [28], GT S AM [37] and recent one S R RG2_S O L V E R [38]. This methodology due to supportive programming frameworks requires only high-level design, for example using graphical representation with Factor Graphs discussed in Sect. 4.4. Therefore, the entire SLAM design is rather straightforward having in mind probabilistic formulation. Unfortunately, there are plenty of situations when we cannot use those frameworks. For this reason, an alternative bottom-up approach is proposed. It starts with the collection of all observation equations for the optimization problem. For this reason, we should set the uniform rotation matrix parameterization for the state variables, measurements and increments. Thus, the system of linear equations is well defined by the design matrix (Jacobian). If we would like to consider the probabilistic and/or robust approach, we should prepare weight matrices. This yields the final system of linear equations that can be solved by any solver discussed in Sect. 4.2.2.

92

4 Simultaneous Localization and Mapping

Fig. 4.48 Two possible approaches a bottom-up and b top-down methodologies for SLAM. Both approaches result in the optimization problem defined with the system of linear equations. The top-down methodology is the probabilistic approach described in Sect. 4.3 that is well-established approach for SLAM in the mobile robotics domain. An alternative bottom-up approach starts with the collection of all observation equations for the optimization problem. Both approaches yield the final system of linear equations that can be solved by any solver discussed in Sect. 4.2.2

References

93

References 1. J. Leonard, H. Durrant-Whyte, Simultaneous map building and localization for an autonomous mobile robot, in Proceedings IROS ’91:IEEE/RSJ International Workshop on Intelligent Robots and Systems ’91 vol. 3 (1991), pp. 1442–1447 2. P. Skrzypczy´nski, Simultaneous localization and mapping: a feature-based probabilistic approach. Int. J. Appl. Math. Comput. Sci. 19(4), 575–588 (2009) 3. P. Agarwal, W. Burgard, C. Stachniss, Survey of geodetic mapping methods: geodetic approaches to mapping and the relationship to graph-based SLAM. Robot. Autom. Mag. IEEE 21, 63–80 (2014) 4. K. Kraus, I.A. Harley, S. Kyle, Photogrammetry: Geometry from Images and Laser Scans (De Gruyter, Berlin, 2011). Accessed 18 Oct 2011 5. A. Gruen, D. Akca, Least squares 3d surface and curve matching ISPRS J. Photogramm. Remote Sens. 59(3), 151–174 (2005). Accessed 11 July 2004; Accessed 16 Feb 2005; Accepted: 16 February 2005; Available online: 29 March 2005 6. B.K.P. Horn, Closed-form solution of absolute orientation using unit quaternions. J. Opt. Soc. Amer. A 4(4), 629–642 (1987) 7. B.K.P. Horn, H.M. Hilden, S. Negahdaripour, Closed-form solution of absolute orientation using orthonormal matrices. J. Opt. Soc. Amer. A 5(7), 1127–1135 (1988) 8. B. Triggs, P.F. McLauchlan, R.I. Hartley, A.W. Fitzgibbon, Bundle adjustment - a modern synthesis, in Proceedings of the International Workshop on Vision Algorithms: Theory and Practice, ICCV ’99 (Springer, Berlin, Heidelberg, 1999), pp. 298–372 9. E.M. Mikhail, F.E. Ackermann, Observations and Least Squares (University Press of America, Washington, D.C., 1982) 10. A.C. Aitken, On least squares and linear combination of observations. 55, 42–48 (1934) 11. R. Branham, A covariance matrix for total least squares with heteroscedastic data. Astron. J. 117, 1942 (2007) 12. R. Zlot, M. Bosse, Efficient large-scale three-dimensional mobile mapping for underground mines. J. Field Robot. 31(5), 758–779 (2014) 13. N.J. Higham, Cholesky factorization. Wiley Interdisciplinary Rev.: Comput. Stat. 1(2), 251– 254 (2009) 14. D.W. Marquardt, An algorithm for least-squares estimation of nonlinear parameters. SIAM J. Appl. Math. 11(2), 431–441 (1963) 15. R. Fletcher, Modified marquardt subroutine for non-linear least squares 16. Z. Zhang, Parameter estimation techniques: a tutorial with application to conic fitting. Image Vis. Comput. 15(1), 59–76 (1997) 17. D.Q.F. de Menezes, D.M. Prata, A.R. Secchi, J.C. Pinto, A review on robust m-estimators for regression analysis. Comput. Chem. Eng. 147, 107254 (2021) 18. S. Agarwal, K. Mierle, et al., Ceres solver. http://ceres-solver.org 19. J.T. Barron, A general and adaptive robust loss function, in 2019 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR) (2019), pp. 4326–4334 20. N. Chebrolu, T. Läbe, O. Vysotska, J. Behley, C. Stachniss, Adaptive robust kernels for nonlinear least squares problems (2020). arXiv:abs/2004.14938 21. J. Sola, J. Deray, D. Atchuthan, A micro lie theory for state estimation in robotics (2020) 22. F. Dellaert, M. Kaess, Factor graphs for robot perception. Found. Trends Robot. 6(1–2), 1–139 (2017) 23. M. Xie, A. Escontrela, F. Dellaert, A factor-graph approach for optimization problems with dynamics constraints (2020). arXiv:abs/2011.06194 24. M. Xie, F. Dellaert, Batch and incremental kinodynamic motion planning using dynamic factor graphs (2020). arXiv:abs/2005.12514 25. L.O. Tiziani, Y. Zhang, F. Dellaert, F.L. Hammond, Factor graph-based trajectory optimization for a pneumatically-actuated jumping robot, in ICRA (IEEE, 2021), pp. 9870–9876 26. S. Yang, G. Chen, Y. Zhang, H. Choset, F. Dellaert, Equality constrained linear optimal control with factor graphs, in ICRA (IEEE, 2021), pp. 9717–9723

94

4 Simultaneous Localization and Mapping

27. A. Cunningham, B. Paluri, F. Dellaert, DDF-SAM: fully distributed SLAM using constrained factor graphs, in IROS (IEEE, 2010), pp. 3025–3030 28. R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige, W. Burgard, G2o: a general framework for graph optimization, in 2011 IEEE International Conference on Robotics and Automation (2011), pp. 3607–3613 29. G. Grisetti, R. Kuemmerle, C. Stachniss, W. Burgard, A tutorial on graph-based SLAM. Intell. Trans. Syst. Mag. IEEE 2(4), 31–43 (2010) 30. W.H. Press, S.A. Teukolsky, W.T. Vetterling, B.P. Flannery, Numerical Recipes 3rd Edition: The Art of Scientific Computing, 3rd edn. (Cambridge University Press, USA, 2007) 31. C.J. Taylor, C.J. Taylor, D.J. Kriegman, Minimization on the lie group so(3) and related manifolds. Technical Report (1994) 32. U. Frese, A proof for the approximate sparsity of slam information matrices, in Proceedings of the 2005 IEEE International Conference on Robotics and Automation (2005), pp. 329–335 33. S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics (MIT Press, Cambridge, 2005) 34. N. Sunderhauf, P. Protzel, Switchable constraints for robust pose graph slam, in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems (2012), pp. 1879–1884 35. E. Olson, P. Agarwal, Inference on networks of mixtures for robust robot mapping. Int. J. Rob. Res. 32(7), 826–840 (2013) 36. P. Agarwal, G.D. Tipaldi, L. Spinello, C. Stachniss, W. Burgard, Robust map optimization using dynamic covariance scaling (2013), pp. 62–69 37. F. Dellaert, Factor graphs and gtsam: a hands-on introduction. Technical Report (2012) 38. G. Grisetti, T. Guadagnino, I. Aloise, M. Colosi, B. Della Corte, D. Schlegel, Least squares optimization: from theory to practice. Robotics 9(3), 51 (2020)

Chapter 5

Trajectory Estimation

5.1 Motion Model Motion model addresses the mobile mapping system pose estimation problem [1]. In mobile robotics [2], it is common practice to send commands as rotational and translational velocities for wheeled robots; thus, the velocity motion model can be used for pose estimation in consecutive time intervals. Alternatively, the odometry measurements can be used. In practice, the odometry readings are more accurate than velocities. Both techniques suffer from drift and obviously slippage. The difference between velocity motion model and odometry motion model is the fact that the odometry readings can be used in retrospect, after motion action. Thus, the disadvantage of the odometry motion model relates to accurate motion planning and control of the mobile robot. For better pose estimation, the IMU can be fused; thus, accurate measurement of the rotations can drastically improve the desired application. Looking from the mobile mapping systems point of view, the motion model should be chosen carefully to model lateral and longitudinal displacements. An important aspect is also the extrinsic calibration parameters of the multi-sensor system; thus, e.g. large displacement between IMU and LiDAR has to be added to the motion model. It is possible to retrieve the trajectory estimates looking from a probability point of view [2] based on velocity controls and velocity motion model or based on odometry and odometry motion model.

5.2 Kalman Fiter The Kalman filter is one of the common estimation algorithms [2]. It produces estimates based on inaccurate and uncertain [3] measurements modeled in the sense of the probabilistic approach. Thus, the Kalman filter fuses sensor signals and system knowledge in an optimal way as shown in Fig. 5.1. Furthermore, KF provides a prediction of the future system state, based on the past estimations. The common © The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_5

95

96

5 Trajectory Estimation

Fig. 5.1 Kalman filter—simple application

applications are location and navigation systems. The design end valuation of the kinematic and odometric approach in mobile robotics was discussed in [4]. This type of filter can reduce noisy measurements from, e.g. encoders and IMU. In order to predict the next pose, an object motion model (e.g. velocity motion model and odometry motion model) and the measurement noise model are crucial. Thus, the final trajectory can be retrieved as a smooth set of consecutive poses. Let us take a linear, dynamic system given with Eqs. (5.1) and (5.2), where x is a state vector and z ∈ R P×1 is an output vector. The system state vector x ∈ R N ×1 . It contains a state of the system at time moment t. The matrix A ∈ R N ×N is called state transition matrix. Vector u ∈ R M×1 represents an input to the system; thus, B ∈ R N ×M . (5.1) x t+1 = At x t + B t ut + vr z t+1 = C t x t+1 + v q

(5.2)

The matrix C ∈ R P×N maps a state vector x to the output of the system. The system given in (5.1) and (5.2) has • N length state vector, • M number of inputs, • P number of outputs. The system model has random variables vr and v q . The first one is called process noise and represents the non-ideal transition of the state. The second one is called sensor noise. Prediction step is given with p(x t+1 |x t , ut+1 ). The variable v p follows Normal distribution so (5.3) x t+1 = At x t + B t ut + N (vr ; 0, Rt ) where Rt is process noise variance. System state in the past is given with N (x; x t ,  t ). To obtain N (x; x t+1 ,  t+1 ), N (x; x t+1 ,  t+1 ) = At N (x; x t ,  t ) + B t ut + N (vr ; 0, Rt )

(5.4)

5.2 Kalman Fiter

97

The linear combination of two Gaussian distributions (5.5) gives (5.6). N (μ3 ,  3 ) = B 1 N (μ1 ,  1 ) + B 2 N (μ2 ,  2 ) 

(5.5) 

N (μ3 ,  3 ) = N (B 1 μ1 + B 2 μ2 , B 1  1 B 1 + B 2  2 B 2 )

(5.6)

¯ t+1 ). Finally, we obtain (5.7) and (5.8), which is a formula for a prediction step ( x¯ t+1 , x¯ t+1 = At x t + B t ut

(5.7)

¯ t+1 = At  t At + Rt 

(5.8)

Estimated value of the output of the system is given by (5.9). 

¯ zt+1 ) = N ( x¯ ; C t+1 x¯ t+1 , C t+1  ¯ t+1 C t+1 + Q t+1 ) N (¯z ; z¯ t+1 , 

(5.9)

The conditional probability of Gaussian distributions : p(x 1 |(x 2 = k)) = N (x j ; μ j ,  j ) where x1 = N (x 1 ; μ1 ,  11 ) and x2 = N (x 2 ; μ2 ,  22 ) can be expressed as Eq. (5.10). This form also exposes the crosscovariance  12 and  21 between N (x 1 ; μ1 ,  1 ) N (x 2 ; μ2 ,  2 ) distributions.  N

     μ  11  12 x1 ; 1 , x2 μ2  21  22

(5.10)

The joint distribution N (x j ; μ j ,  j ) is given with μ j = μ1 +  12  −1 22 (k − μ2 )

(5.11)

 j =  11 −  12  −1 22  21

(5.12)

Conditional probability is given with p(x t+1 |z t+1 ). At this moment, we have a mea¯ zt+1 ); thus, we obtain (5.13), surment z t+1 and estimated measurment N (¯z ; z¯ t+1 ,  (5.14) and (5.15). 

       ¯ t+1 C t+1 ¯ t+1 xt+1  x¯  x¯ t+1 =N ; , ¯ t+1 ¯ zt+1 z t+1 z¯ t+1 z¯ C t+1   −1

¯ t+1 C t+1  ¯ z (z t+1 − z¯ t+1 ) x t+1 = x¯ t+1 +  t+1 

−1

¯ t+1 −  ¯ t+1 C t+1  ¯ z C t+1  ¯ t+1  t+1 =  t+1

(5.13) (5.14) (5.15)

98

5 Trajectory Estimation

¯ t+1 C t+1  ¯ −1 The expression  z t+1 can be substituted with variable called the Kalman ¯ zt+1 and z¯ t+1 can be substigain (5.16) in Eq. (5.14) and (5.15). Further expression  tuted with Eq. (5.9). −1    ¯ t+1 C t+1  ¯ ¯ −1 ¯ K t+1 =  z t+1 =  t+1 C t+1 C t+1  t+1 C t+1 + Q t+1

(5.16)

Finally, x t+1 = x¯ t+1 + K t+1 (z t+1 − C t+1 x¯ t+1 )

(5.17)

¯ t+1 − K t+1 C t+1  ¯ t+1 ¯ t+1 = (I − K t+1 C t+1 )  t+1 = 

(5.18)

The Kalman filter algorithm is given with set of Eqs. (5.19)–(5.23). It can be observed that the error propagation (linear case) discussed in Sect. 6.5 is used for the uncertainty  ¯ t+1 C t+1 in Eq. (5.21). calculation as At  t At in Eq. (5.20) and C t+1  x¯ t+1 = At x t + B t ut

(5.19)



¯ t+1 = 

A  A +Rt  t t t

(5.20)

error propagation

⎛

⎞−1

¯ t+1 C t+1 ⎜ ¯ t+1 C t+1 + Q t+1 ⎟ K t+1 =  ⎝C t+1  ⎠ 



(5.21)

error propagation

x t+1 = x¯ t+1 + K t+1 (z t+1 − C t+1 x¯ t+1 )

(5.22)

¯ t+1  t+1 = (I − K t+1 C t+1 )

(5.23)

5.2.1 Extended Kalman Filter The non-linear system is given with equations x t+1 = g(x t , ut ) + vr

(5.24)

z t+1 = h(x t+1 ) + v q

(5.25)

5.2 Kalman Fiter

99

The function g(x, t) and h(x) are non-linear functions. Random variables vr and v q are under non-linear transformations and the distribution of x t+1 is not normal anymore. The functions g(x t , ut ) can be linearized as follows: g(x t , ut ) ≈ g(x t , ut ) + h(x t+1 ) ≈ h(x t+1 ) +

∂g(x t , ut ) x t = g(x t , ut ) + G t x t ∂ xt

∂h(x t+1 ) x t+1 = h(x t+1 ) + H t+1 x t+1 ∂ x t+1

(5.26)

(5.27)

where H t+1 is Jacobian around x t+1 of sensor model (5.25) and G t is Jacobian around x t , ut of process model (5.24). Equations (5.26) and (5.27) are first element of the Taylor expansion of Eqs. (5.24) and (5.25). With linearization (5.26) and (5.27), the Extended Kalman Filter algorithm can be constructed. Equation (5.28) implements estimation of state using non-linear function g. Equations (5.29)–(5.32) uses the linearization of process model and sensor model. It can be observed that the error propagation (non-linear case) discussed in Sect. 6.5 is used for the uncertainty  ¯ t+1 H t+1 in Eq. (5.30). calculation as G t  t G t in Eq. (5.29) and H t+1  x¯ t+1 = g(x t , ut )

(5.28)

¯ t+1 = G t  t G t +Rt   

(5.29)

error propagation

⎛

⎞−1

¯ t+1 H t+1 ⎜ ¯ t+1 H t+1 + Q t ⎟ K t+1 =  ⎝ H t+1  ⎠ 



(5.30)

error propagation

x t+1 = x¯ t+1 + K t+1 (z t+1 − H t+1 x¯ t+1 )

(5.31)

¯ t+1  t+1 = (I − K t+1 H t+1 )

(5.32)

5.3 Particle Filter It is already more than two decades since the pioneering modern sequential Monte Carlo approach (particle filter) was introduced [5]. Particle filters solve numerous hard perceptual problems in robotics including high-dimensional estimation problems such as robot localization in environments with known maps [6]. The advantage of parallel computing for solving particle filter problem is evident since each particle

100

5 Trajectory Estimation

can be processed individually. Hence, many researchers are developing multi-core solutions for particle filter [7, 8]. Moreover, there are also cloud-based approaches (i.e. [9]) for solving a high computational task without the need for a powerful onboard computer. The efficiency of the particle filter for the mobile robot localization depends on many factors such as the number of particles, motion, perception and map. Thus, proper motion models have to be chosen [2]. Due to the fact that there are plenty of particle filter implementations, it is worth to mention the tutorial [10] containing a detailed description of the Monte Carlo Simulation method. This tutorial is sufficient for particle filter prototyping; thus, it was used as a reference in the presented research. Another important contribution is a tutorial on particle filters for online non-linear/non-Gaussian–Bayesian tracking [11]. It is important to emphasize the fact that the use of 3D semantic data within the context of particle filter localization is not evident in the literature; thus, the major objective of the presented section is to demonstrate it. The main objective of the particle filter is to track the robot pose over time with a non-Gaussian and multi-modal PDF. The basis of the method is to construct a sample-based representation of the entire PDF. A series of actions are taken, and each one is modifying the state of the variable of interest according to a certain model. Multiple copies—particles of the variable of interest—are used, and each one is associated with the weight that signifies the quality of that specific particle measured as an overlap function between the current observation and the reference map. An overlap function uses the map to find the best matches. Thus, an estimate of the robot pose is obtained by the particle with the highest weight. Obviously, there are other possible methods for obtaining estimation such as the weighted sum of all particles. The particle filter is recursive and it operates in two phases: prediction and update. After each action, each particle is modified according to the motion model (prediction). Then, each particle’s weight is re-evaluated based on the latest sensory information assuming overlap function (update). The recursive process determines that some particles reach very small weights. Hence, the resampling process eliminates such particles and duplicates the best ones (with the highest probability weight).

5.3.1 Variable of Interest The variable of interest is the pose of the moving ground vehicle xk = [x k , y k , z k , yaw k , pitch k , r oll k ] at time t = k. It is represented as a set of M samples (particles) (Sik = [xkj , w kj ] : j = 1...M), where the index j denotes the particle. Each particle consists of a copy of the variable of interest and the weight wkj that defines the contribution of this particle to the overall estimation of the variable.

5.3 Particle Filter

101

5.3.2 Prediction Stage (Motion Model) The prediction phase uses a motion model in order to simulate the effect an action has on a set of particles with appropriate noise added. Due to the movement on the ground and the use of the IMU (pitch and roll angles are known), it is assumed that z k , pitch k , r oll k can be arbitrary calculated based on IMU, map and the known geometry of the vehicle. For this reason, z k for a given x k , y k is calculated as Z coordinate of the closest point with semantic label ground/floor from the map. Given pitch k , r oll k angles from IMU are used for the current 3D point cloud transformation. Due to the prediction problem reduction into the 2Dcases, it was possible to reach an online performance. The additive Gaussian noise model is used for the motion (the first robot rotates to face the destination position and then it translates forward for a given distance). Thus, we obtained a typical banana-shaped distribution [2, 5].

5.3.3 Use of Semantic Data in the Particle Filter For each iteration, the weights in the particle filter are updated by the value of the overlap function corresponding to the number of semantic nearest neighbors of the current 3D semantic point cloud to the reference semantic map in relation to the total number of 3D points of local measurement of a certain semantic label (wall, ceiling, ground/floor and edge). Thus, the final value of the overlap is bounded by 0 in the case when there is no overlap and 1 in the case when the nearest neighbor is found for each query point. Figures 5.2 and 5.3 show the particle filter in action. It is possible to modify the particle filter for the map building process [12]. This method is originally called FastSLAM [13], and further work can be found in [14]. It is possible to incorporate semantic maps instead of landmarks into the FastSLAM algorithm, and the result of retrieving trajectory and mapping (map of the best particle) is shown in Fig. 5.4.

5.3.4 Update Stage and Resampling The update phase uses the information obtained from the sensing to update the particle weights in order to accurately describe the moving robot’s PDF. Thus, a high value of the probability corresponds to the perfect match of the current semantic point cloud to a certain location in the semantic map. A very small value of the calculated probability corresponds to the worst scenario where there is no correspondence to the semantic map. Finally, when the weight drops below a certain threshold, these particles take part in the resampling. The resampling procedure eliminates all particles with the weight dropped below a certain threshold and duplicates the best one to maintain

102 Fig. 5.2 Particles over the semantic scene

Fig. 5.3 Particles over the desired pose

Fig. 5.4 Mapping with particle filter, red—trajectory of the winning particle

5 Trajectory Estimation

5.3 Particle Filter

103

the same total number of particles. The online performance was possible due to its capability of putting all the particle filter calculations directly on the GPU. Thus, the data transfer was sufficiently decreased. During the initiation phase, the reference model with the semantic labels is copied to the GPU memory, and the regular grid decomposition that will be used for the entire process of the pose estimation is calculated. The key concept of the approach is that the reference data are stored and decomposed directly in the GPU memory during the entire process of the particle filter calculations. Therefore, the need to copy data between host (CPU) and device (GPU) is drastically reduced. This causes the limitation of the method related to the capacity of the GPU RAM. For each updated stage of the particle filter, the current 3D semantic data is copied from the host to the device. The probabilities for all particles are calculated based on the parallel implementation of the semantic nearest neighborhood search procedure. More information can be found in [15].

5.4 Structure from Motion SFM algorithm assuming a pinhole camera model calculates the 3Dstructure of the environment using consecutive 2Dimages. Obviously different variations of SFM, e.g. for equrectangular cameras exist [16]. We are focused on estimating the trajectory; therefore, we talk about the so-called incremental SFM [17], and as a result, we expect also the 3D structure of the environment. Due to an assumption that there is no initial knowledge about the 3D structure, it is evident to introduce techniques for retrieving relative poses between consecutive images. For this reason, it is possible to use fundamental matrix F or homography matrix H [18]. F and H relate corresponding points in two images x, x  . F describes an epipolar line on which x  on the second image must lie. H describes a relation between the same planar surface in space observed by two images; therefore, it is straightforward to find x  in the second image. Recent SFM algorithms [19] use both techniques and RANSAC method [20] to retrieve best initial guess of the relative pose between consecutive 2Dimages. Having F or H, it is possible to retrieve a relative pose [R, t] between two images but the scale factor is an issue; therefore, this relative pose can be different from the ground truth. Having a set of corresponding points on different images, relative poses between these images and camera projection function, it is possible to reconstruct a 3D scene with the so-called triangulation method [18]. At this stage of the SFM algorithm, we have a trajectory (set of relative poses) estimated based on consecutive 2Dimages and an initial guess for the 3D structure of the environment. This stage of SFM is called the front-end. Front-end typically provides not optimal solution determined by the growing incremental error. To reduce this error, the BA algorithm being back-end of SFM is incorporated. BA simultaneously refines the 3D coordinates describing the scene geometry, the parameters of the relative poses and optionally the optical characteristics of the cameras. It is worth to mention that instead of points, we can use line representation [21, 22], and in such a case, the general scheme of SFM remains the same.

104

5 Trajectory Estimation

5.4.1 Pinhole Camera Model For the pinhole camera model, the scene view is formed by projecting a 3D point into the image plane using a perspective transformation. First, we introduce camera←world [R, t]cw and world←camera [R, t]wc transformation matrices that satisfy the following condition: 

  −1    Rcw t cw Rwc t wc Rwc −Rwc t wc = = 01×3 1 01×3 1 01×3 1

(5.33)

The world←camera [R, t]wc matrix transforms 3D point P local (x l , y l , z l ) in local camera reference system to point P global (x g , y g , z g ) in global reference system. The camera←world [R, t]cw matrix transforms 3D point P global (x g , y g , z g ) in global reference system to point P local (x l , y l , z l ) in local camera reference system. The pinhole camera model incorporates [R, t]cw to transform P global (x g , y g , z g ) to local camera reference system to find projection onto image plane by the following equation: ⎡ g⎤ ⎡ ⎤ ⎡ ⎤ x u f x 0 cx ⎢yg⎥ ⎥ (5.34) s ⎣v ⎦ = ⎣ 0 f y c y ⎦ [R, t]cw ⎢ ⎣zg ⎦ 1 0 0 1 1 where s = 1/z l and z l is z coordinate of point in camera local coordinate system, (u, v) are the coordinates of the projection point P global (x g , y g , z g , 1) in pixels, ( f x , f y ) are the focal lengths expressed in pixel units and (cx , c y ) is a principal point that used to be at the center of the image. Principal point and focal lengths expressed in pixel units are called intrinsic camera parameters and form K ∈ R3×3 camera calibration matrix (5.35). ⎡ ⎤ f x 0 cx K = ⎣ 0 f y cy ⎦ (5.35) 0 0 1 Intrinsic camera parameters are necessary to link the pixel coordinates of an image point with the corresponding coordinates in the camera reference frame. These parameters characterize the optical, geometric and digital characteristics of the camera such as the perspective projection (focal lengths), the transformation between image plane coordinates and pixel coordinates and the geometric distortion introduced by the optics.

5.4 Structure from Motion

105

5.4.2 Essential and Fundamental Matrices The essential matrix is 3 × 3 matrix that encodes epipolar geometry [18]. For a given point x in one image, its multiplication by the essential matrix E gives the epipolar line l’ (e R -x R ) in the second image (Fig. 5.5); therefore, Ex = l

(5.36)

If the point x’ is on the epipolar line l’, then xT l = 0

(5.37)

Moreover, both epipoles e L and e R in their respective image planes and both optical centers O L and O R lie on a single 3D line. Having the camera–camera relative pose [R, t], we can compute x  with the following rigid motion equation: x  = R(x − t)

(5.38)

Due to the assumption that x, t, x  are coplanar, the following condition is satisfied: x  (t × x) = 0

(5.39)

where × is a cross product. Furthermore, we can construct the following coplanarity condition: (5.40) (x − t) (t × x) = 0 since vector (x − t) lies on the same plane as x due to coplanarity. From rigid motion Eq. 5.38, we can extract (x − t) as follows: x  = R(x − t) ⇒ R x  = R R(x − t) ⇒ R x  = (x − t)

Fig. 5.5 Epipolar line in image with optical center O R for point x projected onto image with optical center O L as x L , e L and e R are epipoles

X3

X2

X1

(5.41)

X

XR

XL

OL

eL

eR

OR

106

5 Trajectory Estimation

In this case, we can use ( AB) = B  A , ( A ) = A

(5.42)

Therefore, R x  = (x − t) ⇒ (R x  ) = (x − t) ⇒ x  (R ) = x  R = (x − t) (5.43) Combining Eqs. 5.43 and 5.40, we obtain (x  R)(t × x) = 0

(5.44)

Expressing (t × x) using skew-symmetric matrix t × , we obtain ⎡ ⎤⎡ ⎤ 0 −t3 t2 x1   t × x = t × x = ⎣ t3 0 −t1 ⎦ ⎣x2 ⎦ −t2 t1 0 x3

(5.45)

Therefore as in [23], we obtain essential matrix     (x  R)(t × x) = (x  R)( t × x) = x  (R t × )x = x  Ex = 0

(5.46)

The essential matrix uses camera coordinates. To use image coordinates ( x¯ , x¯  ), the intrinsic camera parameters expressed as K 1 , K 2 ∈ R3×3 have to be considered; therefore, ¯ , x  = K −1 ¯ (5.47) x¯ = K 1 x, x¯  = K 2 x  ⇒ x = K −1 1 x 2 x Equation 5.46 becomes −1  ¯  ) E(K −1 ¯ ) ⇒ x¯  ((K −1 ¯ = x¯  F x¯ = 0 x  Ex = (K −1 2 x 1 x 2 ) E K 1 )x

(5.48)

where F is the fundamental matrix. The relation between fundamental, essential and rigid motion matrices is given in following equation:   −1  F = (K −1 2 ) EK1 , E = R t ×

(5.49)

To compute essential and fundamental matrices, the eight-point algorithm can be used, and it was introduced in [23] and further elaborated in [18]. This algorithm requires at least eight corresponding points in two images. Once the essential matrix is obtained from the fundamental matrix and intrinsic camera parameters, it is possible to retrieve camera–camera relative pose based on SVD [18] method. From consecutive camera-camera relative poses, we can build trajectory and reconstruct a 3D scene with the triangulation method [18]. Having a 3D scene, it is possible to incorporate the BA method for reducing cumulative incremental error for this trajectory.

5.4 Structure from Motion

107

5.4.3 Homography Matrix The homography matrix H ∈ R3×3 transforms points from one plane to another. To derive homography matrix, we start from Eq. 5.34; thus, ⎡ ⎤ ⎤ xg ⎡ ⎤ ⎡ ⎤⎡ u f x 0 cx r11 r12 r13 t1 ⎢ g ⎥ y ⎥ s ⎣v ⎦ = ⎣ 0 f y c y ⎦ ⎣r21 r22 r23 t2 ⎦ ⎢ ⎣zg ⎦ 1 0 0 1 r31 r32 r33 t3 1

(5.50)

If we consider point P g (x g , y g , z g ) that lies on the plane with Z = 0, Eq. 5.50 becomes

⎡ ⎤ ⎤ xg ⎤ ⎡ g⎤ ⎡ ⎤ ⎡ ⎡ ⎤⎡ ⎤⎡ x u f x 0 cx f x 0 cx r11 r12 0 t1 ⎢ g ⎥ r11 r12 t1 y ⎥ ⎣ ⎢ ⎦ ⎦ ⎣ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ r21 r22 0 t2 ⎣ ⎦ = 0 f y c y r21 r22 t2 yg⎦ s v = 0 f y cy 0 1 0 0 1 r31 r32 0 t3 0 0 1 r31 r32 t3 1 1 (5.51) Finally, we obtain H that maps u, v onto x g , y g assuming z g = 0. ⎡ ⎤ ⎡ ⎤ ⎤⎡ f x 0 cx h 11 h 12 h 13 r11 r12 t1 ⎣ 0 f y c y ⎦ ⎣r21 r22 t2 ⎦ = ⎣h 21 h 22 h 23 ⎦ (5.52) 0 0 1 r31 r32 t3 h 31 h 32 h 33 A simplest approach for retrieving camera pose [R, t] from homography matrix H determines incorporating the following equation: ⎤ ⎡ ⎤ ⎡ ⎤ h 11 h 12 h 13 r11 r12 t1 h 11 h 12 h 13 H = ⎣h 21 h 22 h 23 ⎦ ⇒ K−1 H = K−1 ⎣h 21 h 22 h 23 ⎦ = ⎣r21 r22 t2 ⎦ h 31 h 32 h 33 h 31 h 32 h 33 r31 r32 t3 ⎡

(5.53)

Third column of R can be calculated from cross product of the first and second column of normalized K −1 H resulting ||r11 , r21 , r31 || = 1; thus, R3 = R1 × R2

(5.54)

To find homography between two images H 21 , we use the fact that ⎡ g ⎤ ⎡ ⎤2 ⎡ g⎤ ⎡ ⎤1 x x u u ⎣v ⎦ = H 1 ⎣ y g ⎦ , ⎣v ⎦ = H 2 ⎣ y g ⎦ 1 1 1 1 where the same point is observed in two images; therefore,

(5.55)

108

5 Trajectory Estimation

⎡

⎡ ⎤1 ⎤ xg u ⎣ y g ⎦ = H −1 ⎣ v⎦ 1 1 1

(5.56)

and finally, we obtain ⎡ ⎤2 ⎡ ⎤1 ⎡ ⎤1 u u u ⎣v ⎦ = H 2 H −1 ⎣ ⎦ ⎣ ⎦ v = H 21 v 1 1 1 1

(5.57)

To retrieve camera to camera relative pose [R, t] from H 21 , first we apply camera calibration matrices as follows:  21 = K −1 H 2 H 21 K 1

(5.58)

 21 [24, 25]. From consecutive cameraand then apply SVD-based method to H camera relative poses, we can build trajectory and reconstruct the 3D scene. Having a 3D scene, it is possible to incorporate the BA method for improving the trajectory and refine the 3D scene. The homography estimation is based on at least 4-point correspondences where no 3 points are collinear; therefore, we should find at least 4 corresponding points on two different images that are lying on the same plane. The basic homography estimation uses the DLT algorithm [26]. There are plenty of potential applications for homography such as retrieving camera to camera relative pose in SFM, building mosaic from aerial images, augmenting images by adding virtual scenes on top of the marked plane and image rectification.

5.4.4 Bundle of Rays Intersection (Triangulation) Finding intersection [txint , t yint , tzint ] of bundle of rays [27] is crucial for SFM. This method retrieves the coordinates of 3D point in the scene based on the camera poses [R, t]wc and matched keypoints [u, v]. Therefore, ray direction rayc expressed in camera coordinates can be computed from Eq. (5.59)  rayc =

(v − c y )/ f y (u − cx )/ f x 1  , , 2 2 2 2 2 x +y +1 x +y +1 x + y2 + 1

 (5.59)

where u, v are pixel coordinates in rectified image (removed distortion). To express ray in global reference system, it is straightforward to transform rayc by world←camera rotation matrix Rwc ; therefore, rayw = Rwc rayc

(5.60)

5.4 Structure from Motion

109

Fig. 5.6 Green: bundle of rays; red: intersection

Point on the rayw has coordinates of camera center t wc . Having bundle of rays expressed in global reference system (Fig. 5.6), it is possible to incorporate simplified point-to-line observation equation discussed in Sect. 8.2.1. For this reason assuming line representation as Rln =[V x , V y , V z ] as expressed in Eq. 8.49, rayw is V z . Thus, V x and V y can be calculated freely assuming they satisfy the orthogonality condition of Rln and det Rln = 1 and t ln = t wc . The bundle of rays intersection observation equation is given as 5.61.   δx = δy   r esidual

  0 0  

target values

  Vx  int t − t ln  Vy

 

 −

(5.61)

model f unction

    where δ δ are residuals, 0 0 are target values and  [t int ] (V x , V y , t int , t ln ) =   x y  V x  int t − t ln is the model function. From model function, it can be seen that V y int int there are only three parameters to optimize β = [t int x , t y , t z ], and from observation equation, it is straightforward that two equations contribute for each ray in the r

Jacobian J of the objective function. Therefore, this method can efficiently calculate r

intersection for n ∈ 2, m) rays. The Jacobian J of the objective function is given in Eq. 5.62.

110

5 Trajectory Estimation r

J=

 d1,1 d1,2 d1,3 = d2,1 d2,2 d2,3 ⎛ ∂δ ∂δ ∂δ ⎞



x

⎝

x

x

∂txint ∂t yint ∂tzint ∂δ y ∂δ y ∂δ y ∂txint ∂t yint ∂tzint

⎠

(5.62)

r

The Python code for generating the Jacobian J of the objective function for bundle of rays intersection observation equation is given in Listing 5.1. 1

from sympy import *

2 3 4 5 6

t_x_int, t_y_int, t_z_int = symbols(’t_x_int t_y_int t_z_int’) t_x_ln, t_y_ln, t_z_ln = symbols(’t_x_ln t_y_ln t_z_ln’) vxx, vxy, vxz = symbols(’vxx vxy vxz’) vyx, vyy, vyz = symbols(’vyx vyy vyz’)

7 8 9

ray_intersection_symbols = [t_x_int, t_y_int, t_z_int] all_symbols = ray_intersection_symbols

10 11 12 13 14

t_int = Matrix([t_x_int, t_y_int, t_z_int]) t_ln = Matrix([t_x_ln, t_y_ln, t_z_ln]) vxt = Matrix([[vxx, vxy, vxz]]).transpose() vyt = Matrix([[vyx, vyy, vyz]]).transpose()

15 16 17

18 19

target_value = Matrix([0,0]).vec() model_function = Matrix([vxt.dot(t_int - t_ln),vyt.dot(t_int t_ln)]).vec() delta = target_value - model_function delta_jacobian=delta.jacobian(all_symbols) r

Listing 5.1 Python code for generating the Jacobian J of the objective function for bundle of rays intersection observation equation

5.4.5 Bundle Adjustment BA is an important component of the SFM algorithm capable of simultaneously refining a map and correcting all camera poses. Specifically, BA techniques [28– 32] correct 3D coordinates of the primitives (points, lines [33], etc.) describing the scene geometry, refine poses of the cameras and optimize the optical characteristics of the cameras (pinhole camera model, fish-eye, metric camera and equirectangular camera). The optimality criterion involves mainly colinearity and coplanarity observation equations. Thus, typical BA minimizes the reprojection error between the image coordinates of observed and predicted image points. For example, BA can be build based on following colinearity observation Eqs. (7.5), (7.50) and (7.75). These basic observation equations can be extended by coplanarity observation Eqs. (7.63) and (7.84) that can improve the estimation of the camera rotation parameters. Fur-

5.4 Structure from Motion

111 X2

Fig. 5.7 BA in computer vision [28]

X3

X1

u

11

u’ 11 u 21 u

u

41

C1

31

u

u’ 12 u

12

u u

u 22

42 u

43

u

23

u

33

13 u’ 13

32

C3 C2

Fig. 5.8 BA in photogrammetry [29]

z

y x

thermore, georeferencing can be done by the incorporation of the georeferenced data source by following observation Eqs. (7.29) and (9.4). BA is the well-known technique widely used both for commercial and research applications. It can be solved using Weighted Non-Linear Least Square Optimization described in Sect. 4.2. The example BA system form in computer vision is shown in Fig. 5.7 and in Photogrammetry in Fig. 5.8. Further information on how to construct the normal equation system for BA is discussed in Sect. 11.1.

5.5 Lidar Odometry LiDAR odometry calculates relative pose between consecutive LiDAR data without any additional data. Consecutive relative poses build a trajectory; thus, this trajectory can be used for map reconstruction what is shown in Figs. 5.9, 5.10 and 5.11. It is possible to incorporate all LiDAR metrics discussed in Chap. 8 for building LiDAR odometry system. There are plenty of alternative implementations such as [34–38]. The information derived only from LiDAR could cause many issues in relative pose

112 Fig. 5.9 Result of LiDAR odometry; green: computed trajectory; red: reconstructed map

Fig. 5.10 2D map reconstructed using ClearPath Jackal mobile robot odometry and LiDAR

Fig. 5.11 2D map reconstructed using LiDAR odometry and LiDAR

Fig. 5.12 Optimal solution

5 Trajectory Estimation

5.5 Lidar Odometry

113

Fig. 5.13 Initial guess of 5 ◦C

Fig. 5.14 Initial guess of 15 ◦C

Fig. 5.15 Initial guess of 30 ◦C

calculations; therefore additional constraints such as anisotropic motion discussed in Sect. 9.3 can provide robust estimates. Moreover, additional information sources such as odometry readings can also improve the convergence. It is straightforward that LiDAR odometry can use the previously computed relative pose as an initial guess; thus, relative pose observation equations discussed in Sects. 9.5.1 and 9.5.2 can help in preserving motion model. The major issue with LiDAR odometry is the computational complexity of the nearest neighbor search procedure finding matches

114

5 Trajectory Estimation

Fig. 5.16 The convergence of the LiDAR odometry for two consecutive poses from Fig. 5.13 in function of algorithms’ iterations for sufficient initial guess. Point-to-point metric is discussed in Sect. 8.1.1, point to projection onto plane metric is discussed in Sect. 8.1.6, point-to-plane dot product metric is discussed in Sect. 8.2.2 and plane-to-plane metric is discussed in Sect. 8.3.2

Fig. 5.17 The convergence of the LiDAR odometry for two consecutive poses from Fig. 5.14 in function of algorithms’ iterations for bad initial guess. Point-to-point metric is discussed in Sect. 8.1.1, point to projection onto plane metric is discussed in Sect. 8.1.6, point-to-plane dot product metric is discussed in Sect. 8.2.2 and plane-to-plane metric is discussed in Sect. 8.3.2

between primitives (points, lines and planes). Another issue is related to the initial guess and number of iterations required for the optimal solution. To demonstrate it, an experiment was performed; thus Figs. 5.12, 5.13, 5.14 and 5.15 show two consecutive LiDAR data for different initial guesses. The convergence of the LiDAR odometry in function of algorithms’ iterations is shown in Figs. 5.16, 5.17 and 5.18. For the sufficient initial guess shown in Fig. 5.16, all LiDAR metrics result in the convergence into an optimal solution. The cases of bad and very bad initial guesses have problems with convergence. It was observed that the plane-to-plane metric with

5.5 Lidar Odometry

115

Fig. 5.18 The convergence of the LiDAR odometry for two consecutive poses from Fig. 5.15 in function of algorithms’ iterations for very bad initial guess. Only plane-to-plane metric with quaternion parametrization of the rotation matrix converges to the optimal solution. Point-to-point metric is discussed in Sect. 8.1.1, point to projection onto plane metric is discussed in Sect. 8.1.6, point-to-plane dot product metric is discussed in Sect. 8.2.2 and plane-to-plane metric is discussed in Sect. 8.3.2

quaternion parametrization of the rotation matrix is most robust. We are far away from the statement that this particular metric (winning in this experiment) is the preferable one since in the real world there are plenty of edge cases where other metrics could be much more efficient. One prominent solution could be the fusion of the metrics; thus, the LiDAR odometry application will be capable of using the advantages of the particular metrics.

References 1. T.T.Q. Bui, K.-S. Hong, A comparison of using probabilistic motion models for mobile robot pose estimation, vol. 09 (2009), pp. 528–532 2. S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics (MIT Press, Cambridge, Mass, 2005) 3. P. Skrzypczy´nski, D. Belter, M. Nowicki, Modeling spatial uncertainty of point features in feature-based RGB-D SLAM. Mach. Vis. Appl. 29(5), 827–844 (2018) 4. T.D. Larsen, K.L. Hansen, N.A. Andersen, O. Ravn, Design of Kalman filters for mobile robots: evaluation of the kinematic and odometric approach, in Proceedings of IEEE Conference on Control Applications, vol. 2 (IEEE, United States, 1999) 5. N.J. Gordon, D.J. Salmond, A.F.M. Smith, Novel approach to nonlinear/non-gaussian Bayesian state estimation. IEEE Proc. F, Radar Signal Process. 140(2), 107–113 (1993) 6. S. Thrun, Particle filters in robotics, in Proceedings of the 17th Annual Conference on Uncertainty in AI (UAI) (2002) 7. M. Chitchian, A.S. van Amesfoort, A. Simonetto, T. Keviczky, H.J. Sips, Adapting particle filter algorithms to many-core architectures, in IEEE 27th International Symposium on Parallel Distributed Processing (IPDPS) (2013), pp. 427–438 8. M.A. Chao, C.Y. Chu, C.H. Chao, A.Y. Wu, Efficient parallelized particle filter design on CUDA, in 2010 IEEE Workshop On Signal Processing Systems (2010), pp. 299–304

116

5 Trajectory Estimation

9. S. Kamburugamuve, H. He, G. Fox, D. Crandall, Cloud-based parallel implementation of slam for mobile robots, in Proceedings of the International Conference on Internet of Things and Cloud Computing, ICC ’16 (ACM, New York, NY, USA, 2016), pp. 48:1–48:7 10. I. Rekleitis, A particle filter tutorial for mobile robot localization. Technical report TR-CIM-0402, Centre for Intelligent Machines, McGill University, 3480 University St., Montreal, Québec, CANADA H3A 2A7 (2004) 11. M.S. Arulampalam, S. Maskell, N. Gordon, A tutorial on particle filters for online nonlinear/non-gaussian bayesian tracking. IEEE Trans. Signal Process. 50:174–188 (2002) 12. S. Gharatappeh, M. Ghorbanian, M. Keshmiri, H.D. Taghirad, Modified fast-slam for 2D mapping and 3D localization, in 2015 3rd RSI International Conference on Robotics and Mechatronics (ICROM) (2015), pp. 108–113 13. M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit, Fastslam: a factored solution to the simultaneous localization and mapping problem, in Eighteenth National Conference on Artificial Intelligence (American Association for Artificial Intelligence, USA, 2002), pp. 593–598 14. M. Montemerlo, S. Thrun, D. Roller, B. Wegbreit, Fastslam 2.0: an improved particle filtering algorithm for simultaneous localization and mapping that provably converges, in Proceedings of the 18th International Joint Conference on Artificial Intelligence (IJCAI’03, Morgan Kaufmann Publishers Inc., San Francisco, CA, USA, 2003), pp. 1151–1156 15. J. Bedkowski, T. Röhling, Online 3d LIDAR Monte Carlo localization with GPU acceleration. Ind. Robot. 44(4), 442–456 (2017) 16. A. Pagani, D. Stricker, Structure from motion using full spherical panoramic cameras, in 2011 IEEE International Conference on Computer Vision Workshops (ICCV Workshops) (2011), pp. 375–382 17. J.L. Schonberger, J. Frahm, Structure-from-motion revisited, in 2016 IEEE Conference on Computer Vision and Pattern Recognition (CVPR) (2016), pp. 4104–4113 18. R. Hartley, A. Zisserman, Multiple View Geometry in Computer Vision, 2nd edn. (Cambridge University Press, USA, 2003) 19. S. Sumikura, M. Shibuya, K. Sakurada, OpenVSLAM: a versatile visual SLAM framework, in Proceedings of the 27th ACM International Conference on Multimedia (MM ’19, ACM, New York, NY, USA, 2019), pp. 2292–2295 20. M.A. Fischler, R.C. Bolles, Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography. Commun. ACM 24(6), 381–395 (1981) 21. A. Bartoli, P. Sturm, Structure-from-motion using lines: representation, triangulation, and bundle adjustment. Comput. Vis. Image Underst. 100(3), 416–441 (2005) 22. A. Bartoli, P. Sturm, The 3D line motion matrix and alignment of line reconstructions. Int. J. Comput. Vision 57, 159–178 (2004) 23. H.C. Longuet-Higgins, A Computer Algorithm for Reconstructing a Scene from Two Projections (Morgan Kaufmann Publishers Inc., San Francisco, CA, USA, 1987), pp. 61–62 24. E. Malis, M. Vargas, Deeper understanding of the homography decomposition for vision-based control. Research report (2007) 25. O. Faugeras, F. Lustman, Motion and structure from motion in a piecewise planar environment. Int. J. Pattern Recognit. Artif. Intell.—IJPRAI 02, 09 (1988) 26. E. Dubrofsky, R. Woodham, Combining line and point correspondences for homography estimation vol. 12 (2008) pp. 202–213 27. R. Lakemond, C. Fookes, S. Sridharan, Resection-intersection bundle adjustment revisited. ISRN Mach. Vis. 261956, 1–8 (2013) 28. B. Triggs, P.F. McLauchlan, R.I. Hartley, A.W. Fitzgibbon, Bundle adjustment—a modern synthesis, in Proceedings of the International Workshop on Vision Algorithms: Theory and Practice (ICCV ’99, Springer, Berlin, Heidelberg, 1999), pp. 298–372 29. K. Kraus, I.A. Harley, S. Kyle, Photogrammetry: Geometry from Images and Laser Scans (De Gruyter, Berlin, Boston, 18 Oct 2011) 30. M.I.A. Lourakis, A.A. Argyros, Sba: a software package for generic sparse bundle adjustment. ACM Trans. Math. Softw. 36(1) (2009)

References

117

31. S. Agarwal, N. Snavely, S.M. Seitz, R. Szeliski, Bundle adjustment in the large, in Computer Vision—ECCV 2010, ed. by K. Daniilidis, P. Maragos, N. Paragios (Springer Berlin Heidelberg, Berlin, Heidelberg, 2010), pp. 29–42 32. M.I.A. Lourakis, A.A. Argyros, SBA: a software package for generic sparse bundle adjustment. ACM Trans. Math. Soft. 36(1), 1–30 (2009) 33. F. Shevlin, Analysis of orientation problems using plucker lines, vol. 1, issue 09 (1998), pp. 685–689 34. J. Zhang, S. Singh, Loam: lidar odometry and mapping in real-time, in Proceedings of Robotics: Science and Systems (RSS ’14) (2014) 35. J. Zhang, S. Singh, Low-drift and real-time lidar odometry and mapping. Auton. Robot. 41(2), 401–416 (2017) 36. Q. Li, S. Chen, C. Wang, X. Li, C. Wen, M. Cheng, J. Li, (Deep real-time lidar odometry, Lo-net, 2020) 37. Queens Maria Thomas, Oliver Wasenmuller, and Didier Stricker (Decoupled lidar odometry, Delio, 2019) 38. J. Jiao, H. Ye, Y. Zhu, M. Liu, Robust odometry and mapping for multi-lidar systems with online extrinsic calibration (2020) 39. S. Golodetz, S.L. Hicks, P. Pérez, S. Izadi, P.H.S. Torr, The semantic paintbrush: interactive 3D mapping and recognition in large outdoor spaces, in Proceedings of the 33rd Annual ACM Conference on Human Factors in Computing Systems, CHI ’15, Association for Computing Machinery, New York, NY, USA, 2015), pp. 3317–3326

Chapter 6

Errors

6.1 Uncertainty, Accuracy and Precision Considering the given measurement, the uncertainty is the interval of the confidence around the measured value. Thus, the measured value is certainly not to lie outside this stated interval. Uncertainties may also be stated along with probability. In this case, the measured value has the stated probability to lie within the confidence interval. A common example is one SD ‘σ ’ for the average of a random variable. According to the σ rule, if you repeat the measurement 68% of new measurements will fall in this interval. Accuracy refers to the agreement between a measurement and the true or correct value. The true or correct value is typically measured in laboratory conditions with known uncertainties. Precision refers to the repeatability of measurement. It is important to mention that it does not require knowing the expected value. Figures 6.1, 6.2, 6.3 and 6.4 show the examples of accuracy and precision for different cases of hitting the target.

6.2 Evaluation Metrics Most straightforward evaluation metric for the ith observation equation is χ 2 [1], and it is given by Eq. (6.1) χi2 = (z i − f i (x i ))  i−1 (z i − f i (x i ))

(6.1)

where z i is the observed quantity, f i (x) is observation function that predicts the value of the observation given the current state estimate and  i is the uncertainty of the observation. It can be seen that χi2 denotes the squared Mahalanobis distance already discussed in the probabilistic approach to SLAM in Sect. 4.3. For the given number of the observation equations, the total cost is the sum of the χi2 errors expressed as (6.2) © The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_6

119

120

6 Errors

Fig. 6.1 High accuracy, high precision. All hits are clustered around the target area

Fig. 6.2 Low accuracy, high precision. Precise results are clustered together but far from the target area

Fig. 6.3 Low accuracy, low precision. The results are scattered out far away from target area

Fig. 6.4 High accuracy, low precision. The results are scattered out far away close to target area

χ2 =

C 

(z i − f i (x))  i−1 (z i − f i (x))

(6.2)

i=1

where there are C observation equations. χ 2 error estimation is a rather limited than a powerful tool for the optimization result quality quantitative measure. It suffers from the fact that it cannot distinguish if we are in local or global optimum. For this reason, an additional error measure MSE [1] is given by Eq. (6.3)

6.2 Evaluation Metrics

121

MSE =

n 2 1  β i − β i∗ n i=1

(6.3)

where β is the vector of observed values of the optimized parameters (some estimate) and β ∗ is the result of the optimization (state vector of the optimal solution). The potential problem appears when we would like to distinguish translation from rotations, thus M S E tanslation and M S Er otation are considered. In that case, different quantities do not mix units. More specific evaluation metrics such as RPE and ATE are discussed in [2]. These metrics assume a sequence of poses from the estimated trajectory P 1 , . . . , P n ∈ S E(3) and from the ground truth trajectory Q 1 , . . . , Q n ∈ S E(3). It is worth mentioning that the uncertainty of the ground truth trajectory is extremely small  = 0, thus it is considered as a reference one. It is assumed that P and Q sequences are time-synchronized and of the same length. The RPE measures the local accuracy of the trajectory over a fixed time interval  as (6.4)  −1  −1  P i P i+ R P E i = Q i−1 Q i+

(6.4)

Furthermore, the RMSE over all translation components is defined as (6.5)   n 1  trans (R P E i )2 R M S E(R P E 1:n , ) =  n i=1

(6.5)

where trans (R P E) is the translation component of the relative pose error R P E i . RPE can help in identifying the drift and outliers. The global consistency is evaluated by comparing the absolute distances between the estimated and the ground truth poses. The first step is to find optimal transformation S that aligns both trajectories, thus they are expressed in the same global reference system. It can be done using point-to-point source-to-target observation Eq. (8.9) discussed in Sect. 8.1.2 or using georeferencing discussed in Sect. 9.7.1. Given the transformation S, the ith ATE is given by (6.6) (6.6) AT E i = Q −1 S P i and it is a simply relative pose similar to that in (9.14). The RMSE over all ATE is given in (6.7)   n 1  trans (AT E i )2 R M S E(AT E 1:n , ) =  n i=1

(6.7)

122

6 Errors

ATE is commonly used for the trajectory accuracy evaluation based on known ground truth data sources obtained typically with the accurate GPS. ATE and RPE are already well-established and widely accepted evaluation metrics in mobile robotics and computer vision domains.

6.3 Mean and Covariance of the Point Cloud The main assumption is that the measurements are affected by noise with normal distribution N (x; μ, ). It is characterized by the density function of the form 6.8  1 1 p (x) = det (2π )− 2 exp − (x − μ)  −1 (x − μ) 2

(6.8)

where μ is the mean vector,  is a covariance matrix and (x − μ)  −1 (x − μ) denotes the squared Mahalanobis distance. The error estimation process calculates μ and , thus this quantitative result provides necessary information concerning the quality of the optimization process. There are several techniques for an error estimation such as covariance of the point cloud, black-box method, error propagation law and closed-form estimate of the covariance. Each of them is described in the following sections. The mean vector μ and the covariance matrix  is common technique, and it is discussed in Sect. 8.1.5 for semantic classification and in Sect. 8.5 for building NDT. It is also crucial for local features ( planarit y, linearit y) extraction in surfels method discussed in Sect. 8.6. The mean vector μ is calculated using Eq. (6.9) and  is calculated using Eq. (6.10) μ=

m 1  g P m k=1 k

1  g g ( P − μ)( P k − μ) m − 1 k=1 k

(6.9)

m

=

(6.10)

g

where P k is kth point expressed in the global reference system. Such approach can efficiently model point cloud as its position and the shape characteristics (see Table 8.5). This method can be used for the error estimation as  given by (6.11) of the landmark x L positioning. The visualization of such covariances is shown in Fig. 6.5. ⎡

⎤ σx2 σx y σx z  = ⎣σ yx σ y2 σ yz ⎦ σzx σzy σz2

(6.11)

6.4 Black-Box Method

123

Fig. 6.5 Covariances  for landmarks x L . Red ellipse—1σ , green ellipse—2σ , blue ellipse—3σ of normal distribution N (x L ; μ, ), red strip line—trajectory and black crosses—landmarks

6.4 Black-Box Method This method estimates the covariance by examining the shape of the Taylor series of the quadratic error function given in Eq. (6.12). 1 r (β + δβ) ≈ r (β) + g  δβ + δβ  Hδβ 2

(6.12)

∂ r where H ≡ ∂β 2 (β) is the Hessian matrix. The error of the optimal least squares ∗ estimate β can be expressed as covariance β ∗ given by (6.13) [3] 2

 β = ∗

1 ∂ 2r (β) 2 ∂β 2

−1 σ2

(6.13)

R with n number of observations where an unbiased estimate s 2 of σ 2 can be s 2 = SS n−k and k number of optimized parameters β. For the case that the error function is not described analytically or it is not smooth, a robust estimate of the curvature can be performed via its sampling around the minimum. Therefore, the estimate of the Hessian can be used for the covariance calculation. The problem with this approach commented in [4] is that the shape of the error function for one particular observation does not contain all the information needed to compute the covariance.

6.5 Error Propagation Law Error propagation law combines uncertain measurements [5] modeled by normal distribution. For the set of x optimized parameters with known covariance  x , the general solution for calculating output covariance assumes the first-order Taylor expansion of error function f . Thus, the output  y is given by error propagation law (6.14)

124

6 Errors

Fig. 6.6 1D case of a non-linear error propagation law for function y = f (x). Input variable X with normal distribution N (x; μx , σx ), output variable Y with calculated normal distribution N (y; μ y , σ y ). Large uncertainty

Fig. 6.7 1D case of a non-linear error propagation law for function y = f (x). Input variable X with normal distribution N (x; μx , σx ), output variable Y with calculated normal distribution N (y; μ y , σ y ). Small uncertainty

f

f

 y = J x J 

(6.14)

f

where J is Jacobian of the error function f . It can be seen in Fig. 6.6, the 1D case of a non-linear error propagation law for function y = f (x). This plot was generated for input variable X with normal distribution N (x; μx , σx ), thus assuming current state of the optimization process as f (x) calculated output variable Y with normal distribution N (y; μ y , σ y ) corresponds to the large uncertainty. The goal of the optimization process is to minimize the objective function, thus gradient equations are supposed to reach zero values close to the optimal solution. Therefore, such desired situation decreases the uncertainty that is shown in Fig. 6.7. For a linear case expressed as y = Ax, the error propagation law is given in Eq. (6.15)  y = A x A

(6.15)

6.6 Closed-Form Estimate of Covariance

125

6.6 Closed-Form Estimate of Covariance Closed-form estimate of covariance for 2D ICP was introduced in [4] and extended to 3D in [6]. Point-to-point and point-to-plane error metrics were taken into consideration for the uncertainty analysis. The method defines x as input parameters and β ∗ as outputof the optimization algorithm A that minimizes an objective function C ri2 , thus β ∗ = A(x) = min SS R(x). Thus, the approximate value SS R(x) = i=1 of the covariance of β ∗ is given by (6.16)  β ≈ ∗

∂ 2 SS R

−1 

∂β 2

∂ 2 SS R ∂β∂ x

β



 x

∂ 2 SS R ∂β∂ x

 

∂ 2 SS R

−1 (6.16)

∂β 2

since A(x) is a function with input x and output β ∗ that can be approximated using first-order Taylor series expansion at a value x = x 0 as (6.17)   ∂A ∂A ∂A A(x) x=x 0 ≈ A(x 0 ) + (x) − (x 0 ) (x − x 0 ) = A(x 0 ) + ∂ x0 ∂ x0 ∂ x0 (6.17)  where ∂∂xA0 = ∂∂ Ax  x=x . The covariance of an algebraic expression of the form 0 Bx + const is equivalent with error propagation law expressed as (6.14). Thus, after applying (6.17) in (6.14), we obtain (6.18) β∗ =



β∗ = 

 A(x)

≈ x=x 0

∂A  ∂A x ∂ x0 ∂ x0

(6.18)

From the implicit function theorem (see Appendix of [4]), we obtain (6.19) −1  2   2 ∂ SS R ∂A ∂ SS R =− ∂ x0 ∂β∂ x ∂β 2

(6.19)

and finally we obtain (6.20) β∗ = 

 A(x)

 ≈−

∂ 2 SS R

−1 

∂β 2

x=x 0







∂ 2 SS R ∂β∂ x

 



x −



∂ 2 SS R ∂β 2

∂ 2 SS R ∂β 2

−1

−1 

∂ 2 SS R ∂β∂ x



(6.20)

is a symmetric matrix From the property ( AB) = B A and due to the (6.20) become (6.16). This approach can lead to estimate the covariance of any 2 SS R i for the ith algorithm that minimizes an objective function. The calculation of ∂ ∂β 2 instance of the observation equation is given as N × N symmetric matrix (6.21), where N is the number of optimized parameters β.

126

6 Errors

⎡ ∂ 2 SS R i

∂ 2 SS R i ∂β 2

Therefore, the final

=

∂ 2 SS R ∂β 2

∂ 2 SS R i 2 ∂β1 ∂β2 1 ⎢ ∂ 2∂β i 2 i ⎢ ∂βSS∂βR ∂ SS2R ⎢ 2 2 1i 2∂β2 i ⎢ ∂ SS R ∂ SS R ⎢ ∂β3 ∂β1 ∂β3 ∂β2 ⎢ 2 i 2 i ⎢ ∂ SS R ∂ SS R ⎢ ∂β4 ∂β1 ∂β4 ∂β2

⎢ ⎢ ⎢ ⎣

.. .

.. .

∂ 2 SS R i ∂β1 ∂β3 ∂ 2 SS R i ∂β2 ∂β3 ∂ 2 SS R i ∂β32 ∂ 2 SS R i ∂β4 ∂β3

.. .

∂ 2 SS R i ∂β1 ∂β4 ∂ 2 SS R i ∂β2 ∂β4 ∂ 2 SS R i ∂β3 ∂β4 ∂ 2 SS R i ∂β42

.. .

∂ 2 SS R i ∂ 2 SS R i ∂ 2 SS R i ∂ 2 SS R i ∂βn ∂β1 ∂βn ∂β2 ∂βn ∂β3 ∂βn ∂β4

is summed up as

C i=1

∂ 2 SS R i ∂β 2

∂ 2 SS R i ∂β1 ∂βn ∂ 2 SS R i ∂β2 ∂βn ∂ 2 SS R i ∂β3 ∂βn ∂ 2 SS R i ∂β4 ∂βn

··· ··· ··· ··· .. . .. .

.. .

∂ 2 SS R i ∂βn2

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(6.21)

where there are C instances

SS R of the observation equations in the optimization system. The calculation of ∂∂ x∂β is given as N × MC matrix (6.22), where N is the number of optimized parameters β, M is the number of input parameters x and C is the number of the observation equations’ instances in the optimization system.

⎡ ∂ 2 SS R

∂ 2 SS R ∂β1 ∂ x21 ⎢ ∂ SS R ∂ 2 SS R ⎢ ∂β ∂ x 1 ∂β ∂ x 1 ⎢ 22 1 22 2 ⎢ ∂ SS R ∂ SS R ⎢ ∂β3 ∂ x11 ∂β3 ∂ x21 ⎢ ∂ 2 SS R ∂ 2 SS R ⎢ ⎢ ∂β4 ∂ x11 ∂β4 ∂ x21

∂ 2 SS R ∂β1 ∂ x31 ∂ 2 SS R ∂β2 ∂ x31 ∂ 2 SS R ∂β3 ∂ x31 ∂ 2 SS R ∂β4 ∂ x31

∂β1 ∂ x11 2

∂ 2 SS R = ∂β∂ x

⎢ ⎢ ⎣

.. .

.. .

.. .

∂ 2 SS R ∂ 2 SS R ∂ 2 SS R ∂βn ∂ x11 ∂βn ∂ x21 ∂βn ∂ x31

··· ··· ··· ··· .. . ···

∂ 2 SS R ∂β1 ∂ xm1 ∂ 2 SS R ∂β2 ∂ xm1 ∂ 2 SS R ∂β3 ∂ xm1 ∂ 2 SS R ∂β4 ∂ xm1

.. .

∂ 2 SS R ∂βn ∂ xm1

··· ··· ··· ··· ··· ··· ··· ··· ··· ··· ··· ···

2

∂ 2 SS R ∂β1 ∂ x1c ∂ 2 SS R ∂β2 ∂ x1c ∂ 2 SS R ∂β3 ∂ x1c ∂ 2 SS R ∂β4 ∂ x1c

.. .

∂ 2 SS R ∂βn ∂ x1c

··· ··· ··· ··· .. . ···

⎤

∂ 2 SS R ∂β1 ∂ xmc ∂ 2 SS R ⎥ ⎥ ∂β2 ∂ xmc ⎥ ∂ 2 SS R ⎥ ∂β3 ∂ xmc ⎥ ⎥ ∂ 2 SS R ⎥ ∂β4 ∂ xmc ⎥

.. .

∂ 2 SS R ∂βn ∂ xmc

(6.22)

⎥ ⎥ ⎦

Estimation of the  x is given as MC × MC matrix (6.23) where M is the number of input parameters x and C is the number of the observation equations’ instances in the optimization system. It is diagonal in case when elements of x are not correlated. ⎡

σx11

⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ x = ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

⎤ σx21

..

⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

. σxm1

..

. σx1c

..

.

(6.23)

σxmc To demonstrate this method, we consider optimization system shown in Figs. 6.8 and 6.9. It is a typical ICP problem. It is composed of some poses and C landmarks. Each landmark is observed from each pose (x, y, z, ω, ϕ, κ) expressed using the Tait– Bryan angle convention, thus the local coordinates of the landmark are (xs , ys , z s ). It means that we have C observations for each pose. Landmark coordinates expressed

6.6 Closed-Form Estimate of Covariance

127

Fig. 6.8 Large uncertainty (at initial state of optimization process). Visualization of the uncertainty  β ∗ . Red ellipse—1σ , green ellipse—2σ , blue ellipse—3σ of normal distribution N (β; μ, ), red strip line—trajectory and black crosses—landmarks. This example correspond to point-to-point source-to-landmark observation equation discussed in Sect. 8.1.3

in global coordinate system are (x L , y L , z L ). Automatic implementation of the

∂ 2 SS R ∂β 2

SS R and ∂∂β∂ for incorporated point-to-point source-to-landmark observation Eq. (8.20) x discussed in Sect. 8.1.3 is given in Listing 6.1. The Python implementation is rather straightforward. In this particular demonstration for each pose, β = (x, y, z, ω, ϕ, κ) and input parameters for single instance of the point-to-point source-to-landmark observation equation are given with x = (xs , ys , z s , x L , y L , z L ). The elements of the covariance  (x L ,y L ,z L ) are calculated with the method described in Sect. 6.3;  (xs ,ys ,zs ) are set manually to simulate small uncertainty of landmark measurement. The covariance  β ∗ for each pose is calculated based on C instances of the point-to-point source2 SS R i is a 6 × 6 matrix given in (6.24), and to-landmark observation equation, thus ∂ ∂β 2 2

∂ 2 SS R ∂β∂ x

is a 6 × 6C matrix given in (6.25). The uncertainties are given as a diagonal 6C × 6C matrix  (xs ,ys ,zs ,x L ,yL ,z L ) in (6.26). ⎡

∂ 2 SS R i ∂β 2

=

∂ 2 SS R i 2 ⎢ ∂ 2∂SSx R i ⎢ ∂ y∂ x ⎢ 2 i ⎢ ∂ SS R ⎢ ∂z∂ x ⎢ ∂ 2 SS R i ⎢ ∂ω∂ x ⎢ 2 i ⎢ ∂ SS R ⎣ ∂ϕ∂ x ∂ 2 SS R i ∂κ∂ x

∂ 2 SS R i ∂ x∂ y ∂ 2 SS R i ∂ y2 ∂ 2 SS R i ∂z∂ y ∂ 2 SS R i ∂ω∂ y ∂ 2 SS R i ∂ϕ∂ y ∂ 2 SS R i ∂κ∂ y

∂ 2 SS R i ∂ x∂z ∂ 2 SS R i ∂ y∂z ∂ 2 SS R i ∂z 2 ∂ 2 SS R i ∂ω∂z ∂ 2 SS R i ∂ϕ∂z ∂ 2 SS R i ∂κ∂z

∂ 2 SS R i ∂ x∂ω ∂ 2 SS R i ∂ y∂ω ∂ 2 SS R i ∂z∂ω ∂ 2 SS R i ∂ω2 ∂ 2 SS R i ∂ϕ∂ω ∂ 2 SS R i ∂κ∂ω

∂ 2 SS R i ∂ x∂ϕ ∂ 2 SS R i ∂ y∂ϕ ∂ 2 SS R i ∂z∂ϕ ∂ 2 SS R i ∂ω∂ϕ ∂ 2 SS R i ∂ϕ 2 ∂ 2 SS R i ∂κ∂ϕ

∂ 2 SS R i ∂ x∂κ ∂ 2 SS R i ∂ y∂κ ∂ 2 SS R i ∂z∂κ ∂ 2 SS R i ∂ω∂κ ∂ 2 SS R i ∂ϕ∂κ ∂ 2 SS R i ∂κ 2

⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

(6.24)

128

6 Errors

⎡

∂ 2 SS R = ∂β∂ x

∂ 2 SS R 1 ⎢ ∂∂2x∂SSxRs ⎢ ⎢ ∂ y∂ xs1 ⎢ ∂ 2 SS R ⎢ ∂z∂ x 1 ⎢ 2 s ⎢ ∂ SS R ⎢ ∂ω∂ xs1 ⎢ ∂ 2 SS R ⎢ xs1 ⎣ ∂ϕ∂ ∂ 2 SS R ∂κ∂ xs1

∂ 2 SS R ∂ x∂ ys1 ∂ 2 SS R ∂ y∂ ys1 ∂ 2 SS R ∂z∂ ys1 ∂ 2 SS R ∂ω∂ ys1 ∂ 2 SS R ∂ϕ∂ ys1 ∂ 2 SS R ∂κ∂ ys1

∂ 2 SS R ∂ x∂z s1 ∂ 2 SS R i ∂ y∂z s1 ∂ 2 SS R i ∂z∂z s1 ∂ 2 SS R ∂ω∂z s1 ∂ 2 SS R ∂ϕ∂z s1 ∂ 2 SS R ∂κ∂z s1

∂ 2 SS R ∂ x∂ x L1 ∂ 2 SS R ∂ y∂ x L1 ∂ 2 SS R ∂z∂ x L1 ∂ 2 SS R ∂ω∂ x L1 ∂ 2 SS R ∂ϕ∂ x L1 ∂ 2 SS R ∂κ∂ x L1

∂ 2 SS R ∂ x∂ y L1 ∂ 2 SS R ∂ y∂ y L1 ∂ 2 SS R ∂z∂ y L1 ∂ 2 SS R ∂ω∂ y L1 ∂ 2 SS R ∂ϕ∂ y L1 ∂ 2 SS R ∂κ∂ y L1

∂ 2 SS R ∂ x∂z 1L ∂ 2 SS R ∂ y∂z 1L ∂ 2 SS R ∂z∂z 1L ∂ 2 SS R ∂ω∂z 1L ∂ 2 SS R ∂ϕ∂z 1L ∂ 2 SS R ∂κ∂z 1L

··· ··· ··· ··· ··· ··· ··· ··· ··· ··· ··· ···

∂ 2 SS R ∂ x∂ xsc ∂ 2 SS R ∂ y∂ xsc ∂ 2 SS R ∂z∂ xsc ∂ 2 SS R ∂ω∂ xsc ∂ 2 SS R ∂ϕ∂ xsc ∂ 2 SS R ∂κ∂ xsc

··· ··· ··· ··· ··· ···

(6.25) ⎤

⎡

σxs1

 x =  (xs ,ys ,zs ,x L ,yL ,z L )

⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ =⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

⎤

∂ 2 SS R ∂ x∂z cL ⎥ ∂ 2 SS R ⎥ ∂ y∂z cL ⎥ ∂ 2 SS R ⎥ ∂z∂z cL ⎥ ⎥ ∂ 2 SS R ⎥ ∂ω∂z cL ⎥ ⎥ ∂ 2 SS R ⎥ ∂ϕ∂z cL ⎦ ∂ 2 SS R ∂κ∂z cL

σ ys1

σzs1

σx L1

σ yL1

σz 1L

..

⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

. σxsc

..

.

(6.26)

σz cL The visualization of the decreased uncertainty  β ∗ of the positioning after some optimization iterations as ellipses is given in Fig. 6.9. 1 2 3 4 5 6

from s y m p y i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13

x_L , y_L , z_L = s y m b o l s ( ’ x_L y_L z_L ’ ) x_s , y_s , z_s = s y m b o l s ( ’ x_s y_s z_s ’ ) px , py , pz = s y m b o l s ( ’ px py pz ’) om , fi , ka = s y m b o l s ( ’ om fi ka ’) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’)

14 15 16 17 18 19 20

p o s i t i o n _ s y m b o l s = [ px , py , pz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] l a n d m a r k _ s y m b o l s = [ x_L , y_L , z_L ] s o u r c e _ p o i n t _ s y m b o l s = [ x_s , y_s , z_s ]

21 22 23

beta_symbols = position_symbols + orientation_symbols x_symbols = source_point_symbols + landmark_symbols

6.6 Closed-Form Estimate of Covariance

129

Fig. 6.9 Small uncertainty (after some optimization iterations). Visualization of the decreased uncertainty  β ∗ after some optimization iterations. Red ellipse—1σ , green ellipse—2σ , blue ellipse—3σ of normal distribution N (β; μ, ), red strip line—trajectory and black crosses—landmarks. This example correspond to point-to-point source-to-landmark observation equation discussed in Sect. 8.1.3

24 25 26 27

28

29

p o i n t _ L a n d m a r k = M a t r i x ([ x_L , y_L , z_L ]) . vec () p o i n t _ s o u r c e = M a t r i x ([ x_s , y_s , z_s , 1]) . vec () t r a n s f o r m e d _ p o i n t _ s o u r c e = ( m a t r i x 4 4 F r o m T a i t B r y a n ( px , py , pz , om , fi , ka ) * p o i n t _ s o u r c e ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ s o u r c e = ( m a t r i x 4 4 F r o m R o d r i g u e s ( px , py , pz , sx , sy , sz ) * p o i n t _ s o u r c e ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ s o u r c e = ( m a t r i x 4 4 F r o m Q u a t e r n i o n ( px , py , pz , q0 , q1 , q2 , q3 ) * p o i n t _ s o u r c e ) [: -1 ,:]

30 31 32

33 34

35

36

t a r g e t _ v a l u e = M a t r i x ([0 ,0 ,0]) . vec () model_function = transformed_point_source point_Landmark delta = target_value - model_function sum = M a t r i x ([ d e l t a [0 ,0]* delta [0 ,0]+ delta [1 ,0]* d e l t a [1 ,0]+ delta [2 ,0]* delta [2 ,0]]) . vec () d 2 s u m _ d b e t a 2 = sum . j a c o b i a n ( b e t a _ s y m b o l s ) . j a c o b i a n ( beta_symbols ) d 2 s u m _ d b e t a d x = sum . j a c o b i a n ( b e t a _ s y m b o l s ) . j a c o b i a n ( x_symbols ) Listing 6.1 Python code for generating ∂ SS2R and ∂β observation Eq. (8.20) discussed in Sect. 8.1.3 2

i

∂ 2 SS R ∂β∂ x

for point to point source to landmark

The second example relates to PGSLAM based on the relative pose (1) observation equation discussed in Sect. 9.5.1. The rotation is represented as the Tait–Bryan angle convention as ω, ϕ, κ. Figures 6.10, 6.11 and 6.12 show the trajectory built from twelve poses indexed from 1 to 12 and consecutive optimization steps assuming adding the loop closure. For each pose, the parameters of normal distribution N (β; μ, ) of β = (x, y, z, ω, ϕ, κ) are computed and visualized as ellipses (red

130

6 Errors

Fig. 6.10 Large uncertainty (at initial state of optimization process). Visualization of the uncertainty  β ∗ . Red ellipse—1σ , green ellipse—2σ , blue ellipse—3σ of normal distribution N (β; μ, ) and red strip line—trajectory. This example corresponds to relative pose (1) observation equation discussed in Sect. 9.5.1

Fig. 6.11 Small uncertainty (after adding loop closure edge). Visualization of the uncertainty  β ∗ . Red ellipse—1σ , green ellipse—2σ , blue ellipse—3σ of normal distribution N (β; μ, ) and red strip line—trajectory. This example corresponds to relative pose (1) observation equation discussed in Sect. 9.5.1

ellipse—1σ , green ellipse—2σ , blue ellipse—3σ ). It can be seen that adding a loop closure edge between the second and eleventh poses drastically decreases the uncer2 2 SS R SS R and ∂∂β∂ before and after loop closure is tainty. The topology of the matrices ∂ ∂β 2 x shown in Figs. 6.13, 6.14, 6.15 and 6.16. In Figs. 6.13 and 6.14, each square corre2 SS R sponds to ∂ ∂β where β = (x, y, z, ω, ϕ, κ) are six parameters of a single pose. In 2 SS R where β = (x, y, z, ω, ϕ, κ) Figs. 6.15 and 6.16, each square corresponds to ∂∂β∂ x are six parameters of a single pose and x = (x, y, z, ω, ϕ, κ) are also six parame2 SS R before and after loop closure is ters of a single pose. Therefore, the size of ∂ ∂β 2 N × N , N = 6 · 12. Before loop closure, the system consists of eleven edges, thus 2 SS R from N × MC = (6 · 12) × (6 · 11) after adding loop closure edge the size of ∂∂β∂ x became N × M(C + 1) = (6 · 12) × (6 · 12), where N is the number of optimized 2

6.6 Closed-Form Estimate of Covariance

131

Fig. 6.12 Small uncertainty (after some optimization iterations). Visualization of the decreased uncertainty  β ∗ after adding loop closure edge and some optimization iterations. Red ellipse— 1σ , green ellipse—2σ , blue ellipse—3σ of normal distribution N (β; μ, ) and red strip line— trajectory. This example corresponds to the relative pose (1) observation equation discussed in Sect. 9.5.1. It can be seen that adding loop closure edge between second and eleventh poses drastically decreases the uncertainty Fig. 6.13 closure

∂ 2 SS R ∂β 2

before loop

132 Fig. 6.14 closure

Fig. 6.15 closure

6 Errors ∂ 2 SS R ∂β 2

after loop

∂ 2 SS R ∂β∂ x

before loop

parameters β, M is the number of input parameters x and C is the number of the observation equations’ instances (number of edges) in the optimization system. Loop 2 SS R ∂ 2 SS R closure determines update of four blocks in ∂ ∂β 2 , and it expands ∂β∂ x by an entire

matrix before loop column block. The topology of 6C × 6C = 6 · 11 × 6 · 11  1−11 x closure is given in (6.27). It is a diagonal matrix that is composed of eleven blocks of relative pose uncertainty σ x i, j , where i is the index of the preceding pose and j is the index of the successive pose in the relative pose (1) observation equation. matrix after loop Consequently, the topology of 6C × 6C = 6 · 12 × 6 · 12  1−12 x

6.6 Closed-Form Estimate of Covariance Fig. 6.16 closure

∂ 2 SS R ∂β∂ x

133

after loop

closure is given in (6.28). It is a diagonal matrix that is composed of twelve blocks where the last block is related to the uncertainty of the loop closure edge. ⎤

⎡

σx 1,2

1−11 =  (x,y,z,ω,ϕ,κ)  1−11 x

⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ =⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

σ y 1,2

σz 1,2

σω1,2

σϕ 1,2

σκ 1,2

..

. σx 11,12

..

.

⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ σκ 11,12 (6.27)

134

6 Errors ⎡

⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢  1−12 =  1−12 x (x,y,z,ω,ϕ,κ) = ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣

σx 1,2

⎤ σ y 1,2

σz 1,2

σω1,2

σϕ 1,2

σκ 1,2

..

⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦

. σx 11,12

..

. σκ 11,12

..

. σκ 2,11

(6.28)

6.7 Global Positioning System Accuracy Assessment GPS is considered as ground truth information for mobile robotics. The goal of this section is to show how to measure the accuracy of this ground truth based on the aligned mobile mapping data. The challenge is to verify the correctness of GPS measures affected by many factors/errors. These errors include [7] • • • • • • •

selective availability (limited clock correction for civilian use), satellite clock error, relativistic effects on satellite clock, receiver clock errors, ionosphere transport delay that varies on Sun’s activity, weather and time of day, troposphere errors that vary on humidity and weather, Sagnac effect on Earth’s rotation, multipath error, where receiver suffers from signal reflection from obstacles.

Looking from a pragmatic point of view, the SLAM approach is used for aligning LiDAR data, where georeferenced constraints guarantee no systematic drift of the result. To address global positioning system accuracy assessment, a novel methodology is proposed in [8]. The idea is to collect as much overlapping mobile mapping data, execute SLAM assuming georeferenced initial trajectories and apply the evaluation metrics from Sect. 6.2, e.g. ATE comparing initial data to aligned one. For this reason, it is considered that the GPS signal is available and the information concerning the positioning accuracy is taken into the consideration. The research was performed to analyze the potential impact on autonomous driving safety. It is composed of six elements: • Mobile mapping system minimal setup. • GPS data processing. • LiDAR data processing.

6.7 Global Positioning System Accuracy Assessment

135

Fig. 6.17 The scheme of GNSS+INS accuracy assessment for autonomous driving safety analysis [8]

• Alignment algorithm. • Accuracy assessment confirmation. • Autonomous driving safety analysis. The scheme of the experimental verification is shown in Fig. 6.17. The goal was to measure the GNSS+INS offsets from aligned trajectories based on SLAM. The minimal setup of the mobile mapping system is at least one 3D LiDAR, GNSS+INS positioning system and odometry. An example of such a mobile mapping system is MoMa (Mobile Mapping) van—TomTom B.V. proprietary technology is shown in Fig. 6.18. It is composed of NovAtel ProPak6/PwrPak7 GNSS receiver, NovAtel VEXXIS GNSS-850 GNSS antennas, ADIS 16488/KVH 1750 Inertial Measurement Unit, DIY odometer, Velodyne Lidar HDL-32E and FLiR Ladybug 5/5+ LD5P. All data is synchronized, and the relative poses of all sensors are obtained from the in-house calibration procedure. The processing of GPS observations in precise positioning is complex and requires professional surveyors since it must be carried out after each static measurement [9]. There are many different sources of free online GPS

Fig. 6.18 MoMa van, a TomTom B.V. mobile mapping proprietary technology providing calibrated data [8]

136

6 Errors

Table 6.1 Qualities verified using SLAM Quality 3D accuracy (m) % 3D diff 1 2 3 4 5 6

0.0–0.15 0.05–0.4 0.2–1.0 0.5–2.0 1.0–5.0 2.0–10.0

52.5 81.7 91.7 96.1 88.6 98.4

% 2D diff

% Altitude diff

84.0 93.7 97.8 99.0 97.8 99.5

65.6 88.3 94.5 97.3 91.6 99.2

data processing (AUSPOS, OPUS, CenterPoint RTX, APPS, MagicGNSS, CSRSPPP, GAPS and SCOUT). LiDAR data processing includes downsampling for equal 3D point distribution and filtering for traffic noise reduction. The remaining point cloud is classified into the basic primitives (point, cylindrical, plane) and assigned semantic labels related to reflectivity. The result is a set of points with high reflectivity, points with low reflectivity, lines with high reflectivity, lines with low reflectivity, planes with high reflectivity and planes with low reflectivity. This segmentation allows matching similar primitives as corresponding landmarks. The alignment algorithm uses LiDAR metrics for the corresponding landmark observation equation discussed in Sect. 8.1.5. Motion model is constrained with the relative pose observation equation discussed in Sect. 9.5.1. Georeferencing is based on observation equation discussed in Sect. 9.7.1. This SLAM system has the ability to align all trajectories without additional drift due to georeferencing. Therefore, it is possible to use evaluation metrics from Sect. 6.2, e.g. ATE given in Eq. (6.7). This confirms the accuracy assessment, thus we are able to justify the accuracy of the GPS. It is reported in [8] that from all trajectories from the United States mobile mapping survey (total length: 1.159.956 km), 710.958 km is class 1 (61.3%), 378.453 is class 2 (32.63%), 49.335 is class 3 (4.25%), 14.132 km is of class 4 (1.22%) 715 km is of class 5 (0.006%) and 59 km is of class 6 (0.005%). To summarize, 98.17% of processed data belongs to classes 1–3, while 1.83% of data does not belong to classes 1–3. The quantitative results confirming the GPS accuracy assessment with SLAM are collected in Table 6.1. For example, 52.5% of class 1 data are confirmed to be within the error range 0–0.15 m in 3D. To conclude, the proposed methodology is capable of performing GPS accuracy assessment using a mobile mapping survey with LiDAR.

References 1. E. Olson, M. Kaess, Evaluating the performance of map optimization algorithms, in RSS Workshop on Good Experimental Methodology in Robotics (2009) 2. J. Sturm, N. Engelhard, F. Endres, W. Burgard, D. Cremers, A benchmark for the evaluation of RGB-D slam systems, in Proceedings of the International Conference on Intelligent Robot Systems (IROS) (2012)

References

137

3. O. Bengtsson, A.-J. Baerveldt, Robot localization based on scan-matching-estimating the covariance matrix for the IDC algorithm. Robot. Auton. Syst. 44(1), 29–40 (2003). Best Papers of the Eurobot ’01 Workshop 4. A. Censi, An accurate closed-form estimate of icp’s covariance, in Proceedings 2007 IEEE International Conference on Robotics and Automation (2007), pp. 3167–3172 5. R. Siegwart, I.R. Nourbakhsh, Introduction to Autonomous Mobile Robots (Bradford Company, USA, 2004) 6. S.M. Prakhya, L. Bingbing, Y. Rui, W. Lin, A closed-form estimate of 3D ICP covariance, in 2015 14th IAPR International Conference on Machine Vision Applications (MVA) (2015), pp. 526–529 7. M. Karaim, M. Elsheikh, A. Noureldin, Gnss error sources, in Multifunctional Operation and Application of GPS, Chap. 4. ed. by R.B. Rustamov, A.M. Hashimov (IntechOpen, Rijeka, 2018) 8. J. Bedkowski, H. Nowak, B. Kubiak, W. Studzinski, M. Janeczek, S. Karas, A. Kopaczewski, P. Makosiej, J. Koszuk, M. Pec, K. Miksa, A novel approach to global positioning system accuracy assessment, verified on lidar alignment of one million kilometers at a continent scale, as a foundation for autonomous driving safety analysis. Sensors 21(17) (2021) 9. I.H. Mohammed, T.N. Ataiwe, H.A. Sharaa, Accuracy assessment of a variety of gps data processing, online services and software. Geomat. Environ. Eng. 15(4), 5–19 (2021) 10. S. Golodetz, S.L. Hicks, P. Pérez, S. Izadi, P.H.S. Torr, The semantic paintbrush: interactive 3d mapping and recognition in large outdoor spaces, in Proceedings of the 33rd Annual ACM Conference on Human Factors in Computing Systems, (CHI ’15, Association for Computing Machinery, New York, NY, USA, 2015), pp. 3317–3326

Part III

Observation Equations

Chapter 7

Camera Metrics

7.1 Pinhole Camera Model 7.1.1 Pinhole Camera Reprojection Error Reprojection error is related with pinhole camera model where scene view is formed by projecting 3D point into the image plane using a perspective transformation (Fig. 7.1). The pinhole camera model was discussed in Sect. 5.4.1. To build observation equations first we transform point P(x g , y g , z g , 1) to camera local coordinate system: ⎡ g⎤ ⎡ l⎤ ⎡ g⎤ x x x

    g⎥ ⎢ ⎢ yl ⎥ ⎢yg⎥ − R t R wc wc wc ⎢ yg ⎥ ⎢ l ⎥ = [R, t]cw ⎢ g ⎥ = (7.1) ⎣z ⎦ ⎣z ⎦ ⎣z ⎦ 0 1 1 1 1 assuming s=

1 zl

(7.2)

we obtain the model function in form the Eq. (7.3). g

g

g

 [R,t]cw ,x g ,y g ,z g ([R, t]cw , xi , yi , z i , cx , c y , f x , f y ) = i

i

i

     l   u s fx 0 x c = + x v 0 s f y yl cy

(7.3) To build the observation equation incorporating the projection of point P(x g , y g , z g , 1) onto image plane the following optimization problem is considered: ming

[R,t]cw ,x ,y g ,z

C   2 g g g u kp , v kp −  [R,t]cw ,x g ,y g ,z g ([R, t]cw , xi , yi , z i , cx , c y , f x , f y ) g i

i

i

i=1

(7.4) © The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_7

141

142

7 Camera Metrics

Fig. 7.1 Reprojection error. O—camera origin, P—point in 3D space, pˆ - intersection of OP with camera plane, p—measured pˆ

where there are C projections [u, v] of points P(xi , yi , z i ) onto image plane. The observation equation for pinhole camera (perspective camera) reprojection error is formulated as (7.5), g

  uδ = vδ    r esiduals

g

g

 u kp g g g −  [[R,t]cw ,xig ,yig ,zig ] ([R, t]cw , xi , yi , z i , cx , c y , f x , f y ) v kp       model f unction



target values

(7.5)  kp  u are residuals, are target values and where v kp g g g  [[R,t]cw ,xig ,yig ,zig ] ([R, t]cw , xi , yi , z i , cx , c y , f x , f y ) is the model function. Assum  uδ vδ

r

ing pose expressed using Tait-Bryan angles (tx , t y , tz , ω, ϕ, κ) the Jacobian J of the objective function for reprojection error is given in (7.6). r



J=

 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9  ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u  δ

δ

δ

δ

δ

δ

δ

δ

δ

∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂ x g ∂ y g ∂z g ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂ x g ∂ y g ∂z g

(7.6)

For pose expressed using Rodrigues (tx , t y , tz , sx , s y , sz ) parameterization of the rotar

tion matrix R the Jacobian J of the objective function for reprojection error is given in (7.7). r

J=



 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9  ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u  δ

δ

δ

δ

δ

δ

δ

δ

δ

∂tx ∂t y ∂tz ∂sx ∂s y ∂sz ∂ x g ∂ y g ∂z g ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂tx ∂t y ∂tz ∂sx ∂s y ∂sz ∂ x g ∂ y g ∂z g

(7.7)

7.1 Pinhole Camera Model

143

For pose using Quaternion (tx , t y , tz , q0 , q1 , q2 , q3 ) parameterization of the rotation r

matrix R the Jacobian J of the objective function for reprojection error is given in (7.8). r

J=



 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 d2,10  ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u  δ

δ

δ

δ

δ

δ

δ

δ

δ

δ

∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3 ∂ x g ∂ y g ∂z g ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3 ∂ x g ∂ y g ∂z g

(7.8)

Python code for generating pinhole camera observation equations and the Jacobian r

J of the objective function is shown in Listing 7.1. 1 2 3 4 5 6

from sympy i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’ ) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14

fx , fy , cx , cy = s y m b o l s ( ’ fx fy cx cy ’) ; tx , ty , tz = s y m b o l s ( ’ tx ty tz ’ ) om , fi , ka = s y m b o l s ( ’ om fi ka ’ ) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’) px , py , pz = s y m b o l s ( ’ px py pz ’ ) ; u_kp , v_kp = s y m b o l s ( ’ u_kp v_kp ’)

15 16 17 18 19 20 21

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] p o i n t _ s y m b o l s = [ px , py , pz ] all_symbols = position_symbols + orientation_symbols + point_symbols

22 23 24 25

26 27 28

Rt_wc = m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) # Rt_wc = m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) # Rt_wc = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) R_cw = Rt_wc [: -1 ,: -1]. t r a n s p o s e () t _ w c = M a t r i x ([ tx , ty , tz ]) . vec () t_cw = - R_cw * t_wc

29 30 31 32 33 34 35 36 37

p o i n t _ g l o b a l = M a t r i x ([ px , py , pz ]) . vec () p o i n t _ l o c a l = R_cw * p o i n t _ g l o b a l + t_cw ; s = 1.0 / p o i n t _ l o c a l [2]; u = s * fx * p o i n t _ l o c a l [0] + cx v = s * fy * p o i n t _ l o c a l [1] + cy u _ d e l t a = u_kp - u ; v _ d e l t a = v_kp - v ; uv = M a t r i x ([ u , v ]) . vec ()

144

7 Camera Metrics

38 39 40 41 42

t a r g e t _ v a l u e = M a t r i x ([ u_kp , v_kp ]) . vec () m o d e l _ f u n c t i o n = M a t r i x ([ u , v ]) . vec () obs_eq = target_value - model_function obs_eq_jacobian = obs_eq . jacobian ( all_symbols ) r

Listing 7.1 Python code for generating pinhole camera observation equation and the Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or Quaternion) rotation matrix parameterization

7.1.2 Intrinsic Calibration of Perspective Camera This section discusses the intrinsic properties of the perspective camera related with distortions. In practice radial and tangential distortions are considered as major impact into image quality. The effect of radial distortion (not straight lines) is shown in Fig. 7.2, it is caused by optics, and it is larger for image corners. Tangential distortion is caused by not perfectly aligned lense to the imaging plane; therefore, it is not parallel. In this case we observe in the image that some object are nearer than suppose to be, it is visualized in Fig. 7.3. Radial distortion is expressed in Eq. 7.9   xdist = x 1 + k1r 2 + k2 r 4 + k3r 6 (7.9)   ydist = y 1 + k1r 2 + k2 r 4 + k3r 6 and tangential distortion is expressed in Eq. 7.10    xdist = x + 2 p1 x y + p2 r 2 + 2x 2     ydist = y + p1 r 2 + 2y 2 + 2 p2 x y

Fig. 7.2 Radial distortion. Red color: ground-truth chess, green color: distorted chess

(7.10)

7.1 Pinhole Camera Model

145

Fig. 7.3 Tangential distortion. Red color: ground-truth chess, green color: distorted chess

where r 2 = u 2 + v 2 . To build observation equation for intrinsic properties of the perspective camera the pinhole camera model discussed in Sect. 5.4.1 and pinhole camera reprojection error discussed in Sect. 7.1.1 is incorporated. It is assumed that the set of ground-truth terrain points (x g , y g , z g ) arranged in chess pattern is given, furthermore more shots of the camera are available from different angles. In such a case Eq. 7.3 can be rewritten as (7.11) g

g

g

 [[R,t]cw ,k1 ,k2 ,k3 , p1 , p2 ] ([R, t]cw , k1 , k2 , k3 , p1 , p2 , xi , yi , z i , cx , c y , f x , f y )        (7.11) f x 0 (sx)ldist c u dist = + x = vdist cy 0 f y (sy)ldist

where β = [R, t]cw , k1 , k2 , k3 , p1 , p2 is vector of parameters being optimized assuming optimization problem defined in Eq. 7.12. min

[R,t]cw ,k1 ,k2 ,k3 , p1 , p2

C   kp kp u dist , vdist −  [[R,t]cw ,k1 ,k2 ,k3 , p1 , p2 ] i=1

2 g

g

g

([R, t]cw , k1 , k2 , k3 , p1 , p2 , xi , yi , z i , cx , c y , f x , f y ) (7.12) It can be seen that in the same time camera pose [R, t]cw and intrinsic parameters k1 , k2 , k3 , p1 , p2 are optimized. The observation equation for intrinsic calibration of the perspective camera is given in Eq. 7.13

146

7 Camera Metrics   uδ vδ   

⎡ kp ⎤ u g g g

⎣ dist ⎦ −  [R,t]cw ,k1 ,k2 ,k3 , p1 , p2 ([R, t]cw , k1 , k2 , k3 , p1 , p2 , xi , yi , z i , cx , c y , f x , f y ) kp vdist       model f unction

=

r esiduals

target values

 u δ vδ

where

are

  kp kp u dist vdist

residuals,

are

(7.13) values and

target

g g g t]cw , k1 , k2 , k3 , p1 , p2 , xi , yi , z i , cx , c y ,

f x , f y ) is the  [[R,t]cw ,k1 ,k2 ,k3 , p1 , p2 ] ([R, model function. For pose expressed using Tait-Bryan angles (tx , t y , tz , ω, ϕ, κ) the r

Jacobian J of the objective function for this observation equation is given in 7.14. 

r

J=

 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 d2,10 d2,11  ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u  δ

δ

δ

δ

δ

δ

δ

δ

δ

δ

δ

∂k1 ∂k2 ∂k3 ∂ p1 ∂ p2 ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂k1 ∂k2 ∂k3 ∂ p1 ∂ p2 ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ

(7.14)

For pose expressed using Rodrigues (tx , t y , tz , sx , s y , sz ) parameterization of the rotar

tion matrix R the Jacobian J of the objective function is given in Eq. 7.15. 

r

J=

 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 d2,10 d2,11  ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u  δ

δ

δ

δ

δ

δ

δ

δ

δ

δ

δ

∂k1 ∂k2 ∂k3 ∂ p1 ∂ p2 ∂tx ∂t y ∂tz ∂sx ∂s y ∂sz ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂k1 ∂k2 ∂k3 ∂ p1 ∂ p2 ∂tx ∂t y ∂tz ∂sx ∂s y ∂sz

(7.15)

For pose using quaternion (tx , t y , tz , q0 , q1 , q2 , q3 ) parameterization of the rotation r

matrix R the Jacobian J of the objective function is given in Eq. 7.16. r

J=



 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 d1,12 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 d2,10 d2,11 d2,12  ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u  δ

δ

δ

δ

δ

δ

δ

δ

δ

δ

δ

δ

∂k1 ∂k2 ∂k3 ∂ p1 ∂ p2 ∂ Tx ∂ Ty ∂ Tz ∂q0 ∂q1 ∂q2 ∂q3 ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂k1 ∂k2 ∂k3 ∂ p1 ∂ p2 ∂ Tx ∂ Ty ∂ Tz ∂q0 ∂q1 ∂q2 ∂q3

(7.16)

Python code for generating intrinsic parameters of pinhole camera observation equar

tions and the Jacobian J of the objective function is shown in Listing 7.2. 1 2 3 4 5 6

from sympy i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9

fx , fy , cx , cy = s y m b o l s ( ’ fx fy cx cy ’ ) ; tx , ty , tz = s y m b o l s ( ’ tx ty tz ’ )

7.1 Pinhole Camera Model 10 11 12 13 14 15

147

om , fi , ka = s y m b o l s ( ’ om fi ka ’ ) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’) pg_x , pg_y , pg_z = s y m b o l s ( ’ pg_x pg_y pg_z ’ ) ; u_kp , v_kp = s y m b o l s ( ’ u_kp v_kp ’ ) k1 , k2 , k3 , p1 , p2 = s y m b o l s ( " k1 k2 k3 p1 p2 " )

16 17 18 19 20 21 22 23

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] p o i n t _ g l o b a l _ s y m b o l s = [ pg_x , pg_y , pg_z ] i n t r i n s i c s _ s y m b o l s = [ k1 , k2 , k3 , p1 , p2 ] all_symbols = intrinsics_symbols + position_symbols + orientation_symbols + point_global_symbols

24 25 26 27 28 29

Rt_cw = m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) # Rt_cw = m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) # Rt_cw = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) R_cw = Rt_cw [: -1 ,: -1] t _ c w = M a t r i x ([ tx , ty , tz ]) . vec ()

30 31 32 33

p o i n t _ g l o b a l = M a t r i x ([ pg_x , pg_y , pg_z ]) . vec () p o i n t _ l o c a l = R_cw * p o i n t _ g l o b a l + t_cw ; s = 1.0 / p o i n t _ l o c a l [2];

34 35 36 37 38

39

40 41 42 43 44

u1 v1 r2 u2

= s * p o i n t _ l o c a l [0] = s * p o i n t _ l o c a l [1] = u1 * u1 + v1 * v1 ; = u1 * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2 ) + 2* p1 * u1 * v1 + p2 *( r2 + 2 * u1 * u1 ) v2 = v1 * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2 ) + p1 *( r2 + 2* v1 * v1 ) + 2*( p2 * u1 * v1 ) ; u3 = fx * u2 + cx v3 = fy * v2 + cy u _ d e l t a = u_kp - u3 ; v _ d e l t a = v_kp - v3 ; uv = M a t r i x ([ u3 , v3 ]) . vec ()

45 46 47 48 49

t a r g e t _ v a l u e = M a t r i x ([ u_kp , v_kp ]) . vec () m o d e l _ f u n c t i o n = M a t r i x ([ u3 , v3 ]) . vec () obs_eq = target_value - model_function obs_eq_jacobian = obs_eq . jacobian ( all_symbols )

Listing 7.2 Python code for generating intrinsic parameters of pinhole camera observation equation r

and the Jacobian J of the objective function for Tait-Bryan rotation matrix parameterization and alternatively for Ridrigues and Quaternion

148

7 Camera Metrics

7.1.3 Calculating Pinhole Camera External Orientation Using Plücker Lines Line l [1, 2] can be represented as a direction d and a point P that passes through this line (Fig. 7.4). Line in Plücker coordinate’s system is defined as a pair (m, d), where m= P×d

(7.17)

is a moment vector. We choose d to be an unit vector, thus d = 1

(7.18)

d·m=0

(7.19)

and

Therefore, six coordinates of the Plücker line in world coordinate system is: ⎤ m wx w ⎢m y ⎥ ⎢ w⎥ ⎢m ⎥ w z ⎥ L =⎢ ⎢ dw ⎥ ⎢ xw ⎥ ⎣ dy ⎦ dzw ⎡

(7.20)

To move Plücker Line via [R, t]wc following 6 × 6 motion matrix can be incorporated:   Rwc [t wc ]× Rwc (7.21) H wc = 0 Rwc where [t wc ]× is skew-symmetric matrix of the vector t:

Fig. 7.4 Plücker line represented as a direction d, moment m and point P that passes through this line

7.1 Pinhole Camera Model

149

Fig. 7.5 BA system for Plücker Lines

⎡

⎤ 0 −tz t y [t wc ]× = ⎣ tz 0 −tx ⎦ −t y tx 0

(7.22)

The inverse transformation useful for transforming Plücker Line L w into camera local coordinate system L c is given:  H cw = therefore

Rwc −Rwc [t wc ]× 0 Rwc

 (7.23)

⎤ m cx ⎢m cy ⎥ ⎢ c⎥ ⎢m ⎥ w z⎥ Lc = ⎢ ⎢ d c ⎥ = H cw L x ⎢ c⎥ ⎣ dy ⎦ dzc ⎡

(7.24)

For back-projection l c of the Plücker Line L c on image plane (Fig. 7.5) we use only the moment vector mc and following projection matrix: ⎡

thus,

⎤ fy 0 0 0 ⎦ fx K =⎣ 0 − f y cx − f x c y f x f y

(7.25)

⎡ ⎤ lx l c = ⎣l y ⎦ = K mc lz

(7.26)

For building observation equations we use two homogeneous x s = (u s , vs )  x  l c endpoints  xe l c s √ √ and x e = (u e , ve ) of the matched line segment z = l 2 +l 2 l 2 +l 2 to the backx

y

x

y

150

7 Camera Metrics

projected line l c on image plane expressed in pixel coordinates. Following condition for the model function has to occur: 

 x l c  xe l c  s  [R,t]wc ([R, t]wc , K , H cw , L w ) = √l 2 +l 2 √l 2 +l 2 = 0 0 x

y

x

y

(7.27)

therefore, the following optimization problem is considered: min

[R,t]wc

C 2  [0, 0] −  [R,t]wc ([R, t]wc , K , H cw , L w )

(7.28)

i=1

and the observation equation for calculating pinhole camera external orientation using Plücker Lines is given in (7.29),  xs,δ = xe,δ    

r esiduals

  0 0 

−  [R,t]wc ([R, t]wc , K , H cw , L w )   

target values

(7.29)

model f unction

   0 xs,δ are residuals, are target values and  [R,t]wc ([R, t]wc , K , xe,δ 0 H cw , L w ) is the model function. Assuming Tait-Bryan angles for pose 

where

r

(tx , t y , tz , ω, ϕ, κ) the Jacobian J of the objective function is given in (7.30). r



J=

d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d2,1 d2,2 d2,3 d2,4 d2,5 d2,6



 ∂x

s,δ ∂ x s,δ ∂ x s,δ ∂ x s,δ ∂ x s,δ ∂ x s,δ ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ

=  (7.30)

For pose (tx , t y , tz , sx , s y , sz ) using Rodrigues parameterization of the rotation matrix r

R the Jacobian J of the objective function is given in (7.31). r

J=



d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d2,1 d2,2 d2,3 d2,4 d2,5 d2,6

 ∂x



s,δ ∂ x s,δ ∂ x s,δ ∂ x s,δ ∂ x s,δ ∂ x s,δ ∂tx ∂t y ∂tz ∂sx ∂s y ∂sz ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂tx ∂t y ∂tz ∂sx ∂s y ∂sz

=  (7.31)

For pose using quaternion (tx , t y , tz , q0 , q1 , q2 , q3 ) parameterization of the rotation r

matrix R the Jacobian J of the objective function is given in (7.32).

7.1 Pinhole Camera Model r

J=

151



d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7

 ∂x



s,δ ∂ x s,δ ∂ x s,δ ∂ x s,δ ∂ x s,δ ∂ x s,δ ∂ x s,δ ∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3 ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂ xe,δ ∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3

=  (7.32) r

Python code for generating observation equations and the Jacobian J of the objective function is shown in Listing 7.3. Listing 7.4 shows the Python code for generating projection matrix K , and motion matrices H cw and H wc . 1 2 3 4 5 6 7

from sympy i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t * from p l u c k e r _ l i n e _ u t i l s i m p o r t *

8 9 10 11 12 13 14

15 16

fx , fy , cx , cy = s y m b o l s ( ’ fx fy cx cy ’ ) ; tx , ty , tz = s y m b o l s ( ’ tx ty tz ’ ) om , fi , ka = s y m b o l s ( ’ om fi ka ’ ) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’) mx_w , my_w , mz_w , lx_w , ly_w , lz_w = s y m b o l s ( ’ mx_w my_w mz_w lx_w ly_w lz_w ’ ) u_s , v_s = s y m b o l s ( ’ u_s v_s ’ ) u_e , v_e = s y m b o l s ( ’ u_e v_e ’ )

17 18 19 20 21 22 23

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] p l u c k e r _ l i n e _ s y m b o l s = [ mx_w , my_w , mz_w ] all_symbols = position_symbols + orientation_symbols + plucker_line_symbols

24 25 26 27 28 29 30 31 32 33

Rt_wc = m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) # Rt_wc = m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) # Rt_wc = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) K = p l u c k e r _ l i n e _ K ( fx , fy , cx , cy ) mm_cw = p l u c k e r _ l i n e _ m o t i o n _ m a t r i x _ c w ( Rt_wc ) l_w = M a t r i x ([[ m x _ w ] ,[ my_w ] ,[ mz_w ] ,[ lx_w ] ,[ ly_w ] ,[ lz_w ]]) l_c = mm_cw * l_w m_c = M a t r i x ([[ l_c [0 ,0]] ,[ l_c [1 ,0]] ,[ l_c [2 ,0]]]) l = K * m_c

34 35 36

x_s = M a t r i x ([[ u_s , v_s , 1 ] ] ) x_e = M a t r i x ([[ u_e , v_e , 1 ] ] )

37 38

t a r g e t _ v a l u e = M a t r i x ([0 ,0]) . vec ()

152 39

40 41

7 Camera Metrics

m o d e l _ f u n c t i o n = M a t r i x ([( x_s * l ) / sqrt ( l [0 ,0]* l [0 ,0] + l [1 ,0]* l [1 ,0]) ,( x_e * l ) / sqrt ( l [0 ,0]* l [0 ,0] + l [1 ,0]* l [1 ,0]) ]) . vec () obs_eq = target_value - model_function obs_eq_jacobian = obs_eq . jacobian ( all_symbols ) r

Listing 7.3 Python code for generating observation equations and the Jacobian J of the objective function for calculating camera external orientation based on Plücker Line assuming Tait-Bryan (alternatively Rodrigues or Quaternion) rotation matrix parametrization 1

from sympy i m p o r t *

2 3 4

def p l u c k e r _ l i n e _ K ( fx , fy , cx , cy ) : r e t u r n M a t r i x ([[ fy ,0 ,0] ,[0 , fx ,0] ,[ - fy * cx , - fx * cy , fx * fy ]])

5 6 7 8 9 10 11 12 13 14 15

16

17

18 19 20

def p l u c k e r _ l i n e _ m o t i o n _ m a t r i x _ c w ( Rt_wc ) : a = Rt_wc [0 ,3] b = Rt_wc [1 ,3] c = Rt_wc [2 ,3] rt = M a t r i x ([[ R t _ w c [0 ,0] , R t _ w c [1 ,0] , R t _ w c [2 ,0]] , [ Rt_wc [0 ,1] , Rt_wc [1 ,1] , Rt_wc [2 ,1]] , [ Rt_wc [0 ,2] , Rt_wc [1 ,2] , Rt_wc [2 ,2]]]) tx = M a t r i x ([[0 , - c , b ] ,[ c ,0 , - a ] ,[ - b ,a ,0]]) rttx = rt * tx r e t u r n Matrix ([[ rt [0 ,0] , rt [0 ,1] , rt [0 ,2] , - rttx [0 ,0] , rttx [0 ,1] , - rttx [0 ,2]] , [ rt [1 ,0] , rt [1 ,1] , rt [1 ,2] , - rttx [1 ,0] , rttx [1 ,1] , - rttx [1 ,2]] , [ rt [2 ,0] , rt [2 ,1] , rt [2 ,2] , - rttx [2 ,0] , rttx [2 ,1] , - rttx [2 ,2]] , [0 ,0 ,0 , rt [0 ,0] , rt [0 ,1] , rt [0 ,2]] , [0 ,0 ,0 , rt [1 ,0] , rt [1 ,1] , rt [1 ,2]] , [0 ,0 ,0 , rt [2 ,0] , rt [2 ,1] , rt [2 ,2]]])

21 22 23 24 25 26 27 28 29 30 31

32

33

34 35 36

def p l u c k e r _ l i n e _ m o t i o n _ m a t r i x _ w c ( Rt_wc ) : a = Rt_wc [0 ,3] b = Rt_wc [1 ,3] c = Rt_wc [2 ,3] r = M a t r i x ([[ R t _ w c [0 ,0] , R t _ w c [0 ,1] , R t _ w c [0 ,2]] , [ Rt_wc [1 ,0] , Rt_wc [1 ,1] , Rt_wc [1 ,2]] , [ Rt_wc [2 ,0] , Rt_wc [2 ,1] , Rt_wc [2 ,2]]]) tx = M a t r i x ([[0 , - c , b ] ,[ c ,0 , - a ] ,[ - b ,a ,0]]) txr = tx * r r e t u r n M a t r i x ([[ r [0 ,0] , r [0 ,1] , r [0 ,2] , txr [0 ,0] , txr [0 ,1] , txr [0 ,2]] , [ r [1 ,0] , r [1 ,1] , r [1 ,2] , txr [1 ,0] , txr [1 ,1] , txr [1 ,2]] , [ r [2 ,0] , r [2 ,1] , r [2 ,2] , txr [2 ,0] , txr [2 ,1] , txr [2 ,2]] , [0 ,0 ,0 , r [0 ,0] , r [0 ,1] , r [0 ,2]] , [0 ,0 ,0 , r [1 ,0] , r [1 ,1] , r [1 ,2]] , [0 ,0 ,0 , r [2 ,0] , r [2 ,1] , r [2 ,2]]])

Listing 7.4 Python code for generating Plücker Line projection matrix and motion matrices

7.2 Metric Camera Model

153

7.2 Metric Camera Model 7.2.1 Colinearity Observation Equation In this section a central projection in three dimensional space is discussed. There is an analogy to the approach discussed in Sect. 7.1.1. The difference is related with the application of cameras used in photogrammetry, where produced photographs can be considered with adequate accuracy, thus three-dimensional object is projected onto image plane. We start from Fig. 7.6 where O XY Z is world coordinate system. Lets t(tx , t y , tz ) is a center perspective and in the same time camera position in world coordinates. P P is the principal point with coordinates (ξ0 , η0 ) and c is a principal distance, M is a fiducial center (related in photogrammetry with the point of intersection of the straight lines joining the fiducial marks). P(x g , y g , z g ) is a ground point, therefore image coordinates of this point are denoted as (ξ, η) and x, y, z as point on image plane expressed in global coordinate system. The main assumption of

Fig. 7.6 Relation between image and objects’ coordinates. O XY Z is world coordinate system. t(tx , t y , tz ) is a center perspective and in the same time camera position in world coordinates. P P is the principal point with coordinates (ξ0 , η0 ) and c is a principal distance, M is a fiducial center (related in photogrammetry with the point of intersection of the straight lines joining the fiducial marks). Colinearity condition equation assumes that the center perspective t(tx , t y , tz ), ground point P(x g , y g , z g ) and image coordinates ξ, η (incorporating respectively ξ0 , η0 , image coordinates of the principal point P P) lies on the straight line

154

7 Camera Metrics

the metric camera colinearity condition equation assumes that the center perspective t(tx , t y , tz ), ground point P(x g , y g , z g ) and image coordinates ξ, η (incorporating respectively ξ0 , η0 , image coordinates of the principal point P P) lies on the straight line. Using world←camera [R, t]wc matrix we can easily describe the metric camera coordinate system as the certain pose expressed using Tait-Bryan angle representation as (tx , t y , tz , ω, ϕ, κ) in global reference system. Note that ω is rotation around X axis, ϕ is rotation around Y axis, and κ is rotation via Z axis respectively that was discussed in Sect. 3.2. Lets assume the O x  y  z  camera coordinate system that x  y  plane is parallel to X Y and xa , ya , z a are the coordinates of ground point (x g , y g , z g ) projected onto image plane. Within such assumption we can build Eq. (7.33) xa ya z a = = x g − tx y g − ty z g − tz where [t]wc thus, xa ya

 =  =

z a =



(7.33)

⎡ ⎤ tx = ⎣t y ⎦ tz

x g − tx z g − tz y g − ty z g − tz z g − tz z g − tz

  

(7.34)

z a

(7.35)

z a

(7.36)

z a

(7.37)

Having xa , ya , z a we can rotate this point via [R]wc matrix where:

[R]wc

⎡ ⎤ r11 r12 r13 = ⎣r21 r22 r23 ⎦ r31 r32 r33

(7.38)

thus, ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ ⎡ ⎤ xa r11 r12 r13 xa r11 xa + r12 ya + r13 z a xa ⎣ ya ⎦ = [R]wc ⎣ ya ⎦ = ⎣r21 r22 r23 ⎦ ⎣ ya ⎦ = ⎣r21 xa + r22 ya + r23 z a ⎦ (7.39) za z a r31 r32 r33 z a r31 xa + r32 ya + r33 z a finally we obtain:  xa = r11

x g − tx z g − tz



z a + r12



y g − ty z g − tz



z a + r13



z g − tz z g − tz



z a

(7.40)

7.2 Metric Camera Model

 ya = r21  z a = r31

x g − tx z g − tz x g − tx z g − tz

155

 

z a + r22 z a

 

+ r32

y g − ty z g − tz y g − ty z g − tz

 

z a + r23 z a

 

+ r33

z g − tz z g − tz z g − tz z g − tz

 

z a

(7.41)

z a

(7.42)

Using factoring term z a /(z g − tz ) we obtain       xa (z g − tz ) = r11 x g − tx + r12 y g − t y + r13 z g − tz  za

(7.43)

 g     g  ya (z g − tz ) x + r y + r z = r − t − t − t 21 x 22 g y 23 z z a

(7.44)

      z a (z g − tz ) = r31 x g − tx + r32 y g − t y + r33 z g − tz z a

(7.45)

by dividing 7.43 and 7.44 by 7.45 we obtain:   r11 (x g − tx ) + r12 y g − t y + r13 (z g − tz ) xa   = za r31 (x g − tx ) + r32 y g − t y + r33 (z g − tz )

(7.46)

  r21 (x g − tx ) + r22 y g − t y + r23 (z g − tz ) ya   = za r31 (x g − tx ) + r32 y g − t y + r33 (z g − tz )

(7.47)

Due to a principal distance c = z a we finally obtain   r11 (x g − tx ) + r12 y g − t y + r13 (z g − tz )   ξ = ξ0 − x a = ξ0 − c r31 (x g − tx ) + r32 y g − t y + r33 (z g − tz )

(7.48)

  r21 (z g − tx ) + r22 y g − t y + r23 (x g − tz )   η = η0 − ya = η0 − c r31 (x g − tx ) + r32 yg − t y + r33 (z g − tz )

(7.49)

The metric camera colinearity observation equation is given as (7.50),   ξδ ηδ    r esiduals

=

 kp    ξ ξ − = η ηkp

 kp  ξ ηkp   

target values

−  [[R,t]wc ,x g ,y g ,z g ] ([R, t]wc , x g , y g , z g , ξ0 , η0 , c)    model f unction

(7.50)  kp    ξ ξδ are residuals, kp are target values and  [[R,t]wc ,x g ,y g ,z g ] ([R, t]wc , where ηδ η g g g x , y , z , ξ0 , η0 , c) is the model function. For this colinearity observation the following optimization problem can be formulated:

156

7 Camera Metrics C

ming

[R,t]wc ,x

,y g ,z g

ξ kp , ηkp



−  [[R,t]wc ,x g ,y g ,z g ] ([R, t]wc , x g , y g , z g , ξ0 , η0 , c)

2

i=1

(7.51) where there are C correspondences between ground point and its metric camera coordinates. Assuming pose expressed using Tait-Bryan angles (tx , t y , tz , ω, ϕ, κ) r

the Jacobian J of the objective function for the colinearity observation is given in (7.52). 

r

J=

 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9  ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ  δ

δ

δ

δ

δ

δ

δ

δ

δ

∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂ x g ∂ y g ∂z g ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂ x g ∂ y g ∂z g

(7.52)

For pose (tx , t y , tz , sx , s y , sz ) using Rodrigues parameterization of the rotation matrix r

R the Jacobian J of the objective function of the colinearity observation is given by (7.53).   r d d d d d d d d d J = 1,1 1,2 1,3 1,4 1,5 1,6 1,7 1,8 1,9 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9  ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ  (7.53) δ

δ

δ

δ

δ

δ

δ

δ

δ

∂tx ∂t y ∂tz ∂sx ∂s y ∂sz ∂ x g ∂ y g ∂z g ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂tx ∂t y ∂tz ∂sx ∂s y ∂sz ∂ x g ∂ y g ∂z g

For pose (tx , t y , tz , q0 , q1 , q2 , q3 ) using quaternion parameterization of the rotation r

matrix R the Jacobian J of the objective function of the colinearity observation is given with (7.54). r

J=



 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 d2,10  ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ  δ

δ

δ

δ

δ

δ

δ

δ

δ

δ

∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3 ∂ x g ∂ y g ∂z g ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3 ∂ x g ∂ y g ∂z g

(7.54)

Python code for generating metric camera observation colinearity equations and the r

Jacobian J of the objective function is shown in Listing 7.5. 1 2 3 4 5 6

from sympy i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’ ) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11

ksi_0 , eta_0 , c = s y m b o l s ( ’ ksi_0 eta_0 c ’ ) ; tx , ty , tz = s y m b o l s ( ’ tx ty tz ’ ) om , fi , ka = s y m b o l s ( ’ om fi ka ’ ) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’)

7.2 Metric Camera Model 12 13 14

157

# q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’) px , py , pz = s y m b o l s ( ’ px py pz ’ ) ; ksi_kp , e t a _ k p = s y m b o l s ( ’ k s i _ k p e t a _ k p ’)

15 16 17 18 19 20 21

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] p o i n t _ s y m b o l s = [ px , py , pz ] all_symbols = position_symbols + orientation_symbols + point_symbols

22 23 24 25

26 27

Rt_wc = m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) # Rt_wc = m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) # Rt_wc = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) r = Rt_wc [: -1 ,: -1] t = M a t r i x ([ tx , ty , tz ]) . vec ()

28 29

30

31

32 33 34

denom = r [0 ,2]*( px - t [0]) + r [1 ,2]*( py - t [1]) + r [2 ,2]*( pz - t [2]) ksi = ksi_0 - c * ( r [0 ,0]*( px - t [0]) + r [1 ,0]*( py - t [1]) + r [2 ,0]*( pz - t [2]) ) / denom eta = eta_0 - c * ( r [0 ,1]*( px - t [0]) + r [1 ,1]*( py - t [1]) + r [2 ,1]*( pz - t [2]) ) / denom k s i _ d e l t a = k s i _ k p - ksi ; e t a _ d e l t a = e t a _ k p - eta ; obs = M a t r i x ([ ksi , eta ]) . vec ()

35 36 37

t a r g e t _ v a l u e = M a t r i x ([ ksi_kp , e t a _ k p ]) . vec () m o d e l _ f u n c t i o n = M a t r i x ([ ksi , eta ]) . vec ()

38 39 40

obs_eq = target_value - model_function obs_eq_jacobian = obs_eq . jacobian ( all_symbols )

Listing 7.5 Python code for generating metric camera colinearity observation equation and the r

Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or Quaternion) rotation matrix parametrization

7.2.2 Coplanarity Observation Equation Having system of teo cameras shown in Fig. 7.7 composed of two poses ([R, t]1wc , [R, t]2wc ), ground point P(x g , y g , z g ), intersections with cameras’ planes p1 , p2 and the vector of the displacement between cameras b the coplanarity condition is defined as (7.55).   p1 b × p2 = 0 The center of the metric camera is described by

(7.55)

158

7 Camera Metrics

Fig. 7.7 The system of two cameras composed of poses ([R, t]1wc , [R, t]2wc ), ground point P(x g , y g , z g ), intersections with cameras’ planes p1 , p2 and the vector of the displacement between cameras b. The coplanarity  condition is p1 b × p2 = 0

[t]wc

⎡ ⎤ tx = ⎣t y ⎦ tz

(7.56)

in global reference system. Point P(x g , y g , z g ) is observed from two views. Therefore, if we derive p1 , p2 , b from (7.57) ⎡

x g − tx1

⎤

⎡

x g − tx2

⎤

⎡

tx2 − tx1

⎤

⎥ ⎥ ⎢ ⎥ ⎢ ⎢ p1 = ⎣ y g − t y1 ⎦ , p2 = ⎣ y g − t y2 ⎦ , b = ⎣t y2 − t y1 ⎦ z − g

z −

tz1

g

tz2

tz2

−

(7.57)

tz1

and if p1 , p2 , b satisfies (7.55) two camera poses ([R, t]1wc , [R, t]2wc ) are coplanar. Equation (7.55) can be expressed as: 

p1 [b]× p2 = 0

(7.58)

where [b]× is skew-symmetric matrix: ⎡

⎤ 0 −bz by [b]× = ⎣ bz 0 −bx ⎦ −by bx 0

(7.59)

To build metric camera coplanarity observation we have to relate vectors p1 , p2 with central projection of ground point P(x g , y g , z g ) to image planes as image coordinates ξ1 , η1 , ξ2 , η2 related respectively with the intersections p1 , p2 , therefore ⎡

⎤ ⎡ ⎤⎡ ⎤ ξ1 − ξ0,1 1 0 −ξ0,1 ξ1 p1 = R1 ⎣η1 − η0,1 ⎦ = R1 ⎣0 1 −η0,1 ⎦ ⎣η1 ⎦ = R1 C 1 (ξ1 , η1 , 1) 0 − c1 0 0 −c1 1

(7.60)

7.2 Metric Camera Model

159

⎡

⎤ ⎡ ⎤⎡ ⎤ ξ2 − ξ0,2 1 0 −ξ0,2 ξ2 p2 = R2 ⎣η2 − η0,2 ⎦ = R2 ⎣0 1 −η0,2 ⎦ ⎣η2 ⎦ = R2 C 2 (ξ2 , η2 , 1) 0 − c2 0 0 −c2 1

(7.61)

Finally, the coplanarity condition is given in (7.62) kp

kp

kp

kp

 [[R,t]1wc ,[R,t]2wc ] ([R, t]1wc , [R, t]2wc , ξ1 , η1 , ξ2 , η2 , ξ0,1 , η0,1 , c1 , ξ0,2 , η0,2 , c2 ) = ⎛ kp ⎞ ξ2

 kp kp   ξ1 , η1 , 1 C 1 R1 [b]× R2 C 2 ⎝η2kp ⎠ = 0 1 (7.62) therefore, metric camera coplanarity observation equation is given in (7.63), δ 

=

r esidual

− 

0  target value



 ([R, t]1 , [R, t]2 , ξ kp , ηkp , ξ kp , ηkp , ξ , η , c , ξ , η , c ) 0,1 0,1 1 0,2 0,2 2 wc wc 1 1 2 2 [R,t]1wc ,[R,t]2wc





model f unction

(7.63) kp kp where δ is residual, 0 is target value and  [[R,t]1wc ,[R,t]2wc ] ([R, t]1wc , [R, t]2wc , ξ1 , η1 , kp

kp

ξ2 , η2 , ξ0,1 , η0,1 , c1 , ξ0,2 , η0,2 , c2 ) is the model function. It incorporates the desired kp kp kp kp projections (ξ1 , η1 , ξ2 , η2 ) of the terrain point P(x g , y g , z g ) onto two images 1 and two poses ([R, t]wc , [R, t]2wc ), thus the following optimization problem is considered: min

C

[R,t]1wc ,[R,t]2wc i=1



2 0 − 

 ([R, t]1 , [R, t]2 , ξ kp , ηkp , ξ kp , ηkp , ξ , η , c , ξ , η , c ) 0,1 0,1 1 0,2 0,2 2 wc wc 1 1 2 2 [R,t]1wc ,[R,t]2wc

(7.64) r

Assuming Tait-Bryan angles (ω, ϕ, κ) the Jacobian J of the objective function for two poses (tx1 , t y1 , tz1 , ω1 , ϕ 1 , κ 1 ) and (tx2 , t y2 , tz2 , ω2 , ϕ 2 , κ 2 ) of the coplanarity observation equation is given: r   J = d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 d1,12 =

 ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂tx1 ∂t y1 ∂tz1 ∂ω1 ∂ϕ 1 ∂κ 1 ∂tx2 ∂t y2 ∂tz2 ∂ω2 ∂ϕ 2 ∂κ 2

(7.65) r

For Rodrigues (sx , s y , sz ) rotation matrix R parameterization the Jacobian J of the objective function for two poses (tx1 , t y1 , tz1 , sx1 , s y1 , sz1 ) and (tx2 , t y2 , tz2 , sx2 , s y2 , sz2 ) of coplanarity observation equation is given: r   J = d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 d1,12 =

 ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂tx1 ∂t y1 ∂tz1 ∂sx1 ∂s y1 ∂sz1 ∂tx2 ∂t y2 ∂tz2 ∂sx2 ∂s y2 ∂sz2

(7.66)

160

7 Camera Metrics

For quaternion (q0 , q1 , q2 , q3 ) rotation matrix R parameterization the Jacor

bian J of the objective function for two poses (tx1 , t y1 , tz1 , q01 , q11 , q21 , q31 ) and (tx2 , t y2 , tz2 , q02 , q12 , q22 , q32 ) of coplanarity observation equation is given: r



d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 d1,12 d1,13

J=  d1,14 = 

∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂tx1 ∂t y1 ∂tz1 ∂q01 ∂q11 ∂q21 ∂q31 ∂tx2 ∂t y2 ∂tz2 ∂q02 ∂q12 ∂q22 ∂q32

(7.67)

Python code for generating metric camera coplanarity observation equations and r

the Jacobian J of the objective function is shown in Listing 7.6. 1 2 3 4 5 6

from sympy i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14 15 16

tx_1 , ty_1 , tz_1 = s y m b o l s ( ’ tx_1 ty_1 tz_1 ’ ) om_1 , fi_1 , ka_1 = s y m b o l s ( ’ om_1 fi_1 ka_1 ’ ) # sx_1 , sy_1 , sz_1 = s y m b o l s ( ’ sx_1 sy_1 sz_1 ’) # q0_1 , q1_1 , q2_1 , q3_1 = s y m b o l s ( ’ q0_1 q1_1 q2_1 q3_1 ’) tx_2 , ty_2 , tz_2 = s y m b o l s ( ’ tx_2 ty_2 tz_2 ’ ) om_2 , fi_2 , ka_2 = s y m b o l s ( ’ om_2 fi_2 ka_2 ’ ) # sx_2 , sy_2 , sz_2 = s y m b o l s ( ’ sx_2 sy_2 sz_2 ’) # q0_2 , q1_2 , q2_2 , q3_2 = s y m b o l s ( ’ q0_2 q1_2 q2_2 q3_2 ’) ksi_1 , eta_1 , ksi_2 , eta_2 , ksi_01 , eta_01 , ksi_02 , eta_02 , c_1 , c_2 = s y m b o l s ( ’ k s i _ 1 e t a _ 1 k s i _ 2 e t a _ 2 k s i _ 0 1 e t a _ 0 1 k s i _ 0 2 e t a _ 0 2 c_1 c_2 ’ ) ;

17 18 19 20 21 22 23 24 25 26 27

p o s i t i o n _ s y m b o l s _ 1 = [ tx_1 , ty_1 , tz_1 ] o r i e n t a t i o n _ s y m b o l s _ 1 = [ om_1 , fi_1 , ka_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ sx_1 , sy_1 , sz_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ q0_1 , q1_1 , q2_1 , q3_1 ] p o s i t i o n _ s y m b o l s _ 2 = [ tx_2 , ty_2 , tz_2 ] o r i e n t a t i o n _ s y m b o l s _ 2 = [ om_2 , fi_2 , ka_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ sx_2 , sy_2 , sz_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ q0_2 , q1_2 , q2_2 , q3_2 ] c _ s y m b o l s = [ c_1 , c_2 ] all_symbols = position_symbols_1 + orientation_symbols_1 + position_symbols_2 + orientation_symbols_2

28 29 30 31 32 33

bx = tx_2 - tx_1 by = ty_2 - ty_1 bz = tz_2 - tz_1 b = M a t r i x ([[0 , - bz , by ] , [ bz , 0 , - bx ] , [ - by , bx , 0]]) C _ 1 t = M a t r i x ([[1 , 0 , - k s i _ 0 1 ] , [0 , 1 , - e t a _ 0 1 ] , [0 , 0 , - c_1 ]]) . t r a n s p o s e ()

7.3 Equirectangular Camera Model 34

161

C_2 = M a t r i x ([[1 , 0 , - k s i _ 0 2 ] , [0 , 1 , - e t a _ 0 2 ] , [0 , 0 , - c_2 ]])

35 36

37

38

39 40

41

42

43 44 45

c a m e r a _ m a t r i x _ 1 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_1 , ty_1 , tz_1 , om_1 , fi_1 , ka_1 ) # c a m e r a _ m a t r i x _ 1 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_1 , ty_1 , tz_1 , sx_1 , sy_1 , sz_1 ) # c a m e r a _ m a t r i x _ 1 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_1 , ty_1 , tz_1 , q0_1 , q1_1 , q2_1 , q3_1 ) R_1t = c a m e r a _ m a t r i x _ 1 [: -1 ,: -1]. t r a n s p o s e () c a m e r a _ m a t r i x _ 2 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_2 , ty_2 , tz_2 , om_2 , fi_2 , ka_2 ) # c a m e r a _ m a t r i x _ 2 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_2 , ty_2 , tz_2 , sx_2 , sy_2 , sz_2 ) # c a m e r a _ m a t r i x _ 2 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_2 , ty_2 , tz_2 , q0_2 , q1_2 , q2_2 , q3_2 ) R_2 = c a m e r a _ m a t r i x _ 2 [: -1 ,: -1] k s i e t a _ 1 = M a t r i x ([[ ksi_1 , eta_1 , 1]]) k s i e t a _ 2 t = M a t r i x ([[ ksi_2 , eta_2 , 1]]) . t r a n s p o s e ()

46 47 48

49 50

target_value = Matrix ([[0]]) m o d e l _ f u n c t i o n = k s i e t a _ 1 * C_1t * R_1t * b * R_2 * C_2 * ksieta_2t obs_eq = target_value - model_function obs_eq_jacobian = obs_eq . jacobian ( all_symbols )

Listing 7.6 Python code for generating metric camera coplanarity observation equation and the r

Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or Quaternion) rotation matrix parameterization

7.3 Equirectangular Camera Model 7.3.1 Colinearity Observation Equation To build equirectangular camera colinearity observations first we use the camera← world [R, t]cw matrix that transforms 3D point P g (x g , y g , z g , 1) in global reference system to point P l (x l , y l , z l ) in local camera reference system. The idea of transforming spherical coordinates into equirectangular image is shown in Figs. 7.8, 7.9.



−1 (7.68) P l = R, t cw P g = R, t wc P g Secondly, we move point P l (x l , y l , z l ) to the sphere with radius equals 1 ( P lr =0 ). ⎡

plx

⎤

⎢ ⎥ P l = ⎣ ply ⎦ pzl

(7.69)

162

7 Camera Metrics

Fig. 7.8 Transforming spherical coordinates into equirectangular image in photogrammetry

Fig. 7.9 Transforming spherical coordinates into equirectangular image in computer vision

 l P  =

plx 2 + ply 2 + pzl 2 ⎡

P lr =0 =

(7.70)

⎤

plx l ⎢  Pl  ⎥ ⎢ py ⎥ ⎢  Pl  ⎥ ⎣ l ⎦ pz  Pl 

(7.71)

Having image sizes [cols, rows], the coordinates on the image sphere can be derived from: ⎤ ⎡ pl −asin(  Pyl  )     ⎥ ⎢ latitude lat plx ⎥ = (7.72) =⎢ ⎣  Pl  ⎦ longitude lon atan( plz )  Pl      ) cols(0.5 + lon u 2π  [[R,t]wc ,x g ,y g ,z g ] ([R, t]wc , x , y , z ) = = v r ows(0.5 − lat ) π g

g

g

(7.73)

7.3 Equirectangular Camera Model

163

To build the observation equation incorporating the projection of point P g (x g , y g , z g ) onto image sphere (r = 1) the following optimization problem is considered: C 2 kp kp  min u ,v −  [[R,t]wc ,x g ,y g ,z g ] ([R, t]wc , x g , y g , z g )

[R,t]wc

(7.74)

i=1

where there are C projections [u, v] of points (xi , yi , z i ) onto image sphere. Thus, equirectangular camera colinearity observation equation is given in (7.75), g

  uδ = vδ    r esiduals

g

g

 u kp −  [[R,t]wc ,x g ,y g ,z g ] ([R, t]wc , x g , y g , z g ) v kp       model f unction



(7.75)

target values

   kp  uδ u are residuals, are target values and vδ v kp  [[R,t]wc ,x g ,y g ,z g ] ([R, t]wc , x g , y g , z g ) is the model function. Assuming pose where

r

expressed using Tait-Bryan angles (tx , t y , tz , ω, ϕ, κ) the Jacobian J of the objective function is given in (7.76). 

r

J=

 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9  ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u  δ

δ

δ

δ

δ

δ

δ

δ

δ

∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂ x ∂ y ∂z ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂ x ∂ y ∂z

(7.76)

For pose (tx , t y , tz , sx , s y , sz ) with Rodrigues rotation matrix R parameterization the r

Jacobian J of the objective function is given in (7.77). 

r

J=

 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9  ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u  δ

δ

δ

δ

δ

δ

δ

δ

δ

∂tx ∂t y ∂tz ∂sx ∂s y ∂sz ∂ x ∂ y ∂z ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂tx ∂t y ∂tz ∂sx ∂s y ∂sz ∂ x ∂ y ∂z

(7.77)

For pose (tx , t y , tz , q0 , q1 , q2 , q3 ) with quaternion rotation matrix R parameterization r

the Jacobian J of the objective function is given in (7.78). r

J=



 d11 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 = d21 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 d1,10  ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u ∂u  δ

δ

δ

δ

δ

δ

δ

δ

δ

δ

∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3 ∂ x ∂ y ∂z ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂vδ ∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3 ∂ x ∂ y ∂z

(7.78)

164

7 Camera Metrics

Python code for generating equirectangular camera observation colinearity equation r

and the Jacobian J of the objective function is shown in Listing 7.7. 1 2 3 4 5 6

from sympy i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’ ) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14 15

tx , ty , tz = s y m b o l s ( ’ tx ty tz ’ ) om , fi , ka = s y m b o l s ( ’ om fi ka ’ ) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’) px , py , pz = s y m b o l s ( ’ px py pz ’ ) ; cols , rows = s y m b o l s ( ’ cols rows ’) ; pi = s y m b o l s ( ’ pi ’ ) u_kp , v_kp = s y m b o l s ( ’ u_kp v_kp ’)

16 17 18 19 20 21 22

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] p o i n t _ s y m b o l s = [ px , py , pz ] all_symbols = position_symbols + orientation_symbols + point_symbols

23 24 25 26

27 28

Rt_wc = m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) # Rt_wc = m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) # Rt_wc = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) r = Rt_wc [: -1 ,: -1] t = M a t r i x ([ tx , ty , tz ]) . vec ()

29 30 31 32

33 34 35

p o s _ w = M a t r i x ([ px , py , pz ]) . vec () bearing = r * pos_w + t; norm = sqrt ( b e a r i n g [0]* b e a r i n g [0] + b e a r i n g [1]* b e a r i n g [1] + b e a r i n g [2]* b e a r i n g [2]) b e a r i n g = b e a r i n g / norm l a t i t u d e = - asin ( b e a r i n g [1]) l o n g i t u d e = a t a n 2 ( b e a r i n g [0] , b e a r i n g [2])

36 37 38 39

u = cols *(0.5 + l o n g i t u d e / (2.0 * pi ) ) v = rows *(0.5 - l a t i t u d e / pi ) uv = M a t r i x ([ u , v ]) . vec ()

40 41 42 43 44

t a r g e t _ v a l u e = M a t r i x ([ u_kp , v_kp ]) . vec () m o d e l _ f u n c t i o n = M a t r i x ([ u , v ]) . vec () obs_eq = target_value - model_function obs_eq_jacobian = obs_eq . jacobian ( all_symbols )

Listing 7.7 Python code for generating equirectangular camera colinearity observation equation r

and the Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or Quaternion) rotation matrix parameterization

7.3 Equirectangular Camera Model

165

7.3.2 Coplanarity Observation Equation For two poses ([R, t]1wc , [R, t]2wc ) of the equirectangular camera following vector b can be derived: ⎡ ⎤ ⎡2 ⎤ tx − tx1 bx b = ⎣b y ⎦ = t 2wc − t 1wc = ⎣t y2 − t y1 ⎦ (7.79) bz tz2 − tz1 The coplanarity condition (shown in Figs. 7.10, 7.11) incorporates [b]× skew symmetric matrix: ⎡ ⎤ 0 −bz by [b]× = ⎣ bz 0 −bx ⎦ (7.80) −by bx 0 and point P g (x g , y g , z g ) projected onto two spheres P  (x1l , y1l , z l1 ) and P  (x2l , y2l , z l2 ) into the following condition: 

2  δ =  [[R,t]1wc ,[R,t]2wc ] ([R, t]1wc , [R, t]2wc , x g , y g , z g ) = P  R1 wc [b]× R wc P = ⎡ ⎤ ⎡ l⎤ x2 0 −bz by l l l 1 x1 y1 z 1 Rwc ⎣ bz 0 −bx ⎦ R2wc ⎣ y2l ⎦ −by bx 0 z l2 (7.81) Thus, two poses of equirectangular cameras are coplanar when δ = 0. Retrieving projected point P l from pixel coordinates u, v assuming known number of image rows and columns in computer vision is done by following equation:

Fig. 7.10 Coplanarity of two spherical panoramas in photogrammetry

166

7 Camera Metrics

Fig. 7.11 Coplanarity of two spherical panoramas in computer vision

⎡

⎤

⎤ cos(lat)sin(lon) 

v 

u ⎢ l⎥ ⎣ −sin(lat) ⎦ , lon = 2π − 0.5 , lat = −π − 0.5 ⎣ py ⎦ = cols r ows cos(lat)cos(lon) pzl (7.82) To build the observation equation incorporating two poses ([R, t]1wc , [R, t]2wc ) kp kp kp kp and two keypoints (u 1 , v1 , u 2 , v2 ) being desired projections of terrain point P g (x g , y g , z g ) onto these image spheres (r = 1) the following optimization problem is considered: plx

⎡

min

[R,t]1wc ,[R,t]2wc

C

0 −  [R,t]1

2 wc ,[R,t]wc

i=1

([R, t]1 wc

l,kp

l,kp

l,kp

l,kp

l,kp

l,kp

, [R, t]2wc , xi,1 , yi,1 , z i,1 , xi,2 , yi,2 , z i,2 )

2

(7.83) where there are C coplanarity conditions incorporating two poses ([R, t]1wc , [R, t]2wc ) kp kp kp kp l,kp l,kp l,kp and keypoints (u 1 , v1 ), (u 2 , v2 ) related with projections (xi,1 , yi,1 , z i,1 ) and l,kp l,kp l,kp (xi,2 , yi,2 , z i,2 ). Thus, equirectangular camera coplanarity observation equation is given in (7.84), δ =  r esidual

l,kp

l,kp

l,kp

l,kp

l,kp

l,kp

−  [R,t]1 ,[R,t]2 ([R, t]1wc , [R, t]2wc , xi,1 , yi,1 , z i,1 , xi,2 , yi,2 , z i,2 ) wc wc    target value 0 

model f unction

(7.84) where δ is residual, 0 is target value and  [[R,t]1wc ,[R,t]2wc ] ([R, l,kp

l,kp

l,kp

l,kp

t]1wc

, [R,

t]2wc

l,kp

, xi,1 ,

l,kp

yi,1 , z i,1 , xi,2 , yi,2 , z i,2 ) is the model function. Assuming Tait-Bryan angles r

(ω, ϕ, κ) the Jacobian J of the objective function for two poses (tx1 , t y1 , tz1 , ω1 , ϕ 1 , κ 1 ) and (tx2 , t y2 , tz2 , ω2 , ϕ 2 , κ 2 ) is given in (7.85).

7.3 Equirectangular Camera Model

167

r   J = d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 d1,12 =

 ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂tx1 ∂t y1 ∂tz1 ∂ω1 ∂ϕ 1 ∂κ 1 ∂tx2 ∂t y2 ∂tz2 ∂ω2 ∂ϕ 2 ∂κ 2

(7.85) r

For Rodrigues (sx , s y , sz ) rotation matrix R parameterization the Jacobian J of the objective function for two poses (tx1 , t y1 , tz1 , sx1 , s y1 , sz1 ) and (tx2 , t y2 , tz2 , sx2 , s y2 , sz2 ) is given in (7.86). r   J = d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 d1,12 =

 ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂tx1 ∂t y1 ∂tz1 ∂sx1 ∂s y1 ∂sz1 ∂tx2 ∂t y2 ∂tz2 ∂sx2 ∂s y2 ∂sz2

(7.86)

For quaternion (q0 , q1 , q2 , q3 ) rotation matrix R parameterization the Jacor

bian J of the objective function for two poses (Tx1 , Ty1 , Tz1 , q01 , q11 , q21 , q31 ) and (Tx2 , Ty2 , Tz2 , q02 , q12 , q22 , q32 ) is given in (7.87). r



d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 d1,12 d1,13

J=  d1,14 = 

∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂tx1 ∂t y1 ∂tz1 ∂q01 ∂q11 ∂q21 ∂q31 ∂tx2 ∂t y2 ∂tz2 ∂q02 ∂q12 ∂q22 ∂q32

(7.87)

Python code for generating equirectangular camera coplanarity observation equar

tion and the Jacobian J of the objective function is shown in Listing 7.8. 1 2 3 4 5 6

from sympy i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14 15 16

17 18

tx_1 , ty_1 , tz_1 = s y m b o l s ( ’ tx_1 ty_1 tz_1 ’ ) om_1 , fi_1 , ka_1 = s y m b o l s ( ’ om_1 fi_1 ka_1 ’ ) # sx_1 , sy_1 , sz_1 = s y m b o l s ( ’ sx_1 sy_1 sz_1 ’) # q0_1 , q1_1 , q2_1 , q3_1 = s y m b o l s ( ’ q0_1 q1_1 q2_1 q3_1 ’) tx_2 , ty_2 , tz_2 = s y m b o l s ( ’ tx_2 ty_2 tz_2 ’ ) om_2 , fi_2 , ka_2 = s y m b o l s ( ’ om_2 fi_2 ka_2 ’ ) # sx_2 , sy_2 , sz_2 = s y m b o l s ( ’ sx_2 sy_2 sz_2 ’) # q0_2 , q1_2 , q2_2 , q3_2 = s y m b o l s ( ’ q0_2 q1_2 q2_2 q3_2 ’) rows_1 , cols_1 , rows_2 , c o l s _ 2 = s y m b o l s ( ’ r o w s _ 1 c o l s _ 1 rows_2 cols_2 ’) u_1 , v_1 , u_2 , v_2 = s y m b o l s ( ’ u_1 v_1 u_2 v_2 ’ ) pi = s y m b o l s ( ’ pi ’)

19 20 21 22

p o s i t i o n _ s y m b o l s _ 1 = [ tx_1 , ty_1 , tz_1 ] o r i e n t a t i o n _ s y m b o l s _ 1 = [ om_1 , fi_1 , ka_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ sx_1 , sy_1 , sz_1 ]

168 23 24 25 26 27 28

7 Camera Metrics

# o r i e n t a t i o n _ s y m b o l s _ 1 = [ q0_1 , q1_1 , q2_1 , q3_1 ] p o s i t i o n _ s y m b o l s _ 2 = [ tx_2 , ty_2 , tz_2 ] o r i e n t a t i o n _ s y m b o l s _ 2 = [ om_2 , fi_2 , ka_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ sx_2 , sy_2 , sz_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ q0_2 , q1_2 , q2_2 , q3_2 ] all_symbols = position_symbols_1 + orientation_symbols_1 + position_symbols_2 + orientation_symbols_2

29 30

31

32

33 34

35

36

37

c a m e r a _ m a t r i x _ 1 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_1 , ty_1 , tz_1 , om_1 , fi_1 , ka_1 ) # c a m e r a _ m a t r i x _ 1 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_1 , ty_1 , tz_1 , sx_1 , sy_1 , sz_1 ) # c a m e r a _ m a t r i x _ 1 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_1 , ty_1 , tz_1 , q0_1 , q1_1 , q2_1 , q3_1 ) R_1t = c a m e r a _ m a t r i x _ 1 [: -1 ,: -1]. t r a n s p o s e () c a m e r a _ m a t r i x _ 2 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_2 , ty_2 , tz_2 , om_2 , fi_2 , ka_2 ) # c a m e r a _ m a t r i x _ 2 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_2 , ty_2 , tz_2 , sx_2 , sy_2 , sz_2 ) # c a m e r a _ m a t r i x _ 2 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_2 , ty_2 , tz_2 , q0_2 , q1_2 , q2_2 , q3_2 ) R_2 = c a m e r a _ m a t r i x _ 2 [: -1 ,: -1]

38 39 40 41 42 43 44

l o n 1 = ( u_1 / c o l s _ 1 - 0.5) * (2 * pi ) lat1 = -( v_1 / r o w s _ 1 - 0 . 5 ) * pi b e a r i n g 1 _ x = cos ( lat1 ) * sin ( lon1 ) b e a r i n g 1 _ y = - sin ( lat1 ) b e a r i n g 1 _ z = cos ( lat1 ) * cos ( lon1 ) b e a r i n g 1 = M a t r i x ([ bearing1_x , bearing1_y , b e a r i n g 1 _ z ]) . vec ()

45 46 47 48 49 50 51

l o n 2 = ( u_2 / c o l s _ 2 - 0.5) * (2 * pi ) lat2 = -( v_2 / r o w s _ 2 - 0 . 5 ) * pi b e a r i n g 2 _ x = cos ( lat2 ) * sin ( lon2 ) b e a r i n g 2 _ y = - sin ( lat2 ) b e a r i n g 2 _ z = cos ( lat2 ) * cos ( lon2 ) b e a r i n g 2 = M a t r i x ([ bearing2_x , bearing2_y , b e a r i n g 2 _ z ]) . vec ()

52 53 54 55 56

bx = tx_2 - tx_1 by = ty_2 - ty_1 bz = tz_2 - tz_1 b = M a t r i x ([[0 , - bz , by ] , [ bz , 0 , - bx ] , [ - by , bx , 0]])

57 58 59

60 61

target_value = Matrix ([[0]]) m o d e l _ f u n c t i o n = b e a r i n g 1 . t r a n s p o s e () * R_1t * b * R_2 * bearing2 obs_eq = target_value - model_function obs_eq_jacobian = obs_eq . jacobian ( all_symbols )

Listing 7.8 Python code for generating equirectangular camera coplanarity observation equation r

and the Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or Quaternion) rotation matrix parameterization

References

169

References 1. L. Liu, H. Li, H. Yao, R. Zha, Plueckernet: learn to register 3D line reconstructions (2020) 2. F.E. Lijia, L.I. Jianfeng, W.A. Huinan, S.H. Qinghong, C.H. Shuwen, Registration of aerial image with airborne lidar data based on Plucker line. Acta Geod. Cartogr. Sin. 44(7), 761 (2015)

Chapter 8

LiDAR Metrics

8.1 Point to Point Metrics 8.1.1 Point to Point Local point P l (x l , y l , z l , 1) is represented in Euclidean space as point in local reference frame. The matrix [R, t] transforms the local point P l into point P g (x g , y g , z g ) in global reference frame by Eq. (8.1). P g = [R, t] P l

(8.1) l, j

l, j

l, j

Furthermore, Eq. (8.2) transforms ith local point (xi , yi , z i , 1) in jth [R, t] into g, j g, j g, j global reference system (xi , yi , z i ), therefore it can be used for building point to observation Eq. (8.3) two poses [R1 , t 1 ] and [R2 , t 2 ], where   incorporating   δpoint δ δ  x y z are residuals, 0 0 0 are target values and  [ R j ,t j ] (R j , t j , x l, j , y l, j , z l, j ) −  [ Rk ,t k ] (Rk , t k , x l,k , y l,k , z l,k ) is the model function. The optimization problem for point to point observations is defined as Eq. (8.4). Where there are C pairs of r

points contributing into optimization process. The construction of the Jacobian J of the objective function for the system composed of four poses assuming Tait-Bryan parametrization of rotation matrix is shown in Table 8.1. ⎛ l, j

l, j

l, j

 [ R j ,t j ] (R j , t j , xi , yi , z i ) =

⎞

g, j x ⎜ ig, j ⎟ ⎝ yi ⎠ g, j zi

⎛ ⎞ ⎛x l, j ⎞ j j j j r r12 r13 t1 ⎜ il, j ⎟ ⎜ 11 j j j j ⎟ yi ⎟ = ⎝r21 r22 r23 t2 ⎠ ⎜ ⎝ z l, j ⎠ j j j j i r31 r32 r33 t3 1

© The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_8

(8.2)

171

d1,5

d1,6

0

zl,1 , zl,3

d3,6 0

d3,5

d2,6

y l,1 , y l,3

d3,4

d2,5 −1

−1

d2,4

d1,6

x l,1 , x l,3

0

0

d1,5

0

−1

0 d3,4

d2,4 d3,5

d2,5

d3,6

d2,6

0

zl,0 , zl,3

−1

0

d3,6

−1

0

0

d3,5

0

0

y l,0 , y l,3

0

d3,4

d2,6

−1

−1

d2,5

x l,0 , x l,3

0

d2,4

0

0

0

y l,0 , y l,2

zl,0 , zl,2

−1

1

d1,6

0

0

1

0

d1,5

d3,6

d2,6

d1,6

−1

0

d3,5

d2,5

d1,5

x l,0 , x l,2

0

d3,4

d2,4

0

d3,10 d3,11 d3,12

d2,10 d2,11 d2,12

0

1

0

0

−1

0

0

1

0

ty

Pose2 tx

zl,2 , zl,3

0

−1

0

0

1

0

d1,11 d1,12

κ

0

0

−1

0

0

1

0

ϕ

y l,2 , y l,3

0

0

0

0

ω

−1

0

d3,6

d2,6

0

tz

x l,2 , x l,3

0

d3,5

d2,5

1

ty

Pose1 tx

0

d3,4

d2,4

d1,6

κ

zl,1 , zl,2

−1

0

d1,5

ϕ

0

0

−1

0

ω

y l,1 , y l,2

0

0

tz

−1

0

y l,0 , y l,1

zl,0 , zl,1

0

ty

x l,1 , x l,2

−1

x l,0 , x l,1

tx

Pose0

1

0

0

−1

0

0

1

0

0

tz

κ

d1,11 d1,12

ϕ

d3,6

d2,6

d1,6

d1,11 d1,12

d3,5

d2,5

d1,5

d3,10 d3,11 d3,12

d2,10 d2,11 d2,12

0

d3,4

d2,4

0

d3,10 d3,11 d3,12

d2,10 d2,11 d2,12

0

ω

0

0

1

0

0

1

0

0

1

tx

0

1

0

0

1

0

0

1

0

ty

Pose3

Table 8.1 Example of design matrix for point to point observation equation assuming Tait-Bryan parametrization of rotation matrix

1

0

0

1

0

0

1

0

0

tz

κ

d1,11 d1,12

ϕ

d1,11 d1,12

d1,11 d1,12 d3,10 d3,11 d3,12

d2,10 d2,11 d2,12

0

d3,10 d3,11 d3,12

d2,10 d2,11 d2,12

0

d3,10 d3,11 d3,12

d2,10 d2,11 d2,12

0

ω

172 8 LiDAR Metrics

8.1 Point to Point Metrics

⎡ δ⎤ x ⎣yδ ⎦ = zδ    r esiduals

⎡ ⎤ 0 ⎣0⎦ 0 

173

  − [ R j ,t j ] (R j , t j , x l, j , y l, j , z l, j ) −  [ Rk ,t k ] (Rk , t k , x l,k , y l,k , z l,k )    model f unction

target values

(8.3)

min

⎞2 ⎛⎡ ⎤ 0   l, j l, j l, j l,k l,k l,k j k j k ⎝⎣0⎦ −  R j ,t j (R , t , xi , yi , z i ) −  Rk ,t k (R , t , xi , yi , z i ) ⎠ ] ] [ [ 0 i=1

C 

R j ,t j ,Rk ,t k

(8.4) r

Assuming Tait-Bryan angles (ω, ϕ, κ) the Jacobian J of the objective function for two poses (tx1 , t y1 , tz1 , ω1 , ϕ 1 , κ 1 ) and (tx2 , t y2 , tz2 , ω2 , ϕ 2 , κ 2 ) is given in (8.5).

⎛

d1,1 d1,2 d1,3 d1,4 d1,5 ⎝ J = d2,1 d2,2 d2,3 d2,4 d2,5 d3,1 d3,2 d3,3 d3,4 d3,5 ⎛ δ ∂x 0 0 1 ⎜ ∂tx ∂ y δ ⎜0 0 ∂t y1 ⎝ ∂z δ 0 0 ∂t 1 r

z

⎞ d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 d1,12 d2,6 d2,7 d2,8 d2,9 d2,10 d2,11 d2,12 ⎠ = d3,6 d3,7 d3,8 d3,9 d3,10 d3,11 d3,12 ⎞ ∂xδ ∂xδ ∂xδ ∂xδ ∂xδ 0 ∂ϕ 0 0 0 ∂ϕ 1 ∂κ 1 ∂t 2 2 ∂κ 2 x ∂ yδ ∂ yδ ∂ yδ ∂ yδ ∂ yδ ∂ yδ ∂ yδ ⎟ ⎟ 0 0 1 1 1 2 ∂ω ∂ϕ ∂κ ∂t y ∂ω2 ∂ϕ 2 ∂κ 2 ⎠ δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ 0 0 ∂z ∂ω1 ∂ϕ 1 ∂κ 1 ∂t 2 ∂ω2 ∂ϕ 2 ∂κ 2 z

(8.5) r

For Rodrigues (sx , s y , sz ) rotation matrix R parameterization the Jacobian J of the objective function for two poses (tx1 , t y1 , tz1 , sx1 , s y1 , sz1 ) and (tx2 , t y2 , tz2 , sx2 , s y2 , sz2 ) is given in (8.6).

⎛

⎞ d d d d d d d d d d d d 1,1 1,2 1,3 1,4 1,5 1,6 1,7 1,8 1,9 1,10 1,11 1,12 r J = ⎝d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 d2,10 d2,11 d2,12 ⎠ = d3,1 d3,2 d3,3 d3,4 d3,5 d3,6 d3,7 d3,8 d3,9 d3,10 d3,11 d3,12 ⎛ ∂xδ δ δ δ δ δ δ δ⎞ 0 0 ∂∂sx1 ∂∂sx1 ∂∂sx1 ∂∂tx2 0 0 ∂∂sx2 ∂∂sx2 ∂∂sx2 ∂tx1 x y z x x y z ⎜ δ δ δ δ δ δ δ δ ⎟ ⎜ 0 ∂ y1 0 ∂ y1 ∂ y1 ∂ y1 0 ∂ y2 0 ∂ y2 ∂ y2 ∂ y2 ⎟ ∂t ∂s ∂s ∂s ∂t ∂s ∂s ∂s ⎝ y x y z y x y z ⎠ δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ 0 0 ∂z 0 0 ∂t 1 ∂s 1 ∂s 1 ∂s 1 ∂t 2 ∂s 2 ∂s 2 ∂s 2 z

x

y

z

z

x

y

z

(8.6) r

For quaternion (q0 , q1 , q2 , q3 ) rotation matrix R parameterization the Jacobian J of the objective function for two poses (tx1 , t y1 , tz1 , q01 , q11 , q21 , q31 ) and (tx2 , t y2 , tz2 , q02 , q12 , q22 , q32 ) is given in (8.7).

174

8 LiDAR Metrics

⎛

J= ⎞

r

d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 ⎝d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 d2,10 d3,1 d3,2 d3,3 d3,4 d3,5 d3,6 d3,7 d3,8 d3,9 d3,10 ⎛ ∂xδ ∂xδ ∂xδ ∂xδ ∂xδ ∂xδ 0 0 ∂q 1 1 1 1 ∂t 2 ∂tx1 x 0 ∂q1 ∂q2 ∂q3 ⎜ δ δ δ δ δ ∂ y ∂ y ∂ y ∂ y ∂ y ⎜0 0 0 ∂t y1 ∂q01 ∂q11 ∂q21 ∂q31 ⎝ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ 0 0 ∂t 1 ∂q 1 ∂q 1 ∂q 1 ∂q 1 0 z

0

1

2

3

d1,11 d1,12 d1,13 d1,14 d2,11 d2,12 d2,13 d2,14 ⎠ = d3,11 d3,12 d3,13 d3,14 ⎞ ∂xδ ∂xδ ∂xδ ∂xδ 0 0 ∂q 2 2 2 2 0 ∂q1 ∂q2 ∂q3 ∂ yδ ∂ yδ ∂ yδ ∂ yδ ∂ yδ ⎟ ⎟ 0 ∂t y2 ∂q02 ∂q12 ∂q22 ∂q32 ⎠ δ δ δ δ δ ∂z ∂z ∂z ∂z 0 ∂z ∂t 2 ∂q 2 ∂q 2 ∂q 2 ∂q 2 z

0

1

2

r

It is straight forward that for all previous Jacobians J the partial derivatives ∂ yδ ∂t y1

=

∂z δ ∂tz1

= −1 and

∂xδ ∂tx2

=

∂ yδ ∂t y2

=

∂z δ ∂tz2

3

(8.7) ∂xδ ∂tx1

=

= 1. Python code for generating point to point r

observation equations and the Jacobian J of the objective function is shown in Listing 8.1. 1 2 3 4 5 6

from s ym py i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’ ) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14 15 16 17

x_1 , y_1 , z_1 = s y m b o l s ( ’ x_1 y_1 z_1 ’ ) tx_1 , ty_1 , tz_1 = s y m b o l s ( ’ tx_1 ty_1 tz_1 ’ ) om_1 , fi_1 , ka_1 = s y m b o l s ( ’ om_1 fi_1 ka_1 ’ ) # sx_1 , sy_1 , sz_1 = s y m b o l s ( ’ sx_1 sy_1 sz_1 ’) # q0_1 , q1_1 , q2_1 , q3_1 = s y m b o l s ( ’ q0_1 q1_1 q2_1 q3_1 ’) x_2 , y_2 , z_2 = s y m b o l s ( ’ x_2 y_2 z_2 ’ ) tx_2 , ty_2 , tz_2 = s y m b o l s ( ’ tx_2 ty_2 tz_2 ’ ) om_2 , fi_2 , ka_2 = s y m b o l s ( ’ om_2 fi_2 ka_2 ’ ) # sx_2 , sy_2 , sz_2 = s y m b o l s ( ’ sx_2 sy_2 sz_2 ’) # q0_2 , q1_2 , q2_2 , q3_2 = s y m b o l s ( ’ q0_2 q1_2 q2_2 q3_2 ’)

18 19 20 21 22 23 24 25 26 27

p o s i t i o n _ s y m b o l s _ 1 = [ tx_1 , ty_1 , tz_1 ] o r i e n t a t i o n _ s y m b o l s _ 1 = [ om_1 , fi_1 , ka_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ sx_1 , sy_1 , sz_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ q0_1 , q1_1 , q2_1 , q3_1 ] p o s i t i o n _ s y m b o l s _ 2 = [ tx_2 , ty_2 , tz_2 ] o r i e n t a t i o n _ s y m b o l s _ 2 = [ om_2 , fi_2 , ka_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ sx_2 , sy_2 , sz_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ q0_2 , q1_2 , q2_2 , q3_2 ] all_symbols = position_symbols_1 + orientation_symbols_1 + position_symbols_2 + orientation_symbols_2

28 29 30

p o i n t _ 1 = M a t r i x ([ x_1 , y_1 , z_1 , 1]) . vec () p o i n t _ 2 = M a t r i x ([ x_2 , y_2 , z_2 , 1]) . vec ()

31 32

t r a n s f o r m e d _ p o i n t _ 1 = ( m a t r i x 4 4 F r o m T a i t B r y a n ( tx_1 , ty_1 , tz_1 , om_1 , fi_1 , ka_1 ) * p o i n t _ 1 ) [: -1 ,:]

8.1 Point to Point Metrics 33

34

35

36

37

175

# t r a n s f o r m e d _ p o i n t _ 1 = ( m a t r i x 4 4 F r o m R o d r i g u e s ( tx_1 , ty_1 , tz_1 , sx_1 , sy_1 , sz_1 ) * p o i n t _ 1 ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ 1 = ( m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_1 , ty_1 , tz_1 , q0_1 , q1_1 , q2_1 , q3_1 ) * p o i n t _ 1 ) [: -1 ,:] t r a n s f o r m e d _ p o i n t _ 2 = ( m a t r i x 4 4 F r o m T a i t B r y a n ( tx_2 , ty_2 , tz_2 , om_2 , fi_2 , ka_2 ) * p o i n t _ 2 ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ 2 = ( m a t r i x 4 4 F r o m R o d r i g u e s ( tx_2 , ty_2 , tz_2 , sx_2 , sy_2 , sz_2 ) * p o i n t _ 2 ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ 2 = ( m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_2 , ty_2 , tz_2 , q0_2 , q1_2 , q2_2 , q3_2 ) * p o i n t _ 2 ) [: -1 ,:]

38 39 40 41 42

t a r g e t _ v a l u e = M a t r i x ([0 ,0 ,0]) . vec () model_function = transformed_point_1 - transformed_point_2 del ta = t a r g e t _ v a l u e - m o d e l _ f u n c t i o n d e l t a _ j a c o b i a n = delta . j a c o b i a n ( a l l _ s y m b o l s ) r

Listing 8.1 Python code for generating point to point observation equation and the Jacobian J of the objective function for Tait-Bryan (aternatively Rodrigues or Quaternion) rotation matrix parameterization

8.1.2 Point to Point—Source to Target This method simplifies point to point optimization problem expressed as Eq. (8.4) for each kth source point to target point correspondence: ⎡ k⎤ x  [ Rk ,t k ] (Rk , t k , x l,k , y l,k , z l,k ) = ⎣ y k ⎦ zk

(8.8)

as a target point cloud, therefore point to point—source to target observation equation is (8.9), ⎡ δ⎤ x ⎣yδ ⎦ = zδ    r esiduals

⎡ ⎤ 0 ⎣0⎦ 0 

target values

⎡ k ⎤⎞ x − ⎝ [ R j ,t j ] (R j , t j , x l,k , y l,k , z l,k ) − ⎣ y k ⎦⎠ zk    ⎛

(8.9)

model f unction

    where x δ y δ z δ are residuals, 0 0 0 are target values and [ R j ,t j ]  k k k  j j l,k l,k l,k is the model function. Optimization problem (R , t , x , y , z ) − x y z of point to point—source to target approach is given by Eq. (8.10). ⎡ k ⎤⎞⎞2 ⎛⎡ ⎤ ⎛ xi 0 ⎝⎣0⎦ − ⎝ R j ,t j (R j , t j , xil, j , yil, j , z l,i j ) − ⎣ yik ⎦⎠⎠ min ] [ R j ,t j 0 i=1 z ik C 

(8.10)

176

8 LiDAR Metrics

Where there are C pairs of points (source to target) contributing into optimization process. This method does not modify target points and does not relate with any pose that could cause such modification. Therefore, it can be sufficiently used in scenarios where the desired solution is to align the certain part of data set to the reference one. r

The Jacobian J of the objective function for pose (tx , t y , tz , ω, ϕ, κ) expressed using Tait-Bryan angles is given: ⎛

⎞ d11 d12 d13 d14 d15 d16 J = ⎝d21 d22 d23 d24 d25 d26 ⎠ = d31 d32 d33 d34 d35 d36 ⎛ ∂xδ δ δ⎞ 0 0 0 ∂∂ϕx ∂∂κx ∂tx δ δ δ δ ⎜ ∂y ∂y ∂y ∂y ⎟ ⎝ 0 ∂t y 0 ∂ω ∂ϕ ∂κ ⎠ r

0

0

(8.11)

∂z δ ∂z δ ∂z δ ∂z δ ∂tz ∂ω ∂ϕ ∂κ r

Table 8.2 shows an example of the Jacobian J of the objective function of the source to target approach for the problem aligning source data of four poses with δ δ δ = −1. The rotation reference point cloud. The partial derivatives ∂∂tx1 = ∂∂ty1 = ∂z ∂tz1 x y part expressed using Tait-Bryan angles parameterization is given in Eq. (8.12).

 R(ω, ϕ, κ) =

cos(ϕ) cos(κ) − cos(ϕ) sin(κ) sin(ϕ) cos(ω) sin(κ)+sin(ω) sin(ϕ) cos(κ) cos(ω) cos(κ)−sin(ω) sin(ϕ) sin(κ) − sin(ω) cos(ϕ) sin(ω) sin(κ)−cos(ω) sin(ϕ) cos(κ) sin(ω) cos(κ)+cos(ω) sin(ϕ) sin(κ) cos(ω) cos(ϕ)



(8.12) r

It is straight forward to calculate the Jacobian J of the objective function if (8.10) for Tait-Bryan angles parameterization from following equations: ⎛ 0 r tx ⎜0 J =⎜ ⎝0 0

0 0 0 0

0 0 0 0

⎞ −1 0⎟ ⎟ 0⎠ 0

⎛ 0 r ty ⎜0 J =⎜ ⎝0 0

0 0 0 0

0 0 0 0

⎞ 0 −1⎟ ⎟ 0⎠ 0

0 0 0 0

0 0 0 0

⎞ 0 0⎟ ⎟ −1⎠ 0

⎛

0 r tz ⎜0 J =⎜ ⎝0 0

(8.13)

(8.14)

(8.15)

d35

d25

d36

d26

tx

tx

tx

0

d36

d26

d16

κ

z s,3 , z t

d35

d25

d15

ϕ

0

d34

d24

0

ω

−1

−1

0

0

tz

y s,3 , y t

0

−1

0

ty

Pose3

x s,3 , x t

0

d36

d26

d16

κ

z s,2 , z t

d35

d25

d15

ϕ

0

d34

d24

0

ω

−1

−1

0

0

tz

y s,2 , y t

0

−1

0

ty

Pose2

x s,2 , x t

0

d34

d24

d16

κ

z s,1 , z t

−1

0

ϕ

d15

0

0

−1

ω

0

−1

0

tz

0

y s,1 , y t

0

y s,0 , y t

z s,0 , z t

ty

0

Pose1

x s,1 , x t

−1

x s,0 , x t

tx

Pose0

Table 8.2 Example of the Jacobian of the objective function (source to target approach)

0

−1

0

ty

−1

0

0

tz

d34

d24

0

ω

d35

d25

d15

ϕ

d36

d26

d16

κ

8.1 Point to Point Metrics 177

178

8 LiDAR Metrics rω

J = ⎛ ⎞ 0 0 0 0 ⎜cos(ω) sin(ϕ) cos(κ) − sin(ω) sin(κ) − cos(ω) sin(ϕ) sin(κ) − sin(ω) cos(κ) − cos(ω) cos(ϕ) 0⎟ ⎟ −⎜ ⎝sin(ω) sin(ϕ) cos(κ) + cos(ω) sin(κ) − sin(ω) sin(ϕ) sin(κ) + cos(ω) cos(κ) − sin(ω) cos(ϕ) 0⎠ 0 0 0 0

(8.16) ⎞ − sin(ϕ) cos(κ) sin(ϕ) sin(κ) cos(ϕ) 0 rϕ ⎜ sin(ω) cos(ϕ) cos(κ) − sin(ω) cos(ϕ) sin(κ) sin(ω) sin(ϕ) 0⎟ ⎟ J = −⎜ ⎝− cos(ω) cos(ϕ) cos(κ) cos(ω) cos(ϕ) sin(κ) − cos(ω) sin(ϕ) 0⎠ 0 0 0 0 (8.17) ⎛ ⎞

⎛

− cos(ϕ) sin(κ) − cos(ϕ) cos(κ) 0 ⎜− sin(ω) sin(ϕ) sin(κ) + cos(ω) cos(κ) − sin(ω) sin(ϕ) cos(κ) − cos(ω) sin(κ) 0 ⎜ J = −⎝ cos(ω) sin(ϕ) sin(κ) + sin(ω) cos(κ) cos(ω) sin(ϕ) cos(κ) − sin(ω) sin(κ) 0 0 0 0 rκ

0 0⎟ ⎟ 0⎠ 0

(8.18) ∂xδ = J t11x x l + J t12x y l + J t13x z l + J t14x = −1 ∂tx ∂xδ t t t t = = J 11y x l + J 12y y l + J 13y z l + J 14y = 0 ∂t y

d11 = d12

∂xδ t t t t = J 11z x l + J 12z y l + J 13z z l + J 14z = 0 ∂tz ∂xδ = J ω11 x l + J ω12 y l + J ω13 z l + J ω14 = 0 d14 = ∂ω ∂xδ ϕ ϕ ϕ ϕ = J 11 x l + J 12 y l + J 13 z l + J 14 = d15 = ∂ϕ −(− sin(ϕ) cos(κ)x l + sin(ϕ) sin(κ)y l + cos(ϕ)z l + 0) d13 =

∂xδ = J κ11 x l + J κ12 y l + J κ13 z l + J κ14 = ∂κ −(− cos(ϕ) sin(κ)x l − cos(ϕ) cos(κ)y l + 0 + 0) d16 =

∂ yδ = J t21x x l + J t22x y l + J t13x z l + J t24x = 0 ∂tx ∂ yδ t t t t = = J 21y x l + J 22y y l + J 23y z l + J 24y = −1 ∂t y

d21 = d22

∂ yδ t t t t = J 21z x l + J 22z y l + J 23z z l + J 24z = 0 ∂tz ∂ yδ = J ω21 x l + J ω22 y l + J ω23 z l + J ω24 = d24 = ∂ω −((cos(ω) sin(ϕ) cos(κ) − sin(ω) sin(κ))x l +

d23 =

−(cos(ω) sin(ϕ) sin(κ) − sin(ω) cos(κ))y l − cos(ω) cos(ϕ)z l + 0)

8.1 Point to Point Metrics

179

∂ yδ = J ϕ21 x l + J ϕ22 y l + J ϕ23 z l + J ϕ24 = ∂ϕ −(sin(ω) cos(ϕ) cos(κ)x l − sin(ω) cos(ϕ) sin(κ)y l + d25 =

sin(ω) sin(ϕ)z l + 0)

(8.19)

∂ yδ = J κ21 x l + J κ22 y l + J κ23 z l + J κ24 = ∂κ −((− sin(ω) sin(ϕ) sin(κ) + cos(ω) cos(κ))x l + d26 =

(− sin(ω) sin(ϕ) cos(κ) − cos(ω) sin(κ))y l + 0 + 0) ∂z δ = J t31x x l + J t32x y l + J t33x z l + J t34x = −1 ∂tx ∂z δ t t t t = = J 31y x l + J 32y y l + J 33y z l + J 34y = 0 ∂t y

d31 = d32

∂z δ t t t t = J 31z x l + J 32z y l + J 33z z l + J 34z = 0 ∂tz ∂z δ = J ω31 x l + J ω32 y l + J ω33 z l + J ω34 = d34 = ∂ω −((sin(ω) sin(ϕ) cos(κ) + cos(ω) sin(κ))x l +

d33 =

(− sin(ω) sin(ϕ) sin(κ) + cos(ω) cos(κ))y l − sin(ω) cos(ϕ)z l + 0) d35 =

∂z δ ϕ ϕ ϕ ϕ = J 31 x l + J 32 y l + J 33 z l + J 34 = ∂ϕ −(− cos(ω) cos(ϕ) cos(κ)x l +

cos(ω) cos(ϕ) sin(κ)y l − cos(ω) sin(ϕ)z l + 0) ∂z δ = J κ31 x l + J κ32 y l + J κ33 z l + J κ34 = ∂κ −((cos(ω) sin(ϕ) sin(κ) + sin(ω) cos(κ))x l + (cos(ω) sin(ϕ) cos(κ) − sin(ω) sin(κ))y l + 0 + 0)

Python code for generating point to point source to target observation equations and r

the Jacobian J of the objective function is shown in Listing 8.2. 1 2 3 4 5 6

from s y m p y i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13

x_t , y_t , z_t = s y m b o l s ( ’ x_t y_t z_t ’ ) x_s , y_s , z_s = s y m b o l s ( ’ x_s y_s z_s ’ ) tx , ty , tz = s y m b o l s ( ’ tx ty tz ’) om , fi , ka = s y m b o l s ( ’ om fi ka ’) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’)

14 15

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ]

180 16 17 18 19

8 LiDAR Metrics

o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] all_symbols = position_symbols + orientation_symbols

20 21 22 23

24

25

p o i n t _ t = M a t r i x ([ x_t , y_t , z_t ]) . vec () p o i n t _ s = M a t r i x ([ x_s , y_s , z_s , 1]) . vec () t r a n s f o r m e d _ p o i n t _ s = ( m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) * p o i n t _ s ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ s = ( m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) * p o i n t _ s ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ s = ( m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) * p o i n t _ s ) [: -1 ,:]

26 27 28

t a r g e t _ v a l u e = M a t r i x ([0 ,0 ,0]) . vec () model_function = transformed_point_s - point_t Listing 8.2 Python code for generating point to point source to target observation equation and the r

Jacobian J of the objective function for Tait-Bryan (alternatively Ridrigues or Quaternion) rotation matrix parameterization

8.1.3 Point to Point—Source to Landmark This observation equation is similar to point to point—source to target with such a  difference that the landmark position x L y L z L is also updated during optimization process. It is given by (8.20) ⎡ δ⎤ x ⎣yδ ⎦ = zδ    r esiduals

⎡ ⎤ 0 ⎣0⎦ 0 

target values

⎛

⎡

⎤⎞ xL − ⎝ [R,t] (R, t, x s , y s , z s ) − ⎣ y L ⎦⎠ zL   

(8.20)

model f unction

    where x δ y δ z δ are residuals, 0 0 0 are target values and  [R,t] (R, t, x s , y s , z s )     − x L y L z L is the model function, x L y L z L is the position of the landmark and (x s , y s , z s ) is source point expressed in local frame [R, t]. Optimization problem of point to point—source to landmark is given by Eq. (8.21),

min

R,t,x L ,y L ,z L

⎡ L ⎤⎞⎞2 ⎛⎡ ⎤ ⎛ xi 0 ⎝⎣0⎦ − ⎝ [R,t] (R, t, xis , yis , z is ) − ⎣ yiL ⎦⎠⎠ 0 z iL i=1

C 

(8.21)

where there are C source point to landmark pairs contributing into the optimization process. This method updates all poses and landmarks. Table 8.3 shows an example of r

the Jacobian J of source to landmark approach for Tait-Bryan angle parameterization.

y s,3 , y L ,1 z s,3 , z L ,1

z s,2 , z L ,1 x s,3 , x L ,1

x s,2 , x L ,1 y s,2 , y L ,1

y s,1 , y L ,1 z s,1 , z L ,1

z s,0 , z L ,0 x s,1 , x L ,1

x s,0 , x L ,0 y s,0 , y L ,0

0

−1

0

0

−1

0

0

tz

0

ty

−1

tx

Pose0

d24 d34

0

ω

d35

d15 d25

ϕ

d36

d16 d26

κ

−1 0

0

0

ty

0

−1

tx

Pose1

−1

0

0

tz

d24 d34

0

ω

d35

d15 d25

ϕ

d36

d16 d26

κ

0

−1 0

0 0

−1

0

tz

0

ty

−1

tx

Pose2

d24 d34

0

ω

d35

d15 d25

ϕ

d36

d16 d26

κ

−1 0

0

0 0

−1

ty

Pose3 tx

Table 8.3 Example of Jacobian of source to landmark approach for Tait-Bryan angle parameterization

−1

0

0

tz

d24 d34

0

ω

d35

d15 d25

ϕ

d36

d16 d26

κ

0

0

1

0

1

0

1

0

0

Landmar k 0 yL zL xL

0

0

1

0

0

1

0

0

1

0

1

0

0

1

0

0

1

0

1

0

0

1

0

0

1

0

0

Landmar k 1 yL zL xL

8.1 Point to Point Metrics 181

182

8 LiDAR Metrics

It can be seen that Landmar k 0 is paired with point 0 and Landmar k 1 is paired with r

point 1 , point 2 and point 3 . Assuming Tait-Bryan angles (ω, ϕ, κ) the Jacobian J for pose (tx , t y , tz , ω, ϕ, κ) and landmark (x L , y L , z L ) is given in (8.22) ⎞ ⎛ d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 r J = ⎝d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 ⎠ = d3,1 d3,2 d3,3 d3,4 d3,5 d3,6 d3,7 d3,8 d3,9 ⎞ ⎛ ∂xδ δ δ L ,δ (8.22) 0 0 0 ∂∂ϕx ∂∂κx ∂ x∂ x 0 0 ∂tx δ δ δ δ L ,δ ⎟ ⎜ ∂y ∂y ∂y ∂y ∂y ⎝ 0 ∂t y 0 ∂ω ∂ϕ ∂κ 0 ∂ y 0 ⎠ δ δ δ δ L ,δ ∂z ∂z ∂z 0 0 ∂z 0 0 ∂z∂z ∂tz ∂ω ∂ϕ ∂κ where

∂xδ ∂tx

=

∂ yδ ∂t y

=

∂z δ ∂tz

= −1 and

∂ x L ,δ ∂x

=

∂ y L ,δ ∂y

=

∂z L ,δ ∂z

= 1. Similarly for Rodrigues r

(sx , s y , sz ) rotation matrix R parameterization the Jacobian J for pose (tx , t y , tz , sx , s y , sz ) and landmark (x L , y L , z L ) is given in (8.23). ⎛

⎞ d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 J = ⎝d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 ⎠ = d3,1 d3,2 d3,3 d3,4 d3,5 d3,6 d3,7 d3,8 d3,9 ⎞ ⎛ δ δ δ δ L ,δ ∂x 0 0 ∂∂sxx ∂∂sxy ∂∂sxz ∂ x∂ x 0 0 ∂tx ⎟ ⎜ ⎜ 0 ∂ y δ 0 ∂ y δ ∂ y δ ∂ y δ 0 ∂ y L ,δ 0 ⎟ ∂t y ∂sx ∂s y ∂sz ∂y ⎠ ⎝ δ ∂z δ ∂z δ ∂z δ ∂z L ,δ 0 0 ∂z 0 0 ∂tz ∂sx ∂s y ∂sz ∂z r

(8.23)

r

For quaternion (q0 , q1 , q2 , q3 ) rotation matrix R parameterization the Jacobian J for two pose (tx , t y , tz , q0 , q1 , q2 , q3 ) and landmark (x L , y L , z L ) is given in (8.24). r

⎛

J= ⎞

d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 ⎝d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 d2,10 ⎠ = d3,1 d3,2 d3,3 d3,4 d3,5 d3,6 d3,7 d3,8 d3,9 d3,10 ⎛ ∂xδ ⎞ δ δ δ δ L ,δ 0 0 ∂∂qx 0 ∂∂qx 1 ∂∂qx 2 ∂∂qx 3 ∂ x∂ x 0 0 ∂tx ⎜ ⎟ ∂ yδ ∂ yδ ∂ yδ ∂ yδ ∂ yδ ∂ y L ,δ ⎝ 0 ∂t y 0 ∂q0 ∂q1 ∂q2 ∂q3 0 ∂ y 0 ⎠ δ L ,δ ∂z δ ∂z δ ∂z δ ∂z δ 0 0 ∂z 0 0 ∂z∂z ∂tz ∂q0 ∂q1 ∂q2 ∂q3

(8.24)

Python code for generating point to point source to landmark observation equation r

and Jacobian J is shown in Listing 8.3. 1 2 3 4

from s y m p y i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t *

8.1 Point to Point Metrics 5 6

183

from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13

x_L , y_L , z_L = s y m b o l s ( ’ x_L y_L z_L ’ ) x_s , y_s , z_s = s y m b o l s ( ’ x_s y_s z_s ’ ) tx , ty , tz = s y m b o l s ( ’ tx ty tz ’) om , fi , ka = s y m b o l s ( ’ om fi ka ’) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’)

14 15 16 17 18 19

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] l a n d m a r k _ s y m b o l s = [ x_L , y_L , z_L ]

20 21

all_symbols = position_symbols + orientation_symbols + landmark_symbols

22 23 24 25

26

27

p o i n t _ L a n d m a r k = M a t r i x ([ x_L , y_L , z_L ]) . vec () p o i n t _ s o u r c e = M a t r i x ([ x_s , y_s , z_s , 1]) . vec () t r a n s f o r m e d _ p o i n t _ s o u r c e = ( m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) * p o i n t _ s o u r c e ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ s = ( m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) * p o i n t _ s o u r c e ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ s = ( m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) * p o i n t _ s o u r c e ) [: -1 ,:]

28 29 30

31 32

t a r g e t _ v a l u e = M a t r i x ([0 ,0 ,0]) . vec () model_function = transformed_point_source point_Landmark delta = target_value - model_function delta_jacobian = delta . jacobian ( all_symbols ) Listing 8.3 Python code for generating point to point source to landmark observation equation and r

Jacobian J for Tait-Bryan (alternatively Rodrigues or Quaternion) rotation matrix parameterization

8.1.4 Point to Point with Scale In certain scenarios there is a need to find a scale factor between source point cloud and target point cloud. Obvious example is the registration of the 3D map obtained from monocular camera with the 3D map build based on LiDAR data. Source point P l (x l , y l , z l , 1) is represented in Euclidean space as point in local reference frame. The scaled point P ls is obtained by following equation: P = ls

 scale 0 0 0

0 0 0 scale 0 0 0 scale 0 0 0 1

 Pl = S Pl

(8.25)

184

8 LiDAR Metrics

The matrix [R, t] is the transformation of source point P ls into global point P g (x g , y g , z g , 1) in global reference frame, thus (8.26) P g =  [R,t,S] (R, t, S, x l , y l , z l ) = [R, t]S P l = [R, t] P ls

(8.26)

is used for building point to point with scale observation Eq. (8.27) ⎡ δ⎤ x ⎣yδ ⎦ = zδ    r esiduals

⎡ ⎤ 0 ⎣0⎦ 0 

  −  [ R j ,t j ,S j ] (R j , t j , S j , x l, j , y l, j , z l, j ) −  [ Rk ,t k ,Sk ] (Rk , t k , Sk , x l,k , y l,k , z l,k )    model f unction

target values

(8.27)     where, x δ y δ z δ are residuals, 0 0 0 are target values and  [ R j ,t j ,S j ] (R j , t j , S j , x l, j , y l, j , z l, j ) −  [ Rk ,t k ,Sk ] (Rk , t k , Sk , x l,k , y l,k , z l,k ) is the model function. The point to point with scale optimization problem is defined as (8.28). min

R j ,t j ,S j ,Rk ,t k ,Sk

⎞2 ⎛⎡ ⎤ 0   ⎠ ⎝⎣0⎦ −  R j ,t j ,S j (R j , t j , S j , xil, j , yil, j , z l,i j ) −  Rk ,t k ,Sk (Rk , t k , Sk , xil,k , yil,k , z l,k i ) [ [ ] ] 0 i=1

C 

(8.28) r

Table 8.4 shows an example of the Jacobian J of the objective function for point to point with scale observation equation assuming Tait-Bryan parameterization of rotation matrix, where s is the scale parameter that is also optimized. Assuming r

Tait-Bryan angles (ω, ϕ, κ) the Jacobian J of the objective function for two poses with scale (tx1 , t y1 , tz1 , ω1 , ϕ 1 , κ 1 , s 1 ) and (tx2 , t y2 , tz2 , ω2 , ϕ 2 , κ 2 , s 2 ) is given in (8.29).

⎛

d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 J = ⎝d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 d2,10 d3,1 d3,2 d3,3 d3,4 d3,5 d3,6 d3,7 d3,8 d3,9 d3,10 ⎛ δ ∂x ∂xδ ∂xδ ∂xδ ∂xδ 0 0 0 ∂ϕ 1 ∂κ 1 ∂s 1 ∂t 2 ∂tx1 x ⎜ δ δ δ ∂ y ∂ y ∂ y ∂ yδ ∂ yδ ⎜0 1 0 0 1 1 1 1 ∂t y ∂ω ∂ϕ ∂κ ∂s ⎝ δ ∂z δ ∂z δ ∂z δ ∂z δ 0 0 ∂z 0 ∂t 1 ∂ω1 ∂ϕ 1 ∂κ 1 ∂s 1 r

z

⎞ d1,11 d1,12 d1,13 d1,14 d2,11 d2,12 d2,13 d2,14 ⎠ = d3,11 d3,12 d3,13 d3,14 ⎞ ∂xδ ∂xδ ∂xδ 0 0 0 ∂ϕ 2 ∂κ 2 ∂s 2 ∂ yδ ∂ yδ ∂ yδ ∂ yδ ∂ yδ ⎟ ⎟ 0 ∂ω 2 ∂ϕ 2 ∂κ 2 ∂s 2 ∂t y2 ⎠ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ 0 ∂t 2 ∂ω2 ∂ϕ 2 ∂κ 2 ∂s 2 z (8.29) r

For Rodrigues (sx , s y , sz ) rotation matrix R parameterization the Jacobian J of the objective function for two poses with scales (tx1 , t y1 , tz1 , sx1 , s y1 , sz1 , s 1 ) and (tx2 , t y2 , tz2 , sx2 , s y2 , sz2 , s 2 ) is given in (8.30).

d1,6

d1,7

0

0

1

0

d3,7

z l,1 , z l,3

d3,6 0

d3,5

d2,7

y l,1 , y l,3

d3,4

d2,6 −1

−1

d2,5

d1,7

x l,1 , x l,3

0

d2,4

d1,6

0

−1 −1

0 d3,4

d2,4 d3,5

d2,5 d3,6

d2,6

d3,7

d2,7

0

0

d1,5

d3,7

0

0

z l,0 , z l,3

−1

0

d3,6

d2,7

0

0

d3,5

d2,6

y l,0 , y l,3

0

d3,4

d2,5

−1

−1

d2,4

x l,0 , x l,3

0

0

0

−1

0

y l,0 , y l,2

z l,0 , z l,2

d1,7

d3,7

d2,7

d1,7

1

d1,6

d3,6

d2,6

d1,6

0

d1,5

d3,5

d2,5

d1,5

−1

0

d3,4

d2,4

0

d3,11 d3,12 d3,13 d3,14

d2,10 d2,12 d2,13 d2,14

0

1

0

0

−1

0

0

1

0

ty

Pose2 tx

x l,0 , x l,2

0

−1

0

0

1

0

s

d1,12 d1,13 d1,14

κ

z l,2 , z l,3

0

0

−1

0

0

1

0

ϕ

0

d1,5

0

0

0

ω

y l,2 , y l,3

0

d3,7

d2,7

0

tz

−1

0

d3,6

d2,6

1

ty

Pose1 tx

0

d3,5

d2,5

d1,7

s

x l,2 , x l,3

0

d2,4

d3,4

d1,6

κ

z l,1 , z l,2

−1

0

d1,5

ϕ

0

0

−1

0

ω

y l,1 , y l,2

0

0

tz

−1

0

y l,0 , y l,1

z l,0 , z l,1

0

ty

x l,1 , x l,2

−1

x l,0 , x l,1

tx

Pose0

1

0

0

−1

0

0

1

0

0

tz

κ s

d1,12 d1,13 d1,14

ϕ

d3,6

d2,6

d1,6

d3,7

d2,7

d1,7

d1,12 d1,13 d1,14

d3,5

d2,5

d1,5

d3,11 d3,12 d3,13 d3,14

d2,11 d2,12 d2,13 d2,14

0

d3,4

d2,4

0

d3,11 d3,12 d3,13 d3,14

d2,11 d2,12 d2,13 d2,14

0

ω

0

0

1

0

0

1

0

0

1

tx

0

1

0

0

1

0

0

1

0

ty

Pose3

1

0

0

1

0

0

1

0

0

tz

κ

s

d1,12 d1,13 d1,14

ϕ

d1,12 d1,13 d1,14

d1,12 d1,13 d1,14 d3,11 d3,12 d3,13 d3,14

d2,11 d2,12 d2,13 d2,14

0

d3,11 d3,12 d3,13 d3,14

d2,11 d2,12 d2,13 d2,14

0

d3,11 d3,12 d3,13 d3,14

d2,11 d2,12 d2,13 d2,14

0

ω

Table 8.4 Example of the Jacobian of the objective function for point to point with scale observation equation assuming Tait-Bryan parameterization of rotation matrix

8.1 Point to Point Metrics 185

186

8 LiDAR Metrics

⎛

d1,1 d1,2 d1,3 d1,4 d1,5 J = ⎝d2,1 d2,2 d2,3 d2,4 d2,5 d3,1 d3,2 d3,3 d3,4 d3,5 ⎛ ∂xδ 0 ∂t 1 ⎜ x ∂ yδ ⎜0 ∂t y1 ⎝ 0 0 r

⎞ d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 d1,12 d1,13 d1,14 d2,6 d2,7 d2,8 d2,9 d2,10 d2,11 d2,12 d2,13 d2,14 ⎠ = d3,6 d3,7 d3,8 d3,9 d3,10 d3,11 d3,12 d3,13 d3,14 δ δ δ δ δ δ δ δ δ⎞ 0 ∂∂sx1 ∂∂sx1 ∂∂sx1 ∂∂sx1 ∂∂tx2 0 0 ∂∂sx2 ∂∂sx2 ∂∂sx2 ∂∂sx2 x y z x x y z δ δ δ δ δ δ δ δ δ ⎟ 0 ∂∂sy1 ∂∂sy1 ∂∂sy1 ∂∂sy1 0 ∂∂ty2 0 ∂∂sy2 ∂∂sy2 ∂∂sy2 ∂∂sy2 ⎟ ⎠ x y z y x y z ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ 0 0 ∂tz1 ∂sx1 ∂s y1 ∂sz1 ∂s 1 ∂tz2 ∂sx2 ∂s y2 ∂sz2 ∂s 2 (8.30) r

For quaternion (q0 , q1 , q2 , q3 ) rotation matrix R parameterization the Jacobian J of the objective function for two poses with scales (tx1 , t y1 , tz1 , q01 , q11 , q21 , q31 , s 1 ) and (tx2 , t y2 , tz2 , q02 , q12 , q22 , q32 , s 2 ) is given in (8.31). r

⎛

d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 d1,10 d1,11 d1,12 ⎝d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 d2,10 d2,11 d2,12 d3,1 d3,2 d3,3 d3,4 d3,5 d3,6 d3,7 d3,8 d3,9 d3,10 d3,11 d3,12 ⎛ ∂xδ ∂xδ ∂xδ ∂xδ ∂xδ ∂xδ ∂xδ 0 0 ∂q 0 1 1 1 1 ∂s 1 ∂t 2 ∂tx1 x 0 ∂q1 ∂q2 ∂q3 ⎜ δ δ δ δ δ δ δ ⎜ 0 ∂ y1 0 ∂ y 1 ∂ y 1 ∂ y 1 ∂ y 1 ∂ y1 0 ∂ y2 ∂t y ∂t y ∂q0 ∂q1 ∂q2 ∂q3 ∂s ⎝ δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ 0 0 ∂z 0 0 ∂t 1 ∂q 1 ∂q 1 ∂q 1 ∂q 1 ∂s 1 z

0

1

2

3

J= ⎞

d1,13 d1,14 d1,15 d1,16 d2,13 d2,14 d2,15 d2,16 ⎠ = d3,13 d3,14 d3,15 d3,16 ⎞ ∂xδ ∂xδ ∂xδ ∂xδ ∂xδ 0 ∂q 2 2 2 2 ∂s 2 0 ∂q1 ∂q2 ∂q3 ∂ yδ ∂ yδ ∂ yδ ∂ yδ ∂ yδ ⎟ ⎟ 0 ∂q 2 2 2 2 ∂s 2 ⎠ 0 ∂q1 ∂q2 ∂q3 ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ ∂z δ ∂tz2 ∂q02 ∂q12 ∂q22 ∂q32 ∂s 2

(8.31) Python code for generating point to point with scale observation equations and the r

Jacobian J of the objective function is shown in Listing 8.4. 1 2 3 4 5 6

from s ym py i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’ ) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14 15 16 17 18 19

x_1 , y_1 , z_1 = s y m b o l s ( ’ x_1 y_1 z_1 ’ ) s_1 = s y m b o l s ( ’ s_1 ’ ) tx_1 , ty_1 , tz_1 = s y m b o l s ( ’ tx_1 ty_1 tz_1 ’) om_1 , fi_1 , ka_1 = s y m b o l s ( ’ om_1 fi_1 ka_1 ’) # sx_1 , sy_1 , sz_1 = s y m b o l s ( ’ sx_1 sy_1 sz_1 ’) # q0_1 , q1_1 , q2_1 , q3_1 = s y m b o l s ( ’ q0_1 q1_1 q2_1 q3_1 ’) x_2 , y_2 , z_2 = s y m b o l s ( ’ x_2 y_2 z_2 ’ ) s_2 = s y m b o l s ( ’ s_2 ’ ) tx_2 , ty_2 , tz_2 = s y m b o l s ( ’ tx_2 ty_2 tz_2 ’) om_2 , fi_2 , ka_2 = s y m b o l s ( ’ om_2 fi_2 ka_2 ’) # sx_2 , sy_2 , sz_2 = s y m b o l s ( ’ sx_2 sy_2 sz_2 ’) # q0_2 , q1_2 , q2_2 , q3_2 = s y m b o l s ( ’ q0_2 q1_2 q2_2 q3_2 ’)

20 21 22 23

p o s i t i o n _ s y m b o l s _ 1 = [ tx_1 , ty_1 , tz_1 ] o r i e n t a t i o n _ s y m b o l s _ 1 = [ om_1 , fi_1 , ka_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ sx_1 , sy_1 , sz_1 ]

8.1 Point to Point Metrics 24 25 26 27 28 29

187

# o r i e n t a t i o n _ s y m b o l s _ 1 = [ q0_1 , q1_1 , q2_1 , q3_1 ] p o s i t i o n _ s y m b o l s _ 2 = [ tx_2 , ty_2 , tz_2 ] o r i e n t a t i o n _ s y m b o l s _ 2 = [ om_2 , fi_2 , ka_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ sx_2 , sy_2 , sz_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ q0_2 , q1_2 , q2_2 , q3_2 ] all_symbols = position_symbols_1 + orientation_symbols_1 + [ s_1 ] + p o s i t i o n _ s y m b o l s _ 2 + o r i e n t a t i o n _ s y m b o l s _ 2 + [ s_2 ]

30 31 32 33

34

p o i n t _ 1 = M a t r i x ([ x_1 , y_1 , z_1 , 1]) . vec () p o i n t _ 2 = M a t r i x ([ x_2 , y_2 , z_2 , 1]) . vec () m _ s c a l e _ 1 = M a t r i x ([[ s_1 , 0 , 0 , 0] , [0 , s_1 , 0 , 0] , [0 , 0 , s_1 , 0] , [0 ,0 ,0 ,1]]) m _ s c a l e _ 2 = M a t r i x ([[ s_2 , 0 , 0 , 0] , [0 , s_2 , 0 , 0] , [0 , 0 , s_2 , 0] , [0 ,0 ,0 ,1]])

35 36

37

38

39

40

41

t r a n s f o r m e d _ p o i n t _ 1 = ( m a t r i x 4 4 F r o m T a i t B r y a n ( tx_1 , ty_1 , tz_1 , om_1 , fi_1 , ka_1 ) * m _ s c a l e _ 1 * p o i n t _ 1 ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ 1 = ( m a t r i x 4 4 F r o m R o d r i g u e s ( tx_1 , ty_1 , tz_1 , sx_1 , sy_1 , sz_1 ) * m _ s c a l e _ 1 * p o i n t _ 1 ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ 1 = ( m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_1 , py_1 , tz_1 , q0_1 , q1_1 , q2_1 , q3_1 ) * m _ s c a l e _ 1 * p o i n t _ 1 ) [: -1 ,:] t r a n s f o r m e d _ p o i n t _ 2 = ( m a t r i x 4 4 F r o m T a i t B r y a n ( tx_2 , ty_2 , tz_2 , om_2 , fi_2 , ka_2 ) * m _ s c a l e _ 2 * p o i n t _ 2 ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ 2 = ( m a t r i x 4 4 F r o m R o d r i g u e s ( tx_2 , ty_2 , tz_2 , sx_2 , sy_2 , sz_2 ) * m _ s c a l e _ 2 * p o i n t _ 2 ) [: -1 ,:] # t r a n s f o r m e d _ p o i n t _ 2 = ( m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_2 , ty_2 , tz_2 , q0_2 , q1_2 , q2_2 , q3_2 ) * m _ s c a l e _ 2 * p o i n t _ 2 ) [: -1 ,:]

42 43 44 45 46

t a r g e t _ v a l u e = M a t r i x ([0 ,0 ,0]) . vec () model_function = transformed_point_1 - transformed_point_2 del ta = t a r g e t _ v a l u e - m o d e l _ f u n c t i o n d e l t a _ j a c o b i a n = delta . j a c o b i a n ( a l l _ s y m b o l s )

Listing 8.4 Python code for generating point to point with scale observation equation and the r

Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or quaternion) rotation matrix parameterization

8.1.5 Semantic Point to Point In certain applications it is evident that the discrimination of points into certain classes [1, 2] can improve SLAM. Typically such applications produce so called semantic maps [3] and are related with the semantic navigation [4]. Thus, the semantic label sl can be assigned for each query LiDAR point. For example points that satisfy planar condition can be labeled as planes, points that satisfy cylindrical condition can be labeled as cylinders or contour [5]. Other semantic conditions or classification techniques can be used for further discrimination into e.g. walls, floor, ceiling, vegetation etc. Using semantic information [6] extracted from the 3D laser data [7] is a relatively recent research topic in mobile robotics. In [8] a semantic map for a mobile

188

8 LiDAR Metrics

robot was described as a map that contains, in addition to spatial information about the environment, assignments of mapped features to entities of known classes. In [9] a model of an indoor scene is implemented as a semantic net. This approach is used in [10] where a robot extracts semantic information from 3D models built from a laser scanner. In [11] the location of features is extracted by using a probabilistic technique RANSAC [12, 13]. It was shown in [14] that also the region growing approach, extended from [15] using processing of unorganized point clouds. The improvement of the plane extraction from 3D data by fusing laser data and vision is shown in [16]. An interesting approach for detecting corners of openings from terrestrial LiDAR data is discussed in [17]. Large scale implementation of interactive 3D mapping and recognition in large outdoor scene is discussed in [18]. Recent advances in DNN show many applications in 3D point cloud semantic classification [19–22, 22–25]. To build semantic point to point observation equations the same methodology as in Sect. 8.1.1 is used. Therefore, Eq. (8.36) incorporates a semantic label sl of points having the same semantic meaning e.g. wall-to-wall. Semantic discrimination into the certain classes has positive impact into the least squares optimization convergence where it is possible to give different weights for more important classes. For example we can promote walls, ceilings etc. against vegetation. The well-known method for g the semantic classification of the point cloud P k , k ∈ 1..m is the use of covariance matrix [5] defined by (6.10) 1  g g ( P − μ)( P k − μ) m − 1 k=1 k m

= where

(8.32)

m 1  g P μ= m k=1 k

(8.33)

λ1  λ2  λ3 ∈ R

(8.34)

its eigen-values:

and corresponding eigen-vectors: e1 , e2 , e3 ∈ R3

(8.35)

The eigen-values and eigen-vectors can be used for local shape description as features shown in Table 8.5 and most of them are visualized in Figs. 8.1, 8.2, 8.3, 8.4, 8.5, 8.6, 8.7 and 8.8. Therefore it is evident that semantic label sl can be obtained with classical machine learning methods and modern DNN distinguishing local shape description into certain classes. The semantic point to point optimization problem is defined by Eq. (8.37), where there are C sl correspondences of neighboring points with the same semantic label sl.

8.1 Point to Point Metrics Table 8.5 Local neighborhood features Features Sum of eigenvalues Omnivariance Eigenentropy Anisotropy Linearity Cylindrical Likeness Planarity1 Planarity2 Plane likeness, Planarity3 Sphericity Change of curvature Verticality Absolute moment Vertical moment

189

Definitions 3 i=1 λi  1 3 3 λ i i=1 3 − i=1 λi ln(λi ) (λ1 − λ3 ) /λ1 (λ1 − λ2 ) /λ1 3 λi (λ1 − λ2 ) / i=1 (λ2 − λ3 ) /λ1 1 −  3 3 3 3λ1 / i=1 λi · 3λ2 / i=1 λi · 3λ3 / i=1 λi 3 2 (λ2 − λ3 ) / i=1 λi λ3 /λ1 λ3 /(λ1 + λ2 + λ3 ) π   − angle(ei , ez ) 2  i∈(0,2)     k 1  , e p − p   i 0 |N | k i∈(0,1,2)  1  p − p0 , ez |N | |N |

Number of points Average color

1  c |N |  1 (c |N |−1

Color variance

− c) ¯2

Fig. 8.1 Sum of eigenvalues ⎡

⎤ x δ,sl ⎣ y δ,sl ⎦ = z δ,sl    r esiduals

⎡ ⎤ 0 ⎣0⎦ 0 

  − [ R j ,t j ] (R j , t j , x l, j,sl , y l, j,sl , z l, j,sl ) −  [ Rk ,t k ] (Rk , t k , x l,k,sl , y l,k,sl , z l,k,sl )    model f unction

target values

(8.36)

⎞2 ⎛⎡ ⎤ 0   l, j,sl l, j,sl l, j,sl l,k,sl l,k,sl l,k,sl j k j k ⎝⎣0⎦ −  R j ,t j (R , t , xi , yi , z i ) −  Rk ,t k (R , t , xi , yi , zi ) ⎠ ] ] [ [ 0 i=1

C  sl

min

R j ,t j ,Rk ,t k

(8.37)

190

8 LiDAR Metrics

Fig. 8.2 Omnivariance

Fig. 8.3 Eigenentropy

Fig. 8.4 Anisotropy

8.1.6 Point to Projection In this section we consider point on line and point on plane projections. It is evident that ones we calculate these projection the point to point (source to target) approach described in Sect. 8.1.2 can be used as observation equations. Let us assume that we are looking the projection of point P sr c,l (x sr c,l , y sr c,l , z sr c,l , 1) that can be transformed to global coordinate system as point P sr c,g (x sr c,g , y sr c,g , z sr c,g ) using matrix [R, t]. Thus,

8.1 Point to Point Metrics

191

Fig. 8.5 Linearity

Fig. 8.6 Planarity

Fig. 8.7 Sphericity

⎡ sr c,l ⎤ ⎤ x x sr c,g ⎢ y sr c,l ⎥ sr c,l sr c,l sr c,l sr c,g ⎥ ⎣y ⎦ =  [R,t] (R, t, x ,y ,z ) = [R, t] ⎢ ⎣ z sr c,l ⎦ z sr c,g 1 ⎡

(8.38)

To find point P sr c,g to line projection as P pr oj,g in global reference system we use line representation as target direction vector V trg,ln (x trg,ln , y trg,ln , z trg,ln ) and target point on line P trg,g (x trg,g , y trg,g , z trg,g ) expressed in global reference system.

192

8 LiDAR Metrics

Fig. 8.8 Number of neighbors

Therefore, the point to line projection is as follows: P pr oj,g

⎡ sr c,g ⎤ ⎡ trg,ln ⎤ x x − x trg,g a · b b, a = ⎣ y sr c,g − y trg,g ⎦ , b = ⎣ y trg,ln ⎦ = P trg,g + b·b z sr c,g − z trg,g z trg,ln

(8.39)

where (·) is a dot product. The point to projection onto line observation equation is (8.40), ⎡ ⎤ δx ⎣δ y ⎦ = δz    r esiduals

⎡ ⎤ 0 ⎣0⎦ 0 

target values

⎛

⎡

⎤⎞ x pr oj,g − ⎝ [R,t] (R, t, x sr c,l , y sr c,l , z sr c,l ) − ⎣ y pr oj,g ⎦⎠ z pr oj,g   

(8.40)

model f unction

    where x δ y δ z δ are residuals, 0 0 0 are target values and  [R,t]   (R, t, x sr c,l , y sr c,l , z sr c,l ) − x pr oj,g y pr oj,g z pr oj,g is the model function. Thus, the following optimization problem is defined in Eq. (8.41) ⎛⎡ ⎤ ⎛ ⎤⎞⎞2 ⎡ pr oj,g xi C 0  ⎜⎣ ⎦ ⎜ ⎢ pr oj,g ⎥⎟⎟ sr c,l sr c,l sr c,l min ⎝ 0 − ⎝ [R,t] (R, t, xi , yi , z i ) − ⎣ yi ⎦⎠⎠ R,t pr oj,g 0 i=1 zi

(8.41)

pr oj,g

where, there are C correspondences of P isr c,l with its projection onto line P i . For pose [R, t] expressed using Tait-Bryan angles parameterization (ω, ϕ, κ) the r

Jacobian J of the objective function is given in Eq. (8.42).

8.1 Point to Point Metrics

193

⎛

d1,1 d1,2 d1,3 d1,4 J = ⎝d2,1 d2,2 d2,3 d2,4 d3,1 d3,2 d3,3 d3,4 ⎛ ∂δx 0 0 ∂tx ⎜ 0 ∂δ y 0 ⎝ ∂t y z 0 0 ∂δ ∂tz r

⎞ d1,5 d1,6 d2,5 d2,6 ⎠ = d3,5 d3,6 ⎞ x ∂δx 0 ∂δ ∂ϕ ∂κ ∂δ y ∂δ y ∂δ y ⎟ ∂ω ∂ϕ ∂κ ⎠

(8.42)

∂δz ∂δz ∂δz ∂ω ∂ϕ ∂κ

For pose [R, t] expressed using Rodrigues parameterization of rotation matrix r

(sx , s y , sz ) the Jacobian J of the objective function is given in Eq. (8.43). ⎛

⎞ d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 J = ⎝d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 ⎠ = d3,1 d3,2 d3,3 d3,4 d3,5 d3,6 ⎛ ∂δx ⎞ x ∂δx ∂δx 0 0 ∂δ ∂tx ∂sx ∂s y ∂sz ∂δ y ∂δ y ∂δ y ∂δ y ⎟ ⎜ ⎝ 0 ∂t y 0 ∂sx ∂s y ∂sz ⎠ z ∂δz ∂δz ∂δz 0 0 ∂δ ∂tz ∂sx ∂s y ∂sz r

(8.43)

For pose [R, t] expressed using quaternion parameterization of rotation matrix r

(q0 , q1 , q2 , q3 ) the Jacobian J of the objective function is given in Eq. (8.44). ⎛

⎞ d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 J = ⎝d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 ⎠ = d3,1 d3,2 d3,3 d3,4 d3,5 d3,6 d3,7 ⎛ ∂δx ∂δx ∂δx ∂δx ∂δx ⎞ 0 0 ∂q ∂tx 0 ∂q1 ∂q2 ∂q3 ⎜ 0 ∂δ y 0 ∂δ y ∂δ y ∂δ y ∂δ y ⎟ ⎝ ∂t y ∂q0 ∂q1 ∂q2 ∂q3 ⎠ r

0

0

(8.44)

∂δz ∂δ y ∂δz ∂δ y ∂δ y ∂tz ∂q0 ∂q1 ∂q2 ∂q3

Python code for generating point to projection onto line observation equation is given in Listing 8.5. 1 2 3 4 5 6

from s y m p y i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8

9

10

11

x_src_l , y_src_l , z _ s r c _ l = s y m b o l s ( ’ x _ s r c _ l y _ s r c _ l z _ s r c _ l ’) x_trg_g , y_trg_g , z _ t r g _ g = s y m b o l s ( ’ x _ t r g _ g y _ t r g _ g z _ t r g _ g ’) x_trg_ln , y_trg_ln , z _ t r g _ l n = s y m b o l s ( ’ x _ t r g _ l n y_trg_ln z_trg_ln ’) tx , ty , tz = s y m b o l s ( ’ tx ty tz ’)

194 12 13 14

8 LiDAR Metrics

om , fi , ka = s y m b o l s ( ’ om fi ka ’) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’)

15 16 17 18 19 20

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] all_symbols = position_symbols + orientation_symbols

21 22

23

24

25

26 27

p o i n t _ s o u r c e _ l o c a l = M a t r i x ([ x_src_l , y_src_l , z_src_l ,1]) . vec () R t _ w c = m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) [: -1 ,:] # R t _ w c = m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) [: -1 ,:] # R t _ w c = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) [: -1 ,:] point_source_global = Rt_wc * point_source_local p o i n t _ o n _ l i n e _ t a r g e t _ g l o b a l = M a t r i x ([ x_trg_g , y_trg_g , z _ t r g _ g ])

28 29 30

a= point_source_global - point_on_line_target_global b = M a t r i x ([ x_trg_ln , y_trg_ln , z _ t r g _ l n ]) . vec ()

31 32

p _ p r o j = p o i n t _ o n _ l i n e _ t a r g e t _ g l o b a l + ( a . dot ( b ) / b . dot (b))*b

33 34 35 36 37

t a r g e t _ v a l u e = M a t r i x ([0 ,0 ,0]) . vec () model_function = point_source_global - p_proj delta = target_value - model_function delta_jacobian = delta . jacobian ( all_symbols ) Listing 8.5 Python code for generating point to projection onto line observation equations and the r

Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or Quaternion) rotation matrix parameterization

To find point P sr c,g to plane projection as P pr oj,g we will consider following plane equation: ax + by + cz + d = 0 (8.45) where

   abc =1

(8.46)

V pl = (a, b, c) is the unit vector orthogonal to plane, and d is the distance from the origin to the plane. It satisfies following condition with point in 3D space ⎡ ⎤ x   ⎢y⎥ ⎢ abcd ⎣ ⎥ =0 z⎦ 1

(8.47)

8.1 Point to Point Metrics

195

Therefore, the projection P pr oj,g can be computed with: ⎡

P pr oj,g

⎤ x sr c,g     = ⎣ y sr c,g ⎦ − x sr c,g y sr c,g z sr c,g 1 · a b c d V pl z sr c,g

(8.48)

where (·) is a dot product. To build point to projection onto plane observation equation we can incorporate Eq. (8.40), source local point P sr c,l , target global point P pr oj,g and matrix [R, t] that transforms P sr c,l into P sr c,g , thus the optimization problem is as in Eq. (8.41), where there are C correspondences of P sr c,l with its projection onto plane P pr oj,g . For pose [R, t] expressed using Tait-Bryan angles parameterization r

(ω, ϕ, κ) the Jacobian J of the objective function is given in Eq. (8.42), for Rodrigues r

parameterization of rotation matrix (sx , s y , sz ) the Jacobian J of the objective function is given in Eq. (8.43) and the quaternion parameterization of rotation matrix r

(q0 , q1 , q2 , q3 ) the Jacobian J of the objective function is given in Eq. (8.44). Listing 8.6 shows the Python code for point to projection onto plane observation equation. 1 2 3 4 5 6

from s y m p y i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8

9

10 11 12 13 14

x_src_l , y_src_l , z _ s r c _ l = s y m b o l s ( ’ x _ s r c _ l y _ s r c _ l z _ s r c _ l ’) x_trg_g , y_trg_g , z _ t r g _ g = s y m b o l s ( ’ x _ t r g _ g y _ t r g _ g z _ t r g _ g ’) a , b , c = s y m b o l s ( ’ a b c ’) tx , ty , tz = s y m b o l s ( ’ tx ty tz ’) om , fi , ka = s y m b o l s ( ’ om fi ka ’) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’)

15 16 17 18 19 20

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] all_symbols = position_symbols + orientation_symbols

21 22

23

24

25

26

p o i n t _ s o u r c e _ l o c a l = M a t r i x ([ x_src_l , y_src_l , z_src_l , 1]) . vec () R t _ w c = m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) [: -1 ,:] # R t _ w c = m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) [: -1 ,:] # R t _ w c = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) [: -1 ,:] point_source_global = Rt_wc * point_source_local

196 27

28

29 30

8 LiDAR Metrics

p o i n t _ s o u r c e _ g l o b a l _ 1 = M a t r i x ([ p o i n t _ s o u r c e _ g l o b a l [0] , p o i n t _ s o u r c e _ g l o b a l [1] , p o i n t _ s o u r c e _ g l o b a l [2] , 1]) . vec () p o i n t _ o n _ p l a n e _ t a r g e t _ g l o b a l = M a t r i x ([ x_trg_g , y_trg_g , z _ t r g _ g ]) v _ p l = M a t r i x ([ a , b , c ]) . vec () d = - v_pl . dot ( p o i n t _ o n _ p l a n e _ t a r g e t _ g l o b a l )

31 32

p_proj = point_source_global - ( point_source_global_1 . dot ([[ a , b , c , d ]]) ) * v_pl

33 34 35

t a r g e t _ v a l u e = M a t r i x ([0 ,0 ,0]) . vec () model_function = point_source_global - p_proj Listing 8.6 Python code for generating point to projection onto plane observation equations and r

the Jacobian J of the objective function for Tait-Bryan rotation matrix parameterization

8.2 Point to Feature Metrics 8.2.1 Point to Line   Line l can be represented using Rln , t ln matrix (Fig. 8.9), where   Rln = V x , V y , V z

(8.49)

⎡

and t ln

⎤ x t,g = P tg = ⎣ y t,g ⎦ z t,g

(8.50)

as an unit vector V z and point on line t ln =(x t,g , y t,g , z t,g ) . Therefore, we use orthogonal vectors V x , V y and target point on line P tg = (x t,g , y t,g , z t,g ) to build the observation equation of the source point P sg = (x s,g , y s,g , z s,g ) (Fig. 8.9). Points P sg , P tg are expressed in global reference system. Point P sg lies on the line only if: !

" ! " V x 0 s t [ P g − P g] = , P sg = P tg V y 0

(8.51)

Assuming transformation matrix [R, t], P ls = (x s,l , y s,l , z s,l , 1) and P sg = [R, t] P ls following constraint is obtained:

(8.52)

8.2 Point to Feature Metrics

197

" ! "  V x  0 s t [R, t] P = − P l g V y 0 (8.53) To build the point to line observation equation incorporating the transformation [R, t] of corresponding source point P ls , target point on line P tg and orthogonal vectors V x , V y the following optimization problem is considered: !

t,g

t,g

t,g

 [R,t] (R, t, xis,l , yis,l , z is,l , V x,i , V y,i , xi , yi , z i ) =

C  2  t,g t,g t,g s,l s,l s,l  min [0, 0] −  [R,t] (R, t, xi , yi , z i , V x,i , V y,i , xi , yi , z i ) R,t

i=1

(8.54) therefore, point to line observation equation is given in (8.55), ! " δx = δy 

! " 0 0 

target values

r esidual

"  V x  [R, t] P ls − P tg  Vy   

! −

(8.55)

model f unction

        [R, t] P ls − P tg where δx δ y are residuals, 0 0 are target values and V x V y is the model function. Having pose [R, t] expressed using Tait-Bryan angles paramr

eterization (ω, ϕ, κ) the Jacobian J of the objective function is given in Eq. (8.56).  d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 = J= d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 # ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ $ 

r

x

x

x

x

x

x

(8.56)

∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ

For pose [R, t] expressed using Rodrigues parameterization of rotation matrix r

(sx , s y , sz ) the Jacobian J of the objective function is given in Eq. (8.57).  d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 = J= d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 # ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ $ 

r

x

x

x

x

x

x

(8.57)

∂tx ∂t y ∂tz ∂sx ∂s y ∂sz ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂tx ∂t y ∂tz ∂sx ∂s y ∂sz

For pose [R, t] expressed using quaternion parameterization of rotation matrix r

(q0 , q1 , q2 , q3 ) the Jacobian J of the objective function is given in Eq. (8.58). r

J=

 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 # ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ $



x

x

x

x

x

x

x

∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3 ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3

(8.58)

198

8 LiDAR Metrics

The python code for generating point to line observation equation is given in Listing 8.7. 1 2 3 4 5 6

from sympy i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14 15

xsl , ysl , zsl = s y m b o l s ( ’ xsl ysl zsl ’ ) xtg , ytg , ztg = s y m b o l s ( ’ xtg ytg ztg ’ ) tx , ty , tz = s y m b o l s ( ’ tx ty tz ’ ) om , fi , ka = s y m b o l s ( ’ om fi ka ’ ) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’) vxx , vxy , vxz = s y m b o l s ( ’ vxx vxy vxz ’ ) vyx , vyy , vyz = s y m b o l s ( ’ vyx vyy vyz ’ )

16 17 18 19 20 21

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] all_symbols = position_symbols + orientation_symbols

22 23 24

25

26

27 28

p o i n t _ s o u r c e _ l o c a l = M a t r i x ([ xsl , ysl , zsl , 1 ] ) . vec () Rt_wc = m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) [: -1 ,:] # Rt_wc = m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) [: -1 ,:] # Rt_wc = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) [: -1 ,:] p o i n t _ s o u r c e _ g l o b a l = Rt_wc * p o i n t _ s o u r c e _ l o c a l p o i n t _ o n _ l i n e _ t a r g e t _ g l o b a l = M a t r i x ([ xtg , ytg , ztg ])

29 30 31

vxt = M a t r i x ([[ vxx , vxy , vxz ]]) . t r a n s p o s e () vyt = M a t r i x ([[ vyx , vyy , vyz ]]) . t r a n s p o s e ()

32 33 34

35 36

t a r g e t _ v a l u e = M a t r i x ([0 ,0]) . vec () m o d e l _ f u n c t i o n = M a t r i x ([ vxt . dot ( p o i n t _ s o u r c e _ g l o b a l p o i n t _ o n _ l i n e _ t a r g e t _ g l o b a l ) , vyt . dot ( p o i n t _ s o u r c e _ g l o b a l - p o i n t _ o n _ l i n e _ t a r g e t _ g l o b a l ) ]) . vec () delta = t a r g e t _ v a l u e - m o d e l _ f u n c t i o n d e l t a _ j a c o b i a n = delta . j a c o b i a n ( a l l _ s y m b o l s ) r

Listing 8.7 Python code for generating point to line observation equations and the Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or Quaternion) rotation matrix parameterization

8.2 Point to Feature Metrics

199

Fig. 8.9 Point to line. Line represented  ln ln  using R ,t =   V x , V y , V z , t as an unit vector V z and point on line t

8.2.2 Point to Plane   Plane pl can be represented using R pl , t pl matrix (Fig. 8.10), where   R pl = V x , V y , V z ⎤ x t,g = P tg = ⎣ y t,g ⎦ z t,g

(8.59)

⎡

and t pl

(8.60)

as an unit vector V z orthogonal to plane and point on plane P tg = (x t,g , y t,g , z t,g ) . Therefore, we use this vector V z and target point on plane P tg to build the observation equation incorporating the source point P sg = (x s,g , y s,g , z s,g ) . Points P sg , P tg are expressed in global reference system. Point P sg lies on the plane only if: 

 V z [ P sg − P tg ] = 0, P sg = P tg

(8.61)

Assuming transformation matrix [R, t], P ls = (x s,l , y s,l , z s,l , 1) and P sg = [R, t] P ls

(8.62)

following constraint is obtained:     [R,t] (R, t, x s,l , y s,l , z s,l , V z , x t,g , y t,g , z t,g ) = V z [R, t] P ls − P tg = 0 (8.63) To build the point to plane observation equation incorporating the transformation [R, t] of corresponding source point P ls , target point on plane P tg and unit vector V z the following optimization problem is considered:

200

8 LiDAR Metrics

min R,t

C  

t,g

t,g

t,g

0 −  [R,t] (R, t, xis,l , yis,l , z is,l , V z,i , xi , yi , z i )

2 (8.64)

i=1

therefore, point to plane observation equation is given in (8.65), δz =  r esidual

   − V z [R, t] P ls − P tg    target value 0 

(8.65)

model f unction

   where δz is residual, 0 is target value and V z [R, t] P ls − P tg is the model function. For pose [R, t] expressed using Tait-Bryan angles parameterization (tx , t y , tz , r

ω, ϕ, κ) the Jacobian J of the objective function is given in Eq. (8.66). r  J = d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 =   ∂δz ∂δz ∂δz ∂δz ∂δz ∂δz ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ

(8.66)

For pose (tx , t y , tz , sx , s y , sz ) with Rodrigues parameterization of rotation matrix the r

Jacobian J of the objective function is given in Eq. (8.67). r  J = d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 =   ∂δz ∂δz ∂δz ∂δz ∂δz ∂δz ∂tx ∂t y ∂tz ∂sx ∂s y ∂sz

(8.67)

For pose (tx , t y , tz , q0 , q1 , q2 , q3 ) with quaternion parameterization of rotation matrix r

the Jacobian J of the objective function is given in Eq. (8.68). r  J = d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 =   ∂δz ∂δz ∂δz ∂δz ∂δz ∂δz ∂δz ∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3

(8.68)

The Python code for generating point to plane observation equation is given in Listing 8.8. 1 2 3 4 5 6

from s y m p y i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11

xsl , ysl , zsl = s y m b o l s ( ’ xsl ysl zsl ’ ) xtg , ytg , ztg = s y m b o l s ( ’ xtg ytg ztg ’ ) tx , ty , tz = s y m b o l s ( ’ tx ty tz ’) om , fi , ka = s y m b o l s ( ’ om fi ka ’)

8.2 Point to Feature Metrics 12 13 14

201

# sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’) vzx , vzy , vzz = s y m b o l s ( ’ vzx vzy vzz ’ )

15 16 17 18 19 20

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] all_symbols = position_symbols + orientation_symbols

21 22 23

24

25

26 27

p o i n t _ s o u r c e _ l o c a l = M a t r i x ([ xsl , ysl , zsl , 1 ] ) . vec () R t _ w c = m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) [: -1 ,:] # R t _ w c = m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) [: -1 ,:] # R t _ w c = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) [: -1 ,:] point_source_global = Rt_wc * point_source_local p o i n t _ o n _ p l a n e _ t a r g e t _ g l o b a l = M a t r i x ([ xtg , ytg , ztg ])

28 29

vzt = M a t r i x ([[ vzx , vzy , vzz ]]) . t r a n s p o s e ()

30 31 32

33 34

t a r g e t _ v a l u e = M a t r i x ( [ 0 ] ) . vec () m o d e l _ f u n c t i o n = M a t r i x ([ vzt . dot ( p o i n t _ s o u r c e _ g l o b a l p o i n t _ o n _ p l a n e _ t a r g e t _ g l o b a l ) ]) . vec () delta = target_value - model_function delta_jacobian = delta . jacobian ( all_symbols ) r

Listing 8.8 Python code for generating point to plane observation equation and the Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or quaternion) rotation matrix parameterization

Fig. 8.10 Visualization of the point to plane observation equation. Plane using is represented  R pl , t pl =   V x , V y , V z , t as an unit vector V z and point on plane t

202

8 LiDAR Metrics

8.2.3 Distance Point to Plane Distance D of point P sr c,l (x sr c,l , y sr c,l , z sr c,l ) that can be transformed to global coordinate system as point P sr c,g (x sr c,g , y sr c,g , z sr c,g ) using matrix [R, t] to plane parameterized with a, b, c, d is expressed in Eq. (8.69). ⎤ ⎛ ⎡ sr c,l ⎤⎞ x x sr c,g ⎢ y sr c,l ⎥⎟ ⎜   ⎢ y sr c,g ⎥  ⎥ ⎜ ⎥⎟ ⎢ D= abcd ⎢ ⎣ z sr c,g ⎦ = a b c d ⎝[R, t] ⎣ z sr c,l ⎦⎠ 1 1 ⎡

(8.69)

The distance point to plane observation equation is given as (8.70). ⎤⎞ x sr c,l ⎢ y sr c,l ⎥⎟  ⎜ ⎥⎟ ⎢ δ D =  0 − abcd ⎜ ⎝[R, t] ⎣ z sr c,l ⎦⎠  target value r esidual 1    ⎛

⎡

(8.70)

model f unction sr c,l value and , y sr c,l , z sr c,l , a, b, c, d) where   [R,t] (R, t, x  δ D isresidual,   0srisc,ltarget sr c,l sr c,l is the model function. This observation y z 1 = a b c d [R, t] x equation contributes as the following optimization problem (8.71).

min R,t

C  2  0 −  [R,t] (R, t, xisr c,l , yisr c,l , z isr c,l , ai , bi , ci , di )

(8.71)

i=1

where there are C point to plane correspondences. For pose [R, t] expressed using r

Tait-Bryan angles parameterization (tx , t y , tz , ω, ϕ, κ) the Jacobian J of the objective function is given in Eq. (8.72). r  J = d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 =   ∂δ D ∂δ D ∂δ D ∂δ D ∂δ D ∂δ D ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ

(8.72)

For pose (tx , t y , tz , sx , s y , sz ) with Rodrigues parameterization of rotation matrix the r

Jacobian J of the objective function is given in Eq. (8.73). r  J = d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 =   ∂δ D ∂δ D ∂δ D ∂δ D ∂δ D ∂δ D ∂tx ∂t y ∂tz ∂sx ∂s y ∂sz

(8.73)

8.2 Point to Feature Metrics

203

For pose (tx , t y , tz , q0 , q1 , q2 , q3 ) with quaternion parameterization of rotation matrix r

the Jacobian J of the objective function is given in Eq. (8.74). r  J = d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 =   ∂δ D ∂δ D ∂δ D ∂δ D ∂δ D ∂δ D ∂δ D ∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3

(8.74)

The Python code for generating distance point to plane observation equation is given in Listing 8.9. 1 2 3 4 5 6

from s y m p y i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13

x , y , z = s y m b o l s ( ’ x y z ’) a , b , c , d = s y m b o l s ( ’a b c d ’ ) tx , ty , tz = s y m b o l s ( ’ tx ty tz ’) om , fi , ka = s y m b o l s ( ’ om fi ka ’) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’)

14 15 16 17 18 19

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] all_symbols = position_symbols + orientation_symbols

20 21 22

23

24

25

p o i n t _ s o u r c e _ l o c a l = M a t r i x ([ x , y , z , 1]) . vec () R t _ w c = m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) [: -1 ,:] # R t _ w c = m a t r i x 4 4 F r o m R o d r i g u e s ( tx , ty , tz , sx , sy , sz ) [: -1 ,:] # R t _ w c = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx , ty , tz , q0 , q1 , q2 , q3 ) [: -1 ,:] point_source_global = Rt_wc * point_source_local

26 27 28

29 30

t a r g e t _ v a l u e = M a t r i x ( [ 0 ] ) . vec () m o d e l _ f u n c t i o n = M a t r i x ([ a * p o i n t _ s o u r c e _ g l o b a l [0] + b * p o i n t _ s o u r c e _ g l o b a l [1] + c * p o i n t _ s o u r c e _ g l o b a l [2] + d ]) . vec () delta = target_value - model_function delta_jacobian = delta . jacobian ( all_symbols ) Listing 8.9 Python code for generating distance point to plane observation equation and the r

Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or quaternion) rotation matrix parameterization

204

8 LiDAR Metrics

8.2.4 Point to Surface Point to surface scan registration technique [26] is called originally least squares 3D surface matching. Two point clouds are represented as template surface f (x, y, z) and search surface g(x, y, z). The advantage of the method is that we register surface to surface that can be modeled as local planar or bi-linear forms makes it more accurate than methods based on point cloud representation. Adding true error vector e(x, y, z) results following observation Eq. (8.75). f (x, y, z) − e(x, y, z) = g(x, y, z)

(8.75)

To express the geometric relationship between the conjugate surface patches, a 7parameter 3D similarity transformation is used as in Eq. (8.76) ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ x0 tx x ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ y ⎦ = ⎣t y ⎦ + m i Ri ⎣ y0 ⎦ tz i z0 i z i

(8.76)

where 7 parameters are m for scale factor, tx , t y , tz for translation and R expressed using Tait-Bryan (ω, ϕ, κ) angles’ convention given by Eq. (8.77). 

R=

 r11 r12 r13  r21 r22 r23 r31 r32 r33

cos(ϕ) cos(κ) − cos(ϕ) sin(κ) sin(ϕ) cos(ω) sin(κ)+sin(ω) sin(ϕ) cos(κ) cos(ω) cos(κ)−sin(ω) sin(ϕ) sin(κ) − sin(ω) cos(ϕ) sin(ω) sin(κ)−cos(ω) sin(ϕ) cos(κ) sin(ω) cos(κ)+cos(ω) sin(ϕ) sin(κ) cos(ω) cos(ϕ)

= 

(8.77)

After linearization of Eq. (8.75) with first order Taylor expansion we obtain form as (8.78) f (x, y, z) − e(x, y, z) = g 0 (x, y, z) +

∂g 0 (x, y, z) ∂g 0 (x, y, z) ∂g 0 (x, y, z) dx + dy + dz ∂x ∂y ∂z

(8.78)

where we use following notation: ∂g 0 (x, y, z) = gx ∂x ∂g 0 (x, y, z) = gy ∂y ∂g 0 (x, y, z) = gz ∂z The differentiation of (8.76) gives (8.80)

(8.79)

8.2 Point to Feature Metrics

205

d x = dtx + a10 dm + a11 dω + a12 dϕ + a13 dκ dy = dt y + a20 dm + a21 dω + a22 dϕ + a23 dκ

(8.80)

dz = dtz + a30 dm + a31 dω + a32 dϕ + a33 dκ where ai j are given in Eq. (8.81). a10 = r11 x0 + r12 y0 + r13 z 0 a20 = r21 x0 + r22 y0 + r23 z 0 a30 = r31 x0 + r32 y0 + r33 z 0 a11 = 0 a12 = m(− sin(ϕ) cos(κ)x0 + sin(ϕ) sin(κ)y0 + cos(ϕ)z 0 ) a13 = m(r12 x0 − r11 y0 ) a21 = m(−r31 x0 − r32 y0 − r33 z 0 ) a22 = m(sin(ω) cos(ϕ) cos(κ)x0 − sin(ω) cos(ϕ) sin(κ)y0 + sin(ω) sin(ϕ)z 0 ) a23 = m(r22 x0 − r21 y0 ) a31 = m(r21 x0 + r22 y0 + r23 z 0 ) a32 = m(− cos(ω) cos(ϕ) cos(κ)x0 + cos(ω) cos(ϕ) sin(κ)y0 − cos(ω) sin(ϕ)z 0 ) a33 = m(r32 x0 − r31 y0 ) (8.81) Thus, finally we obtain (8.82). −ei (x, y, z) = gx dtx + g y dt y + gz dtz + (gx a10 + g y a20 + gz a30 )dm+ (gx a11 + g y a21 + gz a31 )dω+ (gx a12 + g y a22 + gz a32 )dϕ+

(8.82)

(gx a13 + g y a23 + gz a33 )dκ− ( f (x, y, z) − g 0 (x, y, z)) Equation (8.82) can be expressed in matrix notation (8.83) − e = Ax − l

(8.83)

where A is design matrix, x is parameter vector β = [tx , t y , tz , m, ω, ϕ, κ] and l is constant vector that consists of the Euclidean distances between template and correspondent search surface points. This system can be solved using Gauss-Markoff estimation model. More information about the method and its implementation can be found in [27].

206

8 LiDAR Metrics

8.3 Feature to Feature Metrics 8.3.1 Line to Line Line l can be represented as a direction d and a point P that passes through this line (Fig. 7.4). Line in Plücker coordinate’s system is defined as a pair (m, d)), where m=p×d

(8.84)

is a moment vector. We choose d to be an unit vector, thus d = 1

(8.85)

d·m=0

(8.86)

and

Therefore, six coordinates of the Plücker line is: ⎡ ⎤ mx ⎢m y ⎥ ⎢ ⎥ ⎢m z ⎥ ⎢ ⎥ ⎢ dx ⎥ ⎢ ⎥ ⎣ dy ⎦ dz

(8.87)

To transform Plücker line via [R, t] following 6x6 matrix is used ⎡ r11 ⎢r21 ⎢ ⎢r31 ⎢ ⎢0 ⎢ ⎣0 0 where

r12 r22 r32 0 0 0

⎤ r13 −tz r21 + t y r31 −tz r22 + t y r32 −tz r23 + t y r33 r23 tz r11 − tx r31 tz r12 − tx r32 tz r13 − tx r33 ⎥ ⎥ r33 −t y r11 + tx r21 −t y r12 + tx r22 −t y r13 + tx r23 ⎥ ⎥ ⎥ 0 r11 r12 r13 ⎥ ⎦ 0 r21 r22 r23 0 r31 r32 r33 ⎡ ⎤ r11 r12 r13 R = ⎣r21 r22 r23 ⎦ r31 r32 r33

(8.88)

(8.89)

8.3 Feature to Feature Metrics

207

⎡ ⎤ tx t = ⎣t y ⎦ tz

and

(8.90)

It can be seen that 6x6 Plücker line transformation matrix can be expressed as: !

where

R [t]× R 03×3 R

" (8.91)

⎡

⎤ 0 −tz t y [t]× = ⎣ tz 0 −tx ⎦ −t y tx 0

(8.92)

Finally, the transformation of Plücker line (m, d) into (m , d  ) can be expressed with following equation: ⎡ ⎤ ⎤ mx mx ⎢m y ⎥ ⎢m y ⎥ ⎢ ⎥ ⎢ ⎥ " ! ⎢m z ⎥ R [t]× R ⎢ mz ⎥ ⎢ ⎥ ⎥ ⎢  [R,t] (R, t, m, d) = ⎢ ⎥ = 3×3 ⎢ dx ⎥ R 0 ⎢ ⎥ ⎢ dx ⎥ ⎣ dy ⎦ ⎣ dy ⎦ dz dz ⎡

(8.93)

To build the observation equation incorporating the transformations of corresponding Plücker lines the following optimization problem is considered: ⎛⎡ ⎤ ⎞2 0 ⎜⎢0⎥ ⎟ ⎢ ⎥ C ⎜ ⎟  ⎜⎢0⎥   ⎟ 1 1 2 2 ⎜⎢ ⎥ −  1 1  (R , t , m i,1 , di,1 ) −   2 2  (R , t , m i,2 , di,2 ) ⎟ min R ,t R ,t ⎜⎢0⎥ ⎟ 1 2 1 2 R ,t ,R ,t i=1 ⎜⎢ ⎥ ⎟ ⎝⎣0⎦ ⎠ 0

(8.94) where there are C correspondences between Plücker lines belonging to different poses. Each correspondence is established by two instances of the same Plücker line observed from different poses. The Plücker line to Plücker line observation equation is formulated as (8.95),

208

8 LiDAR Metrics

⎡

⎤ m δ,x ⎢m δ,y ⎥ ⎢ ⎥ ⎢ m δ,z ⎥ ⎢ ⎥ ⎢ dδ,x ⎥ = ⎢ ⎥ ⎣ dδ,y ⎦ dδ,z    r esiduals

⎡ ⎤ 0 ⎢0⎥ ⎢ ⎥ ⎢0⎥ ⎢ ⎥ ⎢0⎥ ⎢ ⎥ ⎣0⎦ 0 

⎛

⎡

m 1x

⎤

⎡

m 2x

⎤⎞

⎜ ⎢m 1 ⎥ ⎢m 2 ⎥⎟ ⎜ ⎢ y⎥ ⎢ y ⎥⎟   ⎜! 1  1  ⎥ ⎢ ⎢ ⎥⎟ ! " " 1 ⎢ 1⎥ 2 2 ⎢ 2 ⎥⎟ 2 ⎜ R t R R R t m z × × ⎜ ⎥ ⎢ ⎢m z ⎥⎟ − ⎜ 3×3 1 2 ⎢ 1 ⎥ − 03×3 ⎢ 2 ⎥⎟ 0 R R ⎜ ⎢ dx ⎥ ⎢ dx ⎥⎟ ⎜ ⎢ 1⎥ ⎢ 2 ⎥⎟ ⎝ ⎣ dy ⎦ ⎣ d y ⎦⎠ 1 dz dz2    target values model f unction

(8.95)     0 0 0 0 0 0 are target values where mδ,x m δ,y m δ,z dδ,x dδ,y dδ,z are residuals,   !

and

" " ! 2 2 R1 t 1 × R1  1 1 1 1 1 1  R t × R2  2 2 2 2 2 2  m x m y m z dx d y dz − 3×3 m x m y m z d x d y dz 3×3 1 2 0 R 0 R

is the model

function. For two poses [R , t ], [R , t ] expressed using Tait-Bryan angles represen1

1

2

2

r

tation (tx1 , t y1 , tz1 , ω1 , ϕ 1 , κ 1 ), (tx2 , t y2 , tz2 , ω2 , ϕ 2 , κ 2 ) the Jacobian J of the objective function for Plücker line transformation is given in Eq. (8.96).

⎛

⎛

0 ⎜d2,1 ⎜ r ⎜d3,1 J =⎜ ⎜ 0 ⎜ ⎝ 0 0 0

⎜ ∂m δ,y ⎜ ∂t 1 ⎜ x ⎜ ∂m δ,z ⎜ ∂tx1 ⎜ ⎜ 0 ⎜ ⎜ ⎝ 0 0

d1,2 0 d3,2 0 0 0

d1,3 d2,3 0 0 0 0

d1,4 d2,4 d3,4 0 d5,4 d6,4

d1,5 d2,5 d3,5 d4,5 d5,5 d6,5

d1,6 d2,6 d3,6 d4,6 d5,6 d6,6

0 d2,7 d3,7 0 0 0

d1,8 0 d3,8 0 0 0

d1,9 d2,9 0 0 0 0

d1,10 d2,10 d3,10 0 d5,10 d6,10

⎞ d1,12 d2,12 ⎟ ⎟ d3,12 ⎟ ⎟= d4,12 ⎟ ⎟ d5,12 ⎠ d6,12 ∂m δ,x ∂m δ,x ⎞

d1,11 d2,11 d3,11 d4,11 d5,11 d6,11

∂m δ,x ∂m δ,x ∂m δ,x ∂m δ,x ∂m δ,x δ,x ∂m δ,x ∂m δ,x 0 ∂m ∂t y1 ∂tz1 ∂ω1 ∂ϕ 1 ∂κ 1 ∂t y2 ∂tz2 ∂ω2 ∂ϕ 2 ∂κ 2 ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ⎟ 0 ∂t 1 ∂ω1 ∂ϕ 1 ∂κ 1 ∂t 2 0 ∂t 2 ∂ω2 ∂ϕ 2 ∂κ 2 ⎟ ⎟ z x z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ⎟ 0 ∂ω1 ∂ϕ 1 ∂κ 1 ∂t 2 ∂t 2 0 ∂ω2 ∂ϕ 2 ∂κ 2 ⎟ ∂t y1 x y ⎟ ∂dδ,x ∂dδ,x ∂dδ,x ⎟ δ,x 0 0 0 ∂ϕ 1 ∂κ 1 0 0 0 0 ∂d ∂ϕ 2 ∂κ 2 ⎟ ∂d ∂d ∂d ∂d ∂d ∂d ⎟ 0 0 ∂ωδ,y1 ∂ϕδ,y1 ∂κδ,y1 0 0 0 ∂ωδ,y2 ∂ϕδ,y2 ∂κδ,y2 ⎠ ∂dδ,z ∂dδ,z ∂dδ,z ∂dδ,z δ,z δ,z 0 0 ∂d 0 0 0 ∂d ∂ω1 ∂ϕ 1 ∂κ 1 ∂ω2 ∂ϕ 2 ∂κ 2

(8.96) For two poses [R1 , t 1 ], [R2 , t 2 ] expressed using Rodrigues parameterization of r

the rotation matrix (tx1 , t y1 , tz1 , sx1 , s y1 , sz1 ), (tx2 , t y2 , tz2 , sx2 , s y2 , sz2 ) the Jacobian J of the objective function for Plücker line transformation is given in Eq. (8.97).

8.3 Feature to Feature Metrics

⎛

⎛

0 ⎜d2,1 ⎜ r ⎜d3,1 J =⎜ ⎜ 0 ⎜ ⎝ 0 0 0

⎜ ∂m δ,y ⎜ 1 ⎜ ∂tx ⎜ ∂m δ,z ⎜ ∂tx1 ⎜ ⎜ 0 ⎜ ⎜ ⎜ 0 ⎝ 0

d1,2 0 d3,2 0 0 0

d1,3 d2,3 0 0 0 0

209

d1,4 d2,4 d3,4 d4,4 d5,4 d6,4

d1,5 d2,5 d3,5 d4,5 d5,5 d6,5

d1,6 d2,6 d3,6 d4,6 d5,6 d6,6

0 d2,7 d3,7 0 0 0

d1,8 0 d3,8 0 0 0

d1,9 d2,9 0 0 0 0

d1,10 d2,10 d3,10 d4,10 d5,10 d6,10

⎞ d1,12 d2,12 ⎟ ⎟ d3,12 ⎟ ⎟= d4,12 ⎟ ⎟ d5,12 ⎠ d6,12 ⎞ ∂m δ,x ∂m δ,x

d1,11 d2,11 d3,11 d4,11 d5,11 d6,11

∂m δ,x ∂m δ,x ∂m δ,x ∂m δ,x ∂m δ,x δ,x ∂m δ,x ∂m δ,x 0 ∂m ∂t y1 ∂tz1 ∂sx1 ∂s y1 ∂sz1 ∂t y2 ∂tz2 ∂sx2 ∂s y2 ∂sz2 ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ⎟ 0 ∂t 1 ∂s 1 ∂s 1 ∂s 1 ∂t 2 0 ∂t 2 ∂s 2 ∂s 2 ∂s 2 ⎟ z x y z x z x y z ⎟ ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ⎟ 0 ∂s 1 ∂s 1 ∂s 1 ∂t 2 ∂t 2 0 ∂s 2 ∂s 2 ∂s 2 ⎟ ∂t y1 x y z x y x y z ⎟ ∂dδ,x ∂dδ,x ∂dδ,x 0 0 ∂s 1 ∂s 1 ∂s 1 0 0 0 ∂d∂sδ,x2 ∂d∂sδ,x2 ∂d∂sδ,x2 ⎟ ⎟ x y z x y z ∂dδ,y ∂dδ,y ∂dδ,y ∂dδ,y ∂dδ,y ∂dδ,y ⎟ 0 0 ∂s 1 ∂s 1 ∂s 1 0 0 0 ∂s 2 ∂s 2 ∂s 2 ⎟ x y z x y z ⎠ ∂dδ,z ∂dδ,z ∂dδ,z ∂dδ,z ∂dδ,z ∂dδ,z 0 0 ∂s 1 ∂s 1 ∂s 1 0 0 0 ∂s 2 ∂s 2 ∂s 2 x y z x y z

(8.97) For two poses [R1 , t 1 ],[R2 , t 2 ] expressed using quaternion parametrization of the r

rotation matrix (tx1 , t y1 , tz1 , q01 , q11 , q21 , q31 ), (tx2 , t y2 , tz2 , q02 , q12 , q22 , q32 ) the Jacobian J of the objective function for Plücker line transformation is given in Eq. (8.98).

⎛

⎛

0 ⎜d2,1 ⎜ r ⎜d3,1 J =⎜ ⎜ 0 ⎜ ⎝ 0 0

0 ⎜ ∂m δ,y ⎜ 1 ⎜ ∂tx ⎜ ∂m δ,z ⎜ ∂t 1 ⎜ x ⎜ 0 ⎜ ⎜ ⎜ 0 ⎝ 0

d1,2 0 d3,2 0 0 0

d1,3 d2,3 0 0 0 0

d1,4 d2,4 d3,4 d4,4 d5,4 d6,4

d1,5 d2,5 d3,5 d4,5 d5,5 d6,5

d1,6 d2,6 d3,6 d4,6 d5,6 d6,6

d1,7 d2,7 d3,7 d4,7 d5,7 d6,7

0 d2,8 d3,8 0 0 0

d1,9 d1,10 d1,11 0 d2,10 d2,11 d3,9 0 d3,11 0 0 d4,11 0 0 d5,11 0 0 d6,11

d1,12 d2,12 d3,12 d4,12 d5,12 d6,12

⎞ d1,14 d2,14 ⎟ ⎟ d3,14 ⎟ ⎟= d4,14 ⎟ ⎟ d5,14 ⎠ d6,14 ⎞ ∂m ∂m

d1,13 d2,13 d3,13 d4,13 d5,13 d6,13

∂m δ,x ∂m δ,x ∂m δ,x ∂m δ,x ∂m δ,x ∂m δ,x δ,x ∂m δ,x ∂m δ,x ∂m δ,x δ,x δ,x 0 ∂m ∂t y1 ∂tz1 ∂t y2 ∂tz2 ∂q01 ∂q11 ∂q21 ∂q31 ∂q02 ∂q12 ∂q22 ∂q32 ∂m ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ∂m ∂m δ,y ∂m δ,y ∂m δ,y ∂m δ,y ⎟ ⎟ 0 ∂tδ,y 0 ∂tδ,y 1 1 1 1 1 2 2 ∂t ∂q ∂q ∂q ∂q ∂q02 ∂q12 ∂q22 ∂q32 ⎟ z x z 0 1 2 3 ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ∂m δ,z ⎟ 0 ∂q 1 ∂q 1 ∂q 1 ∂q 1 ∂t 2 ∂t 2 0 ∂q 2 ∂q 2 ∂q 2 ∂q 3 ⎟ ∂t y1 x y 0 1 2 3 0 1 2 2 ⎟ ∂dδ,x ∂dδ,x ∂dδ,x ∂dδ,x ∂dδ,x ∂dδ,x ∂dδ,x ⎟ δ,x 0 0 ∂q 1 ∂q 1 ∂q 1 ∂q 1 0 0 0 ∂d 2 2 2 2 ⎟ ∂q ∂q ∂q ∂q 0 1 2 3 0 1 2 3 ∂dδ,y ∂dδ,y ∂dδ,y ∂dδ,y ∂dδ,y ∂dδ,y ∂dδ,y ∂dδ,y ⎟ 0 0 ∂q 1 ∂q 1 ∂q 1 ∂q 1 0 0 0 ∂q 2 ∂q 2 ∂q 2 ∂q 2 ⎟ 0 1 2 3 0 1 2 3 ⎠ ∂dδ,z ∂dδ,z ∂dδ,z ∂dδ,z ∂dδ,z ∂dδ,z ∂dδ,z δ,z 0 0 ∂q 1 ∂q 1 ∂q 1 ∂q 1 0 0 0 ∂d 2 2 2 ∂q0 ∂q1 ∂q2 ∂q32 0 1 2 3

(8.98)

1 2 3 4 5 6 7

from s y m p y i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t * from p l u c k e r _ l i n e _ u t i l s i m p o r t *

210

8 LiDAR Metrics

8 9

10 11 12 13

14

15 16 17 18

mx_1 , my_1 , mz_1 , lx_1 , ly_1 , lz_1 = s y m b o l s ( ’ mx_1 my_1 mz_1 lx_1 ly_1 lz_1 ’ ) tx_1 , ty_1 , tz_1 = s y m b o l s ( ’ tx_1 ty_1 tz_1 ’ ) om_1 , fi_1 , ka_1 = s y m b o l s ( ’ om_1 fi_1 ka_1 ’ ) # sx_1 , sy_1 , sz_1 = s y m b o l s ( ’ sx_1 sy_1 sz_1 ’) # q0_1 , q1_1 , q2_1 , q3_1 = s y m b o l s ( ’ q0_1 q1_1 q2_1 q3_1 ’) mx_2 , my_2 , mz_2 , lx_2 , ly_2 , lz_2 = s y m b o l s ( ’ mx_2 my_2 mz_2 lx_2 ly_2 lz_2 ’ ) tx_2 , ty_2 , tz_2 = s y m b o l s ( ’ tx_2 ty_2 tz_2 ’ ) om_2 , fi_2 , ka_2 = s y m b o l s ( ’ om_2 fi_2 ka_2 ’ ) # sx_2 , sy_2 , sz_2 = s y m b o l s ( ’ sx_2 sy_2 sz_2 ’) # q0_2 , q1_2 , q2_2 , q3_2 = s y m b o l s ( ’ q0_2 q1_2 q2_2 q3_2 ’)

19 20 21 22 23 24 25 26 27 28

p o s i t i o n _ s y m b o l s _ 1 = [ tx_1 , ty_1 , tz_1 ] o r i e n t a t i o n _ s y m b o l s _ 1 = [ om_1 , fi_1 , ka_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ sx_1 , sy_1 , sz_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ q0_1 , q1_1 , q2_1 , q3_1 ] p o s i t i o n _ s y m b o l s _ 2 = [ tx_2 , ty_2 , tz_2 ] o r i e n t a t i o n _ s y m b o l s _ 2 = [ om_2 , fi_2 , ka_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ sx_2 , sy_2 , sz_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ q0_2 , q1_2 , q2_2 , q3_2 ] all_symbols = position_symbols_1 + orientation_symbols_1 + position_symbols_2 + orientation_symbols_2

29 30

31

32

33

34

35

36

37

R t _ w c _ 1 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_1 , ty_1 , tz_1 , om_1 , fi_1 , ka_1 ) # R t _ w c _ 1 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_1 , ty_1 , tz_1 , sx_1 , sy_1 , sz_1 ) # R t _ w c _ 1 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_1 , ty_1 , tz_1 , q0_1 , q1_1 , q2_1 , q3_1 ) R t _ w c _ 2 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_2 , ty_2 , tz_2 , om_2 , fi_2 , ka_2 ) # R t _ w c _ 2 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_2 , ty_2 , tz_2 , sx_2 , sy_2 , sz_2 ) # R t _ w c _ 2 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_2 , ty_2 , tz_2 , q0_2 , q1_2 , q2_2 , q3_2 ) plucker_line_motion_matrix_1 = plucker_line_motion_matrix_wc ( Rt_wc_1 ) plucker_line_motion_matrix_2 = plucker_line_motion_matrix_wc ( Rt_wc_2 )

38 39

40

p l u c k e r _ l i n e _ l o c a l _ 1 = M a t r i x ([ mx_1 , my_1 , mz_1 , lx_1 , ly_1 , lz_1 ]) . vec () p l u c k e r _ l i n e _ l o c a l _ 2 = M a t r i x ([ mx_2 , my_2 , mz_2 , lx_2 , ly_2 , lz_2 ]) . vec ()

41 42

plucker_line_global_1 = plucker_line_motion_matrix_1 * plucker_line_local_1

8.3 Feature to Feature Metrics 43

211

plucker_line_global_2 = plucker_line_motion_matrix_2 * plucker_line_local_2

44 45 46

47 48

t a r g e t _ v a l u e = M a t r i x ([0 ,0 ,0 ,0 ,0 ,0]) . vec () model_function = plucker_line_global_1 plucker_line_global_2 delta = target_value - model_function delta_jacobian = delta . jacobian ( all_symbols ) Listing 8.10 Python code for generating Plücker line to Plücker line observation equation and the r

Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or Quaternion) rotation matrix parameterization

8.3.2 Plane to Plane Plane to plane data registration [28] is not so common in mobile robotics for SLAM purpose with some extends as in [29]. It corresponds to automatic plane extraction from LiDAR data [30]. Plane is represented by four parameters:

where

ax + by + cz + d = 0

(8.99)

   abc =1

(8.100)

(a, b, c) is the unit vector orthogonal to plane, and d is the distance from the origin to the plane. It satisfies following condition with point in 3D space ⎡ ⎤ x   ⎢y⎥ ⎥ abcd ⎢ ⎣z ⎦ = 0 1

(8.101)

To find plane transformation matrix we start with: ⎡ ⎤ x      ⎢y⎥ ⎥ a b c d ⎢ ⎣ z ⎦ = 0 1 Therefore, we can build following equation:

(8.102)

212

8 LiDAR Metrics

⎡ ⎤ x ⎢y⎥   −1 ⎥ a b c d [R, t] [R, t] ⎢ ⎣z ⎦ = 0  %   & 1 a  b c d     ⎡ ⎤ x ⎢ ⎥ ⎢y ⎥ ⎢ ⎥ ⎢ ⎥ ⎢z ⎥ ⎣ ⎦ 1 ⎡ ⎤ ⎡ ⎤ x x ⎢y⎥ ⎢y⎥ ⎥ ⎢ ⎥ [R, t] ⎢ ⎣ z ⎦ = ⎣ z ⎦ 1 1

Due to

(8.103)

(8.104)

the plane transformation matrix is given:        a b c d = a b c d [R, t]−1

(8.105)

The inverse matrix [R, t]−1 can be computed with: −1

[R, t]

!

R −R t = 1×3 1 0

" (8.106)

where R is transposed matrix of R. Thus, the plane transformation equation is formulated: " !  R −R t        [R,t] (R, t, a, b, c, d) = a b c d = a b c d (8.107) 01×3 1 To build the observation equation incorporating the transformations of corresponding planes the following optimization problem is considered:

min

R1 ,t 1 ,R2 ,t 2

⎛⎡ ⎤ ⎞2 0   ⎟ ⎜⎢0⎥ ⎜⎢ ⎥ −  R1 ,t 1 (R1 , t 1 , ai,1 , bi,1 , ci,1 , di,1 ) −  R2 ,t 2 (R2 , t 2 , ai,2 , bi,2 , ci,2 , di,2 ) ⎟ [ ] ] [ ⎝⎣0⎦ ⎠ i=1 0

C 

(8.108) where there are C correspondences between planes belonging to different poses. Each correspondence is established by two instances of the same plane observed from different poses. The plane to plane observation equation is formulated as (8.109),

8.3 Feature to Feature Metrics

⎡ ⎤ aδ ⎢bδ ⎥ ⎢ ⎥ = ⎣cδ ⎦ dδ    r esiduals

⎡ ⎤ 0 ⎢0⎥ ⎢ ⎥ ⎣0⎦ 0 

target values

213

⎛ ⎡ 1 ⎤ ⎞ ⎡ 2 ⎤ a a ! ! " "     ⎜⎢b1 ⎥ R1 −R1 t 1 ⎢b2 ⎥ R2 −R2 t 2 ⎟ ⎟ ⎥ ⎢ ⎥ −⎜ −⎢ ⎝⎣ c1 ⎦ 01×3 ⎠ ⎣ c2 ⎦ 01×3 1 1 1 2 d d    model f unction

(8.109)      where aδ bδ cδ dδ are residuals, 0 0 0 0 are target values and a 1 b1 c1 d 1 " ! 1 !  "   R2  −R2  t 2 R −R1 t 1 − a 2 b2 c2 d 2 is the model function. For two 0 1 0 1 poses [R1 , t 1 ], [R2 , t 2 ] expressed using Tait-Bryan angles representation (tx1 , t y1 , tz1 , r

ω1 , ϕ 1 , κ 1 ), (tx2 , t y2 , tz2 , ω2 , ϕ 2 , κ 2 ) the Jacobian J of the objective function for plane to plane observation is given in Eq. (8.110).

⎛

0 r ⎜ 0 J =⎜ ⎝ 0 d4,1

0 0 0 d4,2

0 0 0 d2,4 0 d3,4 d4,3 d4,4 ⎛ 0 0 ⎜ ⎜0 0 ⎜ ⎜ ⎝0 0

d1,5 d2,5 d3,5 d4,5

d1,6 d2,6 d3,6 d4,6

0

0 ∂bδ ∂ω1 ∂cδ ∂ω1 ∂dδ ∂ω1

0 0

∂dδ ∂dδ ∂dδ ∂tx1 ∂t y1 ∂tz1

0 0 0 d4,7

0 0 0 d4,8

0 0 0 d2,10 0 d3,10 d4,9 d4,10

∂aδ ∂ϕ 1 ∂bδ ∂ϕ 1 ∂cδ ∂ϕ 1 ∂dδ ∂ϕ 1

∂aδ 0 0 0 ∂κ 1 ∂bδ 0 0 0 ∂κ 1 ∂cδ 0 0 0 ∂κ 1 ∂dδ ∂dδ ∂dδ ∂dδ ∂κ 1 ∂tx2 ∂t y2 ∂tz2

d1,11 d2,11 d3,11 d4,11 0 ∂bδ ∂ω2 ∂cδ ∂ω2 ∂dδ ∂ω2

⎞ d1,12 d2,12 ⎟ ⎟= d3,12 ⎠ d4,12 ∂aδ ∂aδ ⎞ ∂ϕ 2 ∂bδ ∂ϕ 2 ∂cδ ∂ϕ 2 ∂dδ ∂ϕ 2

∂κ 2 ∂bδ ⎟ ⎟ ∂κ 2 ⎟ ∂cδ ⎟ ∂κ 2 ⎠ ∂dδ ∂κ 2

(8.110) For two poses [R1 , T 1 ], [R2 , T 2 ] expressed using Rodrigues parameterization of the r

rotation matrix (Tx1 , Ty1 , Tz1 , sx1 , s y1 , sz1 ), (Tx2 , Ty2 , Tz2 , sx2 , s y2 , sz2 ) the Jacobian J of the objective function for plane to plane observation is given in Eq. (8.111).

⎛

0 r ⎜ 0 J =⎜ ⎝ 0 d4,1

0 0 0 d4,2

0 0 0 d4,3 ⎛

d1,4 d2,4 d3,4 d4,4 0

⎜ ⎜0 ⎜ ⎜0 ⎝

d1,5 d2,5 d3,5 d4,5 0

0

0

0

0

0

d1,6 d2,6 d3,6 d4,6

∂dδ ∂dδ ∂dδ ∂tx1 ∂t y1 ∂tz1

∂aδ ∂sx1 ∂bδ ∂sx1 ∂cδ ∂sx1 ∂dδ ∂sx1

0 0 0 d4,7 ∂aδ ∂s y1 ∂bδ ∂s y1 ∂cδ ∂s y1 ∂dδ ∂s y1

0 0 0 d4,8

0 0 0 d4,9

d1,10 d2,10 d3,10 d4,10

d1,11 d2,11 d3,11 d4,11

∂aδ 0 0 0 ∂sz1 ∂bδ 0 0 0 ∂sz1 ∂cδ 0 0 0 ∂sz1 ∂dδ ∂dδ ∂dδ ∂dδ ∂sz1 ∂tx2 ∂t y2 ∂tz2

∂aδ ∂sx2 ∂bδ ∂sx2 ∂cδ ∂sx2 ∂dδ ∂sx2

⎞ d1,12 d2,12 ⎟ ⎟= d3,12 ⎠ d4,12 ∂aδ ∂aδ ⎞ ∂s y2 ∂bδ ∂s y2 ∂cδ ∂s y2 ∂dδ ∂s y2

∂sz2 ∂bδ ⎟ ∂sz2 ⎟ ⎟ ∂cδ ⎟ ∂sz2 ⎠ ∂dδ ∂sz2

(8.111)

214

8 LiDAR Metrics

For two poses [R1 , t 1 ], [R2 , t 2 ] expressed using quaternion parameterization of the r

rotation matrix (tx1 , t y1 , tz1 , q01 , q11 , q21 , q31 ), (tx2 , t y2 , tz2 , q02 , q12 , q22 , q32 ) the Jacobian J of the objective function for plane to plane observation is given in Eq. (8.112). ⎛

0 r ⎜ 0 J =⎜ ⎝ 0 d4,1

0 0 0 d4,2

0 d1,4 0 d2,4 0 d3,4 d4,3 d4,4 ⎛ 0 ⎜ ⎜0 ⎜ ⎜0 ⎝

d1,5 d2,5 d3,5 d4,5

d1,6 d2,6 d3,6 d4,6

0

0

0

0

0

0

∂dδ ∂dδ ∂dδ ∂tx1 ∂t y1 ∂tz1

1 2 3 4 5 6

d1,7 d2,7 d3,7 d4,7

∂aδ ∂q01 ∂bδ ∂q01 ∂cδ ∂q01 ∂dδ ∂q01

0 0 0 d4,8

∂aδ ∂q11 ∂bδ ∂q11 ∂cδ ∂q11 ∂dδ ∂q11

0 0 0 0 0 0 d4,9 d4,10

∂aδ ∂q21 ∂bδ ∂q21 ∂cδ ∂q21 ∂dδ ∂q21

d1,11 d2,11 d3,11 d4,11

d1,12 d2,12 d3,12 d4,12

∂aδ 0 0 0 ∂q31 ∂bδ 0 0 0 ∂q31 ∂cδ 0 0 0 ∂q31 ∂dδ ∂dδ ∂dδ ∂dδ 1 ∂t 2 ∂t 2 ∂t 2 ∂q3 x y z

d1,13 d2,13 d3,13 d4,13

∂aδ ∂q02 ∂bδ ∂q02 ∂cδ ∂q02 ∂dδ ∂q02

∂aδ ∂q12 ∂bδ ∂q12 ∂cδ ∂q12 ∂dδ ∂q12

⎞ d1,14 d2,14 ⎟ ⎟= d3,14 ⎠ d4,14 ⎞ ∂a ∂a δ

∂q22 ∂bδ ∂q22 ∂cδ ∂q22 ∂dδ ∂q22

δ

∂q32 ∂bδ ⎟ ⎟ ∂q32 ⎟ ∂cδ ⎟ ∂q32 ⎠ ∂dδ ∂q32

(8.112)

from s ym py i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’ ) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14 15 16 17

a_1 , b_1 , c_1 , d_1 = s y m b o l s ( ’ a_1 b_1 c_1 d_1 ’ ) tx_1 , ty_1 , tz_1 = s y m b o l s ( ’ tx_1 ty_1 tz_1 ’ ) om_1 , fi_1 , ka_1 = s y m b o l s ( ’ om_1 fi_1 ka_1 ’) # sx_1 , sy_1 , sz_1 = s y m b o l s ( ’ sx_1 sy_1 sz_1 ’) # q0_1 , q1_1 , q2_1 , q3_1 = s y m b o l s ( ’ q0_1 q1_1 q2_1 q3_1 ’) a_2 , b_2 , c_2 , d_2 = s y m b o l s ( ’ a_2 b_2 c_2 d_2 ’ ) tx_2 , ty_2 , tz_2 = s y m b o l s ( ’ tx_2 ty_2 tz_2 ’) om_2 , fi_2 , ka_2 = s y m b o l s ( ’ om_2 fi_2 ka_2 ’) # sx_2 , sy_2 , sz_2 = s y m b o l s ( ’ sx_2 sy_2 sz_2 ’) # q0_2 , q1_2 , q2_2 , q3_2 = s y m b o l s ( ’ q0_2 q1_2 q2_2 q3_2 ’)

18 19 20 21 22 23 24 25 26 27

p o s i t i o n _ s y m b o l s _ 1 = [ tx_1 , ty_1 , tz_1 ] o r i e n t a t i o n _ s y m b o l s _ 1 = [ om_1 , fi_1 , ka_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ sx_1 , sy_1 , sz_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ q0_1 , q1_1 , q2_1 , q3_1 ] p o s i t i o n _ s y m b o l s _ 2 = [ tx_2 , ty_2 , tz_2 ] o r i e n t a t i o n _ s y m b o l s _ 2 = [ om_2 , fi_2 , ka_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ sx_2 , sy_2 , sz_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ q0_2 , q1_2 , q2_2 , q3_2 ] all_symbols = position_symbols_1 + orientation_symbols_1 + position_symbols_2 + orientation_symbols_2

28 29

30

31

R t _ w c _ 1 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_1 , ty_1 , tz_1 , om_1 , fi_1 , ka_1 ) # R t _ w c _ 1 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_1 , ty_1 , tz_1 , sx_1 , sy_1 , sz_1 ) # R t _ w c _ 1 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_1 , ty_1 , tz_1 , q0_1 , q1_1 , q2_1 , q3_1 )

8.3 Feature to Feature Metrics 32

33

34

215

R t _ w c _ 2 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_2 , ty_2 , tz_2 , om_2 , fi_2 , ka_2 ) # R t _ w c _ 2 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_2 , ty_2 , tz_2 , sx_2 , sy_2 , sz_2 ) # R t _ w c _ 2 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_2 , ty_2 , tz_2 , q0_2 , q1_2 , q2_2 , q3_2 )

35 36 37 38 39 40

R _ c w _ 1 = R t _ w c _ 1 [: -1 ,: -1]. t r a n s p o s e () t _ w c _ 1 = M a t r i x ([ tx_1 , ty_1 , tz_1 ]) . vec () t_cw_1 =- R_cw_1 * t_wc_1 R t _ c w _ 1 = M a t r i x . h s t a c k ( R_cw_1 , t _ c w _ 1 ) R t _ c w _ 1 = M a t r i x . v s t a c k ( Rt_cw_1 , M a t r i x ([[0 ,0 ,0 ,1]]) )

41 42 43 44 45 46

R _ c w _ 2 = R t _ w c _ 2 [: -1 ,: -1]. t r a n s p o s e () t _ w c _ 2 = M a t r i x ([ tx_2 , ty_2 , tz_2 ]) . vec () t_cw_2 =- R_cw_2 * t_wc_2 R t _ c w _ 2 = M a t r i x . h s t a c k ( R_cw_2 , t _ c w _ 2 ) R t _ c w _ 2 = M a t r i x . v s t a c k ( Rt_cw_2 , M a t r i x ([[0 ,0 ,0 ,1]]) )

47 48 49

p l a n e _ 1 = M a t r i x ([[ a_1 , b_1 , c_1 , d_1 ]]) p l a n e _ 2 = M a t r i x ([[ a_2 , b_2 , c_2 , d_2 ]])

50 51 52 53 54

t a r g e t _ v a l u e = M a t r i x ([0 ,0 ,0 ,0]) . vec () . t r a n s p o s e () model_function = plane_1 * Rt_cw_1 - plane_2 * Rt_cw_2 del ta = t a r g e t _ v a l u e - m o d e l _ f u n c t i o n d e l t a _ j a c o b i a n = delta . j a c o b i a n ( a l l _ s y m b o l s ) r

Listing 8.11 Python code for generating plane to plane observation and the Jacobian J of the objective function for Tait-Bryan (alternatively Rodrigues or quaternion) rotation matrix parameterization

8.4 Reflection Observation Equation Reflection observation equation [31] was introduced for the LiDAR calibration with the reshaped FOV via additional planar reflectors. This technique can help in LiDAR modification to reach expected FOV. For the LiDAR beam that intersects with the reflective surface the plane Eq. (8.113) is considered

where

ax + by + cz + d = 0

(8.113)

   abc =1

(8.114)

V pl = (a, b, c) is the unit vector orthogonal to the plane and d is the distance from the origin to the plane. It satisfies the following condition with the intersection point in 3D space:

216

8 LiDAR Metrics

Fig. 8.11 The direction r d of the LiDAR beam bd after reflecting with the surface with normal vector V pl

⎡ ⎤ x   ⎢y⎥ ⎥ abcd ⎢ ⎣z ⎦ = 0 1

(8.115)

The main assumption is that LiDAR beam starts at origin (0, 0, 0). Assuming the LiDAR beam has unique representation as beam origin bo = (0, 0, 0) and beam  d d d d d direction b = (bx , b y , bz ),  b  = 1 the beam with reflective plane intersection equation is given (8.116). P int

⎡ d⎤ bx d = − ⎣bdy ⎦ pl d bzd V · b

(8.116)

where (·) is the dot product. The direction r d of the LiDAR beam bd after reflecting with the plane with the normal vector V pl is given by Eq. (8.117), and the supportive plot is given in Fig. 8.11. r d = 2(bd · V pl )V pl − bd

(8.117)

The final reflected measurement point P r is given in Eq. (8.118) assuming the in the local coordinate system of the measurement point P l (x l , y l , zl ) expressed  p  P l  and distance from origin to intersection l int = = measurement instrument, l  int   P . P r = −( P int + r d (l p − l int )) (8.118) Finally, the measurement point expressed in the global coordinate system is given as (8.119).

8.4 Reflection Observation Equation

217

! P g = [R, t]

" Pr = (R, t, a, b, c, d, P l ) 1

(8.119)

Thanks to (8.119) it is possible to construct reflection observation Eq. (8.120) similar e.g. to point to point source target observation equation discussed in Sect. 8.1.2. Obviously, other observations equations can also be taken into the consideration.

⎡ δ⎤ x ⎣yδ ⎦ = zδ    r esiduals

⎡ ⎤ 0 ⎣0⎦ 0 

target values

⎛

⎡ k ⎤⎞ x − ⎝ [R,t,a,b,c,d] (R, t, a, b, c, d, P l, j ) − ⎣ y k ⎦⎠ zk    model f unction

(8.120) Listing 8.12 shows the Python code for reflection observation equation. 1 2 3 4 5 6

from s ym py i m p o r t * i m p o r t sys sys . p a t h . i n s e r t (1 , ’ .. ’ ) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12

13

14 15

x_t , y_t , z_t = s y m b o l s ( ’ x_t y_t z_t ’ ) # x_s , y_s , z_s = s y m b o l s ( ’ x_s y_s z_s ’) tx , ty , tz = s y m b o l s ( ’ tx ty tz ’) om , fi , ka = s y m b o l s ( ’ om fi ka ’) ray_dir_x , ray_dir_y , ray_dir_z , r a y _ l e n g t h = s y m b o l s ( ’ ray_dir_x ray_dir_y ray_dir_z ray_length ’) plane_a , plane_b , plane_c , p l a n e _ d = s y m b o l s ( ’ p l a n e _ a plane_b plane_c plane_d ’) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’)

16 17 18 19 20 21 22

p o s i t i o n _ s y m b o l s = [ tx , ty , tz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] p l a n e _ s y m b o l s = [ plane_a , plane_b , plane_c , p l a n e _ d ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] all_symbols = position_symbols + orientation_symbols + plane_symbols

23 24

a = plane_a * ray_dir_x + plane_b * ray_dir_y + plane_c * ray_dir_z

25 26 27 28

intersection_x = - ray_dir_x * ( plane_d /a) intersection_y = - ray_dir_y * ( plane_d /a) intersection_z = - ray_dir_z * ( plane_d /a)

29 30 31 32 33

n = M a t r i x ([ plane_a , plane_b , p l a n e _ c ]) . vec () d = M a t r i x ([ r a y _ d i r _ x , ray_dir_y , r a y _ d i r _ z ]) . vec () rd =2* d . dot ( n ) *n - d

218 34

35 36 37

8 LiDAR Metrics

ll = r a y _ l e n g t h - sqrt ( i n t e r s e c t i o n _ x * i n t e r s e c t i o n _ x + intersection_y * intersection_y + intersection_z * intersection_z ) x_s = -( i n t e r s e c t i o n _ x + rd [0] * ll ) y_s = -( i n t e r s e c t i o n _ y + rd [1] * ll ) z_s = -( i n t e r s e c t i o n _ y + rd [2] * ll )

38 39 40

p o i n t _ s o u r c e = M a t r i x ([ x_s , y_s , z_s , 1]) . vec () p o i n t _ t a r g e t = M a t r i x ([ x_t , y_t , z_t ]) . vec ()

41 42

43 44

t r a n s f o r m e d _ p o i n t _ s o u r c e = ( m a t r i x 4 4 F r o m T a i t B r y a n ( tx , ty , tz , om , fi , ka ) * p o i n t _ s o u r c e ) [: -1 ,:] t a r g e t _ v a l u e = M a t r i x ([0 ,0 ,0]) . vec () model_function = transformed_point_source - point_target

45 46 47

del ta = t a r g e t _ v a l u e - m o d e l _ f u n c t i o n d e l t a _ j a c o b i a n = delta . j a c o b i a n ( a l l _ s y m b o l s ) r

Listing 8.12 Python code for generating reflection observation equations and the Jacobian J of the objective function for Tait-Bryan rotation matrix parameterization (alternatively for Rodrigues and quaternion)

8.5 Normal Distributions Transform NDT is a technique for LiDAR data registration alternative to ICP. The key element of the NDT [32] is the representation of the data. Instead of using point cloud NDT models the environment by the set of normal distributions organized in regular grid over 3D space. These distributions describe the probability of finding a LiDAR point at a certain position. The advantage of the method is that it gives a piece wise smooth representation of the point cloud, with continuous first and second order derivatives. Thus, standard optimization techniques (e.g. described in Sect. 4.2) can be applied. Another advantage of NDT over ICP is much less computational requirements since the consumptive nearest neighborhood search procedure is not needed. Obviously, the 3D space decomposition into the regular grid introduces some minor artifacts, but in many practical applications this is a negligibly small disadvantage. NDT calculates the mean given by Eq. (8.121) and the covariance given by Eq. (8.122) for each bucket of regular grid containing sufficient number of measured points P g expressed in global coordinate system. m 1  g P μ= m k=1 k

1  g g ( P − μ)( P k − μ) m − 1 k=1 k

(8.121)

m

=

(8.122)

8.5 Normal Distributions Transform

219

For each bucket the likelihood of having measured point P mg is given by Eq. (8.123). p( P mg ) =

1 1√

(2 ) 2

  ( P g − μ)  −1 ( P mg − μ) exp − m 2 ||

(8.123)

Each p( P mg ) can be seen as an approximation of the local surface within the range of the bucket. It describes the position μ of the surface as well as its orientation and smoothness given by . Let ([R, t], P lm ) will be a transformation function of the local measurement point [ P lm , 1] via pose [R, t] expressed as (8.124). ! ([R, t], P lm ) = P mg = [R, t]

P lm 1

" (8.124)

Thus, NDT optimization problem is defined as maximization of the likelihood function given in Eq. (8.125). [R, t]∗ = max [R,t]

N '

p(([R, t], P lm ))

(8.125)

k=1

Furthermore, the optimization problem is equivalent to the minimization of the negative log-likelihood given in Eq. (8.126).

Fig. 8.12 Initial state

220

Fig. 8.13 μ and  for each bucket as ellipse

Fig. 8.14 Registered data

8 LiDAR Metrics

8.5 Normal Distributions Transform

221

Fig. 8.15 μ and  for each bucket as ellipse

∗

[R, t] = min − [R,t]

N 

 log p(([R, t], P lm ))

(8.126)

k=1

Thus, the implementation can use point to point—source to target observation Eq. (8.9) described in Sect. 8.1.2. In this particular case the target value is [0, 0, 0] and the model function is ([R, t], P lm ) − μ. To incorporate  into the implementation it is advised to follow the methodology given by the Eq. (4.24) in Sect. 4.2. The example demonstrating the point cloud data registration and visualization of the μ and  for each bucket as ellipse is shown in Figs. 8.12, 8.13, 8.14, 8.15.

8.6 Local Geometric Features—Surfels An effective method for 3D point cloud registration is the use of local features ( planarit y, linearit y) [33] and related observation equations. First we should decompose 3D points into surfels using e.g. regular grid decomposition technique described in Sect. 8.7. For each surfel we compute local features described in Sect. 8.1.5 e.g. planar and linear features. For linear features we can use observation equation described in Sect. 8.2.1. For planar features we can use observation equation described in Sect. 8.2.2. Due to efficient 3D data decomposition the overall computational time is decreased and needed for lines-like and planes-like features extraction. An efficient method for nearest neighborhood search e.g. one from described in Sect. 8.7 efficiently reduces the time needed for obtaining C corre-

222

8 LiDAR Metrics

spondences for fulfilling observation equations. The Algorithm 1 describes the steps of registering the source point cloud to target point cloud that is processed into local features organized by surfels and regular grid decomposition. Each point from source point cloud is expressed in local coordinate system of the pose [R, t], therefore we can use the Eq. (8.1) to transform it into global reference system. For all [R, t] poses the observations equations assuming point to line metric (Eq. (8.54)) and point to plane metric (Eq. 8.64)) are built to obtain optimization system. Thus, the optimization system finds the optimal solution for registering source raw point cloud to local features extracted from target point cloud organized into the surfels. First advantage of this technique is the improved calculation time by efficient data decomposition into surfels, therefore number of observation equations is reduced. Second advantage is that the local features are the alternative solution for the representation of the noisy point cloud. This method is widely used in mobile mapping systems with continuous scanning [34–39]. Algorithm 1 Registering the source point cloud to target point cloud using surfels 1: 2: 3: 4: 5: 6: 7: 8:

decompose target point cloud into surfels using regular grid decomposition compute linear and planar features for surfels (from Table 8.5) classify surfels into planar-like and line-like local features using e.g. Machine Learning method for i:iterations do find point to line, point to plane correspondences calculate observation equations using (8.54), (8.64) solve optimization problem and update all poses end for

8.7 Nearest Observations Search Nearest observations search of basic primitives e.g. points. lines, planes is fundamental aspect of SLAM implementation, since the computational time has to be reduced for reaching on-line performance. The goal is to find nearest primitives in certain radius around the query primitive as shown in Fig. 8.16. For effective search time many approaches of efficient 3D data decomposition were studied, thus there are several approaches for the NNS in SLAM algorithm. An important remark is that NNS takes most of the computational time, to demonstrate it authors of [40] used the pie chart shown in Fig. 8.17 to depict the timing of different stages of one ICP iteration with two point clouds of 68229 points. The basic concept is to decompose 3D space into organized buckets, thus number of calculations can be efficiently reduced by incorporating dedicated bucket structure. An example of such data structure called octree is shown in Fig. 8.18 where each bucket is decomposed recurrently into eight sub buckets till reaching criterion of stop as size of final bucket. An approach from [41] is using the regular grid decomposition inspired by work of [42]. NNS can be efficiently computed by modern GPU processors, therefore it can

8.7 Nearest Observations Search

223

Fig. 8.16 Nearest neighborhood search example. Green: nearest neighbors to the query points. Blue: query point. Red: other points

Fig. 8.17 The pie chart is depicting the timing of different stages of one ICP iteration with two point clouds of 68229 points shown in [40]. The NNS procedure is dominant compared to the rest of ICP algorithm Fig. 8.18 Octree

224

8 LiDAR Metrics

improve the performance of SLAM. Another promising approach—an octree based NNS is shown in [43]. Authors claim that the octree based NNS does not suffer more from the larger maximal distances than the kd-tree based NNS, unfortunately there is observed an increased variance for the computing time. NNS problem is well known also in Computer Graphics, where point-model (often referred to as a point-cloud) usually contains millions of points. For example authors of [44] claim that their NNS approach can be used for the surface normals computation and noise removal. Using NNS for normal vector estimation is also shown in [45], where authors use EGG for finding neighbors. EGG provides balanced neighbors by considering both distance and directional spread. It is emphasized the fact that in recent years there has been a marked shift from using triangles to using points as object modeling primitives especially in computer graphics applications [46, 47]. It is related with the improved scanning technologies [48, 49] that have also meaningful impact into mobile robotic applications from the perspective of mapping [50] and navigation [51]. An important contribution of Andreas Nüchter [52] to NNS problem within the context of 6DSLAM is the efficient kd-tree data decomposition. The kd-tree data structure is commonly used in Computer Graphics, and it is related with the problem of high performance ray tracing [53–56] that can be solved using modern GPU. It is already shown that GPU is more efficient than CPU in NNS problem, for example in [57] authors claim that NNS on GPU is up to 400 times faster than a brute force CPU-based implementation of NNS. The comparison of nearest-neighbor-search strategies and implementations for efficient shape registration is shown in [52]. Authors emphasized the fact that most spatial data structures are hierarchical in nature, thus can be organized with kd-trees [58] and octrees [59]. They mentioned that a special type of NNS employs the Morton order SFC for arranging the point cloud [60]. They also discussed the R-tree [61] based algorithm for nearest neighborhood search. Following algorithms were compared: • kd-tree (implementations: 3DTK [62], ANN [63], CGAL [64], FLANN [65], libnabo [66]), • R-tree (implementation: SpatialIndex [67]), • SFC (implementation: STANN [68]). The result of this comparison is that since most libraries implement only the kd-tree, it is hard to draw final conclusions as to what data structure is better suited for NNS. The octree implementation was among the best performing algorithms. Authors claim that they have contributed their own novel open-source implementation of NNS and have shown these to perform well on realistic as well as artificial shape registration problems.

8.7.1 Anisotropic Nearest Neighborhood Search Anisotropic nearest neighborhood search is sufficient to reduce outliers during nearest observation search assuming motion model—movement in certain direction.

8.7 Nearest Observations Search

225

Fig. 8.19 NNS with rotated ellipsoid. Blue: query point, green: nearest neighbors, red: other points, black line: movement direction

Fig. 8.20 NNS with rotated bounding box. Blue: query point, green: nearest neighbors, red: other points, black line: movement direction

Figures 8.19, 8.20 show examples for rotated ellipsoid function and rotated bounding box. This procedure maintain in this case only nearest neighbors assuming larger longitudinal that lateral displacement what is evident in fast moving mobile map-

226

8 LiDAR Metrics

ping systems. The implementation is simple since it is enough to transform query and neighboring points via inverse current pose of the vehicle and apply ellipsoid or bounding box condition.

References 1. H. Thomas, F. Goulette, J-E. Deschaud, B. Marcotegui, Semantic classification of 3d point clouds with multiscale spherical neighborhoods (2018), pp. 390–398, 09 2. P. Babahajiani, L. Fan, J-K. Kämäräinen, M. Gabbouj, Urban 3d segmentation and modelling from street view images and lidar point clouds. Mach. Vis. Appl. 28(7), 679–694 (2017). EXT=“Babahajiani, Pouria” 3. M. Przybylski, D. Koguciuk, B. Siemiatkowska, B. Harasymowicz-Boggio, L. Chechlinski, Integration of qualitative and quantitative spatial data within a semantic map for service robots, in Progress in Automation, Robotics and Measuring Techniques - Volume 2 Robotics, ed. by R. Szewczyk, C. Zielinski, M. KaliczynskaAdvances in Intelligent Systems and Computing, vol. 351 (Springer, 2015), pp. 223–232 4. A. Borkowski, B. Siemiatkowska, J. Szklarski, Towards semantic navigation in mobile robotics, in Graph Transformations and Model-Driven Engineering - Essays Dedicated to Manfred Nagl on the Occasion of his 65th Birthday, ed. by G. Engels, C. Lewerentz, W. Schäfer, A. Schürr, B. Westfechtel, Lecture Notes in Computer Science, vol. 5765 (2010, Springer), pp. 719–748 5. T. Hackel, J.D. Wegner, K. Schindler, Contour detection in unstructured 3d point clouds, in 2016 IEEE Conference on Computer Vision and Pattern Recognition (CVPR) (2016), pp. 1610–1618 6. M. Asada, Y. Shirai, Building a world model for a mobile robot using dynamic semantic constraints, in Proceedings of 11th International Joint Conference on Artificial Intelligence (1989), pp. 1629–1634 7. A. Nüchter, O. Wulf, K. Lingemann, J. Hertzberg, B. Wagner, H. Surmann, 3D Mapping with semantic knowledge, in In Robocup International Symposium (2005), pp. 335–346 8. Andreas Nüchter, Joachim Hertzberg, Towards semantic maps for mobile robots. Robot. Auton. Syst. 56(11), 915–926 (2008) 9. O. Grau, A scene analysis system for the generation of 3-d models, in NRC ’97: Proceedings of the International Conference on Recent Advances in 3-D Digital Imaging and Modeling, Washington, DC, USA. (IEEE Computer Society, 1997), p. 221 10. A. Nüchter, H. Surmann, K. Lingemann, J. Hertzberg, Semantic scene analysis of scanned 3d indoor environments, in Proceedings of the Eighth International Fall Workshop on Vision, Modeling, and Visualization (VMV 03) (2003), pp. 658–666 11. H. Cantzler, R.B. Fisher, M. Devy, Quality enhancement of reconstructed 3d models using coplanarity and constraints, in Proceedings of the 24th DAGM Symposium on Pattern Recognition, London, UK (Springer, 2002), pp. 34–41 12. M.A. Fischler, R. Bolles, Random sample consensus. A paradigm for model fitting with applications to image analysis and automated cartography, in Proceedings of 1980 Image Understanding Workshop ed. by L.S. Baurnann (College Park, Md., Apr i980) Scmnce Apphcatlons, McLean, Va. (1980), pp. 71–88 13. M..A. Fischler, R..C. Bolles, Readings in Computer Vision: Issues, Problems, Principles, and Paradigms Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography. (Morgan Kaufmann Publishers Inc, San Francisco, CA, USA, 1987), pp. 726–740 14. M. Eich, M. Dabrowska, F. Kirchner, Semantic labeling: classification of 3d entities based on spatial feature descriptors, in IEEE International Conference on Robotics and Automation (ICRA2010) in Anchorage, Alaska, May 3 (2010)

References

227

15. N. Vaskevicius, A. Birk, K. Pathak, J. Poppinga, Fast detection of polygons in 3d point clouds from noise-prone range sensors, in IEEE International Workshop on Safety, Security and Rescue Robotics, SSRR (IEEE, Rome, 2007), pp. 1–6. Retrieved from 12 November 2007 16. H. Andreasson, R. Triebel, W. Burgard, Improving plane extraction from 3d data by fusing laser data and vision, in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (2005), pp. 2656–2661 17. J. Li, B. Xiong, F. Biljecki, G. Schrotter, A sliding window method for detecting corners of openings from terrestrial lidar data. volume 42, pages 97 – 103, Hannover, 2018. International Society for Photogrammetry and Remote Sensing (ISPRS). 13th 3D GeoInfo Conference; Conference Location: Delft, The Netherlands; Conference Date: October 1-2 (2018) 18. O. Miksik, V. Vineet, M. Lidegaard, R. Prasaath, M. Nießner, S. Golodetz, S.L. Hicks, P. Pérez, S. Izadi, P.H.S. Torr, The semantic paintbrush: Interactive 3d mapping and recognition in large outdoor spaces, in Proceedings of the 33rd Annual ACM Conference on Human Factors in Computing Systems, CHI ’15 (Association for Computing Machinery, New York, NY, USA, 2015), pp. 3317–3326 19. Y. Ben-Shabat, M. Lindenbaum, A. Fischer, 3dmfv: 3d point cloud classification in real-time using convolutional neural network. IEEE Robot. Autom. Lett. PP, 1–1 (2018) 20. Y. Guo, H. Wang, Q. Hu, H. Liu, L. Liu, M. Bennamoun, Deep learning for 3d point clouds: A survey (2020) 21. Marcin Daszuta, Ewa Napieralska-Juszczak, Classification of objects in a point cloud using neural networks. J. Appl. Comput. Sci. 27(2), 7–16 (2019). (Dec.) 22. P-H. Hsu, Z-Y. Zhuang, Incorporating handcrafted features into deep learning for point cloud classification. Remote Sens. 12(22) (2020) 23. R. Pierdicca, M. Paolanti, F. Matrone, M. Martini, C. Morbidoni, E.S. Malinverni, E. Frontoni, A.M. Lingua, Point cloud semantic segmentation using a deep learning framework for cultural heritage. Remote Sens. 12(6) (2020) 24. H. Kim, C. Kim, Deep-learning-based classification of point clouds for bridge inspection. Remote Sens. 12(22) (2020) 25. R. Roveri, L. Rahmann, A.C. Oztireli, M. Gross, A network architecture for point cloud classification via automatic depth images generation, in 2018 IEEE/CVF Conference on Computer Vision and Pattern Recognition (Piscataway, NJ, 2018), pp. 4176–4184. 2018 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR 2018); Conference Location: Salt Lake City, UT, USA; Conference Date: June 18-22, 2018 26. A. Gruen, D. Akca, Least squares 3d surface and curve matching. ISPRS J. Photogramm. Remote Sens. 59(3), 151–174 (2005). Received: 11 July 2004; Revised: 16 February 2005; Accepted: 16 February 2005; Available online: 29 March 2005 27. Devrim Akca. Least squares 3d surface matching. Mitteilungen, 92, 2007 28. M. Previtali, L. Barazzetti, R. Brumana, M. Scaioni, Laser scan registration using planar features in the international archives of the photogrammetry, remote sensing and spatial information sciences, ISPRS Technical Commission V Symposium, 23–25 June 2014, vol. XL-5 (Riva del Garda, Italy, 2014), p. 2014 29. Jan Wietrzykowski, Piotr Skrzypczy´nski, Planeloc: probabilistic global localization in 3-d using local planar features. Robot. Auton. Syst. 113, 160–173 (2019) 30. H. Nguyen, D. Belton, P. Helmholz, Planar surface detection for sparse and heterogeneous mobile laser scanning point clouds. ISPRS J. Photogramm. Remote Sens. 151, 141–161 (2019) 31. M. Pelka, J. Bedkowski, Calibration of planar reflectors reshaping lidar’s field of view. Sensors 21(19) (2021) 32. Martin Magnusson, Achim J. Lilienthal, Tom Duckett, Scan registration for autonomous mining vehicles using 3d-NDT. J. Field Robot. 24(10), 803–827 (2007) 33. M. Bosse, R. Zlot, Continuous 3d scan-matching with a spinning 2d laser, in ICRA (IEEE, 2009), pp. 4312–4319 34. Lukas Kaul, Robert Zlot, Michael Bosse, Continuous-time three-dimensional mapping for micro aerial vehicles with a passively actuated rotating laser scanner. J. Field Robot. 33(1), 103–132 (2016)

228

8 LiDAR Metrics

35. Robert Zlot, Michael Bosse, Efficient large-scale three-dimensional mobile mapping for underground mines. J. Field Robot. 31(5), 758–779 (2014) 36. R. Zlot, Efficient and versatile 3d laser mapping for challenging environments, in ICPRAM 2014 - Proceedings of the 3rd International Conference on Pattern Recognition Applications and Methods, ESEO, Angers, Loire Valley, France, 6-8 March, 2014, ed. by M. De Marsico, A. Tabbone, A.L.N. Fred (SciTePress, 2014), pp. IS–11. 37. Michael Bosse, Robert Zlot, Paul Flick, Zebedee: design of a spring-mounted 3-d range sensor with application to mobile mapping. IEEE Trans. Robot. 28(5), 1104–1119 (2012) 38. C. Holenstein, R. Zlot, M. Bosse, Watertight surface reconstruction of caves from 3d laser data, in 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2011, San Francisco, CA, USA, September 25-30, 2011 (IEEE, 2011), pp. 3830–3837 39. M. Bosse, R. Zlot, Continuous 3d scan-matching with a spinning 2d laser, in 2009 IEEE International Conference on Robotics and Automation, ICRA 2009, Kobe, Japan, May 12-17, 2009 (IEEE, 2009), pp. 4312–4319 40. D. Qiu, S. May, A. Nüchter, GPU-Accelerated Nearest Neighbor Search for 3D Registration, in Proceedings of the 7th International Conference on Computer Vision Systems, ICVS09 (Springer, Berlin, Heidelberg, 2009), pp. 194–203 41. J. Bedkowski, A. Maslowski, G. de Cubber, Real time 3D localization and mapping for USAR robotic application. Ind. Robot 39(5), 464–474 (2012) 42. T. Rozen, K. Boryczko, W. Alda, GPU bucket sort algorithm with applications to nearestneighbour search. WSCG 16(1–3), 161–167 (2008) 43. J. Elseberg, D. Borrmann, A. Nüchter, Efficient processing of large 3D point clouds, in 2011 XXIII International Symposium on Information, Communication and Automation Technologies (ICAT) (2011), pp. 1–7 44. Jagan Sankaranarayanan, Hanan Samet, Amitabh Varshney, A fast k-neighborhood algorithm for large point-clouds. In Eurographics Symposium on Point-Based Graphics 2006, 1–11 (2006) 45. J. Park, H. Shin, B. Choi, Elliptic Gabriel graph for finding neighbors in a point set and its application to normal vector estimation. Comput.-Aided Des. 38(6), 619–626 (2006) 46. Mark Pauly, Richard Keiser, Leif P. Kobbelt, Markus Gross, Shape modeling with pointsampled geometry. ACM Trans. Graph. 22(3), 641–650 (2003). (July) 47. Mattias Andersson, Joachim Giesen, Mark Pauly, Bettina Speckmann, Bounds on the kneighborhood for locally uniformly sampled surfaces. In Eurographics Symposium on PointBased Graphics 2004, 1–5 (2004) 48. O. Wulf, B. Wagner, Fast 3d scanning methods for laser measurement systems, in International Conference on Control Systems and Computer Science, Bucharest, Romania (2003) 49. F. Zampa, D. Conforti, Mapping with mobile lidar. GIM Int. 23(4), 35–37 (2009) 50. Andreas Nüchter, Kai Lingemann, Joachim Hertzberg, Hartmut Surmann, 6D SLAM-3D mapping outdoor environments. J. Field Robot. 24(8–9), 699–722 (2007) 51. D. Dolgov, S. Thrun, Detection of principal directions in unknown environments for autonomous navigation. Ann Arbor 1001, 48105 (2008) 52. Jan Elseberg, Stephane Magnenat, Roland Siegwart, Andreas Nüchter, Comparison of nearestneighbor-search strategies and implementations for efficient shape registration. J. Softw. Eng. Robot. (JOSER) 3(1), 2–12 (2012) 53. Stefan Popov, Johannes Günther, Hans-Peter. Seidel, Philipp Slusallek, Stackless KD-tree traversal for high performance GPU ray tracing. Comput. Graph. Forum 26(3), 415–424 (2007) 54. T. Foley, J. Sugerman, KD-tree acceleration structures for a GPU raytracer, in Proceedings of the ACM SIGGRAPH/EUROGRAPHICS Conference on Graphics Hardware, HWWS ’05, (ACM, New York, NY, USA, 2005), pp. 15–22 55. Kun Zhou, Qiming Hou, Rui Wang, Baining Guo, Real-time KD-tree construction on graphics hardware. ACM Trans. Graph. 27(5), 1 (2008) 56. D.R. Horn, J. Sugerman, M. Houston, P. Hanrahan, Interactive k-d tree GPU raytracing, in Proceedings of the 2007 Symposium on Interactive 3D Graphics and Games - I3D ’07 (2007), p. 167

References

229

57. V. Garcia, E. Debreuve, M. Barlaud, Fast k nearest neighbor search using GPU, in IEEE Computer Society Conference on Computer Vision and Pattern Recognition Workshops (2) (2008), pp. 1–6 58. J. Louis Bentley, Multidimensional binary search trees used for associative searching. Commun. ACM 18(9), 509–517 (1975) 59. D. Meagher, Geometric modeling using octree encoding. Comput. Graph. Image Process. 19(2), 129–147 (1982) 60. M. Connor, K. Piyush, Fast construction of k-nearest neighbor graphs for point clouds, in IEEE Transactions on Visualization and Computer Graphics (2009), pp. 1–11 61. A. Guttman, R-trees: a dynamic index structure for spatial searching, in International Conference on Management of Data (ACM, 1984), pp. 47–57 62. Automation Group (Jacobs University Bremen) and Knowledge-Based Systems Group (University of Osnabrück). 3DTK - The 3D Toolkit (2011) 63. D.M. Mount, S. Arya, ANN: A Library for Approximate Nearest Neighbor Searching (2011) 64. CGAL Computational Geometry Algorithms Library (2012) 65. M. Muja, FLANN - fast Library for Approximate Nearest Neighbors 66. S. Magnenat, libnabo 67. M. Hadjieleftheriou, SpatialIndex 68. M. Connor, STANN - The simple, Thread-safe Approximate Nearest Neighbor Library

Chapter 9

Constraints

9.1 Norm of the Quaternion This constraint maintain the norm of unit quaternion q(q0 , q1 , q2 , q3 ) given in  2 Eq. (3.20). Thus, the condition q0 + q12 + q22 + q32 = 1 can be expressed as observation equation (9.1)  δ =  1 − q02 + q12 + q22 + q32     r esidual target value

(9.1)

model f unction

 where δ is residual, 1 is target value and q02 + q12 + q22 + q32 is model function. The optimization problem is defined in Eq. (9.2).  2  minq0 ,q1 ,q2 ,q3 1 − q0 2 + q1 2 + q22 + q32

(9.2)

r

The Jacobian J of the objective function is given in Eq. (9.3) and the Listing 9.1 shows the Python code for this constraint. r

J = d1,1 d1,2 d1,3 d1,4 =  ∂δ ∂δ ∂δ ∂δ ∂q0 ∂q1 ∂q2 ∂q3

1

(9.3)

from s y m p y i m p o r t *

2 3 4 5

q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’) q u a t e r n i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ] target_value = 1 © The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_9

231

232 6 7 8

9 Constraints

m o d e l _ f u n c t i o n = sqrt ( q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 ) d e l t a = M a t r i x ([ t a r g e t _ v a l u e - m o d e l _ f u n c t i o n ]) . vec () delta_jacobian = delta . jacobian ( quaternion_symbols ) Listing 9.1 Python code for generating Quaternion constraint

9.2 Fixed Optimized Parameter The fixed optimized parameter constraint allows to maintain the current value of the optimized parameter, therefore, the desired behavior is that it will not change over optimization iteration. Obviously, the strength of this constraint is determined by the weight in weight matrix W ; see Eq. (4.24). To build observation equation for fixed optimized parameter, we assume that the result of the model function equals target value, therefore, residual equals 0. Model function is expressed as  [β] (x) = x = β, the target value β is current value of optimized parameter x. Therefore, Eq. (9.4) shows the fixed optimized parameter observation equation. δ =  r esidual

β 

target value

−

 [β] (x)   

=β−x =0

(9.4)

model f unction

r

The Jacobian J of the objective function is given in Eq. (9.5) r

J = d1,1 = ∂∂δx = −1

1

(9.5)

from s y m p y i m p o r t *

2 3

4

target_value , model_function = symbols ( ’ target_value model_function ’) all_symbols = [ model_function ]

5 6 7

r e s i d u a l = M a t r i x ([ t a r g e t _ v a l u e - m o d e l _ f u n c t i o n ]) . vec () residual_jacobian = residual . jacobian ( all_symbols )

Listing 9.2 Python code for generating fixed optimized parameter constraint

9.3 Anisotropic Motion In certain scenarios, it is beneficial to incorporate motion information into trajectory’s optimization process to control the trajectory direction updates. In Sect. 4.2, the technique for correlating the observational errors was discussed, thus it is possible by incorporating rotation matrix of current pose [R, t] for weight matrix W , for

9.3 Anisotropic Motion

233

example, fixed  optimized parameter observation equations (9.4) related with position

t = tx t y tz . Therefore, we use it to build anisotropic motion observation equation (9.6) ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ δ⎤ tx tx tx tx ⎣t yδ ⎦ = ⎣t y ⎦ −  [tx ,t y ,tz ] (tx , t y , tz ) = ⎣t y ⎦ − ⎣t y ⎦ (9.6)    tz β tz β tz tzδ model f unction       r esidual

target value

 

where residuals are 0 0 0 ,  [tx ,t y ,tz ] (tx , t y , tz ) is model function and tx t y tz β are target values as current values of the optimized parameters. It means that observation equals measurement. Thanks to this, it is possible to model the impact of the r

optimization process into pose updates for certain directions. The Jacobian J of the objective function for anisotropic motion observation equation is given in (9.7). ⎛

⎞

⎛ ∂t δ

δ δ x ∂tx ∂tx ∂tx ∂t y ∂tz

d1,1 d1,2 d1,3 ⎜ δ ⎜ ∂t J = ⎝d2,1 d2,2 d2,3 ⎠ = ⎜ ∂ty ⎝ x d3,1 d3,2 d3,3 ∂tzδ r

∂t yδ ∂t y ∂tzδ

⎞

⎛ ⎞ −1 0 0 ⎟ ⎟ ⎝ ⎟ = 0 −1 0 ⎠ ∂tz ⎠ 0 0 −1 δ ∂tz ∂t yδ

(9.7)

∂tx ∂t y ∂tz

⎛ ⎞ wx , 0, 0 If we use orthogonal weight matrix W = ⎝0, w y , 0⎠, we control the impact of the 0, 0, wz optimization process for each parameter separately. Therefore, for example, setting wx  w y and w y = wz will cause larger optimization updates in x direction of the global reference system. By incorporating the rotation matrix R of current pose [R, t] and integrating with weight matrix W , we obtain anisotropic weight matrix W a (9.8). (9.8) W a = RW R The same setting wx  w y and w y = wz will cause larger optimization updates in x direction of the local reference system [R, t]. It can be seen in Figs. 9.1 and 9.2 that if heading is not in the direction to the global minimum, the optimization updates are larger in heading direction. The rotation matrix R of current pose [R, t] is visualized as black heading line. The same setting wx  w y and w y = wz is used. It can be seen in Figs. 9.1 and 9.2 that if heading is not in the direction to the global minimum, the optimization updates are larger in x direction of the global reference system and in x direction of the local reference system. In Figs. 9.3 and 9.4, the heading was set within the direction to the global minimum, therefore, optimization process converges within this direction. Figures 9.5 and 9.6 are showing the same setup as in Figs. 9.1 and 9.2 with much smaller value of wx , therefore, large optimization updates can be seen within the direction x of the local reference system.

234

9 Constraints

Fig. 9.1 Green to red color: (x−1)2

(y−2)2

e− 50 − 50 , black mesh: offset = 1, heading not in the direction to the global minimum

Fig. 9.2 Green to red color: visualization of the optimization problem Eq. (4.39). Setup as in Fig. 9.1

Fig. 9.3 Green to red color: (x−1)2

(y−2)2

e− 50 − 50 , black mesh: offset = 1, heading within the direction to the global minimum

9.3 Anisotropic Motion Fig. 9.4 Green to red color: visualization of the optimization problem Eq. (4.39). Setup as in Fig. 9.3

Fig. 9.5 Setup as in Fig. 9.1 with much smaller value of wx

Fig. 9.6 Green to red color: visualization of the optimization problem Eq. (4.39). Setup as in Fig. 9.5

235

236

9 Constraints

Fig. 9.7 Red: 0.1x 3 + 0.5x 2 , blue: observation equation of constrain (9.9), green: optimization behavior, black: offset

9.4 Linear and Square Functions These constraints are useful to add the optimization convergence of the parameter x to the desired target value x target . It can be done by introducing linear (9.9) and square (9.11) functions, where δ is residual, 0 is target value and a(x target − x),

2 target − x) are model functions. The optimization problem is formulated in a(x r

Eqs. (9.10) and (9.12). The Jacobian J of the objective function is given in Eq. (9.13), and the Listing 9.3 shows the Python code for these constraints. Figures 9.7, 9.8 and 9.9 show the impact of the constraint expressed as Eq. (9.9) into optimization problem from one dimensional example discussed in Sect. 4.2.1.3. Figures 9.10, 9.11 and 9.12 show the impact of the constraint expressed as Eq. (9.11). Similarly, Figs. 9.13, 9.14, 9.15 and 9.16, 9.17, 9.18 show constraints (9.9) and (9.11) fused with observation equation (4.40) for 2D function discussed in Sect. 4.2.1.4. The impact of the constraint is the unique solution to the optimization process. It can be seen that the optimizer searches the optimum from many local solutions. To boost the search, in this case, the weights in matrix W (see Eq. (4.24)) can be increased, the result is visualized in Figs. 9.19, 9.20, 9.21 and 9.22, 9.23, 9.24. Another possibility to change optimization behavior is to modify parameter a in constraint observation equations 9.9 and 9.11. δ =  r esidual

0 

target value

− a(x target − x)    model f unction



2 min 0 − a(x target − x) x

δ =  r esidual

0 

target value

(9.9)



2 − a(x target − x)    model f unction

(9.10)

(9.11)

9.4 Linear and Square Functions Fig. 9.8 Red: optimization problem (4.34), blue: optimization problem (9.10), green: optimization behavior, black: offset.

Fig. 9.9 Red: fused optimization problems (4.34) and (9.10), green: optimization behavior, black: offset

Fig. 9.10 Red: 0.1x 3 + 0.5x 2 , blue: observation equation of constraint (9.11), green: optimization behavior, black: offset

237

238

9 Constraints

Fig. 9.11 Red: optimization problem (4.34), blue: optimization problem (9.12), green: optimization behavior, black: offset

Fig. 9.12 Red: fused optimization problems (4.34) and (9.12), green: optimization behavior, black: offset

Fig. 9.13 Red to green: (x−1)2

(y−2)2

e− 50 − 50 , black: offset, blue: optimization behavior

9.4 Linear and Square Functions

239

Fig. 9.14 Optimization problems (4.41) and (9.10), blue: optimization behavior

Fig. 9.15 Red to green: fused optimization problems (4.41) and (9.10), blue: optimization behavior



2 2 min 0 − a(x target − x) x

r

J = d1,1 = ∂∂δx

1

(9.12)

(9.13)

from s ym py i m p o r t *

2 3 4

a ,x , x _ t r g = s y m b o l s ( ’a x x_trg ’) all_symbols = [x]

5 6 7 8

9

o b s _ e q = M a t r i x ([0 - ( a *( x _ t r g - x ) ) ]) . vec () obs_eq_jacobian = obs_eq . jacobian ( all_symbols ) o b s _ e q _ s q = M a t r i x ([0 - ( a *( x_trg - x ) ) *( a *( x_trg - x ) ) ]) . vec () obs_eq_sq_jacobian = obs_eq_sq . jacobian ( all_symbols )

Listing 9.3 Python code for generating linear and square functions constraints

240

9 Constraints

Fig. 9.16 Red to green: (x−1)2

(y−2)2

e− 50 − 50 , black: offset, blue: optimization behavior

Fig. 9.17 Optimization problems (4.41) and (9.12), blue: optimization behavior

9.5 Relative Pose 9.5.1 Variant (1) Relative pose observation equation is a fundamental element of the PGSLAM. Relative pose constraint maintains the desired relative pose as a target value between two nodes of trajectory, for which current relative pose is given by the model function. Thus, it can be used to build graph where vertices are related with trajectory poses and edges correspond to desired relative poses between them. The example of PGSLAM is shown in Figs. 9.25 and 9.26. This constraint can incorporate, for

9.5 Relative Pose

241

Fig. 9.18 Red to green: fused optimization problems (4.41) and (9.12), blue: optimization behavior

Fig. 9.19 Red to green: (x−1)2

(y−2)2

e− 50 − 50 , black: offset, blue: optimization behavior

example, odometry readings and use them as a reference trajectory, therefore, the optimized trajectory will maintain the relative displacements between consecutive poses as a desired relative pose between consecutive odometry readings. Furthermore, these constraints can be used for loop closure. In such case, the optimizer will converge assuming the desired relative pose (target value) between two poses of the trajectory or even more trajectories. Therefore, this constraint is useful for building large scale SLAM systems. Relative pose [R, t]12 from pose [R, t]1 to pose [R, t]2 is given by Eq. (9.14). 

R t 01×3 1



12 =

1 −1  2  1  2 R t R t R t R t = 01×3 1 01×3 1 01×3 1 cw 01×3 1 wc

(9.14)

To build a relative pose observation equation, we need to consider parameterization of the pose [R, t] into Tait–Bryan, Rodrigues or quaternion representation of the rotation matrix. Such parameterization functions m2v were shown in Listings 3.1, 3.2 and 3.3.

242

9 Constraints

Fig. 9.20 Optimization problems (4.41) and (9.10), blue: optimization behavior

Fig. 9.21 Red to green: fused optimization problems (4.41) and (9.10), blue: optimization behavior

Fig. 9.22 Red to green: (x−1)2

(y−2)2

e− 50 − 50 , black: offset, blue: optimization behavior

9.5 Relative Pose

243

Fig. 9.23 Optimization problems (4.41) and (9.12), blue: optimization behavior

Fig. 9.24 Red to green: fused optimization problems (4.41) and (9.12), blue: optimization behavior

Thus, function m2v[β] ([R, t]12 ) retrieves β = (tx , t y , tz , ω, ϕ, κ) for Tait–Bryan parametrization, β = (tx , t y , tz , sx , s y , sz ) for Rodrigues parameterization and β = (tx , t y , tz , q0 , q1 , q2 , q3 ) for quaternion parameterization of the rotation matrix from [R, t]12 . The relative pose observation equations are formulated in (9.15), (9.16) and (9.17) ⎡ ⎤ ⎡ δ⎤ tx tx ⎢t y ⎥ ⎢ t yδ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ tδ ⎥ ⎢ z ⎥ = ⎢ tz ⎥ − m2v t ,t ,t ,ω,ϕ,κ ([R, t]12 ) (9.15) ] [x y z ⎢ω ⎥ ⎢ω δ ⎥    ⎢ ⎥ ⎢ ⎥ ⎣ϕ ⎦ ⎣ϕ δ ⎦ model f unction δ κ κ       r esiduals

target values

244

Fig. 9.25 State of the PGSLAM before optimization

Fig. 9.26 State of the PGSLAM after optimization

9 Constraints

9.5 Relative Pose

245

⎡ δ⎤ tx ⎢ t yδ ⎥ ⎢ δ⎥ ⎢t ⎥ ⎢ zδ ⎥ = ⎢s ⎥ ⎢ xδ ⎥ ⎣s y ⎦ sδ  z  r esiduals

⎡ δ⎤ tx ⎢ t yδ ⎥ ⎢ ⎥ ⎢tδ ⎥ ⎢ zδ ⎥ ⎢q ⎥ = ⎢ 0δ ⎥ ⎢q ⎥ ⎢ 1δ ⎥ ⎣q2 ⎦ qδ  3  r esiduals

⎡ ⎤ tx ⎢ty ⎥ ⎢ ⎥ ⎢ tz ⎥ ⎢ ⎥ ⎢s x ⎥ ⎢ ⎥ ⎣s y ⎦ sz   

− m2v[tx ,t y ,tz ,sx ,s y ,sz ] ([R, t]12 )   

(9.16)

model f unction

target values

⎡ ⎤ tx ⎢ ty ⎥ ⎢ ⎥ ⎢ tz ⎥ ⎢ ⎥ ⎢q0 ⎥ ⎢ ⎥ ⎢q1 ⎥ ⎢ ⎥ ⎣q2 ⎦ q3   

− m2v[tx ,t y ,tz ,q0 ,q1 ,q2 ,q3 ] ([R, t]12 )   

(9.17)

model f unction

target values

  

where txδ t yδ tzδ ωδ ϕ δ κ δ , txδ t yδ tzδ sxδ s yδ szδ , txδ t yδ tzδ q0δ q1δ q2δ q3δ are residu  

als, tx t y tz ω ϕ κ , tx t y tz sx s y sz , tx t y tz q0 q1 q2 q3 are target values and m2v[β] ([R, t]12 ) are model functions. The optimization problems for the relative pose (1) constraints are defined, respectively, in Eqs. (9.18), (9.19) and (9.20) for Tait–Bryan, Rodrigues and quaternion parameterizations of the rotation matrix. min

R1 ,t 1 ,R2 ,t 2

min

R1 ,t 1 ,R2 ,t 2

min

R1 ,t 1 ,R2 ,t 2

C  2  

tx t y tz ω ϕ κ − m2v[tx ,t y ,tz ,ω,ϕ,κ ] ([R, t]12 )

(9.18)

i=1

C  2 

 tx t y tz sx s y sz − m2v[tx ,t y ,tz ,sx ,s y ,sz ] ([R, t]12 )

(9.19)

i=1

C  

t x t y t z q0 q1 q2 q3



− m2v[tx ,t y ,tz ,q0 ,q1 ,q2 ,q3 ] ([R, t]12 )

2 (9.20)

i=1

Table 9.1 shows an example of the design matrix for the optimization problem of four poses expressed as Tait–Bryan parameterization of the rotation matrix. Having a relative pose [R, t]12 of [R, t]1 and [R, t]2 expressed using Tait–Bryan angles r

parameterization (x 1 , y 1 , z 1 , ω1 , ϕ 1 , κ 1 ), (x 2 , y 2 , z 2 , ω2 , ϕ 2 , κ 2 ), the Jacobian J of the objective function is given in Eq. (9.21).

κ δ,23

ϕ δ,23

ωδ,23

z δ,23

y δ,23

x δ,23

κ δ,12

ϕ δ,12

ωδ,12

z δ,12

y δ,12

x δ,12

κ δ,01

ϕ δ,01

ωδ,01

z δ,01

y δ,01

x δ,01

0 d12

0 d22

0 d32 0 d42 0 d52 0 d62

0 d21

0 d31 0 d41 0 d51 0 d61

Y

0 d11

X

Pose0

0 d33 0 d43 0 d53 0 d63

0 d23

0 d13

Z

0 d34 0 d44 0 d54 0 d64

0 d24

0 d14

ω

0 d35 0 d45 0 d55 0 d65

0 d25

0 d15

ϕ

0 d36 0 d46 0 d56 0 d66

0 d26

0 d16

κ

1 d31 1 d41 1 d51 1 d61 1 d11 1 d21 1 d31 1 d41 1 d51 1 d61

1 d21

1 d11

X

Pose1

1 d32 1 d42 1 d52 1 d62 1 d12 1 d22 1 d32 1 d42 1 d52 1 d62

1 d22

1 d12

Y

1 d33 1 d43 1 d53 1 d63 1 d13 1 d23 1 d33 1 d43 1 d53 1 d63

1 d23

1 d13

Z

1 d34 1 d44 1 d54 1 d64 1 d14 1 d24 1 d34 1 d44 1 d54 1 d64

1 d24

1 d14

ω

1 d35 1 d45 1 d55 1 d65 1 d15 1 d25 1 d35 1 d45 1 d55 1 d65

1 d25

1 d15

ϕ

Pose2

2 d11 2 d21 2 d31 2 d41 2 d51 2 d61 2 d11 2 d21 2 d31 2 d41 2 d51 2 d61

1 d26 1 d36 1 d46 1 d56 1 d66

X

1 d16

1 d66

1 d56

1 d46

1 d36

1 d26

1 d16

κ

2 d22 2 d32 2 d42 2 d52 2 d62 2 d12 2 d22 2 d32 2 d42 2 d52 2 d62

2 d12

Y

2 d23 2 d33 2 d43 2 d53 2 d63 2 d13 2 d23 2 d33 2 d43 2 d53 2 d63

2 d13

Z

2 d24 2 d34 2 d44 2 d54 2 d64 2 d14 2 d24 2 d34 2 d44 2 d54 2 d64

2 d14

ω

2 d25 2 d35 2 d45 2 d55 2 d65 2 d15 2 d25 2 d35 2 d45 2 d55 2 d65

2 d15

ϕ

Pose3

3 d11 3 d21 3 d31 3 d41 to d51 3 d61

2 d26 2 d36 2 d46 2 d56 2 d66

X

2 d16

2 d66

2 d56

2 d46

2 d36

2 d26

2 d16

κ

3 d32 3 d42 3 d52 3 d62

3 d22

3 d12

Y

3 d33 3 d43 3 d53 3 d63

3 d23

3 d13

Z

3 d34 3 d44 3 d54 3 d64

3 d24

3 d14

ω

3 d35 3 d45 3 d55 3 d65

3 d25

3 d15

ϕ

3 d66

3 d56

3 d46

3 d36

to d26

3 d16

κ

Table 9.1 Example of design matrix for Tait–Bryan parameterization of the rotation matrix for trajectory composed of four consecutive poses and three consecutive relative poses between them

246 9 Constraints

9.5 Relative Pose

⎛

d1,1 ⎜ d2,1 ⎜ r ⎜ d3,1 J =⎜ ⎜ 0 ⎜ ⎝ 0 0

247

d1,2 d2,2 d3,2 0 0 0

⎞ d1,9 0 0 0 d2,9 0 0 0 ⎟ ⎟ d3,9 0 0 0 ⎟ ⎟= 0 d4,10 d4,11 0 ⎟ ⎟ 0 d5,10 d5,11 0 ⎠ 0 d6,10 d6,11 d6,12 ⎞ δ δ δ ∂t x x ∂tx ∂tx 0 0 0 ∂κ 1 ∂tx2 ∂t y2 ∂tz2 ⎟ ⎟ ∂t yδ ∂t yδ ∂t yδ ∂t yδ 0 0 0 ⎟ ∂κ 1 ∂tx2 ∂t y2 ∂tz2 ⎟ ⎟ ∂tzδ ∂tzδ ∂tzδ 0 ∂t 2 ∂t 2 ∂t 2 0 0 0 ⎟ ⎟ (9.21) x y z ⎟ ∂ω ∂ω ∂ω 0 0 0 ∂ω2 ∂ϕ 2 0 ⎟ ⎟ ∂κ 1 ⎟ ∂ϕ ∂ϕ ∂ϕ 0 0 0 0 ⎠ 1 2 2 ∂κ ∂ω ∂ϕ ∂κ ∂κ ∂κ ∂κ 0 0 0 ∂κ 1 ∂ω2 ∂ϕ 2 ∂κ 2

d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d3,3 d3,4 d3,5 0 d3,7 d3,8 0 d4,4 d4,5 d4,6 0 0 0 d5,4 d5,5 d5,6 0 0 0 d6,4 d6,5 d6,6 0 0 ⎛ ∂t δ ∂t δ ∂t δ ∂t δ ∂t δ ∂t δ x 1

x 1

x 1

⎜ ∂txδ ∂t yδ ∂tzδ ⎜ ∂t y ∂t y ∂t y ⎜ ∂t 1 ∂t 1 ∂t 1 ⎜ x y z ⎜ ∂t δ ∂t δ ∂t δ ⎜ z z z ⎜ ∂tx1 ∂t y1 ∂tz1 ⎜ ⎜ 0 0 0 ⎜ ⎜ ⎝ 0 0 0 0 0 0

x

x

∂ω1 ∂ϕ 1 ∂t yδ ∂t yδ ∂ω1 ∂ϕ 1 ∂tzδ ∂tzδ ∂ω1 ∂ϕ 1 ∂ω ∂ω ∂ω1 ∂ϕ 1 ∂ϕ ∂ϕ ∂ω1 ∂ϕ 1 ∂κ ∂κ ∂ω1 ∂ϕ 1

For two poses [R1 , t 1 ] and [R2 , t 2 ] expressed using Rodrigues parameterization of r

the rotation matrix (tx1 , t y1 , tz1 , sx1 , s y1 , sz1 ), (tx2 , t y2 , tz2 , sx2 , s y2 , sz2 ) the Jacobian J of the objective function for relative pose observation is given in Eq. (9.22). ⎛

d1,1 ⎜ d2,1 ⎜ r ⎜ d3,1 J =⎜ ⎜ 0 ⎜ ⎝ 0 0

d1,2 d2,2 d3,2 0 0 0

d1,3 d1,4 d2,3 d2,4 d3,3 d3,4 0 d4,4 0 d5,4 0 d6,4 ⎛ δ

d1,5 d2,5 d3,5 d4,5 d5,5 d6,5

d1,6 d2,6 d3,6 d4,6 d5,6 d6,6

d1,7 d2,7 d3,7 0 0 0

∂tx ∂txδ ∂txδ ∂txδ 1 1 1 ∂s 1 x ∂t yδ ∂sx1 ∂tzδ ∂sx1 ∂sx ∂sx1 ∂s y ∂sx1 ∂sz ∂sx1

⎜ ∂txδ ∂t yδ ∂tzδ ⎜ ∂t y ∂t y ∂t y ⎜ ∂t 1 ∂t 1 ∂t 1 ⎜ xδ yδ zδ ⎜ ∂tz ∂tz ∂tz ⎜ ∂tx1 ∂t y1 ∂tz1 ⎜ ⎜ 0 0 0 ⎜ ⎜ ⎜ 0 0 0 ⎝ 0 0 0

⎞ d1,9 0 0 0 d2,9 0 0 0 ⎟ ⎟ d3,9 0 0 0 ⎟ ⎟= 0 d4,10 d4,11 d4,12 ⎟ ⎟ 0 d5,10 d5,11 d5,12 ⎠ 0 d6,10 d6,11 d6,12 ⎞ ∂txδ ∂txδ ∂txδ ∂txδ 0 0 0 ∂sz1 ∂tx2 ∂t y2 ∂tz2 ⎟ ∂t yδ ∂t yδ ∂t yδ ∂t yδ ⎟ 0 0 0 ⎟ 1 2 2 2 ∂sz ∂tx ∂t y ∂tz ⎟ δ δ δ δ ⎟ ∂tz ∂tz ∂tz ∂tz 0 0 0 ⎟ ∂sz1 ∂tx2 ∂t y2 ∂tz2 ⎟ (9.22) ∂sx ∂sx ∂sx ∂sx ⎟ 0 0 0 ∂s 2 ∂s 2 ∂s 2 ⎟ ∂sz1 x y z ∂s y ∂s ∂s ∂s ⎟ 0 0 0 ∂s y2 ∂s y2 ∂s y2 ⎟ ∂sz1 x y z ⎠ ∂sz ∂sz ∂sz ∂sz 0 0 0 ∂s 2 ∂s 2 ∂s 2 ∂s 1

d1,8 d2,8 d3,8 0 0 0

∂txδ ∂s y1 ∂t yδ ∂s y1 ∂tzδ ∂s y1 ∂sx ∂s y1 ∂s y ∂s y1 ∂sz ∂s y1

z

x

y

z

Consequently, for two poses [R1 , t 1 ], [R2 , t 2 ] expressed using quaternion parameterization of the rotation matrix (tx1 , t y1 , tz1 , q01 , q11 , q21 , q31 ), (tx2 , t y2 , tz2 , q02 , q12 , q22 , q32 ), r

the Jacobian J of the objective function for relative pose observation is given in Eq. (9.23).

248

9 Constraints ⎛

d1,1 ⎜ d2,1 ⎜ ⎜ d3,1 r ⎜ J =⎜ ⎜ 0 ⎜ 0 ⎜ ⎝ 0 0

d1,2 d2,2 d3,2 0 0 0 0

d1,3 d1,4 d2,3 d2,4 d3,3 d3,4 0 d4,4 0 d5,4 0 d6,4 0 d7,4 ⎛ δ δ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝

d1,5 d2,5 d3,5 d4,5 d5,5 d6,5 d7,5

d1,6 d2,6 d3,6 d4,6 d5,6 d6,6 d6,6

d1,7 d2,7 d3,7 d4,7 d5,7 d6,7 d7,7

d1,8 d2,8 d3,8 0 0 0 0

d1,9 d2,9 d3,9 0 0 0 0

d1,10 d2,10 d3,10 0 0 0 0

∂tx ∂tx ∂txδ ∂txδ ∂txδ ∂txδ ∂txδ ∂txδ ∂tx1 ∂t y1 ∂tz1 ∂q01 ∂q11 ∂q21 ∂q31 ∂tx2 ∂t yδ ∂t yδ ∂t yδ ∂t yδ ∂t yδ ∂t yδ ∂t yδ ∂t yδ ∂tx1 ∂t y1 ∂tz1 ∂q01 ∂q11 ∂q21 ∂q31 ∂tx2 ∂tzδ ∂tzδ ∂tzδ ∂tzδ ∂tzδ ∂tzδ ∂tzδ ∂tzδ ∂tx1 ∂t y1 ∂tz1 ∂q01 ∂q11 ∂q21 ∂q31 ∂tx2 ∂q0 ∂q0 ∂q0 ∂q0 0 0 0 0 ∂q01 ∂q11 ∂q21 ∂q31 ∂q1 ∂q1 ∂q1 ∂q1 0 0 0 0 ∂q01 ∂q11 ∂q21 ∂q31 ∂q2 ∂q2 ∂q2 ∂q2 0 0 0 0 ∂q01 ∂q11 ∂q21 ∂q31 ∂q3 ∂q3 ∂q3 ∂q3 0 0 0 0 ∂q01 ∂q11 ∂q21 ∂q31

0 0 0

0 0 0

0 0 0

0 0 0

⎞

⎟ ⎟ ⎟ ⎟ d4,11 d4,12 d4,13 d4,14 ⎟ ⎟= d5,11 d5,12 d5,13 d5,14 ⎟ ⎟ d6,11 d6,12 d6,13 d6,14 ⎠ d7,11 d7,12 d7,13 d7,14 ⎞ ∂txδ ∂txδ 0 0 0 0 ⎟ ∂t y2 ∂tz2 ⎟ ∂t yδ ∂t yδ ⎟ 0 0 0 0 ⎟ 2 2 ⎟ ∂t y ∂tz ⎟ ⎟ ∂tzδ ∂tzδ 0 0 0 0 ⎟ 2 2 ⎟ ∂t y ∂tz ∂q0 ∂q0 ∂q0 ∂q0 ⎟ ⎟ (9.23) 0 0 2 2 2 2 ∂q0 ∂q1 ∂q2 ∂q3 ⎟ ⎟ ∂q1 ∂q1 ∂q1 ∂q1 ⎟ 0 0 ⎟ 2 2 2 2 ∂q0 ∂q1 ∂q2 ∂q3 ⎟ ∂q2 ∂q2 ∂q2 ∂q2 ⎟ ⎟ 0 0 ∂q02 ∂q12 ∂q22 ∂q32 ⎟ ⎠ ∂q3 ∂q3 ∂q3 ∂q3 0 0 2 2 2 2 ∂q0 ∂q1 ∂q2 ∂q3

The Python code for generating relative pose (1) observation equation is given in Listing 9.4. 1 2 3 4 5 6

from sympy i m p o r t * i m p o r t sys sys . path . insert (1 , ’ .. ’ ) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14 15 16 17 18 19

tx_1 , ty_1 , tz_1 = s y m b o l s ( ’ tx_1 ty_1 tz_1 ’ ) om_1 , fi_1 , ka_1 = s y m b o l s ( ’ om_1 fi_1 ka_1 ’ ) # sx_1 , sy_1 , sz_1 = s y m b o l s ( ’ sx_1 sy_1 sz_1 ’) # q0_1 , q1_1 , q2_1 , q3_1 = s y m b o l s ( ’ q0_1 q1_1 q2_1 q3_1 ’) tx_2 , ty_2 , tz_2 = s y m b o l s ( ’ tx_2 ty_2 tz_2 ’ ) om_2 , fi_2 , ka_2 = s y m b o l s ( ’ om_2 fi_2 ka_2 ’ ) # sx_2 , sy_2 , sz_2 = s y m b o l s ( ’ sx_2 sy_2 sz_2 ’) # q0_2 , q1_2 , q2_2 , q3_2 = s y m b o l s ( ’ q0_2 q1_2 q2_2 q3_2 ’) tx_m , ty_m , tz_m = s y m b o l s ( ’ tx_m ty_m tz_m ’ ) om_m , fi_m , ka_m = s y m b o l s ( ’ om_m fi_m ka_m ’ ) # sx_m , sy_m , sz_m = s y m b o l s ( ’ sx_m sy_m sz_m ’) # q0_m , q1_m , q2_m , q3_m = s y m b o l s ( ’ q0_m q1_m q2_m q3_m ’)

20 21 22 23 24 25 26 27 28 29

30

p o s i t i o n _ s y m b o l s _ 1 = [ tx_1 , ty_1 , tz_1 ] o r i e n t a t i o n _ s y m b o l s _ 1 = [ om_1 , fi_1 , ka_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ sx_1 , sy_1 , sz_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ q0_1 , q1_1 , q2_1 , q3_1 ] p o s i t i o n _ s y m b o l s _ 2 = [ tx_2 , ty_2 , tz_2 ] o r i e n t a t i o n _ s y m b o l s _ 2 = [ om_2 , fi_2 , ka_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ sx_2 , sy_2 , sz_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ q0_2 , q1_2 , q2_2 , q3_2 ] all_symbols = position_symbols_1 + orientation_symbols_1 + position_symbols_2 + orientation_symbols_2

9.5 Relative Pose 31

32

33

34

35

36

249

R t _ w c _ 1 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_1 , ty_1 , tz_1 , om_1 , fi_1 , ka_1 ) # R t _ w c _ 1 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_1 , ty_1 , tz_1 , sx_1 , sy_1 , sz_1 ) # R t _ w c _ 1 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_1 , ty_1 , tz_1 , q0_1 , q1_1 , q2_1 , q3_1 ) R t _ w c _ 2 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_2 , ty_2 , tz_2 , om_2 , fi_2 , ka_2 ) # R t _ w c _ 2 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_2 , ty_2 , tz_2 , sx_2 , sy_2 , sz_2 ) # R t _ w c _ 2 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_2 , ty_2 , tz_2 , q0_2 , q1_2 , q2_2 , q3_2 )

37 38 39 40 41 42

R _ c w _ 1 = R t _ w c _ 1 [: -1 ,: -1]. t r a n s p o s e () t _ w c _ 1 = M a t r i x ([ tx_1 , ty_1 , tz_1 ]) . vec () t_cw_1 =- R_cw_1 * t_wc_1 R t _ c w _ 1 = M a t r i x . h s t a c k ( R_cw_1 , t _ c w _ 1 ) R t _ c w _ 1 = M a t r i x . v s t a c k ( Rt_cw_1 , M a t r i x ([[0 ,0 ,0 ,1]]) )

43 44

relative_pose = Rt_cw_1 * Rt_wc_2

45 46 47 48 49

parametrization = taitBryanFromMatrix44Case1 ( relative_pose ) # parametrization = rodriguesFromMatrix44 ( relative_pose ) # parametrization = quaternionFromMatrix44 ( relative_pose ) t = r e l a t i v e _ p o s e [0 : 3 , 3]

50 51 52 53

54 55 56

t a r g e t _ v a l u e = M a t r i x ([ tx_m , ty_m , tz_m , om_m , fi_m , ka_m ]) # t a r g e t _ v a l u e = M a t r i x ([ tx_m , ty_m , tz_m , sx_m , sy_m , sz_m ]) # t a r g e t _ v a l u e = M a t r i x ([ tx_m , ty_m , tz_m , q0_m , q1_m , q2_m , q3_m ]) model_function =t. col_join ( parametrization ) delta = target_value - model_function d e l t a _ j a c o b i a n = delta . j a c o b i a n ( a l l _ s y m b o l s )

Listing 9.4 Python code for generating relative pose (1) observation equations and the Jacobian r

J of the objective function for Tait–Bryan rotation matrix parameterization

9.5.2 Variant (2) Relative pose constraint can be constructed using Eq. (9.24),

I

 4×4

 =

12 −1  12 R t R t 01×3 1 m 01×3 1

(9.24)

12 where [I 4×4 ] is an identity matrix, [R, t]12 m is a measured relative pose and [R, t] given by (9.14) is an expected relative pose. The expected relative pose supposed to converge to the measured relative pose during the optimization process. The observation equations become (9.25) for Tait–Bryan, (9.26) for Rodrigues and (9.26)

250

9 Constraints

for quaternion parameterization. Function m2v retrieves β = (tx , t y , tz , ω, ϕ, κ) for Tait–Bryan parameterization, β = (tx , t y , tz , sx , s y , sz ) for Rodrigues parame terization   and β = (tx , t y , tz , q0 , q1 , q2 , q3 ) for quaternion parameterization from R t . 01×3 1 ⎡ ⎤ 0 ⎢0⎥ ⎢ ⎥ ⎢0⎥ ⎢ ⎥ ⎢0⎥ ⎢ ⎥ ⎣0⎦ 0 

⎡

⎤ txδ ⎢ t yδ ⎥ ⎢ ⎥ ⎢ tδ ⎥ ⎢ z⎥ = ⎢ωδ ⎥ ⎢ ⎥ ⎣ϕ δ ⎦ κδ    r esiduals

r esiduals

⎡ ⎤ 0 ⎢0 ⎥ ⎢ ⎥ ⎢0 ⎥ ⎢ ⎥ ⎢0 ⎥ ⎢ ⎥ ⎣0 ⎦ 0 

⎛ ⎞  12 −1  12 R t R t ⎠ − m2v[tx ,t y ,tz ,sx ,s y ,sz ] ⎝ 01×3 1 m 01×3 1   

(9.26)

model f unction

target values

⎡ δ⎤ tx ⎢ t yδ ⎥ ⎢ δ⎥ ⎢ tz ⎥ ⎢ δ⎥ ⎢q ⎥ = ⎢ 0⎥ ⎢q δ ⎥ ⎢ 1⎥ ⎣q δ ⎦

⎡ ⎤ 0 ⎢0 ⎥ ⎢ ⎥ ⎢0 ⎥ ⎢ ⎥ ⎢0 ⎥ ⎢ ⎥ ⎢0 ⎥ ⎢ ⎥ ⎣0 ⎦

2

r esiduals

model f unction

target values

⎡ δ⎤ tx ⎢ t yδ ⎥ ⎢ δ⎥ ⎢ tz ⎥ ⎢ ⎥ = ⎢s δ ⎥ ⎢ x⎥ ⎣s δ ⎦ y szδ   

q3δ   

⎛ ⎞  12 −1  12 R t R t ⎠ (9.25) − m2v[tx ,t y ,tz ,ω,ϕ,κ ] ⎝ 01×3 1 m 01×3 1   

0 

⎛ ⎞  12 −1  12 R t R t ⎠ − m2v[tx ,t y ,tz ,q0 ,q1 ,q2 ,q3 ] ⎝ 01×3 1 m 01×3 1   

(9.27)

model f unction

target values

It can be seen that the target values are obtained from m2v([I 4×4 ]). The optimization problems for the relative pose (2) constraints are defined in Eqs. (9.28), (9.29) and (9.30) for Tait–Bryan, Rodrigues and quaternion parameterizations of the rotation matrix. ⎛ ⎛ ⎞⎞2  12 −1  12 C 

 R t R t ⎠⎠ ⎝ ⎝ 0 0 0 0 0 0 − m2v[tx ,t y ,tz ,ω,ϕ,κ ] min 01×3 1 m 01×3 1 R1 ,t 1 ,R2 ,t 2 i=1

(9.28)

9.5 Relative Pose

251

⎛ ⎛ ⎞⎞2  12 −1  12 C 

 R t R t ⎝ 0 0 0 0 0 0 − m2v[t ,t ,t ,s ,s ,s ] ⎝ ⎠⎠ min x y z x y z 01×3 1 m 01×3 1 R1 ,t 1 ,R2 ,t 2 i=1

(9.29) ⎛ ⎞⎞2 12 −1  12  C 

 R t R t ⎝ 0 0 0 0 0 0 0 − m2v[t ,t ,t ,q ,q ,q ,q ] ⎝ ⎠⎠ min x y z 0 1 2 3 01×3 1 01×3 1 m R1 ,t 1 ,R2 ,t 2 ⎛

i=1

(9.30) The Python code for generating relative pose (2) observation equation is given in Listing 9.5. 1 2 3 4 5 6

from s y m p y i m p o r t * i m p o r t sys sys . path . insert (1 , ’ .. ’ ) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14 15 16 17 18 19

tx_1 , ty_1 , tz_1 = s y m b o l s ( ’ tx_1 ty_1 tz_1 ’ ) om_1 , fi_1 , ka_1 = s y m b o l s ( ’ om_1 fi_1 ka_1 ’ ) # sx_1 , sy_1 , sz_1 = s y m b o l s ( ’ sx_1 sy_1 sz_1 ’) # q0_1 , q1_1 , q2_1 , q3_1 = s y m b o l s ( ’ q0_1 q1_1 q2_1 q3_1 ’) tx_2 , ty_2 , tz_2 = s y m b o l s ( ’ tx_2 ty_2 tz_2 ’ ) om_2 , fi_2 , ka_2 = s y m b o l s ( ’ om_2 fi_2 ka_2 ’ ) # sx_2 , sy_2 , sz_2 = s y m b o l s ( ’ sx_2 sy_2 sz_2 ’) # q0_2 , q1_2 , q2_2 , q3_2 = s y m b o l s ( ’ q0_2 q1_2 q2_2 q3_2 ’) tx_m , ty_m , tz_m = s y m b o l s ( ’ tx_m ty_m tz_m ’ ) om_m , fi_m , ka_m = s y m b o l s ( ’ om_m fi_m ka_m ’ ) # sx_m , sy_m , sz_m = s y m b o l s ( ’ sx_m sy_m sz_m ’) # q0_m , q1_m , q2_m , q3_m = s y m b o l s ( ’ q0_m q1_m q2_m q3_m ’)

20 21 22 23 24 25 26 27 28 29

p o s i t i o n _ s y m b o l s _ 1 = [ tx_1 , ty_1 , tz_1 ] o r i e n t a t i o n _ s y m b o l s _ 1 = [ om_1 , fi_1 , ka_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ sx_1 , sy_1 , sz_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ q0_1 , q1_1 , q2_1 , q3_1 ] p o s i t i o n _ s y m b o l s _ 2 = [ tx_2 , ty_2 , tz_2 ] o r i e n t a t i o n _ s y m b o l s _ 2 = [ om_2 , fi_2 , ka_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ sx_2 , sy_2 , sz_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ q0_2 , q1_2 , q2_2 , q3_2 ] all_symbols = position_symbols_1 + orientation_symbols_1 + position_symbols_2 + orientation_symbols_2

30 31

32

33

34

35

R t _ w c _ 1 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_1 , ty_1 , tz_1 , om_1 , fi_1 , ka_1 ) # R t _ w c _ 1 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_1 , ty_1 , tz_1 , sx_1 , sy_1 , sz_1 ) # R t _ w c _ 1 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_1 , ty_1 , tz_1 , q0_1 , q1_1 , q2_1 , q3_1 ) R t _ w c _ 2 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_2 , ty_2 , tz_2 , om_2 , fi_2 , ka_2 ) # R t _ w c _ 2 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_2 , ty_2 , tz_2 , sx_2 , sy_2 , sz_2 )

252 36

37

38

39

9 Constraints

# R t _ w c _ 2 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_2 , ty_2 , tz_2 , q0_2 , q1_2 , q2_2 , q3_2 ) R t _ w c _ m = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_m , ty_m , tz_m , om_m , fi_m , ka_m ) # R t _ w c _ m = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_m , ty_m , tz_m , sx_m , sy_m , sz_m ) # R t _ w c _ m = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_m , ty_m , tz_m , q0_2 , q1_2 , q2_2 , q3_2 )

40 41 42 43 44 45

R _ c w _ 1 = R t _ w c _ 1 [: -1 ,: -1]. t r a n s p o s e () t _ w c _ 1 = M a t r i x ([ tx_1 , ty_1 , tz_1 ]) . vec () t_cw_1 =- R_cw_1 * t_wc_1 R t _ c w _ 1 = M a t r i x . h s t a c k ( R_cw_1 , t _ c w _ 1 ) R t _ c w _ 1 = M a t r i x . v s t a c k ( Rt_cw_1 , M a t r i x ([[0 ,0 ,0 ,1]]) )

46 47 48 49 50 51

R _ c w _ m = R t _ w c _ m [: -1 ,: -1]. t r a n s p o s e () t _ w c _ m = M a t r i x ([ tx_m , ty_m , tz_m ]) . vec () t_cw_m =- R_cw_m * t_wc_m R t _ c w _ m = M a t r i x . h s t a c k ( R_cw_m , t _ c w _ m ) R t _ c w _ m = M a t r i x . v s t a c k ( Rt_cw_m , M a t r i x ([[0 ,0 ,0 ,1]]) )

52 53

mf = R t _ c w _ m * R t _ c w _ 1 * R t _ w c _ 2

54 55 56 57 58

p a r a m e t r i z a t i o n = t a i t B r y a n F r o m M a t r i x 4 4 C a s e 1 ( mf ) # p a r a m e t r i z a t i o n = r o d r i g u e s F r o m M a t r i x 4 4 ( mf ) # p a r a m e t r i z a t i o n = q u a t e r n i o n F r o m M a t r i x 4 4 ( mf ) t = mf [0 : 3 , 3]

59 60 61 62 63 64 65

t a r g e t _ v a l u e = M a t r i x ([0 , 0 , 0 , 0 , 0 , 0]) # t a r g e t _ v a l u e = M a t r i x ([0 , 0 , 0 , 0 , 0 , 0]) # t a r g e t _ v a l u e = M a t r i x ([0 , 0 , 0 , 0 , 0 , 0 , 0]) model_function =t. col_join ( parametrization ) delta = target_value - model_function delta_jacobian = delta . jacobian ( all_symbols )

Listing 9.5 Python code for generating relative pose (2) observation equations and the Jacobian r

J of the objective function for Tait–Bryan rotation matrix parameterization

9.6 Smoothness In mobile mapping scenarios, it is necessary to cope with noisy measurements that cause a not smooth trajectory. The smoothness constraint can address this issue. Therefore, it uses an assumption that three consecutive poses [R, t]1 , [R, t]2 and [R, t]3 composes two consecutive relative poses [R, t]12 and [R, t]23 that satisfy a condition given in an Eq. (9.31).

9.6 Smoothness

253



1 −1 

⎤ txδ ⎢ t yδ ⎥ ⎢ δ⎥ ⎢t ⎥ ⎢ z⎥ = ⎢ω δ ⎥ ⎢ ⎥ ⎣ϕ δ ⎦ κδ   

23

⎡

r esiduals

⎡ ⎤ 0 ⎢0⎥ ⎢ ⎥ ⎢0⎥ ⎢ ⎥ ⎢0⎥ ⎢ ⎥ ⎣0⎦ 0 

⎡ δ⎤ tx ⎢ t yδ ⎥ ⎢ δ⎥ ⎢ tz ⎥ ⎢ ⎥ ⎢q δ ⎥ = ⎢ 0⎥ ⎢q δ ⎥ ⎢ 1⎥ ⎣q δ ⎦ 2

q3δ   

residuals

R t 01×3 1

2 −1 

 − m2v[tx ,t y ,tz ,ω,ϕ,κ ] ([R, t]12 ) − m2v[tx ,t y ,tz ,ω,ϕ,κ ] ([R, t]23 )   

(9.32)

model f unction

target values

⎡ δ⎤ tx ⎢t δ ⎥ ⎢ y⎥ ⎢ δ⎥ ⎢ tz ⎥ ⎢ δ⎥ = ⎢sx ⎥ ⎢ δ⎥ ⎣s y ⎦ szδ    r esiduals

R t 01×3 1

2



3 R t [R, t] = [R, t] ⇒ = 01×3 1 (9.31) It can be seen that we expect that the previous relative pose [R, t]12 will be the same as the consecutive relative pose [R, t]23 , thus noisy trajectory poses should be optimized to the smooth shape. To build smoothness observation equation, we need to consider parameterization of the consecutive relative poses into Tait–Bryan, Rodrigues or quaternion representation of the rotation matrix. Such parameterization functions m2v were shown in Listings 3.1, 3.2 and 3.3. Thus, function m2v[β] ([R, t]12 ) retrieves β = (tx , t y , tz , ω, ϕ, κ) for Tait–Bryan parameterization, β = (tx , t y , tz , sx , s y , sz ) for Rodrigues parameterization and β = (tx , t y , tz , q0 , q1 ,  q2 , q3 ) for Quaternion parameterization of the rotation matrix from [R, t]12 and m2v([R, t]23 ) function retrieves these parameters from [R, t]23 . Finally, smoothness observation equations for different rotation parameterizations are given in (9.32), (9.33) and (9.34). 12

R t 01×3 1

⎡ ⎤ 0 ⎢0 ⎥ ⎢ ⎥ ⎢ ⎥ ⎢0 ⎥ ⎢ ⎥ ⎢0 ⎥ ⎢ ⎥ ⎣0 ⎦ 0 

 − m2v t ,t ,t ,s ,s ,s  ([R, t]12 ) − m2v t ,t ,t ,s ,s ,s  ([R, t]23 ) x y z x y z x y z x y z   

(9.33)

model f unction

target values

⎡ ⎤ 0 ⎢0⎥ ⎢ ⎥ ⎢0⎥ ⎢ ⎥ ⎢0⎥ ⎢ ⎥ ⎢0⎥ ⎢ ⎥ ⎣0⎦

 − m2v[tx ,t y ,tz ,q0 ,q1 ,q2 ,q3 ] ([R, t]12 ) − m2v[tx ,t y ,tz ,q0 ,q1 ,q2 ,q3 ] ([R, t]23 )   

(9.34)

model f unction

0  target values

   where txδ t yδ tzδ ωδ ϕ δ κ δ , txδ t yδ tzδ sxδ s yδ szδ , txδ t yδ tzδ q0δ q1δ q2δ q3δ are residuals, [0] are target values and m2v[β] ([R, t]12 ) − m2v[β] ([R, t]23 ) are model functions. The optimization problems for smoothness constraint assuming different rotation matrices parameterization are defined in Eqs. (9.35), (9.36) and (9.37).

254

9 Constraints ⎛⎡ ⎤ ⎞2 0 ⎜ ⎢0 ⎥ ⎟ ⎜⎢ ⎥  ⎟ ⎜ ⎢0 ⎥ ⎟ ⎜⎢ ⎥ − m2v t ,t ,t ,ω,ϕ,κ ([R, t]12 ) − m2v t ,t ,t ,ω,ϕ,κ ([R, t]23 ) ⎟ min [x y z ] ] [x y z ⎜⎢ ⎥ ⎟ R1 ,t 1 ,R2 ,t 2 ,R3 ,t 3 ⎜⎢0⎥ ⎟ ⎝ ⎣0 ⎦ ⎠ 0

(9.35) ⎛⎡ ⎤ ⎞2 0 ⎜ ⎢0 ⎥ ⎟ ⎜⎢ ⎥  ⎟ ⎜ ⎢0 ⎥ ⎟ ⎜⎢ ⎥ − m2v t ,t ,t ,s ,s ,s ([R, t]12 ) − m2v t ,t ,t ,s ,s ,s ([R, t]23 ) ⎟ min [ x y z x y z] [ x y z x y z] ⎜⎢ ⎥ ⎟ R1 ,t 1 ,R2 ,t 2 ,R3 ,t 3 ⎜⎢0⎥ ⎟ ⎝ ⎣0 ⎦ ⎠ 0

(9.36) ⎛⎡ ⎤ ⎞2 0 ⎜ ⎢0 ⎥ ⎟ ⎜⎢ ⎥ ⎟ ⎜ ⎢0 ⎥  ⎟ ⎜⎢ ⎥ ⎟ ⎜⎢0⎥ − m2v t ,t ,t ,q ,q ,q ,q ([R, t]12 ) − m2v t ,t ,t ,q ,q ,q ,q ([R, t]23 ) ⎟ min [ ] ] [ x y z x y z 0 1 2 3 0 1 2 3 ⎜ ⎢ ⎟ ⎥ R1 ,t 1 ,R2 ,t 2 ,R3 ,t 3 ⎜⎢ ⎥ ⎟ ⎜ ⎢0 ⎥ ⎟ ⎝ ⎣0 ⎦ ⎠ 0

(9.37) For two consecutive relative poses [R, t]12 , [R, t]23 built from three consecutive poses [R, t]1 , [R, t]2 and [R, t]3 expressed using Tait–Bryan angles parameterization (tx1 , t y1 , tz1 , ω1 , ϕ 1 , κ 1 ),(tx2 , t y2 , tz2 , ω2 , ϕ 2 , κ 2 ) and (tx3 , t y3 , tz3 , ω3 , ϕ 3 , κ 3 ), the r

Jacobian J of the objective function is given in Eq. (9.38). ⎛

d1,1 d1,2 d1,3 ⎜d ⎜ 2,1 d2,2 d2,3 ⎜ r d d ⎜d J = ⎜ 3,1 3,2 3,3 ⎜ 0 0 0 ⎜ ⎝ 0 0 0 0 0 0 ⎛ δ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝

d1,4 d2,4 d3,4 d4,4 d5,4 d6,4

∂tx ∂txδ ∂tx1 ∂t y1 ∂t yδ ∂t yδ ∂tx1 ∂t y1

d1,5 d2,5 d3,5 d4,5 d5,5 d6,5

d1,6 d1,7 d1,8 d1,9 d2,6 d2,7 d2,8 d2,9 0 d3,7 d3,8 d3,9 d4,6 0 0 0 d5,6 0 0 0 d6,6 0 0 0

d1,10 d2,10 d3,10 d4,10 d5,10 d6,10

⎞ d1,12 d1,13 d1,14 d1,15 0 0 0 d2,12 d2,13 d2,14 d2,15 0 0 0 ⎟ ⎟ ⎟ 0 d3,13 d3,14 d3,15 0 0 0 ⎟ ⎟= d4,12 0 0 0 d4,16 d4,17 0 ⎟ ⎟ d5,12 0 0 0 d5,16 d5,17 0 ⎠ d6,12 0 0 0 d6,16 d6,17 d6,18 ⎞ ∂txδ ∂txδ ∂txδ ∂txδ ∂txδ ∂txδ 0 0 0 ⎟ ∂ω2 ∂ϕ 2 ∂κ 2 ∂tx3 ∂t y3 ∂tz3 ⎟ ⎟ ⎟ ∂t yδ ∂t yδ ∂t yδ ∂t yδ ∂t yδ ∂t yδ 0 0 0 ⎟ ⎟ 3 3 3 2 2 2 ∂ω ∂ϕ ∂κ ∂tx ∂t y ∂tz ⎟ ⎟ δ δ δ δ δ ⎟ ∂tz ∂tz ∂tz ∂tz ∂tz ⎟ 0 0 0 0 ⎟ ∂ω2 ∂ϕ 2 ∂tx3 ∂t y3 ∂tz3 ⎟ ⎟ ⎟ ∂ω ∂ω ∂ω ∂ω ∂ω 0 0 0 0 ⎟ ∂ω2 ∂ϕ 2 ∂κ 2 ∂ω3 ∂ϕ 3 ⎟ ⎟ ∂ϕ ∂ϕ ∂ϕ ∂ϕ ∂ϕ ⎟ 0 0 0 0 2 2 2 3 3 ⎠ ∂ω ∂ϕ ∂κ ∂ω ∂ϕ ∂κ ∂κ ∂κ ∂κ ∂κ ∂κ 0 0 2 2 2 0 3 3 3

d1,11 d2,11 d3,11 d4,11 d5,11 d6,11

∂txδ ∂txδ ∂txδ ∂txδ ∂txδ ∂txδ ∂txδ ∂tz1 ∂ω1 ∂ϕ 1 ∂κ 1 ∂tx2 ∂t y2 ∂tz2 ∂t yδ ∂t yδ ∂t yδ ∂t yδ ∂t yδ ∂t yδ ∂t yδ ∂tz1 ∂ω1 ∂ϕ 1 ∂κ 1 ∂tx2 ∂t y2 ∂tz2 ∂tzδ ∂tzδ ∂tzδ ∂tzδ ∂tzδ ∂tzδ ∂tzδ ∂tzδ 0 ∂tx1 ∂t y1 ∂tz1 ∂ω1 ∂ϕ 1 ∂tx2 ∂t y2 ∂tz2 0 0 0 0 0 ∂ω1 ∂ω1 ∂ω1 0 ∂ω ∂ϕ ∂κ 0 0 0 ∂ϕ1 ∂ϕ1 ∂ϕ1 0 0 0 ∂ω ∂ϕ ∂κ ∂κ ∂κ ∂κ 0 0 0 0 0 0 ∂ω1 ∂ϕ 1 ∂κ 1 ∂ω

∂ϕ

∂κ

∂ω

∂ϕ

∂κ

(9.38)

9.6 Smoothness

255

For three consecutive poses [R 1 , t 1 ], [R2 , t 2 ] and [R3 , t 3 ] expressed using Rodrigues parametrization of the rotation matrix (tx1 , t y1 , tz1 , sx1 , s y1 , sz1 ), (tx2 , t y2 , tz2 , sx2 , s y2 , sz2 ) and r

(tx3 , t y3 , tz3 , sx3 , s y3 , sz3 ), the Jacobian J of the objective function for smoothness observation is given in Eq. (9.39). ⎛

d1,1 d1,2 d1,3 ⎜d ⎜ 2,1 d2,2 d2,3 ⎜ r d d ⎜d J = ⎜ 3,1 3,2 3,3 ⎜ 0 0 0 ⎜ ⎝ 0 0 0 0 0 0 ⎛ ∂t δ ∂t δ ∂t δ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝

x ∂tx 1 ∂t y δ ∂tx 1 ∂tz δ ∂tx 1

x x ∂t y 1 ∂tz 1 ∂t y δ ∂t y δ ∂t y 1 ∂tz 1 ∂tz δ ∂tz δ ∂t y 1 ∂tz 1

0

0

0

0

0

0

0

0

0

d1,4 d2,4 d3,4 d4,4 d5,4 d6,4

d1,5 d2,5 d3,5 d4,5 d5,5 d6,5

∂tx δ ∂sx1 ∂t y δ ∂sx1 ∂tz δ ∂sx1 ∂sx ∂sx1 ∂s y ∂sx1 ∂sx ∂sx1

∂tx δ ∂s 1y ∂t y δ ∂s 1y ∂tz δ ∂s 1y ∂sx ∂s 1y ∂s y ∂s 1y ∂sx ∂s 1y

d1,6 d1,7 d1,8 d1,9 d2,6 d2,7 d2,8 d2,9 d3,6 d3,7 d3,8 d3,9 d4,6 0 0 0 d5,6 0 0 0 d6,6 0 0 0

d1,10 d2,10 d3,10 d4,10 d5,10 d6,10

∂tx δ ∂tx δ ∂tx δ ∂tx δ ∂sx 1 ∂tx 2 ∂t y 2 ∂tz 2 ∂t y δ ∂t y δ ∂t y δ ∂t y δ ∂sx 1 ∂tx 2 ∂t y 2 ∂tz 2 ∂tz δ ∂tz δ ∂tz δ ∂tz δ ∂sx 1 ∂tx 2 ∂t y 2 ∂tz 2 ∂sx 0 0 0 ∂sx 1 ∂s y 0 0 0 ∂sx 1 ∂sx 0 0 0 ∂sx 1

⎞ d1,12 d1,13 d1,14 d1,15 0 0 0 d2,12 d2,13 d2,14 d2,15 0 0 0 ⎟ ⎟ ⎟ d3,12 d3,13 d3,14 d3,15 0 0 0 ⎟ ⎟= d4,12 0 0 0 d4,16 d4,17 d4,18 ⎟ ⎟ d5,12 0 0 0 d5,16 d5,17 d5,18 ⎠ d6,12 0 0 0 d6,16 d6,17 d6,18 ⎞ ∂tx δ ∂tx δ ∂tx δ ∂tx δ ∂tx δ ∂tx δ 0 0 0 ∂sx2 ∂s 2y ∂sx 2 ∂tx 3 ∂t y 3 ∂tz 3 ⎟ ⎟ ⎟ ∂t y δ ∂t y δ ∂t y δ ∂t y δ ∂t y δ ∂t y δ ⎟ 0 0 0 2 2 2 3 3 3 ⎟ ∂tx ∂t y ∂tz ∂sx ∂s y ∂sx ⎟ ⎟ δ δ δ δ δ δ ∂tz ∂tz ∂tz ∂tz ∂tz ∂tz ⎟ 0 0 0 ⎟ ∂sx2 ∂s 2y ∂sx 2 ∂tx 3 ∂t y 3 ∂tz 3 ⎟ ∂sx ∂sx ∂sx ∂sx ∂sx ∂sx ⎟ ⎟ 0 0 0 2 2 3 3 2 3 ∂sx ∂s y ∂sx ∂sx ∂s y ∂sx ⎟ ⎟ ∂s y ∂s y ∂s y ∂s y ∂s y ∂s y ⎟ ⎟ 0 0 0 ∂sx2 ∂s 2y ∂sx 2 ∂sx3 ∂s 3y ∂sx 3 ⎟ ⎠ ∂sx ∂sx ∂sx ∂sx ∂sx ∂sx 0 0 0 2 2 3 3 2 3 d1,11 d2,11 d3,11 d4,11 d5,11 d6,11

∂sx

∂s y

∂sx

∂sx

∂s y

∂sx

(9.39) Consequently, for three poses [R1 , t 1 ], [R2 , t 2 ] and [R3 , t 3 ] expressed using quaternion parameterization of the rotation matrix (tx1 , t y1 , tz1 , q01 , q11 , q21 , q31 ), (tx2 , t y2 , tz2 , q02 , r

q12 , q22 , q32 ) and (tx3 , t y3 , tz3 , q03 , q13 , q23 , q33 ), the Jacobian J of the objective function for smoothness observation is given in Eq. (9.40).

(9.40) The Python code for generating smoothness observation equation is given in Listing 9.6.

256 1 2 3 4 5 6

9 Constraints

from s y m p y i m p o r t * i m p o r t sys sys . path . insert (1 , ’ .. ’ ) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14 15 16 17 18 19

tx_1 , ty_1 , tz_1 = s y m b o l s ( ’ tx_1 ty_1 tz_1 ’ ) om_1 , fi_1 , ka_1 = s y m b o l s ( ’ om_1 fi_1 ka_1 ’ ) # sx_1 , sy_1 , sz_1 = s y m b o l s ( ’ sx_1 sy_1 sz_1 ’) # q0_1 , q1_1 , q2_1 , q3_1 = s y m b o l s ( ’ q0_1 q1_1 q2_1 q3_1 ’) tx_2 , ty_2 , tz_2 = s y m b o l s ( ’ tx_2 ty_2 tz_2 ’ ) om_2 , fi_2 , ka_2 = s y m b o l s ( ’ om_2 fi_2 ka_2 ’ ) # sx_2 , sy_2 , sz_2 = s y m b o l s ( ’ sx_2 sy_2 sz_2 ’) # q0_2 , q1_2 , q2_2 , q3_2 = s y m b o l s ( ’ q0_2 q1_2 q2_2 q3_2 ’) tx_3 , ty_3 , tz_3 = s y m b o l s ( ’ tx_3 ty_3 tz_3 ’ ) om_3 , fi_3 , ka_3 = s y m b o l s ( ’ om_3 fi_3 ka_3 ’ ) # sx_3 , sy_3 , sz_3 = s y m b o l s ( ’ sx_3 sy_3 sz_3 ’) # q0_3 , q1_3 , q2_3 , q3_3 = s y m b o l s ( ’ q0_3 q1_3 q2_3 q3_3 ’)

20 21 22 23 24 25 26 27 28 29 30 31 32 33

p o s i t i o n _ s y m b o l s _ 1 = [ tx_1 , ty_1 , tz_1 ] o r i e n t a t i o n _ s y m b o l s _ 1 = [ om_1 , fi_1 , ka_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ sx_1 , sy_1 , sz_1 ] # o r i e n t a t i o n _ s y m b o l s _ 1 = [ q0_1 , q1_1 , q2_1 , q3_1 ] p o s i t i o n _ s y m b o l s _ 2 = [ tx_2 , ty_2 , tz_2 ] o r i e n t a t i o n _ s y m b o l s _ 2 = [ om_2 , fi_2 , ka_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ sx_2 , sy_2 , sz_2 ] # o r i e n t a t i o n _ s y m b o l s _ 2 = [ q0_2 , q1_2 , q2_2 , q3_2 ] p o s i t i o n _ s y m b o l s _ 3 = [ tx_3 , ty_3 , tz_3 ] o r i e n t a t i o n _ s y m b o l s _ 3 = [ om_3 , fi_3 , ka_3 ] # r o d r i g u e s _ s y m b o l s _ 3 = [ sx_3 , sy_3 , sz_3 ] # q u a t e r n i o n _ s y m b o l s _ 3 = [ q0_3 , q1_3 , q2_3 , q3_3 ] all_symbols = position_symbols_1 + orientation_symbols_1 + position_symbols_2 + orientation_symbols_2 + position_symbols_3 + orientation_symbols_3

34 35

36

37

38

39

40

41

42

R t _ w c _ 1 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_1 , ty_1 , tz_1 , om_1 , fi_1 , ka_1 ) # R t _ w c _ 1 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_1 , ty_1 , tz_1 , sx_1 , sy_1 , sz_1 ) # R t _ w c _ 1 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_1 , ty_1 , tz_1 , q0_1 , q1_1 , q2_1 , q3_1 ) R t _ w c _ 2 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_2 , ty_2 , tz_2 , om_2 , fi_2 , ka_2 ) # R t _ w c _ 2 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_2 , ty_2 , tz_2 , sx_2 , sy_2 , sz_2 ) # R t _ w c _ 2 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_2 , ty_2 , tz_2 , q0_2 , q1_2 , q2_2 , q3_2 ) R t _ w c _ 3 = m a t r i x 4 4 F r o m T a i t B r y a n ( tx_3 , ty_3 , tz_3 , om_3 , fi_3 , ka_3 ) # R t _ w c _ 3 = m a t r i x 4 4 F r o m R o d r i g u e s ( tx_3 , ty_3 , tz_3 , sx_3 , sy_3 , sz_3 )

9.6 Smoothness 43

257

# R t _ w c _ 3 = m a t r i x 4 4 F r o m Q u a t e r n i o n ( tx_3 , ty_3 , tz_3 , q0_3 , q1_3 , q2_3 , q3_3 )

44 45 46 47 48 49

R _ c w _ 1 = R t _ w c _ 1 [: -1 ,: -1]. t r a n s p o s e () t _ w c _ 1 = M a t r i x ([ tx_1 , ty_1 , tz_1 ]) . vec () t_cw_1 =- R_cw_1 * t_wc_1 R t _ c w _ 1 = M a t r i x . h s t a c k ( R_cw_1 , t _ c w _ 1 ) R t _ c w _ 1 = M a t r i x . v s t a c k ( Rt_cw_1 , M a t r i x ([[0 ,0 ,0 ,1]]) )

50 51 52 53 54 55

R _ c w _ 2 = R t _ w c _ 2 [: -1 ,: -1]. t r a n s p o s e () t _ w c _ 2 = M a t r i x ([ tx_2 , ty_2 , tz_2 ]) . vec () t_cw_2 =- R_cw_2 * t_wc_2 R t _ c w _ 2 = M a t r i x . h s t a c k ( R_cw_2 , t _ c w _ 2 ) R t _ c w _ 2 = M a t r i x . v s t a c k ( Rt_cw_2 , M a t r i x ([[0 ,0 ,0 ,1]]) )

56 57 58

relative_pose12 = Rt_cw_1 * Rt_wc_2 relative_pose23 = Rt_cw_2 * Rt_wc_3

59 60

61 62

63

64 65

66 67

parametrization12 = taitBryanFromMatrix44Case1 ( relative_pose12 ) # parametrization12 = rodriguesFromMatrix44 ( relative_pose12 ) # parametrization12 = quaternionFromMatrix44 ( relative_pose12 ) parametrization23 = taitBryanFromMatrix44Case1 ( relative_pose23 ) # parametrization23 = rodriguesFromMatrix44 ( relative_pose23 ) # parametrization23 = quaternionFromMatrix44 ( relative_pose23 ) t12 = r e l a t i v e _ p o s e 1 2 [0 : 3 , 3] t23 = r e l a t i v e _ p o s e 2 3 [0 : 3 , 3]

68 69 70

r e l _ p o s e 1 2 = t12 . c o l _ j o i n ( p a r a m e t r i z a t i o n 1 2 ) r e l _ p o s e 2 3 = t23 . c o l _ j o i n ( p a r a m e t r i z a t i o n 2 3 )

71 72 73

t a r g e t _ v a l u e = M a t r i x ([0 , 0 , 0 , 0 , 0 , 0]) model_function = rel_pose12 - rel_pose23

74 75 76

delta = target_value - model_function delta_jacobian = delta . jacobian ( all_symbols ) r

Listing 9.6 Python code for generating smoothness observation equations and the Jacobian J of the objective function for Tait–Bryan (alternatively Rodrigues or quaternion) rotation matrix parameterization

9.7 Georeference Georeferencing means that the internal coordinate system of the basic primitives (point, line, plane, etc.) and trajectory poses [R, t] can be related to a GCS, see Sect. 3.5. For the optimization process, we use the transformation from GCS to a CCS. Therefore, all georeferenced data are now in CCS; see Sect. 3.3. There are

258

9 Constraints

two possibilities to incorporate georeferenced data into optimization process. First possibility assumes that there is no need to optimize parameters of georeference data, therefore, the strategy based on Point to Point—Source to Target observation equation discussed in Sect. 8.1.2 can be used. Second possibility assumes the modification of the parameters of georeference data during optimization process. Such modification can be modeled by constraints, thus we can setup optimizer to preserve these parameters, for example, by fusing fixed optimized parameter observation equation discussed in Sect. 9.2. There are plenty of strategies for georeferencing, therefore, some of them are discussed in separate cases.

9.7.1 Case 1: Georeferenced Position (tx , t y , t z ) Georeference data are in form of positions t Gj P S = (txG P S , t yG P S , tzG P S ) j and correspondences to poses [R, t]i of the trajectory. Assuming that the trajectory traverse these GPS position, we obtain the following condition (9.41) t Gj P S = t i

(9.41)

where i is the index of the trajectory pose and j is the index of GPS position, therefore, ji correspondence will be optimized. Figures 9.27 and 9.28 show an example setup of two trajectories and four georeference positions. The ji correspondence can be added to optimization process by adding Point to Point—Source to Target observation equation (8.9) assuming condition (9.41), therefore GPS georeference observation equation is given (9.42). ⎡ δ⎤ tx ⎣t yδ ⎦ = tzδ    r esiduals

⎡ ⎤ 0 ⎣0⎦ 0 

target values

⎛

⎡

⎤⎞ txG P S − ⎝ [β=t j ] (R j , t j , 0, 0, 0) − ⎣t yG P S ⎦⎠ tzG P S   

(9.42)

model f unction

  where txδ t yδ tzδ are residuals, 0 0 0 are target values and  [β=t j ] (R j , t j , 0, 0, 0)

 − txG P S t yG P S tzG P S is the model function. It can be seen that the  [ t j ] (R j , t j , 0, 0, 0) transform local point (0, 0, 0) to t = (tx , t y , tz ); see Eq. (8.2). It means that the trajectory traverse the GPS position. For the GPS antenna offset (xo f f set , yo f f set , z o f f set ), we can use  [ t j ] (R j , t j , xo f f set , yo f f set , z o f f set ). Instead of (9.42), we can use constraint observation equations (9.9) as the following GPS georeference observation equation (9.43)

9.7 Georeference

259

Fig. 9.27 Before optimization

Fig. 9.28 After optimization

⎡ δ⎤ tx ⎣t yδ ⎦ = tzδ    r esiduals

⎡ ⎤ 0 ⎣0⎦ 0 

target values

⎛⎡

⎤ ⎡ ⎤⎞ txG P S tx + xo f f set − ⎝⎣t yG P S ⎦ − ⎣t y + yo f f set ⎦⎠ tz + z o f f set tGPS    z

(9.43)

model f unction

   where txδ t yδ tzδ are residuals, 0 0 0 are target values and txG P S t yG P S tzG P S − 

tx + xo f f set t y + yo f f set tz + z o f f set is the model function assuming the GPS antenna offset (xo f f set , yo f f set , z o f f set ). It can be seen that for both GPS georeference observation equations (9.42) and (9.43) the set of optimized parameters is β = (tx , t y , tz ), thus the rotation of the trajectory pose [R, t]i is not affected by these constraints. Adding rotation (e.g. from INS) to β is discussed in next Sect. 9.7.2.

260

9 Constraints

9.7.2 Case 2: Georeferenced Pose [R, t] The relative pose observation equation (9.15), (9.16), (9.17) can be used for pose [R, t] georeferencing to GPS pose [R, t]G P S . GPS pose is typically delivered by INS, thus desired rotation can also contribute in optimization process. It is done be setting measurement to [0] in relative pose observation equation (9.44) between trajectory pose and corresponding GPS pose  βδ = 

r esiduals

 0 

target values

− m2v[β] ([R, t]1,G P S )   

(9.44)

model f unction

 where β δ is vector of residuals (optimization parameters assuming Tait–Bryan, Rodrigues or quaternion parameterization of the rotation matrix), [0] are target values and m2v[β] ([R, t]1,G P S ) is model function. The detailed description of the relative pose observation equation is given in Sect. 9.5.1. [R, t]1,G P S is a relative pose between pose of trajectory [R, t]1 and GPS pose [R, t]G P S given by Eq. (9.45) 

1,G P S  1 −1  G P S R t R t R t = ⇒ [1]4×4 01×3 1 01×3 1 01×3 1

(9.45)

where [1]4×4 is identity matrix given in Eq. (9.46). ⎡

4×4 1

1 ⎢0 =⎢ ⎣0 0

0 1 0 0

0 0 1 0

⎤ 0 0⎥ ⎥ 0⎦ 1

(9.46)

For the GPS antenna offset (xo f f set , yo f f set , z o f f set ), it is straight forward to modify target values as [xo f f set , yo f f set , z o f f set , β = m2v(R = [1]3×3 ), where β is vector of Tait–Bryan, Rodrigues or quaternion parameters related with the identity rotation matrix R = [1]3×3 . Therefore, in such a case, the desired relative pose is as given in Eq. (9.47). ⎤ ⎡ 1 0 0 xo f f set ⎢0 1 0 yo f f set ⎥ ⎥ (9.47) [R, t]1,G P S = ⎢ ⎣0 0 1 z o f f set ⎦ 000 1 Figures 9.29 and 9.30 compare the result from Figs. 9.27 and 9.28 with georeferencing assuming rotation information. It can be seen that georeferencing to GPS pose [R, t]G P S preserve rotation derived from georeferenced data.

9.7 Georeference

261

Fig. 9.29 Georeferencing to GPS position [t]G P S

Fig. 9.30 Georeferencing to GPS pose [R, t]G P S

9.7.3 Case 3: Georeferenced Primitive (LiDAR Metric) Having georeferenced basic primitives (point, line, plane, etc.) extracted from LiDAR data, it is possible to use source to target approach discussed in Sect. 8.1.2. Therefore, the georeferenced point observation equation is as (9.48) ⎤ xδG E O ⎣ yδG E O ⎦ = zG E O  δ  ⎡

r esiduals

⎡ ⎤ 0 ⎣0⎦ 0 

target values

⎛

⎡

⎤⎞ xGEO − ⎝ [ R j ,t j ] (R j , t j , x l, j , y l, j , z l, j ) − ⎣ y G E O ⎦⎠ (9.48) zG E O    model f unction



 where xδG E O yδG E O z δG E O are residuals, 0 0 0 are target values and  [ R j ,t j ] 

(R j , t j , x l, j , y l, j , z l, j ) − x G E O y G E O z G E O is the model function.

G E O G E O G E O  y z x is georeferenced target point. For the georeferenced Plücker line, the observation equation is a modified equation discussed in Sect. (8.94) and it is as (9.49) ⎡

⎤

⎡ 1 ⎤ ⎡ G E O ⎤⎞ ⎡ ⎤ ⎛ mx mx 0 ⎢m G E O ⎥ ⎢ δ,y ⎥ ⎢m 1y ⎥ ⎢m Gy E O ⎥⎟ ⎢0⎥ ⎜

1 ⎢ GEO⎥ ⎢ ⎥ ⎜  ⎢ 1 ⎥ ⎢ G E O ⎥⎟ 1 ⎥ ⎢m ⎥ ⎢ ⎥⎟ ⎢ ⎥ ⎜ t × R1 ⎢ ⎢ δ,z ⎥ = ⎢0⎥ − ⎜ R ⎢m1z ⎥ − ⎢mGz E O ⎥⎟ 1 ⎢ ⎢ GEO ⎥ ⎢ ⎥ ⎥⎟ ⎢0⎥ ⎜ 03 × 3 R ⎢ lδ,x ⎥ ⎢ l x1 ⎥ ⎢ l xG E O ⎥⎟ ⎢ ⎥ ⎜ ⎢ GEO ⎥ ⎣ ly ⎦ ⎣ ly ⎦⎠ ⎣0⎦ ⎝ ⎦ ⎣l 1 G E O δ,y 0 l lz GEO lδ,z    z     targetvalues model f unction GEO m δ,x

r esiduals

(9.49)

262

9 Constraints

G E O G E O G E O G E O G E O G E O 

 m δ,y m δ,z lδ,x lδ,y lδ,z where m δ,x are residuals, 0 0 0 0 0 0 are   1 1 R t × R1 1 1 1 1 1 1 − m x m y m z lx l y lz target values and 03×3 R1

G E O G E O G E O G E O G E O G E O  my mz lx ly lz mx is the model function. 

G EO GEO GEO GEO GEO GEO  my mz lx ly lz mx is georeferenced Plücker line. For georeferenced plane, the observation equation (8.109) discussed in Sect. 8.3.2 become (9.50) ⎡

⎤ aδG E O ⎢bG E O ⎥ ⎢ δG E O ⎥ = ⎣ cδ ⎦ dδG E O    r esiduals

⎡ ⎤ 0 ⎢0⎥ ⎢ ⎥ ⎣0⎦ 0 

target values

⎛ ⎡ 1 ⎤ ⎡ G E O ⎤ ⎞ a a   ⎜⎢b1 ⎥ R1  −R1  t 1 ⎢bG E O ⎥ ⎟ ⎥ ⎟ ⎢ ⎥ −⎜ −⎢ ⎝⎣ c 1 ⎦ ⎣ cG E O ⎦ ⎠ 0 1 d1 dGEO   

(9.50)

model f unction



 where aδG E O bδG E O cδG E O dδG E O are residuals, 0 0 0 0 are target values and   1 

1 1 1 1  R −R1 t 1  a b c d − a G E O b G E O c G E O d G E O is the model function. 0 1

 Georeferenced plane is given as a G E O b G E O c G E O d G E O .

9.7.4 Case 4: Georeferenced Primitive (Metric Camera) We consider point and line as the primitives that can be useful for georeferencing. In Sect. 7.1.3, the calculation of the pinhole camera external orientation using Plücker lines was discussed. Therefore, the georeferencing can be simply done by using georeferenced Plücker line L w in observation equation (7.29). Georeferencing using point is related with, for example, the GCP (Fig. 9.32) used in photogrammetry (Fig. 9.31) whose positions are known in an object-space reference coordinate system and whose images can be positively identified in the photographs (Fig. 9.33). Therefore, their positions should not be modified during optimization process. For observations equations (7.5), (7.50) and (7.75) the georeferencing is done for each coordinates of a ground points by adding fixed optimized parameter constraint (9.4) discussed in Sect. 9.2 and large vales in weight matrix W . An example of the Jacobian r

J of the objective function for observation equation (7.50) fused with constraint (9.4) assuming Tait–Bryan rotation matrix parameterization is given in (9.51). It can be seen that a ground point (x g , y g , z g ) is in the same time a ground control point, therefore, it contributes into camera pose optimization and its coordinates are preserved by the constraint.

9.7 Georeference

Fig. 9.31 Aerial photos

Fig. 9.32 Example GCP

263

264

9 Constraints

Fig. 9.33 BA system, green lines: from cameras to GCP, black lines: from cameras to tie points

⎛

⎞ d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 ⎜d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9 ⎟ r ⎜ ⎟ ⎟= c3,7 J =⎜ ⎜ ⎟ ⎝ ⎠ c4,8 c5,9 ⎛ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ∂ξ ⎞ δ

δ

δ

δ

δ

δ

δ

δ

δ

(9.51)

∂t ∂t ∂t ∂ω ∂ϕ ∂κ ∂ x g ∂ y g ∂z g ⎜ ∂ηxδ ∂ηyδ ∂ηzδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ∂ηδ ⎟ ⎜ ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂ x g ∂ y g ∂z g ⎟ ⎜ ⎟

⎜ ⎜ ⎝

−1

−1

⎟ ⎟ ⎠

−1

9.8 Distance to Circle Distance to circle observation equation can fit a 2D circle to noisy data as shown in Figs. 9.34, 9.35. Equation (9.52) describes the constraint for the point x, y that lies on the circle with center xc , yc and radius cr . (x − xc )2 + (y − yc )2 = rc2 The observation equation is given in (9.53)

(9.52)

9.8 Distance to Circle

265

Fig. 9.34 Before optimization

dδ =  r esidual

 0 

−  [xc ,yc ,zc ] ( (x − xc )2 + (y − yc )2 − rc )   

(9.53)

model f unction

targetvalue

where, d δ is residual, 0 is target value and  [xc ,yc ,zc ] ( (x − xc )2 + (y − yc )2 − rc ) r

is the model function. The Jacobian J of the objective function is given in Eq. (9.54). r

J = d1,1 d1,2 d1,3 =  δ δ δ ∂d ∂d ∂d ∂ xc ∂ yc ∂rc

(9.54)

Such observation equation can help in building optimization system capable of estimating local curvature of the trajectory. The Python code for generating distance to circle observation equation is given in Listing 9.7. 1

from s y m p y i m p o r t *

2 3 4

x , y = symbols ( ’x y ’) cx , cy , cr = s y m b o l s ( ’ cx cy cr ’ )

5 6 7 8

c i r c l e _ s y m b o l s = [ cx , cy , cr ] all_symbols = circle_symbols

266

9 Constraints

Fig. 9.35 After optimization

9 10

v = M a t r i x ([ x , y ]) . vec () vc = M a t r i x ([ cx , cy ]) . vec ()

11 12

diff = v - vc

13 14 15

target_value = 0 m o d e l _ f u n c t i o n = sqrt ( diff [0] * diff [0] + diff [1] * diff [1] ) - cr

16 17 18

o b s _ e q = M a t r i x ([[ t a r g e t _ v a l u e - m o d e l _ f u n c t i o n ]]) obs_eq_jacobian = obs_eq . jacobian ( all_symbols )

Listing 9.7 Python code for generating distance to circle observation equations and the Jacobian r

J of the objective function

Chapter 10

Metrics’ Fusion

10.1 Introduction Data fusion is an essential aspect of the mobile robotics [1, 2]. Once we fuse data coming from different measurements, we expect to have the result with decreased uncertainty. An example is the loop closing shown in Figs. 6.10, 6.11 and 6.12 where the visualization of the decreased uncertainty  β ∗ of the poses after some optimization iterations as ellipses is given. It can be seen that by adding the loop closure edge to Graph SLAM composed of the relative pose observation equations, the uncertainty of all poses is decreased. Consequently, the uncertainty of localization, map building and overall state estimation can be decreased (improved) by incorporating the probabilistic approach discussed in Sect. 4.3. Common tools are Kalman Filter discussed in Sect. 5.2, Particle Filter discussed in Sect. 5.3 and Graph SLAM discussed in Sect. 4.5. Data fusion is a very important aspect of mobile robotics and geospatial domain. It is difficult to find the exact definition of data fusion, for example, in the geospatial domain, it is equivalent to data integration. In this book, data fusion is performed when all observation equations describing certain optimization systems can be integrated/fused into a single design matrix. This approach covers the probabilistic approach known in mobile robotics.

10.2 Rectangular Object with Unknown Width/Height The rectangular object with an unknown width/height observation equation is useful for finding the location of these shapes in the point cloud reconstructed by SFM. The requirement is that, for each corner, we have bundle of rays that were supposed to intersect exactly at the point of this corner. Thus, the correspondences between corners and bundle of rays are given in Fig. 10.1. The basic assumption is that each

© The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_10

267

268

10 Metrics’ Fusion

corner point P li (xil , yil , 0, 1) lies on the same plane X Y 0 and it is expressed in its local coordinate system [R, t]. The unknown width/height determines scalex and scale y incorporated into transform point equation (10.1). ⎛ ⎞ scalex 0 0 0 ⎜ 0 scale y 0 0⎟ l ⎜ ⎟ P = S Pl P ls i =⎝ i 0 0 1 0⎠ i 0 0 01

(10.1)

Equation (10.2) transforms i th corner point into global reference system. gs

P i = [R, t]S P li

(10.2)

To constrain each corner with a bundle of rays, the point to line observation equation discussed in Sect. 8.2.1 is used. Thus, the rectangular object with an unknown width/height observation equation is given in (10.3).  δx = δy

 r esidual

 0 0



 V x  [R, t]S P l − P tg  Vy 



 −

target values

(10.3)

model f unction

        where δx δ y are residuals, 0 0 are target values and V x V y [R, t]S P l − P tg   is the model function, V x V y is line representation as in Eq. (8.49), P tg is point ner l t on line. The model function is expressed as  cor [R,t,S] (R, t, S, P , P g ). Thus, it is straightforward that the pose [R, t], scalex and scale y are optimized. Having pose [R, t] expressed using Tait–Bryan angles parameterization (tx , t y , tz , ω, ϕ, κ), the r

Jacobian J of the objective function is given in Eq. (10.4). r

J=

 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8  ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ  ∂δ



x

x

x

x

x

x

x

x

(10.4)

∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂scalex ∂scale y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂tx ∂t y ∂tz ∂ω ∂ϕ ∂κ ∂scalex ∂scale y

For pose [R, t] expressed using Rodrigues parameterization of rotation matrix r

(tx , t y , tz , sx , s y , sz ), the Jacobian J of the objective function is given in Eq. (10.5).  d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 = J= d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8  ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ  ∂δ r



x

x

x

x

x

x

x

x

∂tx ∂t y ∂tz ∂sx ∂s y ∂sz ∂scalex ∂scale y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂tx ∂t y ∂tz ∂sx ∂s y ∂sz ∂scalex ∂scale y

(10.5)

10.2 Rectangular Object with Unknown Width/Height

269

Fig. 10.1 Green: bundle of rays, red: rectangle object with unknown width/height and corners intersecting with bundle of rays

For pose [R, t], expressed using quaternion parameterization of rotation matrix r

(tx , t y , tz , q0 , q1 , q2 , q3 ), the Jacobian J of the objective function is given in Eq. (10.6). r

J=

 d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 d1,7 d1,8 d1,9 = d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 d2,7 d2,8 d2,9  ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ ∂δ  ∂δ ∂δ



x

x

x

x

x

x

x

x

x

(10.6)

∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3 ∂scalex ∂scale y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂δ y ∂tx ∂t y ∂tz ∂q0 ∂q1 ∂q2 ∂q3 ∂scalex ∂scale y

The python code for generating design matrix for rectangular object with unknown width/height observation equation is given in Listing 5.1 (Fig. 10.1). 1 2 3 4 5 6

from s y m p y i m p o r t * i m p o r t sys sys . path . insert (1 , ’ .. ’ ) from t a i t _ b r y a n _ R _ u t i l s i m p o r t * from r o d r i g u e s _ R _ u t i l s i m p o r t * from q u a t e r n i o n _ R _ u t i l s i m p o r t *

7 8 9 10 11 12 13 14 15 16

xsl , ysl , zsl = s y m b o l s ( ’ xsl ysl zsl ’ ) xtg , ytg , ztg = s y m b o l s ( ’ xtg ytg ztg ’ ) px , py , pz = s y m b o l s ( ’ px py pz ’ ) om , fi , ka = s y m b o l s ( ’ om fi ka ’ ) # sx , sy , sz = s y m b o l s ( ’ sx sy sz ’) # q0 , q1 , q2 , q3 = s y m b o l s ( ’ q0 q1 q2 q3 ’) vxx , vxy , vxz = s y m b o l s ( ’ vxx vxy vxz ’ ) vyx , vyy , vyz = s y m b o l s ( ’ vyx vyy vyz ’ ) scale_x , s c a l e _ y = s y m b o l s ( ’ s c a l e _ x s c a l e _ y ’ )

17 18 19 20 21

p o s i t i o n _ s y m b o l s = [ px , py , pz ] o r i e n t a t i o n _ s y m b o l s = [ om , fi , ka ] # o r i e n t a t i o n _ s y m b o l s = [ sx , sy , sz ] # o r i e n t a t i o n _ s y m b o l s = [ q0 , q1 , q2 , q3 ]

270 22 23

10 Metrics’ Fusion

s c a l e _ s y m b o l s = [ scale_x , s c a l e _ y ] all_symbols = position_symbols + orientation_symbols + scale_symbols

24 25

26

27

28

29 30

p o i n t _ s o u r c e _ l o c a l = M a t r i x ([ xsl * scale_x , ysl * scale_y , zsl , 1]) . vec () R T _ w c = m a t r i x 4 4 F r o m T a i t B r y a n ( px , py , pz , om , fi , ka ) [: -1 ,:] # R T _ w c = m a t r i x 4 4 F r o m R o d r i g u e s ( px , py , pz , sx , sy , sz ) [: -1 ,:] # R T _ w c = m a t r i x 4 4 F r o m Q u a t e r n i o n ( px , py , pz , q0 , q1 , q2 , q3 ) [: -1 ,:] point_source_global = RT_wc * point_source_local p o i n t _ o n _ l i n e _ t a r g e t _ g l o b a l = M a t r i x ([ xtg , ytg , ztg ])

31 32 33

vxt = M a t r i x ([[ vxx , vxy , vxz ]]) . t r a n s p o s e () vyt = M a t r i x ([[ vyx , vyy , vyz ]]) . t r a n s p o s e ()

34 35 36

t a r g e t _ v a l u e = M a t r i x ([0 ,0]) . vec () m o d e l _ f u n c t i o n = M a t r i x ([ vxt . dot ( p o i n t _ s o u r c e _ g l o b a l p o i n t _ o n _ l i n e _ t a r g e t _ g l o b a l ) , vyt . dot ( p o i n t _ s o u r c e _ g l o b a l - p o i n t _ o n _ l i n e _ t a r g e t _ g l o b a l ) ]) . vec () r

Listing 10.1 Python code for generating the Jacobian J of the objective function for rectangular object with unknown width/height observation equation

10.3 Sheaf of Planes Intersection −−→ Sheaf of plane is a line that is represented in this case as vector abln = bln − a ln −−→ that satisfy the length constraint ||abln || = 1 (Fig. 10.2). Plane is expressed as − → a pl , b pl , c pl , d pl where a pl , b pl , c pl is normal vector n pl of the plane and d pl is the distance of plane to the origin. The constraint that point px , p y , pz lies on the plane is a pl px + b pl p y + c pl pz + d pl = 0. The closest point pln to the origin that lies on the line satisfy the following condition

Fig. 10.2 Green: line as the sheaf of planes

10.3 Sheaf of Planes Intersection

271

−−→ pln · abln = 0

(10.7)

→ −−→ − abln · n pl = 0

(10.8)

and when line is on the plane, then

where (·) is dot product. All these constraints can be fused for building the sheaf of planes observation equation (10.9). ⎤ ⎡ ⎡ 1⎤ −−→ ⎡ ⎤ cδ ||abln || 1 ⎢ pl ln ⎢ 2⎥ pl ln pl ⎥ ⎢0⎥ ⎢a ax + b pl a ln ⎢cδ ⎥ y + c az + d ⎥ ⎢ ⎥ ⎢ pl ln ⎢ 3⎥ pl ln pl ⎥ ⎢c ⎥ = ⎢0⎥ − ⎢ a bx + b pl bln y + c bz + d ⎥ ⎥ ⎢ ⎥ ⎢ ⎢ δ⎥ −−→ ⎥ ⎢ ⎢ 4⎥ ⎣0⎦ a ln · abln ⎦ ⎣ ⎣cδ ⎦ − → − − → 0 5 ln pl c ab · n



 δ



target values

r esiduals

(10.9)

model f unction

− → pl ln ln ln ln pl pl where model function is  aln ,aln ,aln ,bln ,bln ,bln  (axln , a ln y , az , bx , b y , bz , n , d ). It x y z x y z is straightforward that, for a sheaf of two planes, we have nine observation equa−−→ tions since c1 can be used only once constraining ||abln ||. This fusion of constraints can find sheaf of planes intersection by optimizing six parameters β = r  ln ln ln ln ln ln  ax , a y , az , bx , b y , bz . The Jacobian J of the objective function is given in Eq. (10.10). ⎛ ⎞ d1,1 d1,2 d1,3 d1,4 d1,5 d1,6 ⎜d2,1 d2,2 d2,3 d2,4 d2,5 d2,6 ⎟ r ⎜ ⎟ ⎟ J =⎜ ⎜d3,1 d3,2 d3,3 d3,4 d3,5 d3,6 ⎟ = ⎝d4,1 d4,2 d4,3 d4,4 d4,5 d4,6 ⎠ d5,1 d5,2 d5,3 d5,4 d5,5 d5,6 ⎛ ∂c1 ∂c1 ∂c1 ∂c1 ∂c1 ∂c1 ⎞ δ δ δ δ δ δ (10.10) ∂a ln ∂a ln ∂a ln ∂bln ∂bln ∂bln y z ⎜ ∂cx2 ∂cy2 ∂cz2 x ⎟ δ δ ⎜ lnδ ⎟ ⎜ ∂ax ∂alny ∂azln 0 0 0 ⎟ ⎜ 3 3 3 ⎟ ⎜ 0 0 0 ∂clnδ ∂clnδ ∂clnδ ⎟ ∂bx ∂b y ∂bz ⎟ ⎜ ⎜ ∂cδ4 ∂cδ4 ∂cδ4 ∂cδ4 ∂cδ4 ∂cδ4 ⎟ ⎜ ln ln ln ln ln ln ⎟ ⎝ ∂ax ∂a y ∂az ∂bx ∂b y ∂bz ⎠ ∂cδ5 ∂cδ5 ∂cδ5 ∂cδ5 ∂cδ5 ∂cδ5 ∂axln ∂a ln ∂azln ∂bln ∂bln ∂bln y x y z r

The python code for generating the Jacobian J of the objective function for the sheaf of planes observation equation is given in Listing 10.2.

272 1 2

10 Metrics’ Fusion

from s y m p y i m p o r t * from s y m p y . p h y s i c s . v e c t o r . f u n c t i o n s i m p o r t *

3 4

5 6 7

a_x , a_y , a_z , b_x , b_y , b_z = s y m b o l s ( ’ a_x a_y a_z b_x b_y b_z ’) a ,b ,c , d = s y m b o l s ( ’ a b c d ’) l i n e _ s y m b o l s =[ a_x , a_y , a_z , b_x , b_y , b_z ] all_symbols = line_symbols

8 9 10 11

l = M a t r i x ([ b_x - a_x , b_y - a_y , b_z - a_z ]) . vec () p = M a t r i x ([ a_x , a_y , a_z ]) . vec () n = M a t r i x ([ a , b , c ]) . vec ()

12 13 14

15

16

target_value_norm = 1 m o d e l _ f u n c t i o n _ n o r m = sqrt ( l [0]* l [0] + l [1]* l [1] + l [2]* l [2]) r e s i d u a l _ n o r m = M a t r i x ([ t a r g e t _ v a l u e _ n o r m m o d e l _ f u n c t i o n _ n o r m ]) . vec () residual_norm_jacobian = residual_norm . jacobian ( all_symbols )

17 18 19

20

21

t a r g e t _ v a l u e = M a t r i x ([0 ,0 ,0 ,0]) . vec () m o d e l _ f u n c t i o n = M a t r i x ([ a * a_x + b * a_y + c * a_z + d , a * b_x + b * b_y + c * b_z + d , p . dot ( l ) , l . dot ( n ) ]) . vec () r e s i d u a l = M a t r i x ([ t a r g e t _ v a l u e - m o d e l _ f u n c t i o n ]) . vec () residual_jacobian = residual . jacobian ( all_symbols ) r

Listing 10.2 Python code for generating the Jacobian J of the objective function for the sheaf of planes observation equation

10.4 Surface Reconstruction from LiDAR Point Cloud Surface reconstruction [3–7] from LiDAR point cloud is an interesting research topic since it can contribute to many practical applications. Surface can be represented as a regular grid (Fig. 10.3), moreover, such a grid can be implemented as a graph where vertices correspond to poses and edges correspond to spatial relations between them. These spatial relations can be constrained to, e.g. maintain the relative distance or preserve the predefined shape. This regular grid has to approximate the target point cloud, thus it has to minimize the distance of the vertices to the nearest target points. The optimization process will modify the vertices of the regular grid to reach an optimal approximation of the target point cloud. Therefore, the system can be composed of fused point to point observation equation (8.9) with relative pose observation equation (9.15) or with the smoothness observation equation (9.32). Generally, the surface reconstruction (to some extent) from the LiDAR point cloud can be obtained from the fusion of LiDAR metric observation equations (see Chap. 8) with constraints observation equations described in Chap. 9.

10.5 Multi-measurement Instrument System

273

Fig. 10.3 Surface reconstructed from point cloud. Red: point cloud, black: surface

10.5 Multi-measurement Instrument System Most of the mobile mapping systems are composed of multi-measurement instrument system, thus they can offer increased or even full 360◦ field of view [8–15]. There are two possibilities for building observation equations for such a system. First one assumes that we have a single trajectory for the entire system, thus extrinsic parameters of the relative poses between measurement instruments have to be taken into the optimization process. In such a case, each observation equation will contribute for both pose estimation and update of the calibration parameters. Such an approach was addressed in Sect. 10.5.1. It can be observed that the fusion is done on the level of the observation equation. Second case assumes that we have separate trajectory for each measurement instrument. It is mandatory to fuse additional observation equations (e.g. (9.15)) constraining the relative poses between sensors. Thus, in this case, the fusion is evident on the graph level. In practical applications discussed in Chap. 12, it was found that the second approach guarantees better accuracy for the final map. The fact is that the extrinsic parameters could change over time due to vibrations, therefore, it is not sufficient to build the system where calibrations are the same for entire trajectories. Thus, long term, long distance mobile mapping systems should consider changes of the calibration parameters over time. Looking from optimization point of view, the invalid calibration parameters could be compensated to some extent by pose updates. It means that the correction of poses can efficiently fix the sub-optimal calibration parameters and can reach the final map with satisfactory accuracy. The desired solution is the multi-trajectory system constraining the relative poses between sensors. Another challenge is an inaccurate time synchronization between data coming from different sources. To conclude this discussion, it is worth to comment that in most high-end mobile mapping systems, the approach of single trajectory and accurate calibrations is dominant. An interesting recent approach of spatiotemporal calibration of the camera and 3D Laser Scanner is discussed in [16]. This approach causes problem with data post processing since there are limited possibilities to refine a map. For this reason, the time-dependent calibration or multitrajectory approach should be considered. Obviously, there is no guarantee that one

274

10 Metrics’ Fusion

approach will be better than other since it depends on the application characteristics and the desired parameters of the mobile mapping product.

10.5.1 Multi-sensor Extrinsic Calibration Multi-sensor extrinsic calibration [17–19] is a fundamental problem in mobile mapping systems. Finding extrinsic parameters as relative poses between all sensors can be done using calibration patterns or can be retrieved using SLAM methodology. The calibration patterns can be composed of basic primitives (points, lines, planes) that can be identified by measurement instruments. Observation equations elaborated in the next section can be easily extended to address multi-sensor calibration problem by introducing additional parameters of transformation matrix [Rc , t c ] to the optimization system. For example having calibration pattern of the set of lines the calculating pinhole camera external orientation discussed in Sect. 7.1.3 can efficiently retrieve the relative poses between cameras. Consequently, these lines can also be used for LiDAR extrinsic calibration by incorporating observation equation discussed in Sects. 8.1.6, 8.2.1 or 8.3.1. As an example of building the extrinsic calibration observation equation for LiDAR, the point-to-point source to target (Eq. (8.9)) is taken into consideration. Therefore, LiDAR extrinsic calibration observation equation is in form of the Eq. (10.11) ⎡ δ⎤ x ⎣yδ ⎦ = zδ

 r esiduals

⎡ ⎤ 0 ⎣0⎦ 0



target values

⎛

⎡ k ⎤⎞ x − ⎝ [ Rc ,t c ,R j ,t j ] (Rc , t c , R j , t j , x l,k , y l,k , z l,k ) − ⎣ y k ⎦⎠ zk 

model f unction

(10.11)     are residuals, 0 0 0 are target values and  [ Rc ,t c ,R j ,t j ] where, x δ y δ z δ   (Rc , t c , R j , t j , x l,k , y l,k , z l,k ) − x k y k z k ) is the model function. In this case, model function  transforms point P l (x l , y l , z l , 1) expressed in local frame to the global frame as point P g (x g , y g , z g ) with the Eq. (10.12). Rc t c P = [R t ] 1×3 Pl 0 1 

g

j j

(10.12)

Obviously, additional constraints has to be taken into consideration to reach an optimal solution, for example, the boresight and lever-arm tolerances. Target point cloud expressed in Eq. (10.11) as x k , y k , z k is a reference calibration pattern and does not change during the optimization process. The measurement has to be taken from multiple places having similar overlap with the reference calibration pattern. Each LiDAR extrinsic calibration observation equation contributes into the same [Rc , t c ] parameters and different [R j , t j ] related with multiple places of measurements. In

10.5 Multi-measurement Instrument System

275

certain mobile mapping applications such as underground shaft mining mapping, it is evident to incorporate the scale factor into the calibration process, therefore, the observation equation elaborated in Sect. 8.1.4 could improve the accuracy of the final map since LiDAR measurements are affected by heavy environmental conditions (dust, water, humidity and temperature).

References 1. S. Thrun, Particle filters in robotics, in Proceedings of the 17th Annual Conference on Uncertainty in AI (UAI) (2002) 2. S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics (MIT Press, Cambridge, Mass, 2005) 3. M. Berger, A. Tagliasacchi, L.M. Seversky, P. Alliez, J.A. Levine, A. Sharf, C.T. Silva, State of the art in surface reconstruction from point clouds, in Eurographics 2014 - State of the Art Reports. ed. by S. Lefebvre, M. Spagnuolo (The Eurographics Association, 2014) 4. Z. Mi, Y. Luo, W. Tao, Ssrnet: Scalable 3d surface reconstruction network (2020) 5. Y. He, M. Huska, S. Ha Kang, H. Liu, Fast algorithms for surface reconstruction from point cloud (2019) 6. C. Mostegel, R. Prettenthaler, F. Fraundorfer, H. Bischof, Scalable surface reconstruction from point clouds with extreme scale and density diversity (2017) 7. M. Berger, A. Tagliasacchi, L.M. Seversky, P. Alliez, G. Guennebaud, J.A. Levine, A. Sharf, C.T. Silva, A survey of surface reconstruction from point clouds. Comput. Graph. Forum 36(1), 301–329 (2017) 8. S. Ji, Z. Qin, J. Shan, L. Meng, Panoramic slam from a multiple fisheye camera rig. ISPRS J. Photogramm. Remote Sens. 159, 11 (2019) 9. G. Carrera, A. Angeli, A. Davison, Lightweight slam and navigation with a multi-camera rig (2011) 10. G. Carrera, A. Angeli, A.J. Davison, Slam-based automatic extrinsic calibration of a multicamera rig, in 2011 IEEE International Conference on Robotics and Automation (2011), pp. 2652–2659 11. S. Yang, S.A. Scherer, X. Yi, A. Zell, Multi-camera visual slam for autonomous navigation of micro aerial vehicles. Robot. Auton. Syst. 93(C), 116–134 (2017) 12. G. Carrera, A. Angeli, A.J. Davison, IEEE (may 2011) 13. M. Kaess, F. Dellaert, Probabilistic structure matching for visual slam with a multi-camera rig. Comput. Vis. Image Underst. 114(2), 286–296 (2010) 14. H. Yin, Z. Ma, M. Zhong, K. Wu, Y. Wei, J. Guo, B. Huang, Slam-based self-calibration of a binocular stereo vision rig in real-time. Sensors 20(3) (2020) 15. O. Faruk Ince, J.-S. Kim, Tima slam: tracking independently and mapping altogether for an uncalibrated multi-camera system. Sensors 21(2) (2021) 16. M.R. Nowicki, Spatiotemporal calibration of camera and 3d laser scanner. IEEE Robot. Autom. Lett. 5(4), 6451–6458 (2020) 17. D. Zuniga-Noel, J.-R. Ruiz-Sarmiento, R. Gomez-Ojeda, J. Gonzalez-Jimenez, Automatic multi-sensor extrinsic calibration for mobile robots. IEEE Robot. Autom. Lett. 4(3), 2862– 2869 (2019) 18. V. Santos, D. Rato, P. Dias, M. Oliveira, Multi-sensor extrinsic calibration using an extended set of pairwise geometric transformations. Sensors 20(23) (2020) 19. Z. Wei, W. Zou, G. Zhang, K. Zhao, Extrinsic parameters calibration of multi-camera with nonoverlapping fields of view using laser scanning. Opt. Express 27(12), 16719–16737 (2019)

Part IV

Applications

Chapter 11

Building Large-Scale SLAM

11.1 Normal Equation for BA For SLAM applications based on BA related to observation equations (7.5), (7.50) and (7.75), it is possible to express them as Normal equation (11.1) [1, 2]. A Ax = A b

(11.1)

By replacing A A by N and A b by n Normal equation is given as (11.2). Nx = n

(11.2)

Normal equation can be rewritten in the form of (11.3) 

Ncc Nc,t p  Nc,t p Nt p,t p



   xc nc = xt p nt p

(11.3)

where Ncc is a hyper diagonal matrix (in case of Tait–Bryan parametrization) composed of 6 × 6 sub-matrices related with design matrix for camera poses (Ac ), Nt p,t p is also hyper diagonal matrix composed of 3 × 3 sub-matrices related with design matrix (At p ) for landmarks (tie points). Thus, inverting these matrices is straight forward for performing parallel simultaneous calculations [3]. Finally, we obtain Normal equation as (11.4) 

Ac Ac Ac At p  (Ac At p ) At p At p



    xc A c bc =  xt p At p bt p

(11.4)

that can be solved with the so-called Schur Complement method [4–6] as  Ncc x∗c =  nc

© The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_11

(11.5)

279

280

where

11 Building Large-Scale SLAM

  Ncc = Ncc − Nc,t p N−1 t p,t p Nc,t p

(11.6)

 nc = nc − Nc,t p N−1 t p,t p nt p

(11.7)

and the solution x∗c is the same as updates for camera poses xc in (11.3). Therefore, having calculated updates for camera poses the updates for tie points are given by following back substitution (11.8). 

∗ x∗t p = xt p = N−1 t p,t p (nt p − Nc,t p xc )

(11.8)

It can be seen that most of the calculations can be calculated in parallel, thus it is advised to study [1, 3] for implementation details. For LiDAR metrics, we deal with such observation equations that determine different design matrix characteristics than in BA. Since, landmarks (tie points) or other primitives (lines, planes) for LiDAR metrics are expressed in local frames, the design matrix is mainly composed of A A sub-matrices (6 × 6 for Tait–Bryan and Rodrigues parametrization and 7 × 7 for quaternions). The dominant implementation in literature [7] builds the optimization system using parallel reduction, therefore, the set of observation equations can be divided into independent calculation tasks. Finally, the A Ax = A b system is constructed and solved by Cholesky factorization [8]. This approach can be effectively incorporated into building optimization systems assuming fusion of other observation equations such as constraints discussed in Chap. 9.

11.2 Trajectory Centric Versus Map-Centric Approach Large-scale SLAM requires efficient approach for high volume of data processing and high number of poses to be optimized. Moreover, BA requires additional computational effort for landmarks (tie points) corrections. Both requirements can be solved effectively by Schur Complement method for BA and by introducing active and inactive maps [9] for LiDAR metrics. Generally, when we incorporate all poses of the trajectories, the so-called trajectory centric approach is used. In map-centric SLAM approach, aggregating poses and basic primitives (points, lines, planes, etc.) expressed as relative poses into rigid sub-maps can efficiently reduce the computational complexity of the SLAM and can provide other advantages. An interesting approach [10] is the system capable of capturing comprehensive dense globally consistent surfel-based maps, without pose graph optimization or any post-processing steps. This is performed by using dense frame-to-model camera tracking and windowed surfel-based fusion coupled with frequent model refinement, thus non-rigid surface deformations are incorporated. Furthermore, the authors apply local modelto-model surface loop closure optimizations. Authors of [11] observed that in mapcentric SLAM, the process of loop closure differs from that of conventional SLAM

11.2 Trajectory Centric Versus Map-Centric Approach

281

and the result of incorrect loop closure is more destructive and is not reversible. Generally, the most computationally efficient method is to build Graph SLAM system composed of relative pose observation equations discussed in Sects. 9.5.1 and 9.5.2. Therefore, such a system can be optimized online to some extent. The disadvantage of such approach is that in the situations where the information about relative pose is uncertain, the overall result could yield into locally not consistent map and more destructive and not reversible effects. For this reason, the map-centric approach can be incorporated as a method that current map is basically rigid for other regions than region of interest where local calculations are performed using sliding window approach. It means that treating map as rigid, we can omit large number of calculations as point-to-point source to target observation equation discussed in Sect. 8.1.2. In that sense, the online large-scale SLAM system can be built based on map-centric approach and sliding window, performing local map updates and its relative poses to the rest of the rigid map. Obviously, such a system will converge to the optimal solution iteratively and much faster in area of sliding widow, thus entire map will be refined as if all regions will be visited with the local calculations. It is straight forward that this approach can be performed using the parallel implementation.

11.3 Continuous Trajectory SLAM Recent advances in SLAM [9, 12–14] show great interest of continuous trajectory SLAM introduced in [15] where map is built typically based on swiping LiDAR. Generally, if we have an initial trajectory node for each LiDAR measurement, it is possible to build SLAM system refining all points simultaneously. Such approach is extensively computational complex due to large number of required additional constraints preserving the shape of the trajectory. Moreover, most LiDAR measurement instruments derive data in batches for certain timestamps. Therefore, proper data synchronization and interpolation over consecutive stamped poses has to be applied to generate the locally consistent sub-maps. Such sub-maps are considered as rigid since the batch of data is small enough looking from reconstruction error point of view that is comparable with the error of measurement instrument. So, continuous trajectory SLAM, in most of the implementations, organizes the consecutive batches of LiDAR and the associated initial trajectory. The SLAM system can be built based on any LiDAR metric discussed in Chap. 8. Relative pose constraints discussed in Sects. 9.5.1 and 9.5.2 can be used for preserving motion model derived by the initial trajectory. The smoothness constraint discussed in Sect. 9.6 can sufficiently reduce errors caused by local accelerations measured by IMU. The anisotropic motion constraint discussed in Sect. 9.3 can incorporate the motion information into optimization process. Finally, the georeferenced information discussed in Sect. 9.7 can be fused for placing the trajectory in the global reference system. An efficient implementation can incorporate surfels discussed in Sect. 8.6. Furthermore, it can use any efficient 3D data decomposition discussed in Sect. 8.7.

282

11 Building Large-Scale SLAM

11.4 Design Matrix—Programming Example This section elaborates the c++ programming example for point-to-point source to target observation equation discussed in Sect. 8.1.2. The Eigen [16] library is used for organizing data and solving SLAM system. For convenience, the Tait–Bryan parametrization of the rotation matrix is chosen. First step (as for all other observation equations) is to prepare the C++ implementation of the observation equation based on the Python code, in this case, given in Listing 8.2. Thus, in the Listing 11.1, the functions point_to_ point_sour ce_to_target_tait_br yan_wc calculating the residual part (double delta_x, double delta_y, double delta_z) and point_to_ point_sour ce_to_target_tait_br yan_wc_ jacobian calculating the Jacobian (Eigen :: Matri x < double, 3, 6, Eigen :: RowMa jor > jacobian) will be generated automatically. Ones the Jacobian is given the design matrix A as variable tri plet List A can be filled as in Eq. (8.11). The weights P are set to 1 using variable tri plet List P. The residuals are placed in tri plet List B variable. Listing 11.2 shows the construction of A PA and A PB. The solver of the optimization system A PA=A PB using Cholesky factorization is shown in Listing 11.3. The result of the single iteration of the optimization process are stored in the Eigen :: Spar seMatri x < double > x variable. 1 2 3 4 5 6 7

for ( s i z e _ t i = 0 ; i < p o i n t s _ s o u r c e _ l o c a l . size () ; i ++) { E i g e n :: V e c t o r 3 d & p_t = p o i n t s _ t a r g e t _ g l o b a l [ i ]; E i g e n :: V e c t o r 3 d & p_s = p o i n t s _ s o u r c e _ l o c a l [ i ]; double delta_x ; double delta_y ; double delta_z ; p o i n t _ t o _ p o i n t _ s o u r c e _ t o _ t a r g e t _ t a i t _ b r y a n _ w c ( delta_x , delta_y , delta_z , p o s e _ s . px , p o s e _ s . py , p o s e _ s . pz , p o s e _ s . om , p o s e _ s . fi , p o s e _ s . ka , p_s . x () , p_s . y () , p_s . z () , p_t . x () , p_t . y () , p_t . z () ) ;

8 9 10

E i g e n :: Matrix < double , 3 , 6 , Eigen :: RowMajor > j a c o b i a n ; point_to_point_source_to_target_tait_bryan_wc_jacobian ( jacobian , p o s e _ s . px , p o s e _ s . py , p o s e _ s . pz , p o s e _ s . om , p o s e _ s . fi , p o s e _ s . ka , p_s . x () , p_s . y () , p_s . z () ) ;

11 12

int ir = t r i p l e t L i s t B . size () ;

13 14 15 16 17

18 19 20

for ( int row = 0; row < 3; row ++) { for ( int col = 0; col < 6; col ++) { if ( j a c o b i a n ( row , col ) ! = 0 . 0 ) { t r i p l e t L i s t A . e m p l a c e _ b a c k ( ir + row , col , - j a c o b i a n ( row , col ) ) ; } } }

21 22 23 24

t r i p l e t L i s t P . e m p l a c e _ b a c k ( ir , ir , 1) ; t r i p l e t L i s t P . e m p l a c e _ b a c k ( ir + 1 , ir + 1 , 1) ; t r i p l e t L i s t P . e m p l a c e _ b a c k ( ir + 2 , ir + 2 , 1) ;

11.4 Design Matrix—Programming Example

283

25

t r i p l e t L i s t B . e m p l a c e _ b a c k ( ir , 0, t r i p l e t L i s t B . e m p l a c e _ b a c k ( ir + 1 , 0 , t r i p l e t L i s t B . e m p l a c e _ b a c k ( ir + 2 , 0 ,

26 27 28 29

delta_x ); delta_y ); delta_z );

}

Listing 11.1 Example of fulfilling A P B for point to point source to target example 1

2

3

E i g e n :: S p a r s e M a t r i x < double > matA ( t r i p l e t L i s t B . size () , 6) ; E i g e n :: S p a r s e M a t r i x < double > matP ( t r i p l e t L i s t B . size () , t r i p l e t L i s t B . size () ) ; E i g e n :: S p a r s e M a t r i x < double > matB ( t r i p l e t L i s t B . size () , 1) ;

4 5

6

7

matA . s e t F r o m T r i p l e t s ( t r i p l e t L i s t A . b e g i n () , t r i p l e t L i s t A . end () ) ; matP . s e t F r o m T r i p l e t s ( t r i p l e t L i s t P . b e g i n () , t r i p l e t L i s t P . end () ) ; matB . s e t F r o m T r i p l e t s ( t r i p l e t L i s t B . b e g i n () , t r i p l e t L i s t B . end () ) ;

8 9 10

E i g e n :: S p a r s e M a t r i x < double > AtPA (6 , 6) ; E i g e n :: S p a r s e M a t r i x < double > AtPB (6 , 1) ;

11 12

13 14

E i g e n :: S p a r s e M a t r i x < double > AtP = matA . t r a n s p o s e () * matP ; AtPA = AtP * matA ; AtPB = AtP * matB ; Listing 11.2 Constructing the system of equations for point to point source to target example

1

2

E i g e n :: S i m p l i c i a l C h o l e s k y < E i g e n :: S p a r s e M a t r i x < double > > solver ( AtPA ); E i g e n :: S p a r s e M a t r i x < double > x = s o l v e r . s o l v e ( A t P B ) ; Listing 11.3 Solving the system of equations using Cholesky factorization

11.5 Loop Closing and Change Detection Loop closing and change detection tasks are related since once we can calculate the loop, we can verify the changes in overlapping regions. Recent advances in Deep Neural Networks show great interest of this topic [17–20]. An interesting comparison of recent AI-based change detection methods are discussed in [21, 22]. Those methods are based on imagery data. Change detection based on airborne laser scanning point clouds is discussed in [23] also as a method using AI. An interesting approach for LiDAR data is able to partition a 3D point cloud into static and dynamic points using ray traversal of a 3D voxel grid [24]. Generally, loop closure and change detection are related to the situation when the measurement instrument

284

11 Building Large-Scale SLAM

visited the same place for the second time. The current observation is taken into account, therefore, to detect the loop or change, the algorithm has to search all previous observations to find the corresponding one. According to the type of the observation, there are four main approaches for loop and change detection: • • • •

using image information [25–38], using 2D range information [39–43], using 3D range information [44–48], using combined information (video with 3D range information) [49–52].

These methods can also be used as prerequisites for change detection.

11.5.1 Processing Video Information Loop closing using video information is related to the matching of visual features. There are two aspects concerning a visual feature: the detection of the so-called key-point, which identifies an area of interest, and its descriptor, which characterizes its region. The descriptor is typically computed by measuring some relations of the surrounding points, leading to a multidimensional feature vector, which identifies the given key-point. A matching problem can be solved by performing the association of some pairs of key-points between a couple of frames. In computer vision and object recognition, many techniques are based on the detection of points of interests on object or surfaces. This is done through the process called features’ extraction. The main assumption is that, the feature has to be invariant to the image location, scale and rotation. To obtain key-points, we can use the following feature detectors, most of them were compared in [53]: • • • • • •

MSER [54], Harris Corner - a corner detector [55], KLT feature tracker [56–58], Salient Region Detector [59], Geometry Based Regions [60], Intensity-Based Method [61].

Each ROI can be characterized by the following feature descriptors: • • • • • • •

SIFT [62], SURF [63], BRIEF [64], FAST [65], ORB [66], GLOH - an extension of the SIFT descriptor [67], BRISK [68].

11.5 Loop Closing and Change Detection

285

SIFT descriptor is very popular because of its robustness and availability to many implementations [27, 37]. In the work [28] the ROI characterization is extended by the local color histograms. The difficulty in extracting features from an image lies in balancing two competing goals: high-quality description and low computational requirements. This goal was an important issue in developing BRISK methodology [68] that is relevant to SURF [63]. Authors of BRISK claim that it achieves comparable quality of matching at much less computation time. In SLAM, typical approach is related to a combination of SIFT characterization and the bag of words method. For example, authors of work [32] are combining SIFT with the bag of words method shown in [69] to improve the searching of the corresponding ROIs. Similar approach can be found in [36] (SIFT + Bag-of-words Appearance Model [31]), where a bag of words appearance model is used for finding nodes that are likely to have the similar appearance to the current video image. Another interesting approach to obtain characteristics of ROI is to combine the feature detectors with the feature descriptors. One prominent method is shown in [34] where the detector called MSER (Maximally Stable Extremal Regions) [54] offers significant invariance under affine transformations and it is combined with SIFT descriptor. Similar approach can be found in [33] (MSER + SIFT), [30] (KLT-features + SIFT) and [31] (Harris-affine detector [70] + SIFT). In [29], authors use simple Harris points, and find putative matches by normalized cross-correlation of a small patch around the point. Instead of matching techniques, matching strategies are also investigated to find most robust method for loop closing. A comparison of these techniques for matching of video information is shown in [25]. Three methods were compared: • map-to-map [71], • image-to-image [72, 73], • image-to-map [74]. Authors report that the map-to-map matching technique is unsuitable for monocular SLAM because the sparse maps contain too little information to reliably detect true correspondences. The image-to-image method is considered as working well in this sequence. However, authors pointed out that the method is not complete if the relative pose between corresponding images is needed for correcting the metric map. Furthermore, the method would benefit from making some use of the relative positions of the detected visual features to remove some obvious false positives. As a promising approach, the authors claim that the image-to-map method works well and returned the highest number of true positives with no false positives. They predict even better performance can be achieved by taking more of the image into account as outlined in their proposed extension to the method.

11.5.2 Processing LiDAR Information A method using 2D laser range scan matching for incremental mapping of large cyclic environments is shown in [39]. They called it a consistent pose estimation,

286

11 Building Large-Scale SLAM

because it find a set of poses that minimizes the total error of the system. Error is determined by the robot motion, and also overlapping scans. It means the better the scans match, the lower the error. This approach is dedicated for mapping large cyclical structures. Loop closing in large environment was also the main topic in the work [40], where Consistent Pose Estimation method (PCG) was proposed to solve this problem. In the work [41], EM-based algorithm for loop closing is shown. The key is a probability distribution over partitions of feature tracks that is determined in the so-called E-step, based on the current estimate of the motion. 3D range scan matching is proposed in the work [44] as an extension of work [45] where authors proposed an efficient loop closing method for mapping of large environments. In the contrary, in the work [46], authors use the elevation maps for loop closing in 6D. Loop closing is typically performed by adding the additional edges to the graph representing global map. Mapping of large environments determines large graphs. The computational problem has to be solved to compute robot position in acceptable time. One prominent method to find a loop is to use 3D descriptors characterizing 3D cloud of points. There are many available 3D descriptors (some of them were evaluated in [75]), they are listed below: • • • • • • • • • • •

NARF [48], Regional Point Descriptors (3D shape contexts and harmonic shape contexts) [76], PFH [77, 78], FPFH [79], VFH [80], CORS [81], RIFT [82], Spin Image Descriptor [83], Compact Covariance Descriptor [84], Barcode Shape Descriptor [85], PPF [86].

These descriptors can be used both for loop closing and change detection.

References 1. N Demmel, C Sommer, D Cremers, V Usenko, Square root bundle adjustment for large-scale reconstruction, in IEEE Conference on Computer Vision and Pattern Recognition (CVPR) (2021) 2. K. Kraus, I.A. Harley, S. Kyle, Photogrammetry: Geometry from Images and Laser Scans (De Gruyter, Berlin, Boston, 2011) 3. C. Wu, S. Agarwal, B. Curless, S. Seitz, Multicore bundle adjustment, in Proceedings IEEE Conference on Computer Vision and Pattern Recognition (2011), pp. 3057–3064 4. S. Agarwal, N. Snavely, S.M. Seitz, R. Szeliski, Bundle adjustment in the large, in Computer Vision – ECCV 2010. ed. by K. Daniilidis, P. Maragos, N. Paragios (Springer Berlin Heidelberg, Berlin, Heidelberg, 2010), pp. 29–42 5. D.C. Brown, A solution to the general problem of multiple station analytical stereotriangulation in Published Collections Department, Hagley Museum and Library (1958)

References

287

6. F. Zhang, The Schur Complement and its Applications, Numerical Methods and Algorithms, vol. 4. (Springer, New York, 2005) 7. R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige, W. Burgard, G2o: a general framework for graph optimization, in 2011 IEEE International Conference on Robotics and Automation (2011), pp. 3607–3613 8. N.J. Higham, Cholesky factorization. Wiley Interdiscip. Rev. Comput. Stat. 1(2), 251–254 (2009) 9. C. Park, P. Moghadam, J. Williams, S. Kim, S. Sridharan, C. Fookes, Elasticity meets continuous-time: Map-centric dense 3d lidar slam (2020) 10. T. Whelan, R.F. Salas-Moreno, B. Glocker, A. Davison, S. Leutenegger, Elasticfusion: real-time dense slam and light source estimation. Int. J. Robot. Res. 35, 1697–1716 (2016) 11. C. Park, S. Kim, P. Moghadam, J. Guo, S. Sridharan, C. Fookes, Robust photogeometric localization over time for map-centric loop closure. IEEE Robot. Autom. Lett. 4(2), 1768– 1775 (2019) 12. C. Park, P. Moghadam, S. Kim, A. Elfes, C. Fookes, S. Sridharan, Elastic lidar fusion: dense map-centric continuous-time slam (2018) 13. B. Koch, R. Leblebici, A. Martell, S. Jörissen, K. Schilling, A. Nüchter, Evaluating continuoustime slam using a predefined trajectory provided by a robotic arm. ISPRS Ann. Photogramm. Remote Sens. Spat. Inf. Sci. IV-2/W4, 91–98 (2017) 14. A. Nuchter, M. Bleier, J. Schauer, P. Janotta, Improving google’s cartographer 3d mapping by continuous-time slam. ISPRS - Int. Archives of the Photogramm. Remote Sens. Spat. Inf. Sci. XLII-2/W3, 543–549 (2017) 15. M. Bosse, R. Zlot, Continuous 3d scan-matching with a spinning 2d laser, in ICRA (IEEE, 2009), pp. 4312–4319 16. G. Guennebaud, B. Jacob, et al., Eigen v3 (2010). http://eigen.tuxfamily.org 17. L. Khelifi, M. Mignotte, Deep learning for change detection in remote sensing images: Comprehensive review and meta-analysis (2020) 18. Y. Chu, G. Cao, H. Hayat, Change detection of remote sensing image based on deep neural networks (2016) 19. T. Liu, L. Yang, D. Lunga, Change detection using deep learning approach with object-based image analysis. Remote Sens. Environ. 256, 112308 (2021) 20. O. Sefrin, F.M. Riese, S. Keller, Deep learning for land cover change detection. Remote Sens. 13(1) (2021) 21. W. Shi, M. Zhang, R. Zhang, S. Chen, Z. Zhan, Change detection based on artificial intelligence: state-of-the-art and challenges. Remote Sens. 12(10) (2020) 22. L. Khelifi, M. Mignotte, Deep learning for change detection in remote sensing images: comprehensive review and meta-analysis. IEEE Access, p. 1–1 (2020) 23. T. Huong Giang Tran, C. Ressl, N. Pfeifer, Integrated change detection and classification in urban areas based on airborne laser scanning point clouds. Sensors 18(2) (2018) 24. J. Schauer, A. Nuchter, Analytical change detection on the KITTI dataset, in Proceedings of the 14th IEEE International Conference on Control, Automation, Robotics and Vision (ICARCV ’20) (Shenzhen, China, 2020) 25. B. Williams, M. Cummins, J. Neira, P. Newman, I. Reid, J. Tardós, A comparison of loop closing techniques in monocular SLAM. Robot. Auton. Syst. 57(12), 1188–1197 (2009) 26. A. Majdik, D. Galvez-Lopez, G. Lazea, J.A. Castellanos, Adaptive appearance based loopclosing in heterogeneous environments, in 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (2011), pp. 1256 –1263 27. D. Scaramuzza, F. Fraundorfer, M. Pollefeys, Closing the loop in appearance-guided omnidirectional visual odometry by using vocabulary trees. Robot. Auton. Syst. 58(6), 820–827 (2010) 28. S. Bazeille, D. Filliat, Combining odometry and visual loop-closure detection for consistent topo-metrical mapping, in Proccedings of the Conference on COGnitive Systems with Interactive Sensors (COGIS2009) (2009)

288

11 Building Large-Scale SLAM

29. K. Konolige, M. Agrawal, Frame-frame matching for realtime consistent visual mapping, in Proceedings International Conference on Robotics and Automation (ICRA (2007) 30. B. Clipp, J. Lim, J.-M. Frahm, M. Pollefeys, Parallel, real-time visual slam, in 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems, October 18-22, 2010, Taipei, Taiwan (IEEE, 2010), pp. 3961–3968 31. M. Cummins, P.M. Newman, Probabilistic appearance based navigation and loop closing, in 2007 IEEE International Conference on Robotics and Automation, ICRA 2007, 10-14 April 2007, Roma, Italy (IEEE, 2007), pp. 2042–2048 32. A. Angeli, S. Doncieux, J.-A. Meyer, U. Pierre, M. Curie Paris, D. Filliat, Real-time visual loop-closure detection, in IEEE International Conference on Robotics and Automation (ICRA) (2008), pp. 1842–1847 33. L. Kunze, K. Lingemann, A. Nuchter, J. Hertzberg, Salient visual features to help close the loop in 6D SLAM, in The 5th International Conference on Computer Vision Systems (2007) 34. K. Ho, P. Newman, SLAM-loop closing with visually salient features, in IEEE International Conference on Robotics and Automation (ICRA) (April 2005) 35. S. Frintrop, A.B. Cremers, Top-down attention supports visual loop closing, in Proceedings of European Conference on Mobile Robotics (ECMR) (2005) 36. E.D. Eade, T.W. Drummond, Unified loop closing and recovery for real time monocular SLAM, in Proceedings of the British Machine Vision Conference (BMVA Press, 2008), pp. 6.1–6.10 37. V. Pradeep, G.G. Medioni, J. Weiland, Visual loop closing using multi-resolution SIFT grids in metric-topological SLAM, in 2009 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR 2009), 20-25 June 2009, Miami, Florida, USA (IEEE, 2009), pp. 1438–1445 38. D. Gossow, D. Paulus, P. Decker, An evaluation of open source SURF implementations, in RoboCup 2010: Robot Soccer World Cup XIV (Springer, Berlin, Heidelberg, 2010), pp. 169– 179 39. J.-S. Gutmann, K. Konolige, Incremental mapping of large cyclic environments, in Proceedings of IEEE International Symposium on Computational Intelligence in Robotics and Automation CIRA99 (1999), pp. 318–325 40. K. Konolige, Large-scale map-making, in Proceedings of the 19th national conference on Artifical intelligence, AAAI’04 (AAAI Press, 2004), pp. 457–463 41. M. Kaess, F. Dellaert, A Markov chain Monte Carlo approach to closing the loop in SLAM, in ICRA’05 (2005), pp. 643–648 42. C. Stachniss, D. Hahnel, W. Burgard, Exploration with active loop-closing for FastSLAM, in IEEE/RSJ International Conference on Intelligent Robots and Systems, (IROS 2004), vol. 2 (2004), pp. 1505–1510 43. C. Stachniss, D. Hahnel, W. Burgard, G. Grisetti, On actively closing loops in grid-based FastSLAM. Adv. Robot. 19, 2005 (2005) 44. J. Sprickerhof, A. Nüchter, K. Lingemann, J. Hertzberg, A heuristic loop closing technique for large-scale 6D SLAM. Automatika - J. Control Meas. Electron. Comput. Commun. 52(3), 199–222 (2011) 45. J. Sprickerhof, A. Nuechter, K. Lingemann, J. Hertzberg, An explicit loop closing technique for 6D SLAM, in Proceedings of the 4th European Conference on Mobile Robots, ECMR 09, September 23-25, 2009, Mlini/Dubrovnik, Croatia. ed. by I. Petrovic, A.J. Lilienthal (KoREMA, 2009), pp. 229–234 46. P. Pfaff, R. Triebel, W. Burgard, An efficient extension to elevation maps for outdoor terrain mapping and loop closing. Int. J. Rob. Res. 26(2), 217–230 (2007) 47. K. Granström, T.B. Schön, Learning to close the loop from 3D point clouds, in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (Taipei, Taiwan, 2010) 48. B. Steder, R.B. Rusu, K. Konolige, W. Burgard, NARF: 3D range image features for object recognition, in Workshop on Defining and Solving Realistic Perception Problems in Personal Robotics at the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (Taipei, Taiwan, 2010)

References

289

49. C. Chen, H. Wang, Large-scale loop-closing by fusing range data and aerial image. Int. J. Robot. Autom. 22(2), 160–169 (2007) 50. P. Newman, D. Cole, K. Ho, Outdoor SLAM using visual appearance and laser ranging, in Proceedings IEEE International Conference on Robotics and Automation, ICRA (2006), pp. 1180–1187 51. P. Henry, M. Krainin, E. Herbst, X. Ren, D. Fox, RGB-D mapping: using depth cameras for dense 3D modeling of indoor environments, in Proceedings of the 12th International Symposium on Experimental Robotics (2010) 52. F. Steinbruecker, J. Sturm, D. Cremers, Real-time visual odometry from dense RGB-D images, in Workshop on Live Dense Reconstruction with Moving Cameras at the International Conference on Computer Vision (ICCV) (2011), pp. 1–4 53. K. Mikolajczyk, C. Schmid, Comparison of affine-invariant local detectors and descriptors, in 12th European Signal Processing Conference (EUSIPCO ’04) (Vienna, Austria, 2004), pp. 1729–1732 54. J. Matas, O. Chum, M. Urban, T. Pajdla, Robust wide baseline stereo from maximally stable extremal regions, in In British Machine Vision Conference (2002), pp. 384–393 55. C. Harris, M. Stephens, A combined corner and edge detector, in Proceedings of Fourth Alvey Vision Conference (1988), pp. 147–151 56. B.D. Lucas, T. Kanade, An iterative image registration technique with an application to stereo vision, in Proceedings of the 7th International Joint Conference on Artificial Intelligence Volume 2, IJCAI’81 (Morgan Kaufmann Publishers Inc, San Francisco, CA, USA, 1981), pp. 674–679 57. C. Tomasi, T. Kanade, Detection and Tracking of Point Features. Shape and motion from image streams. School of Computer Science, Carnegie Mellon Univ. (1991) 58. J. Shi, C. Tomasi, Good features to track, in IEEE Conference on Computer Vision and Pattern Recognition (1994), pp. 593–600 59. T. Kadir, A. Zisserman, M. Brady, An affine invariant salient region detector, in Computer Vision-ECCV 2004, 8th European Conference on Computer Vision, Prague, Czech Republic, May 11-14, 2004. Proceedings, Part I, ed. by T. Pajdla, J. Matas. Lecture Notes in Computer Science, vol. 3021 (Springer, 2004), pp. 228–241 60. T. Tuytelaars, L. Van Gool, Content-based image retrieval based on local affinely invariant regions, in International Conference on Visual Information Systems (1999), pp. 493–500 61. T. Tuytelaars, L. Van Gool, Wide baseline stereo matching based on local, affinely invariant regions, in Proceedings of BMVC (2000), pp. 412–425 62. D.G. Lowe, Distinctive image features from scale-invariant keypoints. Int. J. Comput. Vis. 60(2), 91–110 (2004) 63. H. Bay, A. Ess, T. Tuytelaars, L. Van Gool, Speeded-up robust features (SURF). Comput. Vis. Image Underst. 110(3), 346–359 (2008) 64. M. Calonder, V. Lepetit, C. Strecha, P. Fua, BRIEF: binary robust independent elementary features, in Proceedings of the 11th European Conference on Computer Vision: Part IV, ECCV’10 (Springer, Berlin, Heidelberg, 2010), pp. 778–792 65. E. Rosten, T. Drummond, Machine learning for high-speed corner detection, in European Conference on Computer Vision (2006), pp. 430–443 66. E. Rublee, V. Rabaud, K. Konolige, G. Bradski, ORB: an efficient alternative to SIFT or SURF, in International Conference on Computer Vision (Barcelona, 2011) 67. K. Mikolajczyk, C. Schmid, A performance evaluation of local descriptors. IEEE Trans. Pattern Anal. Mach. Intell. 27(10), 1615–1630 (2005) 68. S. Leutenegger, M. Chli, R. Siegwart, BRISK: binary robust invariant scalable keypoints, in Proceedings of the IEEE International Conference on Computer Vision (2011) 69. D. Filliat, A visual bag of words method for interactive qualitative localization and mapping, in Proceedings of the International Conference on Robotics and Automation (ICRA) (2007) 70. K. Mikolajczyk, C. Schmid, Scale and affine invariant interest point detectors. Int. J. Comput. Vis. 60(1), 63–86 (2004)

290

11 Building Large-Scale SLAM

71. L. Clemente, A. Davison, I. Reid, J. Neira, J. Domingo Tardós, Mapping large loops with a single hand-held camera, in Proceedings of Robotics: Science and Systems Conference (2007) 72. M. Cummins, P. Newman, Accelerated appearance-only SLAM, in IEEE International Conference on Robotics and Automation, 2008. ICRA 2008 (2008), pp. 1828–1833 73. M. Cummins, P. Newman, FAB-MAP: probabilistic localization and mapping in the space of appearance. Int. J. Robot. Res. 27(6), 647–665 (2008) 74. B. Williams, M. Cummins, J. Neira, P. Newman, I. Reid, J. Tardós, An image-to-map loop closing method for monocular slam, in proc. IEEE International Conference on Intelligent Robots and Systems (2008) 75. L.A. Alexandre, 3D descriptors for object and category recognition: a comparative evaluation, in Workshop on Color-Depth Camera Fusion in Robotics at the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (Vilamoura, Portugal, 2012), pp. 1–6 76. A. Frome, D. Huber, R. Kolluri, T. Bulow, J. Malik, Recognizing objects in range data using regional point descriptors, in European Conference on Computer Vision (2004), pp. 224–237 77. R. Bogdan Rusu, N. Blodow, Z. Csaba Marton, M. Beetz, Aligning point cloud views using persistent feature histograms, in Proceedings of the 21st IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (Nice, France, 2008), pp. 1–8 78. R. Bogdan Rusu, Z. Csaba Marton, N. Blodow, M. Beetz, Learning informative point classes for the acquisition of object model maps, in Proceedings of the 10th International Conference on Control, Automation, Robotics and Vision (ICARCV) (Hanoi, Vietnam, December 17–20 2008), pp. 643–650 79. R. Bogdan Rusu, N. Blodow, M. Beetz, Fast point feature histograms (FPFH) for 3D registration, in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA) (Kobe, Japan, May 12-17 2009), pp. 3212–3217 80. R. Bogdan Rusu, G. Bradski, R. Thibaux, J. Hsu, Fast 3D recognition and pose using the viewpoint feature histogram, in Proceedings of the 23rd IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (Taipei, Taiwan, October 18–22 2010), pp. 2155– 2162 81. H. Van Nguyen, F. Porikli, Concentric ring signature descriptor for 3D objects, in 2011 18th IEEE International Conference on Image Processing (ICIP) (2011), pp. 2893–2896 82. L.J. Skelly, S. Sclaroff, Improved feature descriptors for 3D surface matching, in Proceedings of SPIE 6762, Two- and Three-Dimensional Methods for Inspection and Metrology V, 67620A (2007), pp. 67620A–12 83. A.E. Johnson, M. Hebert, Using spin images for efficient object recognition in cluttered 3D scenes. IEEE Trans. Pattern Anal. Mach. Intell. 21(5), 433–449 (1999) 84. D. Fehr, A. Cherian, R. Sivalingam, S. Nickolay, V. Morellas, N. Papanikolopoulos, Compact covariance descriptors in 3D point clouds for object recognition, in ICRA (IEEE, 2012), pp. 1793–1798 85. A. Collins, A. Zomorodian, G. Carlsson, L.J. Guibas, A barcode shape descriptor for curve point cloud data. Comput. Graph. 28(6), 881–894 (2004) 86. B. Drost, M. Ulrich, N. Navab, S. Ilic, Model globally, match locally: efficient and robust 3D object recognition, in IEEE Conference on Computer Vision and Pattern Recognition (CVPR) (2010), pp. 998–1005

Chapter 12

Final Map Qualitative and Quantitative Evaluation

12.1 Measurement Instrument The measurement instrument used in most experiments is multi-sensor system (Fig. 12.1) composed of Intel RealSense Tracking Camera T265, three Velodynes VLP16, custom electronics for multi-lidar synchronization and VR Zotac GO backpack computer equipped with GeForce GTX1070. This configuration allows the operator to collect and process 3D point clouds to obtain a consistent 3D map. RealSense Tracking Camera T265 provides an initial trajectory based on VO. The goal of the experiments was to verify the observations equations and SLAM optimization system elaborated in this book.

12.2 TLS Matching TLS data before registration is shown in Figs. 12.2 and 12.3. Figures 12.4, 12.5 and 12.6 show the result of the registration as a consistent point cloud. Different LiDAR observation equations: point-to-point Eq. (8.3), point-to-projection onto plane Eq. (8.41) and point-to-plane Eq. (8.65) are compared looking from convergence measured as RMS point of view. Also, different parametrization: Tait–Bryan, Rodrigues and quaternion of the rotation matrix is evaluated. It can be seen in Fig. 12.7 that the fastest convergence and, at the same time, the most accurate result is obtained for point-to-point observation equation with Rodrigues and Tait–Bryan parametrization (plots are almost the same). The result of the fusion of the point-to-point and point-to-plane observation equations with quaternion parametrization is shown in Fig. 12.8. This experiment demonstrates that the fusion of the LiDAR observations equations can improve both the performance and the convergence. I am far away from the statement that LiDAR is best looking from any metric point of view since in many

© The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5_12

291

292

12 Final Map Qualitative and Quantitative Evaluation

Fig. 12.1 Experimental mobile backpack system composed of three LiDARs Velodyne VLP16, IMU and tracking camera RealSense T265

Fig. 12.2 Perspective view of entire point cloud

practical applications there are plenty of factors determining accuracy, robustness and convergence. The choice of LiDAR metric and rotation matrix parametrization is crucial, and it could cause not similar results.

12.2 TLS Matching

Fig. 12.3 Top view of intersection

Fig. 12.4 Result of the registration

293

294

12 Final Map Qualitative and Quantitative Evaluation

Fig. 12.5 Zoom of data before registration

Fig. 12.6 Zoom of data after registration

12.3 Mobile Backpack Mapping System Qualitative Evaluation

295

Fig. 12.7 Registration with different LiDAR observation equations

Fig. 12.8 Registration with fused point-to-point (Eq. (8.3)) and point-to-plane (Eq. (8.65)) LiDAR observation equations

12.3 Mobile Backpack Mapping System Qualitative Evaluation In this section, several qualitative experiments are shown for demonstrating the SLAM pipeline composed of VO, LO and finally the map refinement based on full simplest Graph SLAM system composed of point-to-point Eq. (8.3) and relative pose constraint equation (9.15). It can be seen that, for all cases, the scale factor of VO is evident, thus an additional step in SLAM pipeline integrated with LO was incorporated. Figures 12.9, 12.10 and 12.11 show mapping of the INDOOR environment. The result of the OUTDOOR mapping is shown in Figs. 12.12, 12.13, 12.14 and

296

12 Final Map Qualitative and Quantitative Evaluation

Fig. 12.9 Trajectories for SLAM pipeline, 1: VO, 2: LO, 3: Graph SLAM

Fig. 12.10 Perspective view of the map

12.15. An interesting experiment demonstrating the usage of mobile backpack mapping system for digitization of the complex multi-level structure of NPP is shown in Figs. 12.16, 12.17, 12.18, 12.19, 12.20 and 12.21 (Fig. 12.22).

12.3 Mobile Backpack Mapping System Qualitative Evaluation

Fig. 12.11 Top view of the map

Fig. 12.12 Graph SLAM trajectory

297

298

12 Final Map Qualitative and Quantitative Evaluation

Fig. 12.13 Perspective view of the map

Fig. 12.14 Perspective view of the sub map

12.3 Mobile Backpack Mapping System Qualitative Evaluation

Fig. 12.15 Perspective view of the sub map

Fig. 12.16 VO trajectory top view

299

300

Fig. 12.17 VO trajectory side view

Fig. 12.18 Perspective view of the NPP

12 Final Map Qualitative and Quantitative Evaluation

12.3 Mobile Backpack Mapping System Qualitative Evaluation

Fig. 12.19 Top view of the NPP

301

302

12 Final Map Qualitative and Quantitative Evaluation

Fig. 12.20 Perspective view of the sub map Fig. 12.21 Top view of the sub map

12.4 Improving State-of-the-Art Visual SLAM

303

Fig. 12.22 Top view of registered point clouds (red: OpenVSLAM, white: improved) to reference TLS data. Incorporating equirectangular camera colinearity observation equation (7.75) discussed in Sect. 7.3.1 to OpenVSLAM [1]

12.4 Improving State-of-the-Art Visual SLAM This experiment shows that the State-of-the-Art Visual SLAM algorithm [1] can be improved by incorporating observation equation elaborated in this book. Data were collected with an equirectangular camera GoPro and processed with OpenVSLAM, an open-source software. The result are the 3D point cloud composed of all landmarks (tie points) and the trajectory composed of consecutive set of camera’s poses. This point cloud was registered to reference map from Fig. 12.4. Thus, displacements for landmarks can be measured as in Fig. 12.22, Fig. 12.23 and Fig. 12.24. The result of OpenVSLAM is almost 20 cm error. After incorporating equirectangular camera colinearity observation equation (7.75) discussed in Sect. 7.3.1 and registration to the reference point cloud, the error was reduced to 7 cm as in Fig. 12.24. This experiment shows that it is possible to improve the robust Visual SLAM algorithm by additional calculations. It is important to mention that Visual SLAM is an online system, therefore, we should not expect high accuracy as it is desired in Photogrammetry. For this reason, we can say that incorporating additional observation equations into post-processing makes a transition from SLAM to Photogrammetry.

304

12 Final Map Qualitative and Quantitative Evaluation

Fig. 12.23 OpenVSLAM error: XY = 0.18 m. Incorporating equirectangular camera colinearity observation equation (7.75) discussed in Sect. 7.3.1 to OpenVSLAM [1]

Fig. 12.24 After improvement error: XY = 0.067 m. Incorporating equirectangular camera colinearity observation equation (7.75) discussed in Sect. 7.3.1 to OpenVSLAM [1]

Reference 1. S. Sumikura, M. Shibuya, K. Sakurada, OpenVSLAM: a versatile visual SLAM framework, in Proceedings of the 27th ACM International Conference on Multimedia, MM ’19 (ACM, New York, NY, USA, 2019), pp. 2292–2295

Appendix

Table A.1 shows the list of C++ examples as a complementary educational material to this book [1]. These examples use synthetic data to demonstrate the use of observation equations. Table A.2 shows the list of programs used to generate figures, thus the intention is that the navigation via examples will be much easier.

© The Editor(s) (if applicable) and The Author(s), under exclusive license to Springer Nature Singapore Pte Ltd. 2022 J. B¸edkowski, Large-Scale Simultaneous Localization and Mapping, Cognitive Intelligence and Robotics, https://doi.org/10.1007/978-981-19-1972-5

305

306

Appendix

Table A.1 The list of C++ examples as a complementary educational material to this book [1] Program Observation Sect. equations adaptive_r obust_loss_ f unction_demo bundle_o f _rays_intersection distance_to_cir cle equir ectangular _camera_colinearit y_ba equir ectangular _camera_coplanarit y_ba geor e f er ence − case1

(4.84) (5.61) (9.53) (9.1), (7.75) (9.1), (7.84) (9.1), (9.15), (9.16), (9.17), (8.9) geor e f er ence − case2 (9.15), (9.4) geor e f er ence − case4 (9.1), (7.2.1), (9.4) metric_camera_colinearit y_ba (9.1), (7.50) metric_camera_coplanarit y_ba (9.1), (7.63) optimi zing_scale_V O_with_lidar _data (8.27) per spective_camera_ba (9.1), (7.5) per spective_camera_exter nal_orientation_ plucker _line (9.1), (7.29) per spective_camera_intrinsic_calibration (9.1), (7.13) plane_to_ plane (9.1), (8.109) plucker _line_to_ plucker _line (9.1), (8.95) point_to_line (9.1), (8.55) point_to_ plane (9.1), (8.65) distance_ point_to_ plane (8.70) point_to_ point_sour ce_to_target (9.1), (8.9) point_to_ point_sour ce_to_landmar k (8.20) point_to_ point_with_scale (9.1), (8.27) point_to_ point (9.1), (8.3) point_to_ pr ojection_onto_line (9.1), (8.40) point_to_ pr ojection_onto_ plane (9.1), (8.40) mirr or _sim (8.120) r ectangular _object_with_unknown_width_height (9.1, 10.3) r elative_ pose(1) (9.1), (9.15), (9.16), (9.17) r elative_ pose(2) (9.1), (9.25), (9.26), (9.27) shea f _o f _ planes (10.9) simple_optimi zation_ pr oblem_ f unc_x_with_constraints (4.33) simple_optimi zation_ pr oblem_ f unc_x y_with_constraints (4.38) smoothness (9.1), (9.32), (9.33), (9.34) sur f ace_r econstr uction_ f r om_lidar _ point_cloud (9.1), (9.15), (9.32), (8.9)

4.2.3 5.4.4 9.8 7.3.1 7.3.2 9.7.1 9.7.2 9.7.4 7.2.1 7.2.2 8.1.4 7.1.1 7.1.3 7.1.2 8.3.2 8.3.1 8.2.1 8.2.2 8.2.3 8.1.2 8.1.3 8.1.4 8.1.1 8.1.6 8.1.6 8.4 10.2 9.5.1 9.5.2 10.3 4.2.1.3 4.2.1.4 9.6 10.4

1.2, 1.3, 9.25, 9.26 4.1 4.3, 4.4, 4.5, 4.6, 4.7, 4.8, 4.9, 4.10, 4.11, 4.21, 4.22 9.7, 9.8, 9.10, 9.12, 9.11, 9.12 4.12, 4.13, 4.14, 4.15, 4.16, 4.17, 4.18, 4.19, 4.20, 9.1, 9.2, 9.3, 9.4, 9.5, 9.6 9.16, 9.17, 9.18, 9.19, 9.20, 9.21, 9.22, 9.23, 9.24, 9.13, 9.14, 9.15 4.23, 4.24, 4.25 4.26, 4.27, 4.28 4.29, 4.30, 4.31, 4.32, 4.33 4.34, 4.35 4.39, 4.40, 4.41 5.6 8.12, 8.13, 8.14, 8.15 8.16, 8.19, 8.20 9.27, 9.28 9.29, 9.30 9.33 9.34, 9.35 6.5, 6.8, 6.9 6.10, 6.11, 6.12 10.1 10.2 10.3

pose2d_graph_slam hor n. py simple_optimi zation_ pr oblem_ f unc_x_with_constraints

m_estimator s_ f unctions_viewer cer es_r obust_loss_ f unction_viewer adaptive_r obust_loss_ f unction_viewer adaptive_r obust_loss_ f unction_demo point_cloud_registration_lie_algebra bundle_o f _rays_intersection point_cloud_registration view_nns geor e f er ence − case1 geor e f er ence − case2 photogrammetr y distance_to_cir cle point_to_ point_sour ce_to_landmar k r elative_ pose_covariances r ectangular _object_with_unknown_width_height shea f _o f _ planes sur f ace_r econstr uction_ f r om_lidar _ point_cloud

simple_optimi zation_ pr oblem_ f unc_x y_with_constraints

Figs.

Program

Table A.2 The list of programs used to generate figures, thus the intention is that the navigation via examples will be much easier

Appendix 307

308

Appendix

Reference 1. J. Bedkowski, Observation observation_equations

equations

(2021).

https://github.com/JanuszBedkowski/