Contributions to metric-topological localization and mapping in mobile robotics

Doctoral Dissertation Contributions to metric-topological localization and mapping in mobile robotics Eduardo Fernández-Moral 2014 Tesis doctoral en Ingeniería Mecatrónica Dpto. de Ingeniería de Sistemas y Automática Universidad de Málaga AUTOR: Eduardo Fernández Moral EDITA: Publicaciones y Divulgación Científica. Universidad de Málaga Esta obra está sujeta a una licencia Creative Commons: Reconocimiento - No comercial - SinObraDerivada (cc-bync-nd): Http://creativecommons.org/licences/by-nc-nd/3.0/es Cualquier parte de esta obra se puede reproducir sin autorización pero con el reconocimiento y atribución de los autores. No se puede hacer uso comercial de la obra y no se puede alterar, transformar o hacer obras derivadas. Esta Tesis Doctoral está depositada en el Repositorio Institucional de la Universidad de Málaga (RIUMA): riuma.uma.es UNIVERSIDAD DE MÁLAGA DEPARTAMENTO DE INGENIERÍA DE SISTEMAS Y AUTOMÁTICA El Dr. D. Javier González Jiménez y el Dr. D. Vicente M. Arévalo Espejo, directores de la tesis titulada “Contributions to metrictopological localization and mapping in mobile robotics” realizada por D. Eduardo Fernández-Moral, certifican su idoneidad para la obtención del título de Doctor en Ingeniería Mecatrónica. Málaga, 10 de Septiembre de 2014 ——————————————– Dr. D. Javier González Jiménez ——————————————– Dr. D. Vicente M. Arévalo Espejo Dept. of System Engineering and Automation University of Málaga Studies in Mechatronics Contributions to metric-topological localization and mapping in mobile robotics AUTHOR: Eduardo Fernández Moral SUPERVISORS: Javier González Jiménez Vicente M. Arévalo Espejo A mi tío Hilario, por mostrarme el camino... Acknowledgements First, I would like to express my immense gratitude to my supervisors Prof. Dr. Javier González Jiménez and Dr. Vicente Arévalo for guiding me through the hard but exciting adventure of working for a PhD. They are responsible of the invaluable technical and theoretical knowledge that I have acquired, which gives me professional independence. The uncountable hours we have spent together discussing about different research problems and their results have today a reward in this thesis. Also, they were always comprehending at the hard moments of this PhD, offering their best. I also have to thank Guillaume Charpiat and Alexander Davies for the time they spent reviewing this thesis and for the useful comments they have provided me. I am also grateful to the committee members for having accepted to be members of this thesis’ tribunal. These four years of research probably would not have been successful without the great people in our lab. Thanks to Raul Ruiz, Javier G. Monroy, Ana Gago, Francisco Moreno, Mariano Jaimez, Francisco Meléndez, Carlos Sánchez, Manuel López, Rubén Gómez and Jesús Briales for being good colleagues and friends. Thanks to them for their support and for creating an atmosphere of joy where it was a pleasure to work. I also have to thank the senior researchers at the MAPIR group, Cipriano Galindo and Juan Antonio Fernández, and to our former colleague Jose Luis Blanco for sharing always their knowledge and expertise when I had needed it. During 2012 I had the opportunity to work as a research visitor at the Vision Information Laboratory at the University of Bristol. I acknowledge Dr. Walterio Mayol Cuevas and his team for giving me this opportunity. I do not forget José Martínez Carranza for the fruitful discussions during this period. Also, my deep gratitude goes to Dr. Patrick Rives who received me at the Lagadic Team, at INRIA Sophia-Antipolis for a research visit during 2013. I need to thank as well the rest of the staff and PhD students in his team to make me feel like at home during this period. Finally, I feel extremely grateful to my family, which has always supported me along this long period at university and during my PhD. And I will not forget to mention my good friends Manuel J. García and Manuel J. Nájera because despite my research work keeps me far from them, I know that they will be there when I need them. i Resumen de la Tesis Doctoral Sumario La presente tesis doctoral aborda el problema de localización y cartografía (o mapeo) en robótica móvil. La capacidad de un robot móvil para crear un mapa de su entorno a partir de la información obtenida por sus sensores es necesaria para que dicho robot pueda localizarse y para que éste pueda navegar de forma autónoma. Este problema ha sido ampliamente estudiado en las últimas décadas, sin embargo, las soluciones obtenidas presentan aún importantes limitaciones. Estas limitaciones afectan principalmente a la operación en entornos a gran escala y en la adaptación a entornos dinámicos. En este contexto, esta tesis representa otro paso más en el camino hacia soluciones de localización y mapeo eficientes. Una primera contribución de este trabajo es una nueva estrategia de cartografía que combina dos características principales: una representación compacta de la información métrica, y la organización de esta información métrica en una estructura topológica que mejora la eficiencia en la localización y la optimización del mapa. Para ello se propone un mapa basado en segmentos planos que son extraídos de las imágenes de rango o RGB-D. Este mapa basado en planos (PbMap) es especialmente adecuado para escenarios de interior, y tiene la ventaja de ser altamente descriptivo a pesar de su compacidad, lo cuál permite reconocer escenarios en tiempo real y cerrar bucles, siendo éstas tareas clave para la localización y mapeo simultáneos (SLAM). Ambas operaciones se basan en el emparejamiento de segmentos planos teniendo en cuenta sus relaciones geométricas. Por otro lado, la abstracción de la información métrica es necesaria para abordar el problema de SLAM en gran escala y para la navegación en entornos complejos. En este sentido, esta tesis propone organizar el mapa en tiempo real en una estructura métrica-topológica de acuerdo a la co-visibilidad de las observaciones. Esta tesis también presenta un sistema de localización y cartografía simultánea (SLAM) empleando un nuevo sensor omnidireccional RGB-D que combina varios sensores Asus Xtion Pro Live. Este dispositivo permite construir rápidamente modelos densos del entorno basados en nubes de puntos a un bajo coste con respecto a 1 2Resumen de la Tesis Doctoral otras alternativas previas. Nuestro enfoque en SLAM se basa en la gestión de una estructura jerárquica de keyframes consistente en una capa de bajo nivel con información métrica y varias capas superiores con información topológica que son útiles para SLAM en gran escala y para navegación. Este sistema de SLAM funciona a una frecuencia de 30 Hz y permite la obtención de mapas de alta consistencia. También se propone una técnica de calibración extrínseca para obtener las poses relativas de una combinación de sensores de rango 3D, como aquellos empleados en el dispositivo RGB-D omnidireccional mencionado anteriormente. La calibración se obtiene a partir de la observación de superficies planas en un entorno estructurado de una manera rápida, fácil y robusta. Esta nueva técnica presenta ventajas cualitativas y cuantitativas con respecto a los enfoques anteriores. Esta solución es ampliada para calibrar cualquier combinación de sensores de rango en cualquier configuración, incluyendo sensores 2D y 3D. La calibración de tales conjuntos de sensores es interesante no sólo en robótica móvil, sino también en el campo de vehículos autónomos. Introducción En los últimos años del siglo XX y principios del XXI se había generalizado en el mundo desarrollado la impresión de que en la época actual viviríamos rodeados de robots inteligentes que harían nuestras vidas más fáciles, o deberíamos decir, que nos ahorrarían tediosas tareas rutinarias. Las películas de ciencia ficción han contribuido a crear dicha imagen de nuestro futuro, en el que convivimos con complejos robots móviles. Por otro lado, los avances en microelectrónica, el aumento del rendimiento de computadores, y la aparición de nuevos materiales y sensores, que hoy tienen sus resultados en los actuales teléfonos móviles (o smartphones) por ejemplo, también han ayudado a pensar que la robótica móvil aparecería pronto en nuestras vidas. Sin embargo la realidad sigue siendo muy diferente de aquella imagen futurista, la cuál necesitará probablemente un largo tiempo para hacerse realidad. Una de las principales razones para no tener robots móviles entre nosotros es la dificultad para procesar e interpretar información del entorno del robot. Esto es clave para la robótica móvil ya que un robot debe entender su entorno para poder interactuar con él. En este contexto, la capacidad de un robot móvil para crear un mapa de su entorno al mismo tiempo que se localiza en dicho mapa es crucial. Este problema, conocido como localización y cartografía simultáneas (SLAM), ha recibido una gran atención en las últimas décadas, dando como resultado una amplia literatura sobre este tema que abarca diferentes condiciones de trabajo y diferentes tipos de sensores. Sin embargo, a pesar del gran esfuerzo dedicado a solventar este problema, las soluciones presentadas tienen todavía importantes limitaciones que impiden la creación de robots fiables que puedan ejecutar tareas útiles en condiciones generales. La investigación en SLAM se ha centrado principalmente en el uso de dos tipos de sensores exteroceptivos: cámaras y sensores de rango. Las cámaras presentan im- Resumen de la Tesis Doctoral 3 portantes ventajas como su bajo coste, su compacidad y su bajo consumo, lo que las hace adecuadas para diversas aplicaciones en robótica móvil además de SLAM, como el reconocimiento de objetos. Además, las cámaras proporcionan información similar a la captada por nuestros ojos, siendo por tanto una herramienta intuitiva de percepción. Sin embargo, la información real proporcionada por una cámara es una matriz rectangular donde cada celda (pixel) captura la luminosidad procedente de una dirección del espacio. Por lo tanto, el primer problema es representar esta información de una manera compacta y estructurada que pueda ser interpretada fácilmente. La mayoría de las soluciones encontradas en la literatura abordan este problema mediante la extracción de diferentes tipos de características de bajo nivel que pueden ser identificadas desde diferentes puntos de vista a lo largo de la trayectoria del robot. Otros enfoques recientes tratan de identificar objetos significativos para ser utilizados como referencias en el entorno. En cualquier caso, el problema de SLAM visual implica algunas limitaciones intrínsecas para operar en escenarios con poca textura, con repetición de texturas (visual aliasing), o donde existen reflejos especulares. Con respecto a los sensores de rango, la percepción de profundidad permite a un robot crear representaciones del espacio y evitar colisiones, creando mapas útiles para la auto-navegación y para llevar a cabo el reconocimiento de objetos y lugares. Existen distintas maneras de obtener información de profundidad del entorno, en el que podemos diferenciar entre los métodos activos o pasivos. Las estrategias activas proyectan y capturan luz de la escena para inferir la profundidad, mientras que las estrategias pasivas sólo capturan luz. Ejemplos de este último son los sistemas de visión estéreo o multi-cámaras; mientras que algunos ejemplos de visión activa son LIDAR, cámaras de tiempo de vuelo y sensores basados en luz estructurada como Kinect. Diferentes soluciones se han presentado para SLAM con un robot que se mueve sobre un mismo plano en un entorno estático utilizando un sensor 2D. El uso de sensores 3D también ha sido introducido para operar en condiciones más complejas. Para este último, algunas desventajas comunes son el alto precio de los sensores, las bajas frecuencias de observación o la escalabilidad de las soluciones de SLAM. El problema de SLAM también se ha tratado combinando información visual y de rango, especialmente después de la aparición de cámaras RGB-D de bajo coste como Asus Xtion Pro Live o Microsoft Kinect. La fusión de la información de profundidad e intensidad mejora la capacidad de SLAM proporcionando robustez a situaciones donde la intensidad o la profundidad por sí solas tienen un bajo rendimiento. Dichos sensores se han empleado por ejemplo para odometría mediante el registro denso de las imágenes RGB-D. Cuando estos sensores se utilizan en SLAM, uno de los principales problemas es cómo representar y almacenar el gran flujo de datos que éstos proporcionan. Differentes estrategias de cartografía basadas en keyframes han proporcionado buenos resultados en pequeños entornos, pero aún existen problemas de escalabilidad y por lo tanto, representaciones del entorno más compactas son deseables para realizar otras tareas junto con SLAM. Con respecto a la robótica móvil, los desafíos actuales en localización y mapeo están relacionados principalmente con limitaciones en el tamaño del entorno de trabajo y con la fiabilidad de funcionamiento a lo largo del tiempo. Otro problema clave 4Resumen de la Tesis Doctoral es la integración de información simbólica para aumentar la robustez y el rendimiento a la vez que se proporciona información útil para otros tareas. Pero como se puede intuir, estos desafíos requieren avances incrementales en las soluciones actuales hasta llegar al objetivo de localización y mapeo robusto en condiciones más generales. Ámbito de la tesis La investigación del problema de localización y mapeo simultáneo ha recibido una amplia atención en los últimos años y se han presentado diferentes soluciones al problema. En este contexto, la contribución de esta tesis debe ser considerada como un paso más en un largo camino hacia la obtención de soluciones más generales, robustas y eficientes para SLAM, que doten a los robots de autonomía real en una variedad de escenarios. El problema de SLAM implica diferentes subproblemas, desde la calibración de los sensores del robot a la representación de la información en el mapa junto con la localización y re-localización (cierre de bucle) eficientes. Estos problemas se abordan en los capítulos siguientes mediante el uso de diferentes sensores visuales y de rango. Concretamente, esta tesis presenta una nueva metodología para la calibración de conjuntos de sensores de rango que está basada en la observación de superficies planas. Dicha metodología es utilizada para calibrar un nuevo sensor que consta de varias cámaras de RGB-D, permitiendo también calibrar otras combinaciones de sensores de rango con escáneres láser 2D y sensores de rango 3D con los que están equipados muchos robots móviles, incluyendo los robots empleados en esta tesis. La observabilidad de los diferentes problemas (dependiendo del tipo de sensores) es analizada, y se definen las condiciones para resolver la calibración extrínseca a partir de un conjunto mínimo de observaciones. En todos los casos, la solución propuesta permite calibrar los sensores fácilmente y de forma robusta después de unos segundos observando una escena estructurada. Uno de los principales retos en SLAM es cómo extraer las características más útiles de la escena para mantener una representación compacta de esta, descartando al mismo tiempo información redundante. Esto es necesario para operar eficientemente en tiempo real, tal como se requiere para muchas aplicaciones de robótica móvil. Para ello presentamos un mapa métrico basado en la extracción de superficies planas de la escena, que almacena un conjunto de características geométricas y radiométricas de manera compacta. Dicha representación ha demostrado ser útil para el registro robusto de imágenes, para odometría usando cámaras de rango y para el reconocimiento automático de lugares. El uso de superficies planas para la localización y mapeo presenta ventajas en cuanto a la reducción de memoria y procesamiento. Por el contrario, existen limitaciones de inobservabilidad cuando no hay suficientes planos visibles, siendo la localización ambigua. Esta restricción puede ser resuelta en general aumentando el campo Resumen de la Tesis Doctoral 5 de visión de los sensores utilizados. En esta línea, esta tesis presenta un nuevo dispositivo para capturar imágenes omnidireccionales RGB-D a una frecuencia de 30Hz. Las imágenes capturadas por este dispositivo son adecuadas para su representación en coordenadas esféricas, lo cual ofrece una serie de ventajas como un mejor condicionamiento de la localización, el desacople natural entre la rotación y la traslación, la creación de mapas compactos basados en keyframes, o su adecuación para clasificación topológica de imágenes. La organización del mapa es una cuestión clave para el funcionamiento de SLAM en gran escala. La literatura sobre este tema es amplia, y se pueden encontrar diferentes estrategias que proponen estructuras topológicas, métrico-topológicas o jerárquicas para los mapas. La necesidad de este tipo de estructuras se justifica porque un sistema SLAM para gran escala debe abstraerse de la información que no es significativa (por ejemplo, teniendo solo en cuenta información métrica relativa a la localización actual del robot). En esta tesis, se presenta una estrategia para organizar dinámicamente la información métrico-topológica en tiempo real que agrupa las observaciones que están más interrelacionadas, formando lugares topológicos. Esta estructura mejora la eficiencia y permite la escalabilidad en SLAM. Por último, esta tesis presenta un nuevo enfoque en SLAM con el uso de imágenes omnidireccionales RGB-D que combina los avances descritos arriba en cuanto a calibración, localización y mapeo. Re-localización y cierre de bucle son dos problemas inherentes en SLAM que se tratan aquí. El primero se refiere a la capacidad para estimar la ubicación del robot cuando se ha perdido la localización (un problema similar es conocido como robot awakening), mientras que el segundo implica que el robot pueda reconocer una ubicación previamente visitada a través de una trayectoria diferente. La detección del cierre de bucle permite reducir la incertidumbre del robot y mejorar la coherencia global del mapa. Ambos problemas se abordan en esta tesis mediante el uso de una representación de la escena basada en planos. Conclusiones Las contribuciones más relevantes de esta tesis son: • Una nueva metodología para calibrar diferentes tipos de sensores de rango basada en la observación de superficies planas. Esta metodología permite calibrar los parámetros extrínsecos de los sensores de forma fácil y robusta en unos pocos segundos [Fernández-Moral et al., 2014b], [Fernández-Moral et al., 2015b]. • Una representación del entorno altamente compacta basada en superficies planas que sintetiza información geométrica y radiométrica (PbMap). Esta representación es útil para el modelado de entornos estructurados, para la localización del 6Resumen de la Tesis Doctoral robot y para la detección del cierre de bucle [Fernández-Moral et al., 2013b], [Fernández-Moral et al., 2014a]. • Una técnica de registro para PbMaps basado en el emparejamiento de conjuntos de planos vecinos mediante un árbol de interpretación. Esta técnica no restringe el emparejamiento a una sola imagen, sino que cualquier conjunto local de los planos es válido para ser emparejados lo que permite usar la información de varias observaciones [Fernández-Moral et al., 2013b]. • Una estrategia de mapeo métrico-topológica, basada en corte normalizado de grafos, que re-organiza dinámicamente el mapa en diferentes regiones topológicas mientras este se actualiza simultáneamente. Esta estrategia de mapeo permite la operación de SLAM en gran escala y ofrece ventajas para la navegación y la planificación de tareas [Fernández-Moral et al., 2013a], [Fernández-Moral et al., 2015a]. • El desarrollo de un nuevo sensor para la adquisición de imágenes omnidireccionales RGB-D a 30Hz consistente en un conjunto de 8 sensores Asus Xtion Pro Live montados en una configuración radial [Fernández-Moral et al., 2014b], junto con un nuevo sistema de SLAM basado en un mapa métricotopológico de keyframes [Gokhool et al., 2014]. Todas las publicaciones derivadas de esta tesis están disponibles en: http:// mapir.isa.uma.es Marco de esta tesis Esta tesis es el resultado de cuatro años de actividad investigadora de su autor como miembro del grupo de investigación MAPIR1, dentro del Departamento de Ingeniería de Sistemas y Automática de la Universidad de Málaga. Esta investigación ha sido financiada por el Gobierno español a través del “Fondo Regional de Desarrollo Europeo FEDER” dentro de los contratos DPI2008-03527 y DPI2011-25483, en los que se enmarcan los proyectos “Construcción de mapas topológicos métrica-visuales para robótica móvil” y “TAROTH: Nuevos avances hacia un robot en el hogar", respectivamente. El primer proyecto enfoca la creación de una representación del entorno para una variedad de sensores visuales, y comprende los dos primeros años de investigación de esta tesis. El segundo comprende el resto de esta tesis, y abarca la calibración de conjuntos de sensores y la explotación de los mapas previos para aplicaciones de localización y SLAM. Durante el doctorado el autor completó el programa doctoral titulado “Ingeniería Mecatrónica” coordinado por el Departamento de Ingeniería de Sistemas y Automática 1http://mapir.isa.uma.es Resumen de la Tesis Doctoral 7 de la Universidad de Málaga. Este programa de doctorado le proporciona al autor una visión general del campo multidisciplinar de la mecatrónica que combina mecánica, eléctrica, control e ingeniería informática, y lo más importante un conocimiento profundo acerca de la robótica móvil, algo que ha resultado fundamental a lo largo de estos años de investigación. Además, el autor ha completado su formación académica con su participación en el curso de Visión por Computador (BMVA 2010) de la Universidad de Kingston, Londres. El autor desarrolló parte de su investigación en colaboración con dos grupos de investigación internacionales. En 2012, estuvo 4 meses con el grupo Visual Information Laboratory en la Universidad de Bristol (Reino Unido), bajo la supervisión de Dr. Walterio Mayol-Cuevas. Durante este período, su investigación se centró en la explotación de estructuras planas para la construcción de mapas y SLAM. En 2013, estuvo 9 meses con el equipo Lagadic en INRIA Sophia-Antipolis (Francia), bajo la supervisión de Dr. Patrick Rives. Durante este tiempo, el autor trabajó en el desarrollo de un dispositivo RGB-D omnidireccional concebido para la construcción de mapas y la navegación de robots. Por último, es necesario mencionar que el marco científico de esta tesis es un área de investigación muy competitiva, que es impulsada por una creciente industria en visión por computador y robótica. En la opinión del autor, las continuas contribuciones en estas dos áreas que están altamente interrelacionadas permitirán la progresiva integración de robots móviles en nuestra sociedad. Estructura de la tesis Con el objetivo de obtener la mención de Doctorado Internacional por la universidad de Málaga, el desarrollo completo de esta tesis está escrito en español e inglés. Así, el texto está dividido en dos partes. La primera parte, escrita en español, describe de forma resumida el contenido del trabajo, mientras que la segunda parte, redactada íntegramente en inglés, presenta una descripción completa del mismo. Esta segunda parte se compone de los siguientes capítulos: El capítulo 2 introduce los conceptos básicos relativos a la calibración de diferentes sensores, y aporta una nueva metodología para calibrar conjuntos de sensores de rango. La estrategia presentada no requiere ningún patrón específico ya que se basa en la detección de superficies planas del entorno. Esta técnica de calibración es rápida, fácil de usar y robusta, presentando importantes ventajas con respecto a alternativas previas. El capítulo 3 propone una nueva representación de la escena basada en superficies planas que son segmentadas de imágenes de rango. Este mapa basado en planos (PbMap) es descrito por un grafo que contiene una serie de características geométricas y radiométricas. También se propone un descriptor compacto de color basado en 8Resumen de la Tesis Doctoral el color dominante del plano que contribuye a la eficiencia y robustez en el emparejamiento de planos. Una técnica de reconocimiento de escenas es propuesta basada en el registro de tales planos. Los resultados cuantitativos y cualitativos aportados en el ámbito de reconocimiento de lugar confirman las ventajas de esta representación. El capítulo 4 aborda el problema de la cartografía métrico-topológica. La estructura topológica se representa mediante un grafo no dirigido donde los nodos contienen información métrica y los arcos definen la conectividad entre regiones locales. Esta estructura tiene ventajas para el manejo eficiente del mapa, ya que sólo la información métrica local es utilizada para la localización y mapeo. Este capitulo propone una estrategia dinámica para gestionar el mapa, donde las diferentes regiones locales son agrupadas en función de la interconexión de las diferentes observaciones. Esta técnica es evaluada en el marco de SLAM monocular (usando PTAM [Klein and Murray, 2007]) y de SLAM omnidireccional RGB-D (capítulo 5). El capítulo 5 presenta un nuevo sensor para capturar imágenes omnidireccionales RGB-D a 30 Hz, junto con una solución SLAM para este tipo de sensor. El enfoque SLAM se basa en una estructura métrico-topológica de keyframes que se organizan en una red jerárquica de mapas locales. Los keyframes se describen a través de un PbMap que es utilizado para la localización y para el cierre de bucle eficientes. La localización obtenida del registro de PbMaps es refinada por una técnica de registro denso. La consistencia del mapa se mejora mediante la optimización de mapa global teniendo en cuenta todas las conexiones de los keyframes. El capítulo 6 concluye la tesis, proporcionando un resumen de la investigación presentada y expone el panorama de los retos futuros para la localización autónoma y la cartografía. En este contexto, en el futuro se espera la continuación de la investigación llevada a cabo en esta tesis en diferentes aspectos. Por un lado, el tratamiento probabilístico de la representación mediante PbMap supondría un avance en precisión, que además permitiría fusionar de forma coherente los planos observados por sensores diferentes. Por otro lado, la incorporación de información semántica al PbMap y a los diferentes niveles topológicos en los que se estructura el mapa son otra línea de investigación que previsiblemente ganará popularidad en los años venideros. Chapter 1 Introduction 1.1 Motivation Mobile robots are far from the state of development imagined a few years ago in our society. There has existed the general impression in the late years of the 20th century and beginning of the 21st, that intelligent robots would be spread in our society making our lives easier, or should we say, releasing us from tiresome routines. But the reality is still quite different. Science fiction films have contributed to build such an image of our future, where we live side by side with intelligent mobile robots. Also, the rapid technological development in the miniaturization of electronics, the increase of processors’ computing performance, and the appearance of new materials and sensors, which today have their results in compact smartphones for instance, suggested that mobile robotics would come along undoubtedly. But unfortunately, such a futuristic image will need some more time to come true. One of the main reasons for not having mobile robots around us nowadays is the difficulty to process and interpret exteroceptive information from the world. This is key for mobile robotics since a robot must understand its environment before it can interact with it. In this context, the ability of a mobile robot to create a map of its environment at the same time that it performs self-localization within such a map becomes crucial. This problem, known as simultaneous localization and mapping (SLAM), has received great attention during the last decades, and there exists a vast literature on the subject for a variety of working conditions using different sensors. However, despite the large effort dedicated to this topic, the solutions presented have still important limitations that prevent robots to work reliably in uncontrolled conditions. Research in SLAM has mainly focused on two kinds of data: photometric information from regular cameras or multi-camera systems, and depth information from range sensors. The use of cameras have some nice advantages as they are inexpensive, compact and consume low power, what makes them suitable for other applications in mobile robotics besides SLAM, e.g. object recognition. Also, cameras provide information similar to that captured by our eyes, being thus an intuitive way to perform robot perception. However, the actual information provided by a camera is a rectangu9 Chapter 2 Calibration of sensor rigs Abstract The operation of a robotic system requires knowing the characteristics and configuration of its sensor and motor systems. Such information is defined by the intrinsic and extrinsic parameters. The intrinsic parameters refer generally to a single sensor or actuator, describing how the measurements are modelled or how the actions are executed, respectively. On the other hand, the extrinsic parameters describe the relative poses among the sensors/actuators. Such parameters can be obtained from calibration, which is usually a prerequisite to perform other tasks as localization, mapping or navigation. This chapter reviews some relevant methods for intrinsic and extrinsic calibration, and presents several solutions for the extrinsic calibration of different combinations of range sensors developed within the work of this thesis. 17 18 Chapter 2. Calibration of sensor rigs 2.1 Introduction Many applications in the field of mobile robotics employ a variety of sensors, from proprioceptive sensors (like GPS, Inertial Measurement Units (IMU) or shaft encoders) to exteroceptive sensors (including vision, range or contact devices). In order to exploit efficiently the information provided by such sensorial systems, the sensors must be calibrated to: a) interpret correctly the acquired data (intrinsic calibration), and to put all the measurements in a common reference frame (extrinsic calibration). Such a calibration is also required for the robot’s actuators in order to provide the right commands towards the goal. This chapter is focused on sensor calibration, though many concepts can also be applied to the calibration of actuators. For more specific literature on this the reader is referred to [Whitehouse and Culler, 2003]. The intrinsic calibration of a sensor consists of providing a model to interpret the raw measurements, so that the data is put in correspondence with world properties. Examples of intrinsic parameters are the focal length of a camera, its distortion parameters, the error model of a laser scanner, or the radius of the robot’s wheels. Such parameters are usually provided by the manufacturer of the respective device, however, it may still be interesting to calibrate them in order to model deviations from the construction parameters or particular circumstances of the system (e.g. wheel inflation degree). The intrinsic calibration is required prior to the extrinsic calibration since it is needed to interpret the sensor measurements. It has been shown that computing both calibrations in a coupled manner can be helpful to reduce the errors of both [Zhang and Pless, 2004]. This section describes briefly two models employed along this thesis to calibrate the intrinsic parameters of a regular camera using a checkerboard [Zhang, 2000], and a method to calibrate the parameters of a depth camera using SLAM [Teichman et al., 2013]. The extrinsic calibration among the robot’s sensors (i.e. finding their relative poses) is required to exploit effectively all the sensor measurements and to perform data fusion. There exist a vast literature about this problem. These works can be classified according to the devices to be calibrated, for instance, the extrinsic calibration of regular cameras was one of the first to be investigated due to its utility for stereo vision and multiview geometry [Faugeras and Toscani, 1986]. A rig consisting of a camera and an IMU is calibrated in [Mirzaei and Roumeliotis, 2008; Guo and Roumeliotis, 2013], what is required for applications in the fields of wearable devices and aerial robotics (UAVs). Several solutions have been presented as well for the calibration of RGB cameras and laser scanners or LIDAR (e.g. [Zhang and Pless, 2004]), which have been used to build coloured point clouds of the scene [Forkuo and King, 2004]. A 3D LIDAR and a camera have been calibrated with the same purpose of registering depth and intensity information [Mirzaei et al., 2012]. A Velodyne 3D LIDAR and an omnidirectional camera were calibrated in [Pandey et al., 2010]. Methods for calibrating both the intrinsic and extrinsic parameters of a RGB and depth cameras have also become popular in the last years with the release of consumer RGB-D sensors like those developed by Primesense (e.g. Microsoft Kinect) [Smisek et al., 2013; Herrera et al., 2011]. 2.2. Intrinsic calibration 19 The knowledge of the robot trajectory, and/or a map of its environment, provide valuable information to compute the calibration, and at the same time, such calibration contributes to improve the localization and mapping [Foxlin, 2002]. The problem of simultaneous calibration and localization (or SLAM) has also received considerable attention by the research community. This strategy has been applied to calibrate regular cameras [Larsen et al., 1998; Heng et al., 2013], laser scanners [Martinelli et al., 2007], and RGB-D cameras [Teichman et al., 2013; Brookshire and Teller, 2012]. Despite the large volume of literature covering different calibration problems, only a few works have addressed the extrinsic calibration between range sensors. For example, the extrinsic calibration of a set of 2D laser scanners (or laser rangefinders -LRFs-) is a problem which is present in the field of autonomous vehicles [Thrun et al., 2006; Campbell et al., 2010; Bohren et al., 2008; Petrovskaya and Thrun, 2009; Miller et al., 2011; Leonard et al., 2008], where these sensors are necessary for safe navigation. But most works employing such a combination of LRFs obtain the extrinsic calibration from manual measurements or from non-general ad-hoc solutions, in tedious and time consuming procedures. This is a result of the difficulty to establish some kind of data association between range sensors. A new strategy for calibrating such combinations of sensors is presented in this thesis (section 2.3), which is employed to calibrate different combinations of range sensors. The technique is based on the observation of planar surfaces at different orientations, and offers a series of advantages over previous alternatives such as its ease of use, robustness and accuracy. 2.2 Intrinsic calibration A large body of literature can be found about the problem of intrinsic calibration of different sensors. In this section we review two methods for the intrinsic calibration of a regular RGB camera and for the depth sensor of a structure light sensor like e.g. Microsoft Kinect. These two methods are used along this thesis. 2.2.1 Calibration of a regular camera (RGB) Finding the intrinsic parameters of a camera is a basic problem in computer vision, and it is present in many robotic applications. Such parameters describe the projection between the 3D coordinates of the scene to the 2D coordinates of the image sensor. This problem involves finding the parameters of the camera model (in general the pinhole model is used, which is defined by the focal length and principal point), and the distortion parameters (radial and tangential distortions) of the lens [Heikkila and Silvén, 1997]. The pinhole camera model is usually represented through the camera projection matrix K K=  α γ u0 0βv0 0 0 1  ,(2.1) 20 Chapter 2. Calibration of sensor rigs Figure 2.1: Checkerboard calibration pattern. a 3×3 homogeneous matrix where αand βrepresent the scale factors (focal length) in the xand yaxes, γdescribes the skewness of these axes, and (u0,v0)are the coordinates of the principal point. Thus, the homogeneous images coordinates w= (u,v,1)> corresponding a 3D point p= (x,y,z)>in the camera reference frame are obtained from   u v 1 =K  x/z y/z 1 (2.2) The lens distortion can be modelled as the combination of radial and tangential distortion. The radial distortion consists of a radially symmetric artefact induced from the lens light diffraction, while the tangential distortion is caused by misalignment of the lens or lenses. Radial distortion models are more commonly applied since its effect is generally more significant than the tangential component. The latter is not considered here, for which the reader is referred to [Devernay and Faugeras, 1995]. The radial distortion can be modelled as u0=u+(u−u0)(k1r2+k2r4+...) v0=v+(v−v0)(k1r2+k2r4+...)(2.3) where uand vare the ideal pixel coordinates given by the pinhole model, u0and v0 are the real observed pixel coordinates affected by radial distortion and r r=q(u−u0)2+(v−v0)2(2.4) is the Euclidean distance between the pixel (u,v)and the principal point. Generally, only the two first parameters k1and k2of radial distortion are computed since higher order parameters have a negligible effect. There is a vast literature treating this problem, where a common approach is to employ a checkerboard calibration pattern (see figure 2.1) that must be observed from different orientations of the camera. Point correspondences are extracted from these observations, which are used to define geometrical equations to constrain the problem 2.2. Intrinsic calibration 21 Figure 2.2: RGB-D sensor: Asus Xtion Pro Live. [Zhang, 2000]. This strategy has been applied along this thesis to find the intrinsic parameters of different RGB cameras. Concretely, it is used in sections 4.4 and 5.2.1. The method employed here for calibration is publicly available1within the project MRPT [Blanco, 2008]. 2.2.2 Calibration of depth cameras Depth cameras are increasingly popular in mobile robotics thanks to the arrival of low-cost sensors like Asus Xtion Pro. Some other technologies for depth imaging include time-of-flight (ToF) cameras and 3D LIDAR, with considerable differences in price and working conditions. In this section, we focus on the calibration of structuredlight cameras like Asus Xtion Pro or Microsoft Kinect since these sensors are used along this thesis. For that, we outline some relevant works and explain in more detail the one followed here. Structured-light sensors are composed of an infrared (IR) camera and an IR projector (see figure 2.2). The depth image is obtained from stereo matching of the infrared projected pattern, and the specifications of this process are unknown to the user. Thus, a model for the depth image formation like the one of the previous section for regular intensity cameras is not available. As a consequence, many works that address the calibration of RGB-D sensors solve for the intrinsic parameters of RGB and IR cameras and for the relative pose of these, but do not deal with the intrinsic parameters of depth imaging [Herrera et al., 2011]. More recent works have addressed the intrinsic calibration of this type of depth sensors proposing to treat each pixel (in fact, regions of pixels) individually, to identify the bias of them through statistic methods which imply the observation of a static scene using SLAM [Teichman et al., 2013], or the observation of a checkerboard pattern [Basso et al., 2014b]. In this thesis we employ the first of this methods to calibrate the depth provided by the RGB-D sensors used along this thesis. The depth error of RGB-D sensors from PrimeSense (including Kinect and Asus Xtion) increases with distance, introducing a bias in the measurements. Such a bias is evident when we see the deformation of a flat surface as it is observed from increas1http://www.mrpt.org/list-of-mrpt-apps/application_camera-calib/ 22 Chapter 2. Calibration of sensor rigs Figure 2.3: Intrinsic calibration. Image of multipliers for the 8 different sensors at the reference depth of 4 m. ing distance. In [Teichman et al., 2013], the authors propose to model the intrinsic parameters using a discrete image of multipliers, where every pixel is updated according to its coordinates and depth. This solution, which is publicly available2, has been applied in this thesis to estimate the intrinsic parameters of different RGB-D sensors employed in our experiments. This technique was chosen because it was the only one which models the bias in the depth measurements. A sample of the resulting images of multipliers for different sensors Asus Xtion Pro Live is shown in figure 2.3, where we can see that the bias of each sensor is different even though they are the same sensor type. 2.3 Extrinsic calibration of range sensors The extrinsic calibration of different sensors is of very practical interest in robotics. This problem has been widely studied and different solutions have been presented for a variety of sensor configurations [Zhang and Pless, 2004; Le and Ng, 2009; Ha, 2012; Heng et al., 2013; Schneider et al., 2013]. The case of extrinsic calibration of range sensors is a problem with fewer results in the literature, where only solutions to very specific problems have been presented. The reasons for that are the difficulty to establish some kind of data association between range sensors in arbitrary orientations, and the high cost of many of these sensors, though this cost is being reduced as new technological advances appear [AsusXPL, 2011]. A new strategy to calibrate a combination of range sensors is proposed in this section. The calibration methods proposed here are all based on the observation of planar surfaces at different orientations to establish constraints on the sensor relative poses. The calibration problem is tackled as a maximum likelihood estimation (MLE) for the graph of constraints inferred between the different sensors. This formulation permits to solve the calibration of different types of sensors, as each measurement 2http://cs.stanford.edu/people/teichman/octo/clams/ 2.3. Extrinsic calibration of range sensors 23 is weighted according to its uncertainty. The uncertainty of the resulting calibration can also be estimated, what is useful for SLAM and to evaluate the precision of the calibration. The observability of these problems is analysed through the Fisher information Matrix [Van Trees and Bell, 2007], presenting minimal solutions which only require a single observation of the sensors. The calibration of different combination of range sensors is dealt with in this chapter: section 2.3.1 tackles the extrinsic calibration of a set of 2D LRFs; the calibration of a set of range cameras is addressed in section 2.3.2; and finally, the calibration between range cameras and 2D LRFs is presented in section 2.3.3. The experimental results confirm the efficacy and robustness of these calibration methods. These three problems are solved separately as they are stated upon different constraints, thus having different observability and convergence conditions. 2.3.1 Calibration of several 2D laser rangefinders The extrinsic calibration of several 2D laser rangefinders or LIDAR is of very practical interest for autonomous vehicles and for mobile robotics. Combinations of LRFs have been employed for 3D mapping in outdoor [Borrmann et al., 2008; Barber et al., 2008; Haala et al., 2008] and indoor environments [Thrun et al., 2000], and also for safe navigation [Victorino et al., 2003]. This calibration problem becomes more relevant with the advent of autonomous cars [Thrun et al., 2006; Campbell et al., 2010; Bohren et al., 2008; Petrovskaya and Thrun, 2009; Miller et al., 2011; Leonard et al., 2008], where the information provided by such sensors is essential to avoid possible collisions. This section presents a novel solution for the general problem of extrinsic calibration of 2D LRFs, which is based on the observation of perpendicular planes from any structured scene (i.e. Manhattan like world). Then, the calibration is computed by imposing co-planarity and perpendicularity constraints on the line segments extracted by the different laser scanners. No external information in the form of calibration patterns or auxiliary sensors is required. Only a rough approximation of the sensor relative poses must be provided, which can be guessed from simple visual inspection of the rig. This method can be used to calibrate any set of rigidly joined LRFs where there are at least two sensors with non-parallel scanning planes. The flexibility of our method permits its application to different problems. For example, it can be used to re-calibrate the LRFs mounted on an autonomous car, where the sensors relative poses may change over time as a result of the vehicle vibrations [Bohren et al., 2008]. 2.3.1.1 Related works Among the robotic systems found in the literature that employ a combination of LRFs, only a few of them report a calibration technique [Huang et al., 2010; Blanco et al., 2009b; Gao and Spletzer, 2010]. Many other works like [Thrun et al., 2006; Miller et al., 2011; Campbell et al., 2010; Bohren et al., 2008; Petrovskaya and Thrun, 2009], do not report any calibration process, so, it is reasonable to suppose 24 Chapter 2. Calibration of sensor rigs that they obtain the sensor’s relative poses from manual measurements on their setups, like in [Blanco-Claraco et al., 2014]. Such procedures are prone to errors that may severely affect the performance of mapping and navigation methods, especially when the laser scanners have a long working range, so that small rotation errors can produce significant distortions in the map [Miller et al., 2011; Leonard et al., 2008]. Apart from the limitations in accuracy, measuring the sensors relative poses by hand is also tedious and time consuming. Generally, the preferred strategy to calibrate exteroceptive sensors is to use their own measurements to establish some kind of data association between their observations. The extrinsic calibration of 2D range scanners in arbitrary poses proves to be more difficult than for RGB or depth cameras, since distinctive features are significantly more scarce in the first. This calibration strategy has been demonstrated for LRFs in some particular problems, but the solutions reported share one or more of the next limitations: they need supervised data association in controlled conditions; they need external information (extra sensors, a pattern or landmarks placed manually in the environment); or they are specific for a particular configuration of the sensor rig. For example, vertical posts of traffic signs are segmented and matched in a supervised way in [Huang et al., 2010]. In [Gao and Spletzer, 2010], a solution is presented based on the matching of reflecting landmarks that are manually placed in the environment. Without using particular targets, calibration is achieved in [Blanco et al., 2009b] by making use of the vehicle’s odometry to maximize the fitting of the 3D point clouds built from the different LRFs, what requires extra sensors (cameras or high precision GPS) to improve the accuracy of the vehicle’s odometry. Another approach consists of matching the trajectories of dynamic objects (or people) in the scene [Glas et al., 2010; Schenk et al., 2012]. For that, the trajectory of one, or several objects, is tracked independently by each LRF, and these trajectories are registered to constraint the sensor’s relative poses. This solution is suitable for static systems where all the LRFs scan a common space (nearly in the same plane), but like the approaches above, it is not valid to calibrate LRFs in arbitrary poses. In contrast to those works, a general method to calibrate LRFs like the one proposed in this thesis, is useful to deal with many robot and autonomous vehicle configurations. Ego-motion approaches have also been exploited to calibrate different combinations of sensors. For instance, visual and range odometry [Brookshire and Teller, 2012; Heng et al., 2013; Schneider et al., 2013] have been employed to minimize the fitting error of the independently estimated sensor trajectories. Also, ego-motion has been used in combination with wheel odometry to determine the intrinsic parameters of the odometry together with the relative pose of a laser sensor with respect to the robot’s frame [Censi et al., 2013]. However, this strategy is only applicable when the laser scanner moves in its own plane of measurement (typically, planar movement of a vehicle with an horizontal LRF), otherwise the laser ego-motion cannot be estimated. This last work addresses a different, but complementary problem to the one tackled here. Therefore, a combination of this technique with the one proposed here would be interesting in many problems to calibrate several LRFs with respect to the vehicle’s frame. 2.3. Extrinsic calibration of range sensors 25 Figure 2.4: Observation of a corner structure by a rig with two LRFs. Contribution This section presents, to the best of our knowledge, the first general approach to calibrate a set of LRFs in arbitrary positions. This solution only requires observing perpendicular planes from any structured scene. The observability and the convergence of the method are studied. A C++ implementation of this method is also provided together with a testing dataset34. In comparison to previous approaches, our method is applicable to almost any geometric configuration of the sensors (with at least two non-parallel sensors); it does not require auxiliary sensors or a calibration pattern; it is accurate and fast, indeed, the calibration can be achieved from a single observation; and finally, our method provides an estimation of the calibration uncertainty. In the following section we describe the calibration method using constraint equations derived from the co-planarity and perpendicularity of the observed planes (section 2.3.1.2). Section 2.3.1.3 addresses the optimization problem stated upon these constraints within a probabilistic framework that takes into account the precision of the sensor measurements. We analyse the observability conditions (section 2.3.1.4) and study the convergence region for the solution (section 2.3.1.5). Experimental results are presented to validate our approach with both simulated and real data (section 2.3.1.6). Finally, the results are discussed and the conclusions are outlined. 32 Chapter 2. Calibration of sensor rigs being σab ithe standard deviation of the error of (eq. 2.12). Both standard deviations (σa iand σab i) are computed through linearisation from a first order Taylor approximation of the error functions. Their derivation is detailed in the appendix C. When these standard deviations are constant with respect to the model parameters, the solution of the MLE in (2.13) coincides with that of the weighted, non-linear least squares problem expressed as argmin {R,t} N ∑ i=1ωa i(na jk ·(Rjca j+tj−Rkca k−tk))2+ ωb i(nb jk ·(Rjcb j+tj−Rkcb k−tk))2+ ωab i(na jk ·nb jk)2(2.18) where ωx i(the superindex xstands for a,bor ab) is the weight of the corresponding residual from COi ωx i=1 (σx i)2(2.19) This problem is reformulated using Lie algebra (see appendix B) to represent the poses with a minimal parametrization on a manifold. For that, the rotations are represented as the composition of a guessed rotation and a rotation increment represented with the exponential map (eµjRj), with the rotation increment eµj∈SO(3). The translations are also represented as the sum of a guessed translation plus an increment (tj+∆tj), both in R3. The resulting non-linear least squares problem is solved iteratively using Levenberg-Marquardt [µk 2,∆tk 2,...,µk m,∆tk m]>=−(H+λdiag(H))−1g(2.20) being λthe Levenberg-Marquardt’s damping factor. His the Hessian (a symmetric matrix of dimension 6(m−1)) and gis the Gradient (a column vector of dimension 6(m−1)) of the cost function, which are calculated as H= N ∑ i=1 J> iωiJi,g= N ∑ i=1 J> iωiri(2.21) where Nis the number of constraints of this optimization, being rithe residual defined above and Jithe Jacobian for each constraint (remember that each CO provides three constraints). The Jacobian Ja icorresponding to the constraint from eq. 2.11 is calculated as Ja i=[...;(laW j×(laW k×(caW j−caW k))+Rjca j×na jk | {z } Jµj ;na jk |{z} J∆tj ;...; ...;−(laW k×(laW j×(caW j−caW k))−Rkca k×na jk | {z } Jµk ;−na jk |{z} J∆tk ;...]>(2.22) 2.3. Extrinsic calibration of range sensors 33 where the superindex Wrefers to the common system of coordinates, so that6 laW j=Rjla j,laW k=Rkla k caW j=Rjca j+tj,caW k=Rkca k+tk This Jacobian (a row vector of dimension 6(m−1)) contains four blocks of 1 ×3 vectors corresponding to the derivatives of the residual with respect to µj,∆tjand µk, ∆tk, respectively. On the other hand, the Jacobian Jab iof the residual from eq. 2.12 is given by Jab i= [...;lbW j×(lbW k×na jk) +laW j×(laW k×nb jk) |{z } Jµj ;... ...;−lbW k×(lbW j×na jk)−laW k×(laW j×nb jk) | {z } Jµk ;...]>(2.23) The two blocks of 1×3 vectors of this Jacobian correspond to the derivatives of the residual with respect to µjand µkrespectively, the blocks corresponding to the rest of elements in {R,t}being zero. This optimization is solved iteratively Rk+1 j=eµk jRk j,tk+1 j=∆tk j+tk j,j∈[2,m](2.24) from an initial guess for the sensor relative poses, which may be obtained from a rough measurement of the rig. Once the problem is solved, the covariance of the resulting calibration is calculated as the inverse of the Hessian of the cost function in (2.18) [Fernández-Madrigal and Claraco, 2013]. For a more detailed derivation of Maximum Likelihood Estimation and Least Squares the reader is referred to appendix A. 2.3.1.4 Observability The problem of estimating the relative poses has 6(m−1)degrees of freedom (DoF), with mbeing the number of LRFs. From the formulation presented in the previous section, we have seen that each CO leads to three constraints for the relative poses of the corresponding pair of LRFs. Therefore, at least 2(m−1)COs are needed to provide as many equations as unknowns for solving the problem. We are also interested in knowing how these observations should be taken in order to provide the necessary information to solve the calibration. The analysis of the observability of calibration problems provides valuable information about the procedure to gather such data [Martinelli, 2011; Censi et al., 2013]. Such analysis is carried out here by studying the rank of the Fisher Information Matrix (FIM) of the estimation problem(see appendix D). The key concept here is that when the FIM is singular, 6For clarity, the CO index iwill be omitted in subsequent operations that affect only to the same CO. 34 Chapter 2. Calibration of sensor rigs Figure 2.7: Observation of a corner with three perpendicular planes by two LRFs. the information carried by the data (observations) is not sufficient and the problem is under-constrained ({R|t}is unobservable). We wish to identify these situations of unobservability in order to avoid them in practice. The FIM can be expressed in matrix notation by transforming the sum term in (2.21) to FIM =J>ΩJ(2.25) where Jis a matrix concatenating the Jacobians Jiof the residuals, and Ωis a diagonal matrix containing the weights ωiof such residuals. Since Ωis diagonal with all the elements being positive, the rank of FIM is the same as the rank of J rank(FIM) = rank(J>J) = rank(J)(2.26) Therefore, the problem has a solution when rank(J) = 6(m−1)(2.27) By analysing the structure of the different Jacobians Jifor eachCO, we can notice that a CO provides three linearly independent rows for Jwhen a corner is observed in a new orientation (eqs. 2.22 and 2.23). To get a deeper insight into this, consider a block of the Jacobian in eq. 2.23, for instance Jµj. Each corner observation in a new, linearly independent direction, expressed by na jk ×nb jk, contributes to constrain the problem for µj. For the Jacobian in eq. 2.22, it can be verified that each plane observation providing a linearly independent njk results in a linearly independent Jiwhich constrains both the relative rotation and the relative translation between the sensors Sjand Sk. Therefore, two observations of a corner from different orientations suffice to solve the calibration of a pair of sensors. Moreover, a single observation of a corner with three perpendicular planes (as shown in figure 2.7), provides enough information to solve the problem since it contains already 3 independent normal vectors of the plane, and 3 independent orthogonal constraints. 2.3. Extrinsic calibration of range sensors 35 −π 0 π −π 0 π Roll Pitch −5 0 5 −π 0 π −π 0 π Pitch Yaw −5 0 5 −π 0 π −π 0 π Yaw Roll −5 0 5 −0.5 0 0.5 −0.5 0 0.5 tx ty −8 −6 −4 −2 0 −0.5 0 0.5 −0.5 0 0.5 ty tz −8 −6 −4 −2 0 −0.5 0 0.5 −0.5 0 0.5 tz tx −8 −6 −4 −2 0 2 Figure 2.8: Error maps of the calibration of a pair of LRFs for 6 different combinations of 2 DoFs using 100COs. The configuration of the rig corresponds to the one depicted in figure 2.12 for the sensors S1and S2. The heat maps show the residual error from eq. 2.18 in logarithmic scale, with the correct calibration at the center of each graph, lying on a local minimum. The free DoFs in rotation take all possible values in the domain τ∈[−π,π], while for the translation the DOFs are given values δ∈[−0.5 m,0.5 m]. An interesting case of unobservability occurs for planar movement of a sensor rig, when all the visible planes are perpendicular to the plane of movement. In such a case, it can be clearly seen that there is a free degree of freedom for the translation since the rank of the matrix concatenating the plane’s normal vectors will always be deficient. This situation arises for a vehicle with an horizontal LRF which only observes vertical planes. In order to calibrate such a system, the scene should contain oblique planes, or the rig should be tilted in order to take observations from non-vertical planes. Finally, the ratio η=µ6(m−1)/µ1between the smallest and largest eigenvalues of the FIM is also an indicator of how well distributed the measurements are along the different directions (DoF) of the domain (η=condition_number−1). So, in the best case η=1 which means that all plane observations are equally distributed in the space, while when η→0, the system becomes ill-conditioned. 2.3.1.5 Convergence Considering that the calibration is observable, another important issue is to know if the solution converges to the correct value. This problem is not trivial for a nonlinear optimization whose domain is not convex, containing local minima, as it is the case here. The local convexity of the error function around the solution depends on a number of parameters including: the configuration of the sensor rig, the amount of 36 Chapter 2. Calibration of sensor rigs corner observations and their positions, and the noise in the sensor measurements. Thus, a mathematical condition for the convergence cannot be established in general. However, given a configuration of the rig and a set of observations (COs), we can sample the optimization domain to conduct a qualitative analysis of its convexity. In figure 2.8 we display the residual error of eq. (2.18) for a rig with two LRFs which observe 20 COs from different orientations. The error is shown with respect to the six parameters of the calibration by grouping pairs of DoFs in rotation and translation. The first row of figure 2.8 shows three sections of the sphere of possible rotations, corresponding to the planes x−y,y−zand z−x, while the second row shows the translation domain, where each DoF takes values in [0.5, -0.5] meters around their true value at the center {0, 0} of each graph. The resulting residuals are shown with a 2D heat map with the contour lines. In all the graphs, we see clearly a minimum at the correct calibration. We observe that there are local minima in the orientation domain, what implies that the initial values for the relative rotation must be given in a local region around the solution. On the other hand, the problem is convex for the translation as it is inferred from the formulation (the error depends linearly on the translation), therefore, the result does not depend on its initialization. The test above has been repeated for a number of rig’s configurations and for different COs at different positions, and the results are qualitatively similar. For all such tests, we observe a similar trend for the error surfaces, indicating that there is at least one local minimum for the error function at the correct solution, and that there are local and global minima distributed in the domain. A particularly interesting case of wrong convergence occurs when the relative rotation of a pair of LRFs is initialized in a way that their scanning planes coincide. Note that there exists a global minimum for such set of parameters, where the error will be zero no matter theCOs. Apart from this point of degeneracy, initializing the calibration near this local or global minima can drive the optimization to an undesirable result, but in general, we observe that if the optimization starts from a point near the solution it will converge to the correct calibration. From these results, we can conclude that the convergence region is wide enough to be able to provide good initial values for the relative poses from simple visual inspection of the rig. Another interesting point is to know if the calibration can be performed only from plane observations since, in fact, co-planarity constraints already restrict both the rotation and the translation of the relative poses in the rig. Several tests have indicated that this form of calibration is not possible because the cost function is not locally convex near the solution, and thus the problem cannot be properly constrained. As shown in figure 2.9, the introduction of the orthogonality constraint (eq. 2.12) makes the correct solution lie on a local minimum. Figure 2.9 shows the convergence error maps of different cost functions from: a) co-planarity constraints, b) orthogonality constraints, and c) a combination of both, which is actually the sum of the previous two. Note how these two functions complement well making the correct calibration lie on a local minimum. 2.3. Extrinsic calibration of range sensors 37 −π 0 π −π 0 π Co−planarity constraints Pitch Roll −5 0 5 −π 0 π −π 0 π Orthogonality constraints Pitch Roll −8 −6 −4 −2 0 2 4 6 −π 0 π −π 0 π Combined constraints Pitch Roll −5 0 5 += Figure 2.9: Error maps for different cost functions in logarithmic scale: a) co-planarity constraints, b) orthogonality constraints, c) the combination of these two. These graphs correspond to the previous simulation for two DoFs in the rotation domain (the graph cis the same as the graph shown at the top-left of figure 2.8). Note that the correct calibration at (0,0) lies on a local minimum only when the orthogonality constraints are applied. 2 16 30 44 58 72 86 100 0 0.2 0.4 0.6 0.8 1 Deg Rotation error Num COs No weights Cov. weighting 2 16 30 44 58 72 86 100 0 5 10 15 20 25 30 Translation error mm Num COs No weights Cov. weighting Figure 2.10: Calibration error of 2 LRFs with (red) and without (blue) covariance weighting, represented by the norms of the rotation (left) and translation (right) error vectors. 2.3.1.6 Experiments A number of experiments has been carried out to validate the present approach from both simulated and real LRF rigs. Simulation In our simulation environment, a rig consisting of two non-parallel LRFs is placed at different distances and orientations with respect to a corner in order to gather measurements from several poses. The sensors are modelled according to the parameters of the Hokuyo UTM-30LX rangefinder, and the observations are generated with unbiased, uncorrelated Gaussian noise with σ=0.03 m. The line features and their covariances are extracted from these synthetic observations. The calibration is esti- 38 Chapter 2. Calibration of sensor rigs −2 −1 0 1 x 10−4 −2 −1 0 1 2 x 10−4 Roll Pitch Roll−Pitch Monte Carlo 1 Sample −2 −1 0 1 2 x 10−4 −2.5 −2 −1.5 −1 −0.5 0 0.5 1 1.5 2 2.5 x 10−4 Pitch Yaw Pitch−Yaw Monte Carlo 1 Sample −6 −4 −2 0 2 4 6 x 10−5 −8 −6 −4 −2 0 2 4 6 8 x 10−5 Yaw Pitch Yaw−Roll Monte Carlo 1 Sample −4 −2 0 2 4 x 10−4 −6 −4 −2 0 2 4 6x 10−4 x y x−y Monte Carlo 1 Sample −4 −2 0 2 4 x 10−4 −5 −4 −3 −2 −1 0 1 2 3 4 5 x 10−4 y z y−z Monte Carlo 1 Sample −4 −2 0 2 x 10−4 −4 −3 −2 −1 0 1 2 3 4 5x 10−4 z x z−x Monte Carlo 1 Sample Figure 2.11: Error distribution and 2σcovariance ellipses of the calibration of two LRFs. A Monte Carlo simulation of 103samples is shown (blue). One of this samples is also drawn (red cross) with the covariance computed by the calibration method. mated for the cases of weighted (MLE) and unweighted optimization (standard least squares) for a varying number of COs. The average errors of the calibration with respect to the true poses are obtained from a Monte Carlo simulation with 103trials for every set of COs. For each test, the initial relative pose is uniformly generated around the groundtruth at distance d∈[0,1m]and at an angle |τ| ∈ [0,π/4]. The average errors of the relative rotation and translation are shown in figure 2.10 in degrees and millimeters, respectively. We observe that these errors diminish asymptotically with the number of COs. Also, we see how the MLE solution that takes into account the covariance of the measurements is consistently more accurate than the solution which ignores that information. This test was repeated for several configurations of the LRF rig (different relative poses between the sensors) obtaining similar results. We also study the bias and covariance of our method from the above Monte Carlo simulation by analysing the distribution of the calibration results. The six dimensional 2.3. Extrinsic calibration of range sensors 39 S1 S2 S3 Figure 2.12: Test LRF rig with three Hokuyo UTM-30LX. errors of the calibrated poses are shown in figure 2.11, by grouping pairs of DoF for the rotation and the translation, respectively. This figure shows the distribution of the 103samples around the groundtruth (blue dots), and the 2σconfidence ellipses of Monte Carlo (blue ellipse), and the one corresponding to the estimated covariance of one sample (red ellipse) through the Cramér-Rao Bound (see appendix D). We can see how the bias of the method is very small with respect to the covariance. Real data We have also validated the proposed calibration method in real case scenarios employing: 1) a rig with three LRFs and 2) the sensors mounted on two autonomous cars. The characteristics of the calibrated LRFs are shown in table 2.1. Table 2.1: Properties of the LRFs calibrated in this section. Hokuyo UTM-30LX Sick LMS 291-S05 Range (m) [0, 60] [0, 80] σ(m) 0.03 0.01 Resolution 0.25◦0.25◦ Field of view 270◦180◦ Test rig In the first case, the test rig is composed of 3 Hokuyo UTM-30LX (see figure 2.12). The sensors’ synchronization effect is neglected in this test since the rig is smoothly waved at a low velocity while the LRFs scan at a frame rate of 40 Hz (see the video at http://youtu.be/YG4ShgyIUHQ). The accuracy of the resulting calibration cannot be estimated directly since a groundtruth for the sensors relative poses is not available. Instead, we evaluate the accuracy of the method by checking that the pose 40 Chapter 2. Calibration of sensor rigs composition from calibrating the different pairs closes a loop (R12R23R31 =Iand t12 +t23 +t31 =~ 0). In this test we validate our approach using a varying amount of COs. The initial poses required by our method are given around a guess obtained from visual check of the rig, in a range of [0,40]degrees for the rotation and [0,1]meters for the translation, with respect to the correct calibration (several initializations are tested to check the robustness of our method). Table 2.2 shows the results of this test for different numbers of COs, from a minimum of 2 COs (that were extracted from a single observation, like the one represented in figure 2.7), to 100 COs. The first three columns show the average residuals of the calibration of each pair of sensors, and the last two columns show the average deviation with respect to the loop closure condition of the three independent calibrations. From this table, we observe that, as expected, the residuals and the loop closure deviations decrease with the number of COs. Table 2.2: Residual errors for different calibrations for a varying amount of COs. (*from one single observation). COsres12 res23 res31 Rdev (deg) tdev (cm) 2* 2.74 3.81 1.41 1.03 5.31 20 1.48 1.70 1.25 0.63 1.72 40 1.46 1.66 1.23 0.51 0.54 60 1.39 1.66 1.22 0.49 0.34 80 1.33 1.62 1.22 0.48 0.29 100 1.32 1.62 1.21 0.47 0.27 In this experiment, we have also calibrated the three LRFs by optimizing the full graph of constraints between them, so that the above loop closure condition is guaranteed. This way, the calibration should be more accurate since it uses all the information available. Table 2.3 shows the deviation of the relative pose between each pair of sensors and this global calibration. The deviations between the relative poses are expressed in degrees for the rotations (r12,r23 and r31) and in centimetres for the translations (t12,t23 and t31). The covariance of the resulting calibration depends on the information provided by the COs. In general, providing more COs contributes to reduce the uncertainty of the solution. This is confirmed in figure 2.13, which displays the maximum eigenvalue of the calibration covariance with respect to the number of COs for the experiment above. We observe how the value of the variance decreases asymptotically with the number of COs. This feature is relevant since it allows the user to set the maximum uncertainty for the calibration, so that the process of gathering COs stops after such a limit is reached. 2.3. Extrinsic calibration of range sensors 41 0 20 40 60 80 100 0 0.5 1 1.5 2x 10−6 Number COs Σ2 Max eigenvalue of covariance Figure 2.13: Maximum eigenvalue of the calibration covariance with respect to the number of COs. Table 2.3: Deviations between global calibration and the calibration of each pair for a varying amount of COs (*from one single observation). COsr12(deg) t12(cm) r23(deg) t23(cm) r31(deg) t31(cm) 2* 0.84 1.21 0.54 1.03 0.65 0.83 20 0.41 1.10 0.40 0.93 0.52 0.72 40 0.40 0.96 0.36 0.91 0.50 0.74 60 0.39 0.89 0.35 0.79 0.43 0.65 80 0.33 0.88 0.33 0.68 0.29 0.66 100 0.32 0.73 0.34 0.67 0.27 0.61 Autonomous car datasets We have also validated our method by calibrating the sensors mounted on two different autonomous vehicles, using two publicly available datasets7,8. For the dataset in [Blanco-Claraco et al., 2014], the vehicle has five LRFs in total, three Hokuyo UTM-30LX and two Sick LMS 291-S05, whose configuration is shown in figure 2.14. The Sick sensors scan horizontal planes, and therefore, the calibration cannot be fully constrained unless they observe non-vertical planes (for that, either the rig must be tilted or the scene should contain oblique planes like shop awnings). This situation does not occur in the dataset, so, only the Hokuyo sensors are considered. Note that two of these three sensors (labelled as Hokuyo2 and Hokuyo3) scan almost the same vertical plane, however they can still be calibrated since the sensor Hokuyo1 7http://www.mrpt.org/MalagaUrbanDataset 8http://grandchallenge.mit.edu/wiki/index.php?title=PublicData 48 Chapter 2. Calibration of sensor rigs Orientation constraint: A constraint for the relative orientation between the two sensors is stated from the observed normal vectors n−Rn0=~ 0 (2.33) being nand n0the observed normal vectors seen by the camerasCandC0respectively. Position constraint: A constraint for the relative position is given by d−d0+n·t=0 (2.34) where dand d0are the observed distances from the plane to the optical centers of the depth cameras Cand C0. Obtaining plane correspondences Similarly as in the previous calibration problem (section 2.3.1.2), the sensor relative poses must be known in order to establish the plane correspondences. Thus, the problem consists of estimating the calibration and plane correspondences simultaneously. For that, all the plane observations gathered from a single observation of the rig are matched between them, so that they will contain both correct and wrong correspondences. Then RANSAC [Fischler and Bolles, 1981] is applied to find the extrinsic calibration with a larger number of supporting correspondences, discarding the rest of correspondences as outliers. This procedure is carried out in two steps: first, the outliers showing a large error in the orientation are discarded, and second, those outliers in distance are removed (this order is chosen since the noise in the orientation of the normal vectors is typically smaller than that in the plane position). For these RANSAC processes, the relative poses between the pair of cameras are calculated from a sample of 3 non-degenerate plane correspondences using the models defined in section 2.3.2.3. Note that giving an initial estimation for the relative position of the cameras can also be applied to facilitate the matching of plane observations. Also, there exist other plane matching strategies can be applied avoiding the need of an initial estimate for the calibration [Pathak et al., 2010b; Fernández-Moral et al., 2013b]. The process for gathering correspondences is performed automatically while the camera rig is moving until the problem is well conditioned according to the Fisher Information Matrix, as explained in section 2.3.2.4. The range cameras synchronization effect is neglected in this work since the images are captured at a minimum frame rate of 30 Hz, and the camera rigs are never moved abruptly. 2.3.2.3 Problem formulation Given a set of plane correspondences gathered from two rigidly jointed range cameras Cand C0, as defined above, and provided that the correspondences fulfill the observability condition (concretely the one represented by eq. 2.49), we want to estimate 2.3. Extrinsic calibration of range sensors 49 the optimal [R|t]assuming that the measurements are affected by unbiased Gaussian noise as modelled in 2.3.2.2. This problem can be divided into two separate ones since the rotation and the translation restrictions are decoupled. Solving for the rotation The maximum likelihood estimation (MLE) of the relative rotation R is given by the maximization of the log-likelihood argmax R ln N ∏ i=1 p(ni,n0 i|R)!(2.35) for Nplane correspondences, where the likelihood of the rotation for the i-th correspondence is expressed as p(ni,n0 i|R) = 1 p(2π)3|Σi|exp−1 2(ni−Rn0 i)>Σ−1 i(ni−Rn0 i)(2.36) being niand n0 ithe observed normal vectors from the plane ias seen by the camerasC and C0respectively; R is the rotation matrix in SO(3), and Σiis the 3×3 covariance block corresponding to the normal vector of the plane correspondence (calculated from the fusion of both observations [Pathak et al., 2010c], see appendix C). Considering independent errors of the plane correspondences, the derivation of this MLE coincides with the solution of the least squares problem expressed as argmin R N ∑ i=1 ωikni−Rn0 ik2(2.37) where ωiis the weight of the plane correspondence ωi=1 |Σi|(2.38) This problem is similar to the one of estimating the rotation of a registered set of 3D points [Arun et al., 1987]. Thus, employing the same procedure, the above equation can be expressed as R=argmin R N ∑ i=1 ωin> ini−2 N ∑ i=1 ωin> iRn0 i+ N ∑ i=1 ωin0> in0 i! =argmin R −2 N ∑ i=1 ωin> iRn0 i! =argmax R N ∑ i=1 ωin> iRn0 i(2.39) 50 Chapter 2. Calibration of sensor rigs that can be denoted as N ∑ i=1 ωin0> iRni=trace(WY >RX)(2.40) where W=diag(ω1,...,ωn)is an n×ndiagonal matrix containing the weights ωi; and Yand Xare 3×nmatrices with the normal vectors n0 iand nias their columns, respectively. This problem is solved with singular value decomposition (SVD) over the 3×3 covariance matrix S=XWY >(2.41) From the singular value decomposition S=UΣV>, the rotation is obtained as R=V1 0 0 0 1 0 0 0 det(VU>) | {z } A U>(2.42) where the matrix Ais used to convert the degenerate case of a reflection det(VU>) = −1 (2.43) into a valid rotation in SO(3). For further details on the mathematics, please refer to [Arun et al., 1987; Sorkine, 2009]. Solving for the translation The MLE of the translation is obtained by maximizing the log-likelihood associated to the probability p(ni,n0 i,di,d0 i|t) = 1 √2πσi exp−1 2 (di−d0 i+ni·t)2 σ2 i(2.44) where diand d0 iare the observed distances from the plane ito the optical centers of the depth cameras Cand C0respectively, σ2 iis the error variance, and tis the relative translation we are looking for. This is equivalent to the least squares problem argmin t N ∑ i=1 ωi(di−d0 i+t·ni)2(2.45) with the weight given by ωi=1/σ2 i. This has a closed form solution given by t=−H−1g(2.46) where Hand gare the Hessian and the Gradient of the error function respectively, which are calculated as H= N ∑ i=1 J> iWiJi,g= N ∑ i=1 J> iWiri(2.47) 2.3. Extrinsic calibration of range sensors 51 Figure 2.17: A particular set-up from which we can calibrate the cameras with a single observation. The planar patches on the left and right are those extracted from the two cameras. where the Jacobians, the weights and the residuals are calculated from Ji=n> i,Wi=1 σ2 i ,ri=di−d0 i(2.48) 2.3.2.4 Observability It can be seen that each plane correspondence imposes three new constraints between the pair of sensors: two for the relative rotation and one for the relative translation. Thus, we need at least 3 measurements from linearly independent plane observations (i.e the observed normal vectors of the planes must be linearly independent) to compute the relative pose of a pair of sensors (only two measurements are needed to compute the rotation), and a minimum of 3(N−1)correspondences to calibrate a rig with Nsensors. To put a simple example, let’s consider a single sensor observing the corner of a room. The observation of the three perpendicular planes gives us enough information to localize the camera and its relative motion with respect to a previous pose. Analogously, the relative pose between two cameras can be obtained if they observe 3 plane correspondences with linearly independent normal vectors (either observed from one view, like in figure 2.17, or from several ones). The most simple and convenient procedure of calibration would be to take a short sequence of images of one big plane at different orientations of the rig (see the video at http://youtu.be/MGydi5R7ldA). Similarly as for the calibration of 2D LRFs, here we make use of the Fisher Information Matrix (FIM) to identify those unobservable cases in which the calibration cannot be determined. This analysis defines how the measurements should be taken to avoid these situations. As we will see, the probability of the MLE is given by an unbiased Gaussian distribution (this assumption is realistic only after intrinsic correction). For this estimator (called efficient [Fernández-Madrigal and Claraco, 2013]), the FIM coincides with the Hessian of the least squares problem resulting from the MLE, and 52 Chapter 2. Calibration of sensor rigs Cases of study [R,t] plane z x y x [R,t] plane a) b) Figure 2.18: Different sensor configurations with a pair of cameras: a) Adjacent cameras, b) Opposite cameras. its inverse is the covariance of the resulting calibration (see the appendix D). When the FIM is singular, the information provided is not sufficient and the MLE does not exist. For a pair of cameras, it can be verified that when the FIM is not singular, then rank( N ∑ i=1 nin> i) = 3 (2.49) where niis the normal vector of the plane ias seen from one of the cameras in the pair. From our experiments, we have verified that the covariance of both the rotation and the translation estimations decrease asymptotically as the number of plane correspondences increases. The covariance is used as the condition to control the calibration convergence, and hence, to stop gathering plane correspondences. In our tests, we stop this calibration when the maximum eigenvalue of the covariance is under 10−3, which has shown to be a good compromise between accuracy and effort to obtain plane correspondences. 2.3.2.5 Practical study cases 1. Adjacent cameras This case is interesting to provide a larger field of view of the scene, being specially practical for low cost sensors like Asus Xtion (see figure 2.18.a). This case serves us to illustrate the conditioning of the problem, and so to show different possibilities for calibration. One of this situations is the calibration of the pair from one 2.3. Extrinsic calibration of range sensors 53 single observation, i.e. without moving the rig. This is only possible if three planar patches whose normal vectors span through the different directions of the space are visible at the same time by both cameras. This case can be easily set-up, as the example shown in figure 2.17. In practice, however, it is even more convenient to take several images from different orientations pointing to one single plane (the floor, for example), since we can gather more quickly enough plane correspondences that help to reduce the error from the measurement noise. This may take no longer than 2 or 3 seconds. In table 2.5 we show an example of how the average residual error is reduced when raising the number of plane correspondences. The alignment errors in rotation and translation are measured in a dataset containing 2K correct plane correspondences for the five calibrations, the plane correspondences were taken in all directions of the space. Table 2.5: Residual errors for different calibrations using a different number plane correspondences. Correspondences Av rot error (deg) Av trans error (cm) 3 1.12 1.89 10 0.68 1.01 30 0.52 0.82 60 0.49 0.74 100 0.49 0.61 2. Cameras in opposite directions This case is interesting, for instance, for vehicles that need to observe the scene forward and backward. We address this case here also since it probably represents the most challenging case to obtain plane correspondences in different directions (notice that the further the viewing directions of the cameras are, the more difficult is to find plane correspondences). Figure 2.18.b shows how the plane correspondences can still be obtained to add constraints in the different directions of the space, for example, by rotating the camera rig. Calibration was performed automatically while the user waved the camera near the floor. After 5 seconds from the start of the experiment, the calibration finishes with 29 plane correspondences (see the video at http://youtu. be/MGydi5R7ldA). In this case the deviation with respect to the rig parameters is less than 1 deg for the rotation, and in the order of millimeters for the translation. 3. Sensors of different types Though most of our experiments are carried out with structured light Primesense cameras, other range sensors can also be calibrated with our method. Concretely, a time-of-flight camera and a Kinect sensor mounted on a robot are calibrated by moving the robot (figure 2.19, left) around to gather plane correspondences. The errors in the plane observations from both sensors will follow different distributions, so that they are weighted accordingly as said in section 2.3.2.2. 54 Chapter 2. Calibration of sensor rigs Figure 2.19: Robots which mount rigs of range and RGB-D cameras. 2.3.2.6 Extrinsic calibration of an arbitrary number of range cameras This section extends the previous formulation for an arbitrary number Mof range cameras. Note that for the case when there are no loop closures between the sensors, i.e. there is only one possible way to correlate the relative pose of any pair of sensors. The extrinsic calibration can be calculated as in the previous section by estimating the relative pose between each pair of adjacent sensors, and performing pose composition to place them in a common reference. Instead, this section is dedicated to the case in which there are plane correspondences that create loop closures between sensors. For the sake of space, we present directly the least squares equations, which as in the previous section, derive from the ML estimation. The relative rotation between the different sensors can be formulated as argmin {R,t} M ∑ j=1 M ∑ k=j+1 N ∑ i=1 λi(j,k)(Rjnj i−Rknk i)>Σ−1 i(Rjnj i−Rknk i)+ 1 σ2 i (dj i−dk i−tjRjnj i+tkRknk i)2(2.50) where jand kare indices of the Msensors and iis the index of each one of the N planes observed; λi(j,k)is a binary variable that equals 1 when the plane iis observed by sensors jand k, being 0 otherwise; nj iand nk iare the normal vectors, and dj iand dk i are the distances of the camera optical center to the plane iobserved from sensors j and k, respectively; Σiand σiare the covariance and the variance of the probabilities in eq. 2.36 and 2.44, respectively; and the relative poses between the sensors are represented by {R,t}∈SE(3). 2.3. Extrinsic calibration of range sensors 55 This least squares system has a different structure from the one in the previous section, that can not be solved with the strategy used from equations 2.39 to 2.42. Instead, we rewrite the problem to represent the relative rotations in minimal parametrization with the exponential map from Lie algebra (see appendix B), similarly as it was done for the calibration of 2D range scanners. This is a non-linear least squares system that is solved iteratively with LevenbergMarquardt as in equations 2.20-2.21, where the Jacobian and the vector of residuals are given by Ji= [0... 0J(j) i0... 0J(k) i0... 0] J(j) i=skew(nj i),J(k) i=skew(−nk i)(2.51) thus, the Hessian Hand the gradient gare calculated incrementally as H= N ∑ i=1 Hi,g= N ∑ i=1 gi(2.52) which have the form Hi=       JjT iωiJj iJkT iωiJj i ... JjT iωiJk iJkT iωiJk i        ,gi=       JjT iωiri . . . JkT iωiri        (2.53) 2.3.2.7 Calibration of a rig for omnidirectional image acquisition We have designed a camera rig for omnidirectional RGB-D acquisition which comprises 8 Asus Xtion Pro Live (Asus XPL) sensors mounted in a radial configuration (see figure 2.19.b). This device motivated at the origin the work described in this section, since the parameters from the construction design were not accurate for our application. Existing calibration approaches like those based on SLAM [Brookshire and Teller, 2012] are very time consuming and impose important restrictions on the trajectory, since planar movement (as we have in our robot) is a degenerate case where calibration cannot be achieved. Thus, we employed the calibration method described in the previous section, which was applied on a sequence of images taken with the robot (planar movement is not a degenerate case in our approach). The relative positions between the RGB cameras is the same as these between their corresponding depth cameras for our sensor configuration. Therefore, both RGB and depth omnidirectional images can be built as it is illustrated in chapter 5 (see figure 5.4). The 3D point cloud can be also built from such images as it is shown in figure 5.6(a). The precision of calibration is tested with an experiment where the robot moves in a small circular trajectory (∅∼0.5 m) in the center of a room, taking 200 images. 56 Chapter 2. Calibration of sensor rigs In table 2.6, the average residual in orientation and translation for the plane correspondences of these images is presented for different extrinsic calibrations: design parameters (no extrinsic calibration) with and without intrinsic correction, and the extrinsic calibration also with and without intrinsic correction. The residual of Iterative Closest Point (ICP) alignment of the spherical point clouds from these images is also shown. For all the cases, the combination of intrinsic and extrinsic calibration offers the best results. Table 2.6: Residual errors for different combinations of intrinsic and extrinsic calibrations. Calib / Error type Res. rot (deg) Res. trans (cm) Res ICP (cm) Design Specs 3.17 3.0 0.49 Design S.+Intrinsic 2.95 3.1 0.45 Extrinsic calib 1.78 2.9 0.34 Extrinsic+Intrinsic 1.60 2.5 0.29 2.3.2.8 Discussion A new methodology for calibrating the extrinsic parameters of range camera rigs has been presented in this section. The method relies on the matching of plane observations from the different sensors. No constraints are put on the position of the cameras, where the only requirement for the system is that there is a planar surface that can be observed simultaneously. The observability conditions are analyzed, and a solution is presented based on MLE. With our method, performing calibration becomes very fast and easy for the user, avoiding problems of previous solutions which rely either on calibration patterns or trajectory estimation methods. The method has been tested for different configurations of cameras, including a camera rig designed for omnidirectional image acquisition. All the experiments have validated the claimed features of our proposal. 2.3.3 Calibrating a 3D range camera and a 2D laser scanner After the previous solutions for calibrating sets of 2D and 3D range scanners, we present in this section a new method to calibrate a combination of these ones. The same strategy of the previous sections is applied to make use of plane observations to constrain the relative poses of the sensors. We also follow a similar procedure as before to formulate the problem and present an analysis on the restrictions and the observability conditions. Still, a full description of the approach is given since the problem characteristics are different from the previous ones. Experimental results are provided with both simulation and real mobile platforms with several range sensors. 2.3. Extrinsic calibration of range sensors 57 2.3.3.1 Related works There are some methods in the literature that have addressed the problem of extrinsic calibration between a laser and different sensors like RGB cameras, wheel odometry, etc. One that has received considerable attention is that of finding the relative pose between a laser and a RGB camera. The first solution to this problem was reported in [Zhang and Pless, 2004]. This solution employed a checkerboard as a calibration pattern, and estimates the extrinsic calibration by restricting that points observed with the laser lie on the same 3D plane where the checkerboard lies. When these assumptions hold, the relative position and orientation between both sensors can be estimated through these geometric restrictions. Some improvements of this former solution have been presented along the last decade by exploring different calibration patterns [Li et al., 2007; Ha, 2012; Moreno et al., 2013], decoupling rotation from translation [Zhou and Deng, 2012], presenting a minimal closed form solution [Vasconcelos et al., 2012], and adopting different optimization strategies [Zhou, 2014]. Recently, a solution was presented to the problem of extrinsic calibration between a laser and a Kinect [Devaux et al., 2013]. In this work, the authors extend a previous solution for laser-to-camera calibration [Zhang and Pless, 2004] modifying the typical checkerboard calibration pattern and testing different error metrics. Note that the above problem in which a range camera and a 2D laser are calibrated is very similar to the one calibrating a RGB camera and a laser [Zhang and Pless, 2004], where the plane parameters can be easily extracted from the range images [Fernández-Moral et al., 2013b] without the need of any specific calibration pattern. Thus, only a common 3D plane should be sufficient to estimate the extrinsic calibration. As it was demonstrated in [Vasconcelos et al., 2012], three plane-line correspondences are required for that, with the planes having linearly independent normal vectors (they intersect in only one point). Such plane-line correspondences can be obtained through the observation of the same plane from different orientations. In this section we address the above problem in a probabilistic framework which can be easily generalized to other kind of sensors. For that, the internal calibration of the sensors are assumed to be known. A keypoint of this contribution in reference to previous approaches is the derivation of the approximated maximum likelihood estimation for the calibration, which propagates the uncertainty of the sensor measurements providing the calibration uncertainty itself. This is useful for other problems in the field of mobile robotics like map construction, range-based odometry or simultaneous localization and mapping (SLAM) [Trevor et al., 2012]. Also, the calibration approach presented here is generalized for any geometric configuration of sensors (which may have very divergent fields of view), what is highly interesting for autonomous vehicles. Contribution A new method for extrinsic calibration of range cameras and laser rangefinders is presented in this section. This method shares the advantages of the two solutions 64 Chapter 2. Calibration of sensor rigs 4 6 8 10 12 14 0 0.002 0.004 0.006 0.008 0.01 Covariance Num corresp Rotation Cov Translation Cov Figure 2.22: Maximum eigenvalues of rotation and translation covariance with respect to the number of plane-line correspondences. the bigger the error. This makes sense, since sensors far away will have few common observations. Another interesting point is to evaluate the convergence of the algorithm, so that plane-line correspondences are gathered until a threshold for the calibration’s uncertainty is reached. For that we draw the maximum eigenvalue of the calibration’s covariance as the number of correspondences grows for both rotation and translation, see figure 2.22. We see how the covariance decreases asymptotically to zero as more correspondences are gathered. The threshold to stop calibration can be set easily by the user according to his needs. Real data We present here the results of a real case experiment to illustrate the precision of the calibration method. We have calibrated a Hokuyo UTM-30LX laser rangefinder with respect to a Kinect RGB-D camera. Both sensors are mounted on a Pioneer PatrolBot as is shown in figure 2.23. The sensors’ synchronization effect is neglected in this test since the laser scans and range images are captured at a minimum frame rate of 30 Hz, and the robot is moved at low speed. The accuracy of the resulting calibration from our method cannot be evaluated directly since we do not have a groundtruth for the sensors relative positions. Thus, following the works in the literature, we provide some qualitative results based on the residual errors. In table 2.7 we show an example of how the average residual error is reduced when raising the number of plane-line correspondences. The residual error in rotation and translation are measured in a dataset containing 1000 correct correspondences for several calibrations with varying number of correspondences. In this experiment we placed the robot near a wall, so that correspondences could be ac- 2.3. Extrinsic calibration of range sensors 65 Kinect Hokuyo UTM-30LX Figure 2.23: Robot which has a 3D range camera and a 2D laser rangefinder. quired quickly in different directions (from the wall and the floor) as it is shown in the video http://youtu.be/1VIeP5h_4h4. In this situation, calibration is performed in a few seconds with around 12-100 plane-line correspondences. Table 2.7: Residual errors for different calibrations using a different number correspondences. Correspondences Av rot error (deg) Av trans error (cm) 3 1.31 3.17 10 0.71 2.01 20 0.65 1.52 40 0.60 1.34 80 0.55 1.33 2.3.3.6 Discussion A new methodology for calibrating the extrinsic parameters of a rig of range sensors has been presented in this section, namely a 3D range camera and a 2D laser rangefinder. The method relies on the matching of plane and line observations from the different sensors. The extrinsic calibration problem is solved in a probabilistic framework that takes into account the uncertainty in the measurements of the sensors, and provides also the uncertainty of the resulting calibration. No constraints are put on the position of the cameras, where the only requirement for the system is that there is a planar surface that can be observed simultaneously by all the sensors. This approach can be easily extended to other types of sensors always when a plane can segmented from the scene. The observability conditions and some insight in the 66 Chapter 2. Calibration of sensor rigs convergence of the problem is presented. With our method, performing calibration becomes very fast and easy for the user, avoiding problems of previous solutions which rely either on calibration patterns or trajectory estimation methods. The method has been tested in simulation and real case experiments validating the claimed features of our proposal. 2.4 Conclusions This chapter reviews some relevant methods for calibrating different combinations of sensors which are usually employed in mobile robotics. A new methodology for calibrating combinations of range sensors has been presented. Three different problems are tackled here to calibrate: a) a set of 2D range scanners, b) a set of 3D range cameras, and c) a 2D and a 3D range sensors. Different methods are presented for each problem which can be easily combined to calibrate any combination between them. All these methods are based on the observation of planar surfaces from structured environments. The proposed methods have a number of advantages with respect to previous approaches, namely: they can be applied to any geometric configuration of the rig of sensors; they do not need external information (calibration patterns, special landmarks, etc.); they are easy to apply, performing calibration in a few seconds; and they provide the uncertainty of the resulting calibration. The method to calibrate combinations of 2D range scanners are mainly interesting in the context of autonomous cars, where most prototypes in the literature make use of several 2D LRFs. On the other hand, the calibration of 3D range cameras looks more interesting for indoor robotic applications, providing large field of view at low cost of the sensor system. The observability of each one of the proposed methods is analysed, showing that a single observation of the rig may be enough to calibrate the sensors. Regarding the experimental evaluation carried out here, the presented techniques are not compared to previous approaches in the literature since they cannot be compared in the same conditions. Still, an interesting aspect would be to measure the accuracy of the techniques presented in the real experiments to set some bounds on their applicability and to provide some quantitative information for future comparisons. Obtaining such results would imply a complex and expensive procedure to obtain some kind of groundtruth that was not available during the research of this thesis. Therefore, this aspect stays as a possible line of future research. Chapter 3 Plane-based maps for fast localisation and place recognition Abstract Different kinds of maps have been used in mobile robotics for selflocalization, navigation or scene reasoning. A new type of map is proposed in this chapter which is based on the registration of planar surfaces, and which is extremely compact in comparison with previous approaches. This world representation is organized in a graph where the nodes represent the planar patches and the edges connect neighbouring planes. This map structure allows to work efficiently with local regions by selecting subgraphs of neighbour planes that can be quickly compared for real-time place recognition and scene registration. For that, an interpretation tree is employed to find a candidate match between two subgraphs by searching the best combination of planar patches that fulfils a series of geometric and radiometric constraints. Such a strategy permits working with partially observed and missing planes, offering invariance to viewpoint and robustness against changes in the scene. The proposed approach constitutes an efficient way to solve loop closure detection and scene registration, working satisfactorily even when there are substantial changes in the scene (lifelong maps). 67 68 Chapter 3. Plane-based maps for fast localisation and place recognition 3.1 Introduction As discussed in the first chapter of this thesis, compact scene representations are highly interesting for efficient SLAM operation, among other utilities. Take into account that a mobile robot can gather information from the environment continuously like we humans do. Besides, lightweight mapping solutions are desired when the computation burden is limited, like in wearable applications or in robotic solutions which integrate other demanding functionalities (e.g. task planning and execution, or events anticipation). In such a context, efficient memory and processing mapping strategies are highly advantageous. Different mapping strategies have been proposed in mobile robotics depending on the purpose of the robot and the available sensors. A popular alternative to represent the world when using RGB-D sensors (like Microsoft Kinect) is through point clouds, where coloured points are rendered to the map according to the sensor’s pose [Kerl et al., 2013a]. Such a representation offers a high degree of detail, achieving nice visualizations of the scene that can be employed in several fields apart from mobile robotics, like scene modelling, augmented reality or video games. However, the compactness of this representation is not suitable for applications with more limited memory and processing resources. The problem of re-localization is a good example where compact maps are highly interesting. The ability to quickly recognize a previously visited place is a major problem in mobile robotics since, among other things, it allows to accomplish topological localization and loop closure detection in SLAM. In contrast to the typical localization problem in SLAM where the robot tracks a local map around its last position, a re-localization algorithm will check generally a much larger part of the map, requiring more computation. The map being checked for that will depend on the uncertainty of the robot trajectory. For instance, in the case of robot awakening problem, where information about the current location is not available, the whole map will need to be checked. The main issue here is how to describe the generally big amount of information present in the scene in order to recognize a place in a robust and affordable way when it is visited again. This chapter proposes a compact plane-based representation of the scene from RGB-D data that we name PbMap (Plane-based Map) [Fernández-Moral et al., 2013b], which is specially useful for place recognition and re-localization. The PbMap stores only the planar skeleton of point clouds, and thus, it avoids the redundancy of information in them, where coplanar points are represented compactly by a convex hull. This representation was partly inspired by the CAD models, that encode metric information in a compact fashion and still, they can be easily interpreted by humans despite the lack of information about scale, texture, etc. (see figure 3.1.a). Also, a plane constitutes a higher level feature of semantic information with respect to 3D points, as planes normally correspond to meaningful objects (e.g. a wall, or a door) and a few planes can compose a semantically meaningful object (e.g. a table, a desktop, etc.), that can be exploited for semantic mapping [Ruiz-Sarmiento et al., 2014]. On the other hand, this model loses descriptiveness with respect to point clouds since 3.1. Introduction 69 a) b) Figure 3.1: Example of a typical scene that can be represented with planar patches. a) CAD model b) PbMap of the scene where a local neighbourhood of planes is represented, which is defined by a reference plane (green), and includes its closest planes up to a distance threshold of 1 m (blue). the information from non planar areas is discarded. Thus, our approach assumes that there are enough planar patches, and then structured indoor scenes are more adequate for this representation. A PbMap is organized as an annotated graph where each node corresponds to a planar patch (described by simple features: normal vector, centroid, area, colour, etc.) and the edges connect neighbour patches according to their proximity and covisibility. These planar patches (or planes, for short) can be extracted in real-time from the range video streaming provided by a hand-held range camera or a RGB-D sensor. Such planes are integrated into the map in their respective poses according to the sensor location. The sensor pose can be obtained in different ways, in this chapter it is estimated from the sensor observations using an odometry algorithm [Steinbrücker et al., 2011]. The use of odometry only for constructing our maps implies that our representation is topological in nature, but note that re-localization and place recognition do not require fully consistent maps to work. Place recognition in PbMaps is addressed here as a problem of matching subgraphs, which represent the so-called “contexts of planes”. For loop closure detection, the subgraphs representing the current observed planes are compared with other ones from the PbMap. Such subgraphs can be defined by one reference plane together with their closest neighbours, up to a distance threshold (see figure 3.10). For solving the graph matching problem we rely on an interpretation tree [Grimson, 1990] that exploits the geometric and radiometric characteristics of the planes and their relative positions to generate a set of constraints that guide efficiently the search. A registration method for aligning two matched places is also presented here. This registration is applied after graph matching to check the consistency of the matched places. Thus, it improves the robustness of the recognition since a good registration is required to accept the validity of the matched place. This consistency test computes 70 Chapter 3. Plane-based maps for fast localisation and place recognition a) b) Figure 3.2: Plane-based representation. a) Point cloud representation of a living room. b) Point cloud representation with the segmented planar patches superimposed. the adjustment error together with the relative pose of the sensor with respect to the place recognized, thus providing an estimate for the localization. Experimental results are provided demonstrating the effectiveness of our method for recognizing and localizing places in a dataset composed of several home and work-place scenes (e.g. offices, living rooms, kitchens, bedrooms, corridors, etc.). The proposed registration has also been tested for omnidirectional RGB-D images, showing a performance (accuracy vs. computation) suitable for odometry and SLAM (this is addressed in chapter 5). Also, in order to test the concept of “lifelong map” [Konolige and Bowman, 2009] we also show how the recognition is affected by the fact that the scene suffers some changes. 3.1.1 Related works Mapping Different mapping solutions have been presented depending on the characteristics of the problem. One of the first ones being employed were the 2D occupancy grid maps, in which the space is organized in cells which keep the probability to be occupied [Moravec and Elfes, 1985]. This representation has been very popular for navigation of wheel robots equipped with 2D laser rangefinders moving on a plane [Dellaert et al., 1999]. A 3D version of this map makes use of cubic cells (or voxels) [Lozano Albalate et al., 2002]. The main limitation of this kind of maps comes from its storage requirements which becomes a problem in large scenarios. Octomaps are a way of organizing such information where each voxel is subdivided in 8 sub-voxels depending on the variability of neighbouring cells and the precision required [Wurm et al., 2010]. An alternative to the grid representations are the maps based on distinctive features or landmarks that can be extracted from a textured scene with one or more cameras [Se et al., 2002]. In this line, most works in the literature have focused on 3.1. Introduction 71 point-based maps, where different invariant point descriptors have been used, from the popular SIFT [Lowe, 2004], or SURF [Bay et al., 2008], to patch descriptors like in [Davison and Murray, 2002]. Feature maps from range data have also been proposed more recently with the spread of 3D range sensors [Rusu et al., 2008]. Dense point maps have also become very popular in the last years with the arrival of low cost RGB-D sensors like Microsoft Kinect. These representations offer a fair degree of detail that permits creating point cloud models with nice visualizations which have many applications beyond robotics. However, the scalability of such maps becomes an issue for many mobile applications, especially for large-scale SLAM operation. A common approach to reduce data redundancy is by using keyframes, in a similar fashion to previous monocular SLAM approaches [Klein and Murray, 2007]. In this line, recent combinations of the state-of-the-art techniques exploiting the power of modern computers have resulted in impressive 3D maps from hand-held RGB-D sensors [Kerl et al., 2013b]. Continuous surface representations have also been proposed based on polygonal representations [Lafarge and Alliez, 2013], and piecewise continuous radial basis functions (RBFs) [Carr et al., 2001]. Such models are useful for scene reconstruction and augmented reality, but are not suitable for fast localization in comparison to other approaches. Besides metric mapping strategies, pure topological representations have been employed when accurate metric localization is not required [Ulrich and Nourbakhsh, 2000]. Also, semantic information maps have been presented as a way to integrate useful knowledge about the objects and the environment [Galindo et al., 2005]. Hybrid maps integrating metric and topological information have also been proposed to deal with large and complex scenarios [Blanco et al., 2009a]. These kind of maps are discussed in the following chapter of this thesis. Compact scene representations are interesting for efficiency and scalability. In general, the more compact the model is, the less information and weaker description it offers. The balance between compactness, and accuracy (or descriptiveness) of the model must be taken into account according to the application. For example, this balance is adjusted with the size of the cell in a gridmap [Elfes, 1989], with the number of features in keypoint maps [Dissanayake et al., 2001] and point clouds [Rusu et al., 2008], or with the size of discretization for piecewise continuous RBFs representations [Carr et al., 2001]. In this thesis we address the creation of a compact representation which only contains planar patches. This is the first work, to the best of our knowledge, in which planes are integrated to a map at a high frame rate with unconstrained camera movement. This plane-based representation constitutes a useful framework for object detection and grasping [Klank et al., 2009], visual servoing [Cowan and Koditschek, 1999] and Manhattan-like modelling [Furukawa et al., 2009]. Such planar patches can be efficiently extracted from depth images [Poppinga et al., 2008], [Holz and Behnke, 2013]. In the context of SLAM, some approaches have already used planar surfaces as the only map features [Weingarten and Siegwart, 2006; Trevor et al., 2012], or along with other features (like point features) [Chekhlov et al., 2007; Gee et al., 2008; Martinez-Carranza and Calway, 2012]. Our approach differs from the 72 Chapter 3. Plane-based maps for fast localisation and place recognition above in the graph-based representation that we employ to characterize the scene, which permits to take into account the relations between neighbouring planes to perform fast and robust place recognition. Place recognition The problem of place recognition has been addressed previously from different perspectives using different sensors, from laser-range finders to different kind of cameras (e.g. consumer cameras, stereo vision, omnidirectional imaging and range cameras). Most solutions for this problem employ intensity images as input data. Appearance based methods, applied previously for object recognition [Murase and Nayar, 1995], have been largely studied in this sense. We can distinguish two kind of approaches here depending on whether the scene is described with local descriptors (local appearance) or using a global descriptor (global appearance). Local appearance methods, like the popular bag-of-words (BoW), represent the images as an unordered set of visual features (words), that are generally collected in a dictionary in a previous stage. Such a dictionary is built by clustering similar descriptors to create visual words that are repeatable. Then, different places can be recognized by classifying the images according to the frequency of their words [Sivic and Zisserman, 2003; Csurka et al., 2004]. A relevant example which makes use of bag-of-words is the work of [Cummins and Newman, 2008], which recognizes places quickly by capturing the fact that certain combinations of appearance words tend to co-occur. Also, [Angeli et al., 2008] presented an incremental, real-time system to detect loop-closures within a Bayesian filtering framework. The orderless bag-of-words technique is extended to take into account geometric correspondence in [Lazebnik et al., 2006], improving the recognition performance. In contrast to the solutions above, global appearance methods describe the scene as a whole. In this line, the method presented in [Ulrich and Nourbakhsh, 2000] makes use of an omnidirectional camera to find the location in a topological map employing maximum likelihood estimation to match the current image with a database of images acquired beforehand. Contemporary with this, the work of [Kröse et al., 2001] presented a probabilistic localization method that employs linear image features extracted using Principal Component Analysis. A context-based vision system for place and object recognition was presented in [Torralba et al., 2003] which identifies familiar locations employing a low-dimensional global image representation. In [Oliva and Torralba, 2001], a holistic representation of the scene’s spatial envelope is proposed. This work is extended in [Oliva and Torralba, 2006] introducing a scene centred image global descriptor to find places based on configuration of spatial scales. This method has common aspects with ours since it relies on the global scene layout and it can be combined with local image analysis (like BoW) to constrain the search space and to improve performance. An important difference however, is that our technique is not restricted to work with individual images, and thus, it can integrate several sensor observations in the same scene description, providing inherently a high invariance to viewpoint. 3.1. Introduction 73 There exist other methods in the literature which also address the place recognition problem from range data: the work of [Bosse and Zlot, 2008] presents a solution for place recognition which employs distinctive keypoints from 2D lidar observations. This approach is extended to 3D laser point clouds in [Bosse and Zlot, 2010]. The work in [Granström et al., 2011] also employs range data to extract features that capture important geometric and statistical properties to detect loop closures. Our approach differs from the ones above in different aspects: a) our method exploits contextual information of nearby planes, b) it does not require a training step, and c) it describes the scene in a more continuous way with a plane-based representation which is useful beyond place recognition (e.g. scene modelling). The recent availability of low cost RGB-D sensors has given rise to new approaches for the problem. In [Biswas and Veloso, 2012] the depth image from a Kinect sensor is used for localization and navigation. This approach extracts planar regions to reduce the computation load of using dense point clouds, and projects the points and planes in a 2D vector map to localize the robot and to avoid obstacles in previous 2D-range maps. However, this approach does not exploit the implicit description of planar regions and neglects important 3D information in the scene description. The method proposed in [Koppula et al., 2011] segments the scene and automatically labels these segments using a machine learning approach that takes into account local visual appearance and geometric cues, together with contextual information. Our method resembles the one above in the use of geometric information and proximity to establish the context regions, however, this method is focused on scene understanding, and therefore, requires a training stage, while ours aims to recognize previous places and does not need any off-line preparation. 3.1.2 Contribution We propose a highly compact map representation of the scene based on planar patches that can be built online from the streaming data of a RGB-D sensor. The novelty of our representation is that such planes are described with a compact geometric and radiometric descriptor, and that such planes are integrated in a graph representation which stores the “topological” relations between planes, which permits quick checking for similar place descriptions. This new representation is highly efficient for recognizing and registering places, having the following advantages: 1. the description of the scene through a PbMap is very compact, requiring little memory and reducing the computational cost of search operations; 2. it is robust to changes of viewpoint since the scene planes can be detected from very different poses, and the context of planes to match can be chosen with flexibility (it is not restricted to single-image discretization); 3. it tolerates reasonably well changes in the scene, and therefore is adequate for the so called “lifelong maps”, i.e. maps that are still valid after the scene changes. This characteristic particularly holds for indoor scenarios, where the 80 Chapter 3. Plane-based maps for fast localisation and place recognition histograms corresponding to the same plane by means of a chi-squared (χ2) distance measure [Pele and Werman, 2010]. This measure is used to compute the histogram distances of all pairs of views of the same plane. Then, the mean distance of all analysed pairs is averaged for all tested planes to obtain a global measure of the colour space stability (see table 3.2). Histogram dispersion. The histograms of planes with a well defined dominant colour must be unimodal and with little dispersion. However, such characteristics do not apply to all the planes in the environment, and also, it varies depending on the colour space. To accept that a plane has a dominant colour we make use of a simple heuristics which requires that at least 50% of the patch pixels are contained in a bandwidth of ±5% of the histogram range, centred at such dominant colour. Thus, we define the concentration rate Cas the number of planes that fulfils this condition in all colour components divided by the total number of planes. We have found that the condition above is fulfilled in 97.5% for planes represented with rgb and 92.8% for planes represented with c1c2c3, while the other colour spaces present much lower rates. Table 3.2 shows the dispersion rate in this experiment, defined as (1−C). Computation time. Another important criterion to consider is the computation time required to transform the original colour space to the target one. This is less critical because this cost is small in comparison with the whole process of segmenting the planes, whichever the chosen colour space is. The average of this time for this dataset is also indicated in table 3.2. Table 3.2: Suitability of different colour spaces to represent planar patches according to: histogram stability, histogram dispersion and computation time. The values shown correspond to the average of 100 different planes, with 10 observations each. For all properties, smaller values mean better performance. rgb c1c2c3l1l2l3HS Stability χ20.10 0.11 0.13 0.14 Dispersion (1−C)0.03 0.07 0.74 0.77 Comp. time (µs) 10.7 104.9 23.0 11.3 Taking into account the criteria studied above, we notice that rgb is the one with the best properties, and therefore, it is the one adopted here. 3.4.3 Computing the dominant colour We note that the dominant colour is a discriminative property since its value is repetitive over different observations of the patch (with different viewpoints, lighting conditions, shades and partial occlusions), and it is also distinctive with respect to other 3.4. Compact colour descriptor 81 patches (different patches have generally different colours). Figure 3.5 shows an example where 5 different planes are observed in different conditions, and still they are easily distinguishable. These observations are represented by their dominant colour, expressed as the histogram mode in each channel in the rgb triangle. G R B Figure 3.5: Representation of several observations of 5 planar surfaces through their rgb mode in the triangular domain of rgb. Each planar surface is indicated with a different type of marker. There exist several ways to define the dominant colour for a planar patch. In this work we have tested the mode of the histograms, and the centroid of the largest cluster extracted with two variants of the mean shift algorithm: with fixed (FMS) and variable bandwidth (VMS), respectively. Mean shift has been broadly used for colour segmentation [Comaniciu and Meer, 1997]. Though it has limitations for real-time applications due to its computational cost, in our case the cost of the mean shift is affordable since most histograms present unimodal distributions and we only extract one cluster, so that it converges in very few iterations. We compare the distinctiveness of the dominant colour obtained with the above techniques using a binary classifier based on the colour difference of two patches, expressed as kri−rjk. This difference is actually computed as the L1-norm for each one of the three components in the rgb space. Thus, when this difference is larger than a threshold (for any colour component) the patches are considered to belong to different physical planes. This classifier is tested, for a range of thresholds, with the previous dataset in which we know beforehand which observation corresponds to each plane, i.e. we know the classification groundtruth. From this experiment we obtain the distinctiveness of this classifier in terms of its sensitivity (ratio of actual positives which are correctly identified) and the specificity (ratio of negatives which are correctly rejected) for the different techniques to obtain the dominant colour. These results are depicted as ROC (Receiver Operating Characteristic) curves in figure 3.6. Every point of each curve represents a different threshold for the classifier, thus, more restrictive thresholds result in higher sensitivity 82 Chapter 3. Plane-based maps for fast localisation and place recognition 0.4 0.5 0.6 0.7 0.8 0.9 1 0.6 0.7 0.8 0.9 1 Specificity Sensitivity ROC color classifier VMS FMS Mode Figure 3.6: ROC curves (sensitivity vs. specificity) of the colour constraints as binary classifier. and lower specificity. Note that the nearer the curve is to the optimum point (1,1) the better the classifier. From this test we conclude that VMS provides the most distinctive dominant colour since both, sensitivity and specificity, are higher than for the mode and FMS for any threshold. 3.4.4 Choosing a robust colour descriptor We have seen that the dominant rgb colour of a plane is a distinctive property for patch matching in many cases. However, non saturated colours (i.e. r=g=b= 0.33), as for instance black and white planes, which are present in many scenarios, cannot be distinguished. Thus, we propose to include in the descriptor the average intensity of the plane so that another loose restriction can be applied to differentiate between such planes. Note that a minimum illumination of the scene is required to use colour in PbMaps, and such a minimum illumination is enough to make a difference between black and white surfaces. This value is calculated as the average ((R+G+ B)/3) of the inliers supporting the dominant colour given by the previous mean shift segmentation. This parameter permits also recovering the plane’s original main colour in RGB for visualization purposes. Also, an important issue when describing patches with their dominant colour is dealing with those cases where this description is not applicable (e.g., textured regions without a prevalent colour). In order to take into account this condition we add a boolean to our colour descriptor to specify whether the distribution of the plane histogram in rgb has a low dispersion, as explained in the previous subsection. To sum up, the resulting descriptor contains 4 elements that are stored in a word of 4 bytes: 2 bytes for normalized colour rand g(note that bdepends on these two since r+g+b=1), 1 byte for the average intensity and 1 byte to specify the existence of a dominant colour. 3.4. Compact colour descriptor 83 0.4 0.5 0.6 0.7 0.8 0.9 1 0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 1 Specificity Sensitivity ROC color constraints Normalized rgb Dominant Brgb Hue Histogram Figure 3.7: ROC curves (sensitivity vs. specificity) of different colour descriptors: dominant rgb, robust dominant rgb ++ and hue histogram. 3.4.5 Comparison with the hue histogram In this section we evaluate the distinctiveness of the proposed colour descriptor by comparing it with the dominant rgb colour and with the normalized, saturated hue histogram proposed by [Pathak et al., 2012]. For this last, the histogram distance between h1and h2is computed with the Bhattacharyya distance [Bhattacharyya, 1946]: B(h1,h2) = s1− N ∑ k=1ph1[k]·h2[k](3.3) The sensitivity and specificity of a binary classifier based on the compared descriptors are evaluated using different thresholds as we did in the previous section (see figure 3.7). As expected, we observe that the proposed descriptor is significantly more distinctive than the rgb dominant colour, since the latter lacks the robust information added to the first. By comparing our descriptor with the hue based histogram we observe that their distinctiveness are similar despite the richer information of the latter. The reason for this is that most planes have a dominant colour in our test environment, as in most indoor scenarios. The fact that the sensitivity of the hue histogram is slightly lower is explained because the histogram is less robust to partial viewing. Contrarily, this descriptor should perform better for textured surfaces and when the patches present no occlusion, however, such cases are rare in the home and office environments we are working in, where our dominant colour descriptor is more suitable. 84 Chapter 3. Plane-based maps for fast localisation and place recognition Figure 3.8: 2D representation of the map construction scheme. a) RGB-D capture with segmentated planes (blue). b) Current PbMap with segmented planes (blue) superimposed according to the sensor pose. c) PbMap updated: the planes updated are highlighted d) PbMap graph updated: the planes updated are highlighted in blue, the new plane P7is marked in green and, the new edges are represented with dashed lines. 3.5 PbMap construction After the previous segmentation stage, each detected planar patch is integrated into the PbMap according to the sensor pose, either by updating an already existing plane or by initializing a new one when it is first observed. The sensor pose needed to locate the planes in a common frame of reference can be obtained in different ways. For instance, the current pose may be obtained from the observation of a sufficient number of planes of the PbMap [Fernández-Moral et al., 2013b]. Otherwise, range, visual or combined range and visual odometry may be used [Kerl et al., 2013b; Gokhool et al., 2014]. The PbMap construction procedure is illustrated in figure 3.8. For every new frame, a subsampled point cloud (160×120) is built relative to the sensor, and planar patches are segmented from it. The segmented patches are then placed in the PbMap according to the sensor pose (figure 3.8.b). If the new patch overlaps a previous one and their normal vectors coincide, then they are merged and the parameters of the resulting plane are updated. In other case, a new plane is initialized in the PbMap (figure 3.8.c). The graph connections of the observed planes are also updated at every new observation by calculating the minimum distance between the current planes in view and the surrounding planes (figure 3.8.d). An example of a PbMap built from a 3.6. Place recognition and localization in PbMaps 85 Figure 3.9: Plane based representation of a living room. The coloured planes at the right have been extracted from the point cloud at the left. short RGB-D video sequence in a home environment is shown in figure 3.9 where we can distinguish the different planes segmented. 3.6 Place recognition and localization in PbMaps The identification of a place using PbMaps is based on matching and aligning a set of neighbour planes that are represented by a graph. This process can be divided into three different stages: first, the scope and the size of the subgraphs that are to be compared has to be chosen; second, an interpretation tree is applied employing geometric and radiometric constraints to match the maximum number of planes between the two subgraphs; and finally, the matched planes are aligned rigidly, providing an error measurement and the relative rigid transformation between the matched places. These two last stages constitute the technique for scene registration, that can be applied when the first stage is not required as in the registration of PbMaps extracted from single images (i.e. PbMap odometry), or to find the correspondence between PbMaps that have already been localized in the same local region. 3.6.1 Choosing the scope of search The first question implies that we have to select a set of planes (or subgraph) which defines a place as a distinctive entity. The key to select a subgraph from the multiple combinations that are possible in a PbMap lies in the graph connections, as they link highly related planes in terms of distance and co-visibility. Thus, a subgraph is selected by choosing a reference plane and taking its k-order neighbours which are defined by a distance threshold. In the experiments of this chapter we use the first 86 Chapter 3. Plane-based maps for fast localisation and place recognition Figure 3.10: Example of the graph representation of a PbMap, where the arcs indicate that two planes are neighbour. Two subgraphs are indicated: the ones generated by the reference planes P1and P5, respectively. order neighbours and we set a distance threshold of 1 m to define neighbour planes (see figure 3.10). This strategy permits to describe a place in a piecewise continuous fashion, so that different subgraphs can be possible around a local area, providing flexibility to recognize places that are partially observed. The number of possible subgraphs grows linearly with the map size, that is, the maximum number of subgraphs in the PbMap is limited by the number of planes, though in practice, this number is smaller, since one particular subgraph can be generated from two –or more– neighbour planes (e.g. the subgraphs generated by P8and P9in figure 3.10 are the same). Also, when a subgraph is contained in other subgraph, only the largest one is considered for matching a place. Thus, in order to achieve a scalable solution for place recognition or loop closure we just need to guarantee bounded time for graph matching. 3.6.2 Graph matching The problem addressed here is that of matching local neighbourhoods of planes, represented as subgraphs in the PbMap. Thus, we aim to solve a graph matching problem allowing for inexact matching to be robust to occlusions and viewpoint changes. Several alternatives are found in the literature for this problem, from tree search to continuous optimization or spectral methods [Hansen et al., 2008]. Here, we employ a tree search strategy because it does not require further information like the probability of the graph attributes, it is easy to implement and it is extremely fast to apply when the subgraphs to be compared have a limited size. In order to match two subgraphs we rely on an interpretation tree [Grimson, 1990], which employs weak restrictions represented as a set of unary and binary constraints. On the one hand, the unary constraints are used to check the correspondence of two single planes based on 3.6. Place recognition and localization in PbMaps 87 the comparison of their geometric and radiometric features. On the other hand, the binary constraints serve to validate that two pairs of connected planes present the same geometric relationship. An important advantage of this strategy is that it allows to recognize places when the planes are partially observed or missing (inexact matching), resulting in high robustness to changes of viewpoint. 3.6.2.1 Unary constraints The unary constraints presented here are designed to reject incorrect matches of two planes, and thus, to prune the branches of the interpretation tree. Thus, the unary constraints serve to speed-up the search process. These are weak constraints, meaning that the uncertainty about the plane parameters is high, so the thresholds are very relaxed to avoid rejecting a correct match. In other words, a unary constraint should validate that two planes are distinct when their geometric or radiometric characteristics are too different, but they lack information to confirm that two observations belong to the same plane, since even different planes can have the same characteristics. Three unary constraints have been used here, which perform direct comparisons of the plane’s area, elongation, and dominant colour if available. For example, the area constraint checks that the ratio between the areas of two observed planes are under certain bounds, and similarly for the other constraints. That is 1 threshold <areaPi areaPj <threshold (3.4) In order to determine appropriate thresholds for such constraints, we analyse their performance in a dataset containing 1000 observations of plane surfaces from different scenarios, spanning diverse viewing conditions (changing viewpoint and illumination, partial occlusion, etc.). We have manually classified these planes, so that the correspondences of all plane observations are known. Then, we analyse the classification results of our constraints in terms of the sensitivity (ratio of actual positives which are correctly identified) and the specificity (ratio of negatives which are correctly rejected), for a set of different thresholds. The result of this experiment are shown with a ROC curve, which shows the sensitivity with respect to the specificity for a given threshold, see figure 3.11. The curves show that higher values of specificity correspond to smaller values of sensitivity and viceversa. Note that the nearer the curve is to the optimum point (1,1) the better the classification of the weak constraint. From this graph we can see that the colour is the most discriminative constraint. Also, since all unary restrictions require similar computation, we arrange them according to their discrimination power, thus the first constraint applied is the radiometric one, followed by the area and the elongation, respectively. The thresholds for each constraint are determined consistently by choosing a minimum sensitivity of 99%. We notice that those planes that are incorrectly rejected by a unary constraint correspond to planes which have been partially observed (e.g. the 88 Chapter 3. Plane-based maps for fast localisation and place recognition 0.3 0.4 0.5 0.6 0.7 0.8 0.9 0.6 0.7 0.8 0.9 1 Specificity Sensitivity ROC unary constraints Area Elong Color Figure 3.11: Comparison of the different unary constraints by their ROC curves (sensitivity vs. specificity). corner of a table). The fact that some planes might be rejected incorrectly is not critical to recognize a place since not all of the planes are required to be matched. The thresholds obtained here depend on the amount and variety of the training samples used. But since most indoor scenes have planes of similar sizes and with similar configurations, such thresholds must be valid for most similar environments. Besides, we have observed that variations of one order of magnitude in the thresholds do not affect significantly the results for place recognition. 3.6.2.2 Binary constraints The binary constraints impose geometric restrictions about the relative position of two pairs of neighbour planes (e.g. the angle between the normal vectors of both pairs must be similar, up to a given threshold, to match the planes). These constraints are responsible to provide robustness in our graph matching technique, enforcing the consistency of the matched scene. Three binary constraints are imposed to each pair of planes in a matched subgraph. First, the angle difference between the two pairs being compared should be similar. This is arccos(nC i·nC j)−arccos(nM ii ·nM j j)<threshold (3.5) where nC iand nC jare the normal vectors of a pair of nearby planes from the subgraph C, and similarly nM ii and nM j j are the normal vectors of a pair of planes from the subgraph M. Also, the distances between the centroids of the pair of planes must be bounded (cC j−cC i)−(cM ii −cM j j)<threshold (3.6) 3.6. Place recognition and localization in PbMaps 89 0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 1 0.5 0.6 0.7 0.8 0.9 1 Specificity Sensitivity Sensitivity vs Specificity Distance Angle Height Figure 3.12: Comparison of the different binary constraints by their ROC curves (sensitivity vs. specificity). The other binary constraint takes into account the perpendicular distance from one plane to the centroid of its neighbour. This distance must be similar when the two pair of planes are correctly matched, nC i·(cC j−cC i)−nM ii ·(cM j j −cM ii )<threshold (3.7) Other constraints have been tested employing the distance between planes, and the direction of the principal vectors, however, these constraints did not improve significantly the search since they are highly sensitive to partial observation of planes. Nevertheless, these constraints can be used when partial observation is not a big issue, like when matching nearby omnidirectional frames as in chapter 5. Similarly to the previous subsection, the classification performance of these constraints is analysed for a range of thresholds. For that, we estimate the ROC curves to show the balance between sensitivity and specificity of these binary constraints, which are shown in figure 3.12. 3.6.2.3 Interpretation tree Algorithm 1 describes the recursive function for matching two subgraphs. This function checks all the possible combinations, defined by the edges among the planes of the subgraphs SCand SB, to find the one with the maximum number of matches. In order to assign a new match between a plane from SCand a plane from SBthe unary constraints are verified first (their result is stored in a look-up table to speed up the search), and if they are satisfied, the binary constraints are checked with the already matched planes. If all the constraints are satisfied, a match between the planes is accepted and the recursive function is called again with the updated arguments. The algorithm finishes when all the possibilities have been explored, returning a list of pairs of corresponding planes. 96 Chapter 3. Plane-based maps for fast localisation and place recognition 4 5 6 7 8 9 10 0 2 4 6 8x 104 Num planes to match Num restrictions checked Geometry Geometry & Color Figure 3.15: Performance of the place recognition process (in terms of the number of restrictions checked until matching with respect to the size of the subgraph to match) for both: only geometry and colour and geometry in PbMaps. The computing time is directly proportional to the number of restrictions checked. Table 3.5: Robustness to wrong recognition by using colour information. Scenario Failure rate (depth) Failure rate (depth+colour) Office2 10% 0% Office3 10% 0% Hall2 10% 0% Bedroom1 10% 5% Bedroom2 20% 10% Bedroom3 20% 5% Bathroom 35% 30% 3.8 Discussion This chapter presents a highly compact map representation of the scene based on planar patches (PbMap). Such planes are efficiently extracted from range images with a region growing procedure, permitting the use of this representation for real-time mapping and SLAM. The planes are described by simple geometric attributes, and also colour information if it is available. A PbMap is structured as an undirected graph where the nodes represent planes and the edges store the neighbour relations between them, so that they contain contextual information. This arrangement permits to operate quickly on local neighbourhood of planes for scene registration and place recognition. A new methodology for real-time place recognition in indoor environments using PbMaps has been proposed. The recognition process is tackled with an interpretation tree, which matches efficiently local neighbourhoods of planes based on weak constraints that prune the match space. This matching process employs unary and binary 3.8. Discussion 97 constraints. The unary constraints restrict the individual correspondence of pairs of matched planes, and its main contribution is the speed-up of our solution. On the other hand, the binary constraints check that the layout of the scenes being compared are geometrically consistent, and so, they are responsible of the robustness of this technique. This kind of map is interesting for representing indoor scenes, where the amount of planar surfaces dominates over non-planar structures. The proposed solution can work with range cameras, by generating a geometric description of the scene, or with RGB-D sensors, adding radiometric information to the planes to improve the description and the recognition performance through the unary constraints. A colour descriptor consisting of the dominant colour, the average intensity and a parameter indicating the robustness of dominant colour was employed. A comparison of both alternatives (only geometry vs. geometry and colour) has been presented, which shows an average speed-up of 6 times in scene recognition by using colour information. A registration technique is proposed to further check the metric consistency of two matched places, and to recover the metric localization. This technique aligns the scenes through the minimization of a cost function, whose residual is used to validate the proposed match. This minimization provides the relative pose between the matched places, which can be used for loop closure for instance. We provide experimental results demonstrating the effectiveness of our approach for recognizing and localizing places in a dataset composed of 20 home and work-place scenes: offices, living rooms, kitchens, bathrooms, bedrooms and corridors. Apart from the above mentioned advantages, this strategy to describe and identify places is robust to changes in the scene. The point is that most of the movable objects in indoor environments are not planar, and regarding the planar structure, the larger (dominant) planes are generally static. Thus, the approach presented is conceptually adapted to lifelong mapping. In order to test this idea, we performed an experiment to measure how the recognition performance is affected by the fact that objects can be moved by the users. The results confirm the intuition, though a deeper study must be carried out to evaluate the applicability of our representation for such a problem. This constitutes a field for future research after this thesis. A future improvement for the PbMap will consist of its integration into a probabilistic framework to represent the plane parameters. This will permit to deal with sensor noise to obtain more robust and accurate PbMap registration. Another important improvement would be the ability to detect planar patches that fully cover the represented surface. This implies that the surface is seen with no occlusions, so that information about other dimensions can be exploited to improve localization and to address related tasks like object recognition. Also, an interesting open issue from this research is how to use the compact description of a PbMap for semantic inference, which can provide extra capabilities in mobile robotics and better communication interfaces human-robot. Since the PbMap’s compact geometric (and radiometric) description is useful to match scenes, it is reasonable that they can be useful to identify classes of scenes (e.g. kitchens, bedrooms, etc.) what is interesting for example for domestic service robotics. However, this problem has a total different perspective, 98 Chapter 3. Plane-based maps for fast localisation and place recognition and a whole research to find distinctive cues must be carried out before evaluating the potential capabilities of PbMaps for this problem. 3.8. Discussion 99 Figure 3.16: Different scenarios where place recognition has been tested. These pictures show the point clouds of some of the maps created previously, showing their PbMap right below of each scenario. Chapter 4 Hybrid metric-topological mapping Abstract Efficient map representations are required in mobile robotics to perform complex tasks. The integration of metric and topological information has been proposed to create multi-layer hybrid maps, where the metric layer is generally used for accurate localization in a local environment, and the topological layer stores high level symbolic information which may be required for planning and task reasoning. This chapter presents a new methodology to structure dynamically a metric map into a topological arrangement of local maps, while the topological structure keeps information about the connectivity of these local maps. The division is carried out based on the co-visibility of map features, and is executed with graph cut. This strategy permits scalable localization and mapping approaches by considering only the set of local maps which are closer to the robot. Experimental results are presented with a monocular SLAM system in indoor and outdoor scenarios. 101 102 Chapter 4. Hybrid metric-topological mapping 4.1 Introduction Different kinds of mapping strategies are needed in mobile robotics to perform different actions. For example, grasping an object requires a metric map of the object and its environment, while for navigation, besides the local metric map required to execute immediate movements, a global map with topological information is generally required to decide the most appropriate path or to reason about the aimed tasks. Such topological representation could encode information that is meaningful also for humans, like the connectivity of rooms in a building. Hybrid metric-topological maps have been proposed for dealing with these two types of information [Thrun, 1998]. In this chapter we build up on previous works to present a dynamic arrangement of the metric-topological representation according to the sensed space [Blanco et al., 2006]. The problem of scalable SLAM is present when coping with some real autonomous robotics applications. Such ability to operate in large scale brings the need of appropriate strategies for managing the map. This problem may be addressed using metric-topological or multilayer hierarchical representations. In this sense, applying abstraction (as humans do) is an effective way of dealing with the huge amount of detail present in large metric maps. The result of such abstraction process is a metrictopological map, consisting of a two-layer representation, one containing pure geometrical information and a second one containing higher level symbolic information [Thrun, 1998]. Thus, the benefit of a metric-topological arrangement is twofold: first, it offers a natural integration with symbolic planning that permits a robot to reason about the world and to execute high level tasks [Galindo et al., 2008]; second, the efficiency and scalability of SLAM is improved by limiting the scope of localization and mapping to the region of the environment where the robot is operating. Also, loop closure and re-localization can be more efficiently solved using topological information [Savelli and Kuipers, 2004; Angeli et al., 2009]. Here, we present a mapping strategy where the metric map is dynamically divided into regions (submaps) with highly connected observations, resulting in a topological structure where each node stores a local metric map, and the arcs represent the relations with neighbouring submaps. The key idea is to cluster in the same submap those features which are more interrelated according to the sensor’s visibility, that is, grouping co-visible features, or keyframes with higher overlap depending on the metric map chosen. The map division is performed using a graph cut technique which can be executed online (with every new observation), allowing efficient and scalable SLAM operation. The mapping approach presented here is tested in the framework of a monocular SLAM, where the experiments show that this hybrid metric-topological approach outperforms the efficiency and scalability of the pure metric approach. Concretely, we will focus on the benefits of such hybrid mapping applied to a well known monocular SLAM system based on Bundle Adjustment on keyframes (PTAM [Klein and Murray, 2007]). Our approach has also been applied in a SLAM solution based on PbMaps [Fernández-Moral et al., 2013b] which is presented in the next chapter. 4.1. Introduction 103 4.1.1 Related works Hybrid mapping and map partitioning Hybrid maps that combine metric and topological information have been proposed for SLAM in large and complex robot environments. Such maps are usually composed of local metric maps (suitable for robot localization) organized in a topological graph structure, which stores the relations between local maps, and/or other high level symbolic information [Bosse et al., 2003; Estrada et al., 2005; Blanco et al., 2009a]. A key question for such hybrid mapping is how the map should be partitioned into local maps. Map division has been addressed in a number of works. Some relevant examples are: the Atlas framework [Bosse et al., 2003], where a new local map is started whenever localization performs poorly in the current local map, or the hierarchical SLAM presented in [Estrada et al., 2005], where sensed features are integrated into the current local map until a given number of them is reached. However, none of these provides a mathematically grounded solution based on the particular perception of the scene. In [Eade and Drummond, 2007], the map is divided in nodes where the landmarks are represented in a local coordinate frame and, these landmarks are updated using an information filter. This method uses the common features between adjacent nodes to calculate their relative pose. A different approach called Tectonic-SAM [Ni et al., 2007] uses a “divide and conquer” approach with locally optimized submaps in a Smoothing and Mapping framework (SAM). This approach is improved in [Ni and Dellaert, 2010] to build a hierarchy of multiple-level submaps using nested dissection. Other works employ “graph cut” to divide the map according to a measurable property of the map observations. On that mathematically sound basis, [Zivkovic et al., 2005] addresses the problem of automatic construction of a hierarchical map from images; [Blanco et al., 2008] generates metric-topological maps using a range scanner, and generalizes the approach for other sensors; and [Rogers and Christensen, 2009] splits the map within a Bayesian monocular SLAM framework to reduce the problem complexity. Our method, which also relies on graph cut, differs from the works above in the way the graph is updated, which is specifically tailored for online SLAM operation. Our approach resembles also the stereo-SLAM framework of [Lim et al., 2011] who divide the map keyframes into groups (called segments) according to their geodesic distances in the graph. On the contrary, our map partitioning is independent of the keyframe positions, and is only based on observations acquired from the scene. Concretely, the map is split where there are fewer shared observations, minimizing the loss of information and therefore, enforcing the coherency and consistency of the submaps. 104 Chapter 4. Hybrid metric-topological mapping Monocular SLAM Many solutions have been presented to build metric maps with monocular SLAM since [Davison, 2003] presented the first real-time solution for the problem in 2003. Two main strategies have been applied since then: Bayesian filtering (following the work of Davison) and Bundle Adjustment (BA) on keyframes, as introduced in [Klein and Murray, 2007]. The latter represents the base for the current state-of-the-art since it allows handling denser maps and generally offers a better ratio accuracy/cost [Strasdat et al., 2010]. BA, traditionally used as an offline method for Structure from Motion (SfM), is now widely used in visual SLAM thanks to the introduction of parallel processing and efficient algorithms which exploit the sparse structure of the problem. Its application to visual SLAM was inspired by real time visual odometry and tracking [Nistér et al., 2004], where the most recent camera poses where optimized to achieve accurate localization. In a similar vein, PTAM selects keyframes and applies BA in a fixed size window, around the last keyframe incorporated, to optimize the metric map and the camera trajectory. Then, once the local optimization is performed, a low priority global BA is run to improve the map consistency. This approach is extended in [Holmes et al., 2009] by combining it with relative bundle adjustment (RBA) [Sibley et al., 2009], allowing fixed-time, consistent exploration. An improvement of the latter to exploit the problem’ sparse structure was recently presented by [Blanco et al., 2013]. The work of [Strasdat et al., 2011] which is also related to RBA, proposes a double window optimization: a first window as in PTAM and a second one including the periphery of the first to improve consistency by optimizing a pose-graph of keyframes. Despite the impressive results obtained, such unique map solution has intrinsic limitations for managing maps of real large environments. To prevent this limitation, we propose a topological arrangement of the map into local metric maps. 4.1.2 Contribution In this chapter we present a mapping strategy where the metric map is dynamically divided into regions with highly connected observations, resulting in a topological structure which permits the efficient augmentation and optimization of the map. With such map division, the current submap always contains the most relevant metric information about the current robot location, which is useful to improve the efficiency of SLAM. This hybrid mapping solution has been integrated with a monocular SLAM system to demonstrate the advantages of our approach. This strategy can be applied to other types of SLAM as it is demonstrated in the next chapter, where it is applied to an omnidirectional RGB-D SLAM approach. Subsequently, we describe our metric-topological mapping approach and the map partitioning procedure (section 4.2) and show how it is combined with the monocular SLAM system of PTAM (section 4.3). The experiments and their results are presented next (section 4.4), and finally, we expose our conclusions from these results. 4.2. Hybrid metric-topological mapping approach 105 4.2 Hybrid metric-topological mapping approach Splitting a map into locally consistent metric representations and globally coherent regions provides some relevant advantages for SLAM. Next, we explain the benefits of such map structure (subsection 4.2.1), and describe our proposal to obtain such metric-topological arrangement of the map (subsection 4.2.2). In order to be consistent with the experiments in this chapter, the next subsections tackle specifically SLAM based on bundle adjustment, and for a mapping approach based on keyframes and point features. However, both can be easily generalized to be applied to other types of SLAM approaches, like e.g. pose-graph SLAM or EKF-SLAM, and for other kinds of maps, e.g. PbMaps. 4.2.1 SLAM Improvements through hybrid mapping The advantages of applying a coherent map partition in SLAM are diverse: a) all the metric data in each submap (which may include the keyframe poses, landmark positions, point clouds, etc.) can be referred to a local coordinate system to reduce error accumulation and to avoid numerical instability; b) localization can be achieved more efficiently since only those map features in the nearer regions are used to estimate the pose of the robot; and c) this map structure permits to approximate the global map optimization by the individual optimization of the different submaps, thus reducing the computational cost of this process. This last advantage is of special relevance due to the demanding nature of map optimization. For bundle adjustment, its complexity ranges from linear to cubic in the number of keyframes depending on the particular structure of the problem [Konolige, 2010]. Let us now explain the details of this approximation for BA global optimization. Having a map of nlandmarks obtained from mkeyframes, bundle adjustment can be expressed as min Tj,pi n ∑ i=1 m ∑ j=1 vi j d(Q(Tj,pi),xi j)2(4.1) where •d(x,x0)denotes the Euclidean distance between the image points represented by vectors xand x0, • Tjis the pose of camera at keyframe jand pithe position of landmark i, •Q(Tj,pi)is the predicted projection of landmark ion the image associated to keyframe j, •xi j represents the observation of the i-th 3D landmark by keyframe j, and •vi j stands for a binary variable that equals 1 if landmark iis visible in keyframe jand 0 otherwise. 112 Chapter 4. Hybrid metric-topological mapping Figure 4.3: Tracking and mapping threads of PTAM. Blue boxes correspond to the embedded stages to perform the map partitioning. 4.3.2 Combination of map partitioning and PTAM A scheme of the proposed partitioning method interacting with PTAM is depicted in figure 4.3. Our submapping procedure takes action in both of PTAM threads. In the tracking thread, it selects the current submap and the nearest keyframe to the estimated pose after a new frame is analysed. In the mapping thread, after a new keyframe is selected and new landmarks are detected in it, the SSO is evaluated with respect to all the keyframes of the neighbourhood. Such neighbourhood includes all the submaps directly connected to the current submap (see figure 4.4). The partitioning procedure comes into play after the SSO has been updated, then, the min-Ncut is evaluated, and if it results in a different partition, the map is rearranged. This procedure is described in algorithm 2. This partitioning method is applied dynamically while the map is built and may create new submaps as well as merge existing submaps to maintain the division coherency by grouping keyframes with high overlap. The result is a metric-topological map, where two different topological areas will be connected by a rigid transformation if there are common observations between them. The partitioning process, including SSO computation, min-NCut evaluation and map rearrangement depends on the number of keyframes and landmarks in the neighbourhood, taking up to 100 ms in our experiments, which is a short time in comparison with the map optimization stage through BA. 4.4. Experiments 113 Figure 4.4: Topological representation of the map, showing the neighbourhood of a reference submap. Figure 4.5: Experimental set up: laptop with attached camera. 4.4 Experiments In this section we present some experiments which show the advantages, in terms of efficiency and scalability, of using the proposed metric-topological arrangement of the map instead of a single metric map. The experiments have been carried out using a Philips SPC640NC webcam, connected by USB to a linux-based laptop with an Intel Core2 Duo 2.4 GHz processor, 2Gb of memory and a nVidia GeForce-9400 graphics card. The camera intrinsic parameters were calibrated using the methodology described in section 2.2.1. Figure 4.5 shows the set up of our monocular SLAM system. A first experiment is aimed to illustrate the increase of efficiency in localization (tracking of the camera’s pose). For that, we compare the time needed to reproject 114 Chapter 4. Hybrid metric-topological mapping 0 0.5 1 1.5 2 2.5 3 3.5 4 x 104 0 1 2 3 4 5 6 Map projection time Number of Map Points Time(ms) Submapping Single Map Figure 4.6: Map projection time for localization with and without map partitioning. map points into the current frame with and without partitioning as the map grows. Both tests have been performed in the same environment, building maps composed of about 45K points and 1K keyframes, distributed in 52 submaps for the partitioning case. Figure 4.6 shows that the time with a unique map grows linearly with the number of map points, whereas with the metric-topological submapping this time is bounded since only those points in local maps close to the camera are evaluated. This improvement in efficiency becomes more evident when the map grows non-stop (note that this process is performed with each new frame captured by the camera, at 30 Hz). The goal of the second experiment is to quantify the efficiency in the global optimization of the map with our submapping approximation. For that, we have run BA offline after every new keyframe is selected from a recorded video (that is, sequential SfM), measuring the times of each BA completion with and without partitioning. At the end of these tests, the maps created were composed of about 22K points and 400 keyframes, distributed in 9 submaps for the partitioning case. In order to compare both alternatives in the same conditions, we have included the time of partition management in the BA time for the partitioning test. Figure 4.7 shows the computing time of the optimization vs. the number of keyframes of the whole map for both cases. As expected, for the case of a single metric map, the computational cost follows an increasing polynomial trend with the number of keyframes. Conversely, when applying hybrid mapping, the computational burden is bounded since the BA is applied only on the current submap. For this case, we can observe some abrupt changes in the cost which are produced when the reference submap (the one where the system is localized) switches to a neighbour of different size. Figures 4.8.a and 4.8.b show the maps built with both alternatives (different colors represent different submaps in 4.8.b). We can verify visually their high similarity, and their good alignment, as a result of the continuous optimization previous to the map partition. 4.5. Discussion 115 0 50 100 150 200 250 300 350 400 0 50 100 150 200 250 300 350 400 Key−Frames Time (s) Bundle Adjustment Single Map Submapping Figure 4.7: Bundle adjustment computation time (offline) with and without partitioning. Additionally, we are interested in comparing the accuracy of the generated metric map. Due to the lack of a reliable metric to evaluate the map’s quality, we have compared visually the different maps considering as ground truth the map obtained offline in the previous experiment (figure 4.8.a), which is the most accurate we can get. In the map obtained with PTAM (figure 4.8.c), we can appreciate some regions with depth errors and many outliers (e.g. landmarks detected behind physical walls). These inconsistencies appear as a consequence of the premature interruption of global BA that happens when a new keyframe is selected, which leads to data association errors and the subsequent loss of accuracy with the map size. On the contrary, the map obtained with our approach (figure 4.8.d) presents no inconsistencies and considerably fewer outliers than the unique map solution (figure 4.8.c). This results from the higher efficiency of the submap local optimization, which optimizes regions with highly correlated observations to produce locally accurate submaps. The results shown in this section have been supported in several tests performed under different conditions: exploring different rooms, re-visiting previous maps, traversing a corridor, zooming to get more detail of the scene, etc. The reader may refer to http://youtu.be/-zK05EcOjX4 for a video that illustrates the operation of our submapping approach with PTAM in different environments. 4.5 Discussion This chapter presents an online metric-topological mapping technique which maintains a structure of local metric maps by grouping highly connected observations. Such local maps are obtained from graph cut, by grouping co-visible observations. This hybrid metric-topological structure improves the scalability of SLAM in two aspects: first, the system rules out unnecessary metric information to perform localization more efficiently; and second, it permits to approximate the global map op- 116 Chapter 4. Hybrid metric-topological mapping Figure 4.8: Top view of maps generated in our experiments. All the maps are composed of more than 400 keyframes and 22.000 landmarks. The different colors in b) and d) represent different submaps. timization by a local optimization to reduce computational cost while maintaining the map consistency. Experimental results have demonstrated the potential of our approach to obtain efficient map representations in large environments, permitting a monocular SLAM system designed for small environments to operate in large scale. Furthermore, the topological arrangement of the map is useful for other tasks, as loop closure, global localization or navigation. A possible line of future work after this thesis may include exploiting the topological structure of the proposed mapping technique for loop closure and relocalization. Chapter 5 A SLAM system for omnidirectional RGB-D sensors Abstract Simultaneous Localization and Mapping (SLAM) is a central problem for autonomous mobile robotics. It requires building a map from the sensor measurements at the same time that the robot is localized in such map. This chapter presents a new indoor SLAM solution employing an omnidirectional RGB-D device. The solution presented here is based on a hybrid metric-topological mapping approach consisting of a graph where the nodes are keyframes, corresponding to the omnidirectional (or spherical) RGB-D images, and the arcs represent the relative poses of pairs of keyframes. Each node is described through a plane-based map (PbMap), and localization is performed through PbMap registration. The map is optimized in a pose-graph framework applying dense pixelwise matching of the keyframes. This hybrid map is also structured in a second topological layer where closely related keyframes are clustered. Such higher level organization permits efficient re-localization and loop closure for optimizing the global consistency of the map. 117 118 Chapter 5. A SLAM system for omnidirectional RGB-D sensors 5.1 Introduction Omnidirectional images are traditionally defined as those images whose field of view (FOV) comprises 360◦in the horizontal plane. They are also referred to as spherical images [Meilland et al., 2010] since they can be warped on a sphere covering a large area (in this chapter we use these two terms with no distinction). Such images have some important advantages in computer vision and robotics, since problems as optical flow, or feature selection and matching are better conditioned. Furthermore, spherical vision provides a natural decoupling between rotation and translation, which is useful for localization in mobile robotics. These advantages have already been exploited during the last decades for scene modelling [Micušık et al., 2003], vision-based navigation [Gaspar et al., 2000], robot localization [Tamimi et al., 2006; Menegatti et al., 2006; Meilland et al., 2011], visual odometry [Scaramuzza and Siegwart, 2008], place recognition [Ulrich and Nourbakhsh, 2000; Jogan and Leonardis, 2000], and SLAM [Kim and Chung, 2003; Rituerto et al., 2010]. The availability of depth images is much more recent than for intensity ones (RGB) due to the latter development of dense depth perception. One solution for obtaining spherical depth images is omnidirectional LIDAR, as Velodyne [Glennie and Lichti, 2010]. However, the expensive price of this sensor (about $75000) prevents more extended applicability. A different strategy to obtain spherical RGB-D images is by using a rig of RGB cameras [Meilland et al., 2010], where the depth is obtained from dense stereo matching [Hirschmuller, 2005]. In such case, this omnidirectional RGB-D device allows to build realistic representations of the world, permitting also accurate localization through dense image alignment [Meilland et al., 2011]. However, the need to construct the RGB-D spheres offline puts an important limitation for its application in SLAM. In this thesis we propose to use a rig of RGB-D sensors to obtain omnidirectional intensity and depth images at video frame rate (30 Hz). This approach presents some advantages with respect to the above ones like real-time acquisition, easy calibration and lower price (around $1800). On the other hand, the approach presented here is only valid for indoor environments. Outdoor environments could nonetheless be treated given that the depth can be obtained and that the scene contains planar surfaces. Regarding the first aspect, the main drawbacks of the RGB-D sensors used here is that they cannot compute the depth with direct sunlight and that they have a short useful range. These problems will likely be alleviated with the future versions of these sensors, but by now, they can only be avoided using more expensive sensors like Velodyne. Regarding the lack of planar structure, our approach can still work thanks to the pixelwise registration, however, the frame rate will drop in such case as this technique is considerably slower than PbMap registration. This sensor set-up is intended for quick scene reconstruction and for the creation of hybrid maps (metrictopological-semantic) for autonomous navigation. In this chapter it is used for SLAM employing the mapping approaches introduced in previous chapters of this thesis, to allow efficient operation through a compact and descriptive map. Concretely, the localization procedure is based on plane-based map (PbMap) registration, where a 5.1. Introduction 119 PbMap descriptor is computed online for each omnidirectional image. The map consists of a set of keyframes which are selected when they provide new information about the scene, either when they observe an unexplored place or when the scene has changed considerably with respect to the previous observation. We perform some preliminary experiments in office and home environments confirming that despite the big volume of data acquired by the sensor, SLAM can still perform in real-time. 5.1.1 Related works A variety of SLAM approaches have been presented for different sensors and conditions, a good introduction on these methods is given in [Durrant-Whyte and Bailey, 2006] and [Bailey and Durrant-Whyte, 2006]. In this chapter we focus on omnidirectional RGB-D SLAM, which has its own particularities with its pros and cons. Visual SLAM from omnidirectional cameras has already been investigated following the approaches based on the Extended Kalman Filter [Rituerto et al., 2010], or pure topological SLAM [Goedemé et al., 2007]. Previously, this source of data has been used for visual odometry with perspective omnidirectional vision [Tardif et al., 2008] and with a catadioptric camera [Scaramuzza and Siegwart, 2008]. For the latter, loop closure was also proposed in [Scaramuzza et al., 2010]. Topological mapping has been addressed with omnidirectional cameras like in [Menegatti et al., 2002] where the authors employ a spatially semantic hierarchy. These images have also advantages for semantic inference and image classification [Oliva and Torralba, 2006], [Rituerto et al., 2012]. A SLAM system only from omnidirectional depth images acquired with Velodyne was proposed by [Moosmann and Stiller, 2011]. Their Velodyne SLAM is based on Iterative Closest Point (ICP) registration [Chen and Medioni, 1992] which is performed on planar surfaces characterized by low uncertainty. This solution achieves nice point cloud representations with good global consistency over long trajectories in outdoor environments. Despite the registration stage is focused on planar surfaces, the compactness of such features is not exploited here where ICP is still performed using a classical pointwise cost function. As a result this is only useful with low frame rates or for offline mapping. Similarly to this work, we make use of planar patches for registration of RGB-D images, but we employ a compact description of them which abstracts from the 3D points (pixels). This strategy allows to perform faster inter-frame registration in real-time (30 Hz). Also, our technique does not require any initial estimation and furthermore, it can register frames further apart, so that it can be applied for re-localization and loop closure detection. In the context of omnidirectional RGB-D data, probably the first mapping system to create large models is that of [Meilland et al., 2010], which is intended for urban autonomous navigation. This work was extended in [Meilland et al., 2011] to create dense representations of large environments which are used later for robot localization with a regular monocular camera. Recently, another omnidirectional RGB-D sensor rig, very similar to the one presented in this thesis, was built by [Schwarz and Behnke, 2014] to perform navigation in rough terrain. Besides mapping and navigation, om- 120 Chapter 5. A SLAM system for omnidirectional RGB-D sensors nidirectional RGB-D images provide very rich information for SLAM. However, it still constitutes an open problem where the main question is how to manage the big volume of data captured by the sensors. The approach to SLAM presented here is based on PbMaps, which are used as a descriptor to solve quickly the registration of omnidirectional RGB-D frames. Other SLAM approaches can be found in the literature based on planar patch features extracted from a rotating laser scanner [Weingarten and Siegwart, 2006; Pathak et al., 2010a]. These employ a probabilistic framework to build a map of planar patches which is updated at a low frame rate limited by the frequency of the sensor. The solution proposed here differs from those in a few aspects, mainly in the mapping approach, which is based on a nested structure of keyframes with different topological levels to allow for large scale operation with efficient re-localization and loop-closure. Also, our localization strategy takes into account the spatial relations of neighbouring planes for higher robustness, and finally, the depth and intensity information is exploited through pixelwise registration in a back-end process to refine the keyframe’s poses through pose-graph optimization. 5.1.2 Contribution We present a new sensor rig to capture omnidirectional RGB-D images at video rate (30 Hz), and a new SLAM system employing such omnidirectional RGB-D data. This novel device has important prospective applications for scene reconstruction and mobile robotics, including SLAM. The SLAM approach proposed here is based on hybrid metric-topological mapping, where localization is achieved by PbMap registration. Its main advantage comes from the rapid online registration of spherical RGB-D images using a compact plane-based description of the scene. The map is organized in a metric-topological structure of keyframes which is rearranged dynamically as new observations are available. This map structure permits to perform efficient relocalization and loop closure with sub-linear computation time on the map size. Also, the global consistency of the map is improved through pose-graph optimization, for which the connections between keyframes are refined by dense RGB-D alignment. This dense alignment method is a modified version of a previous method to take into account occlusions and thus, be able to register frames that are further apart. Next, we present the details of our sensor set-up and the acquisition of spherical images, analysing the pros and cons between the different alternatives to obtain spherical RGB-D images. Then, we describe our SLAM approach (section 5.3), where we detail: the localization technique based on fast registration of PbMaps and dense RGB-D alignment; the mapping process, which is based on a hierarchical structure of keyframes; and the loop closure approach. Some preliminary experiments are presented next within home and office environments (section 5.4). Finally, we expose the conclusions of this work and advance some lines of future research. 5.2. Omnidirectional RGB-D device 121 Figure 5.1: Omnidirectional RGB-D camera rig. 5.2 Omnidirectional RGB-D device 5.2.1 Sensor set-up Our device for omnidirectional RGB-D acquisition is composed of 8 Asus Xtion Pro Live (Asus XPL) sensors, which are mounted vertically in a radial configuration at an angle of 45◦as shown in figure 5.1. An example of the images captured by this sensor is shown in figure 5.2. This device is connected to a computer through two PCIe cards with 4 USB ports each. This set-up permits to capture 360◦field of view in the horizontal plane with no overlap among sensors, avoiding problems of interference in the infrared images (the vertical FOV of the Asus XPL sensor is 45◦). The vertical FOV of our device correspond to the horizontal FOV of Asus XPL, being 63◦and its maximum resolution is 3840 x 640 (2.46 Mpx). The whole system works at 30 Hz without synchronization between the different sensors. The latter is not an issue here since the system is mounted on a robot moving at a maximum speed of 1 m/s, what permits to approximate the reconstructed spherical images considering that the 8 pairs of RGB and depth images are taken simultaneously. Previous alternatives to capture omnidirectional depth or RGB-D images include 3D LIDAR (like e.g. Velodyne), and multicamera rigs. Table 5.1 shows a comparison between these options and the device proposed here. Regarding the 3D LIDAR, it still constitutes a very expensive option, so that its use has been mainly limited to complex projects in the field of autonomous cars. This sensor can provide images of about 1 Mpx at 15 Hz with a vertical FOV (26.8◦). This reduced vertical FOV is more amenable for outdoor applications. Also, in order to obtain RGB-D images, the radiometric information must be captured with a separate sensor, requiring calibration between both [Mirzaei et al., 2012]. A different option to obtain spherical RGB-D images through a rig of RGB cameras was presented in [Meilland et al., 2010] which reconstructs the scene based on dense stereo matching. In this way, the corresponding intensity and depth images are available with no need of further geo- 128 Chapter 5. A SLAM system for omnidirectional RGB-D sensors result of specular reflections for instance. On the other hand, the depth consistency minimizes the cost FD= n ∑ i=1 ηHUB (D(w(T(x);P∗ i))−kT(x)P∗ ik)2(5.3) where Dis the depth source image and k·kis the L2-norm operator. This cost function is equivalent to the formulation of point-to-plane ICP with projective lookup. The optimization of such RGB-D image alignment is computationally demanding because all the pixels have to be reprojected along several iterations of the method. In order to speed-up the estimation, we only consider the salient pixels of both intensity and depth images (i.e. pixels with high gradient on the image) since the rest of the pixels provide little or no information. The resulting speed-up is directly related to the proportion of pixels used, which in our case has been manually set to 10 %, qualitatively producing similar registration results in our experiments. The cost functions above have been used for registration of both: projective RGBD images like those captured by Kinect [Kerl et al., 2013b], and spherical RGBD images as in [Meilland et al., 2010]. The only difference between them lies in the warping function which is specific for each case. In any case, the images to be registered are supposed to be taken from very close positions, so that the possible occlusions are neglected. This is not the case here, where keyframes which are further apart are to be registered, so that the co-visibility of these should be enough to be able to compute the alignment, but occlusions must be handled as they may introduce important deviations in the registration. In order to cope with this situation, a depthbuffer of the projected pixels (as in [Lieberknecht et al., 2011]) is employed here to discard the occluded points from the sums above. Both cost functions for intensity and depth consistency depend on the relative pose xbetween the frames, but they have different scales. Several methods can be found in the literature to weight these two functions. Here, we follow the work of [Kerl et al., 2013a] to weight the intensity and depth errors within a probabilistic approach depending on the error variances, which are assumed to be independent. However, since both errors are considered to be independent, we employ different Huber estimators for each, instead of using a common bivariate weight as in [Kerl et al., 2013a]. Thus, the resulting least squares problem corresponds to a robust maximum likelihood estimation (see appendix A). 5.3.2 Mapping The map creation process consists of building a network of keyframes described by individual PbMaps, where each keyframe is connected to those keyframes near-by with which relative localization is possible through PbMap registration. The map is organized in a structure of local submaps which contain highly related keyframes. These submaps are also connected themselves when there exist connections among their keyframes. Figure 5.7 shows a representation of this structure, where we can see 5.3. SLAM approach 129 the two levels of topological information (local and global). This mapping strategy is based on previous works focused on scalable mapping and navigation in complex environments [Blanco, 2009]. The lower block of figure 5.5 depicts the different actions carried out by our mapping approach. Thus, when a new keyframe is provided by the localization process, the current local map is updated to include this keyframe, establishing also the new connections with the registered keyframes in the current submap and its one-connected submaps, for what dense RGB-D registration is also applied. Each keyframe connection stores a relative pose in SE(3)and its 6×6 covariance matrix which are obtained from the registration stage, storing also a scalar which represents the co-visibility of the pair of keyframes. Following [Blanco et al., 2006] we call this value sensed-space-overlap (SSO), which we define as SSO =Ashared Ashared +Adi f f (5.4) where Ashared represents the total area of the matched planar patches, and Adi f f represents the sum of the areas of the non-matched patches (for both measures we take the average of the two PbMaps,). Thus, the SSO ∈[0,1], where SSO =1 when all the planes in both PbMaps are matched, while SSO =0 when the frames are not registered (no keyframe connection). The SSO is used here to organize the map into a higher level topological structure following the methodology presented in chapter 4. Thus, after a new keyframe is added to the map, the submap arrangement is reevaluated to maintain a structure of local maps containing highly related keyframes (this may result in a larger, equal, or even smaller number of local maps depending on the new keyframe connections). This process affects the current local map and its first order neighbors, and it is performed very quickly since the map division strategy is very fast (see chapter 4) and re-organizing the map only implies re-arranging the keyframe indices. The concept of sensed space overlap also permits to define what we call the most representative keyframe of a local map. This keyframe corresponds to the one with the highest index of shared information ISI, which is computed for each keyframe as the sum of the SSO with all its connections. The ISI can be intuitively seen as the connection score of a keyframe within a submap. For example, considering a local map corresponding to a single room of a building, the highest ISI will generally correspond to a keyframe in the center of the room which observes most of the planes and with the minimum occlusions. Identifying the most representative keyframe has two different advantages: it permits to summarize the information of the map with a reduced number of keyframes, and as a consequence, problems like re-localization or loop closure can be performed more efficiently by considering the most representative keyframes first. Each submap stores also its pose with respect to a global coordinate system, and each keyframe stores its pose relative to the submap’s reference system. Such poses allow to represent all the keyframes unequivocally in a common reference frame. This 130 Chapter 5. A SLAM system for omnidirectional RGB-D sensors Figure 5.7: Hybrid map structure with two topological layers: a higher layer where each node represents a local map of highly related keyframes, and a lower layer with a network of keyframes. The most representative keyframe of each local map (the one with highest ISI) is coloured in green. is useful to build consistent metric maps, e.g. a single point cloud or PbMap combining the information of all keyframes into a global map (see figure 5.9). For that, the poses of both the submaps and the keyframes are obtained from pose-graph optimization of the global and local maps respectively, taking into account all its keyframe connections and their covariances. This graph optimization is carried out using the publicly available library g2o2[Kummerle et al., 2011]. For that, every time a new keyframe is added to the map, whether the topological structure is modified or not, the local map is optimized to update the keyframe positions. Also, loop closure is searched for with every new keyframe, and if it is found, the map division is rearranged and the relative poses of the local maps are updated also through pose-graph optimization similarly as it is done for the keyframe poses inside a local map. Such loop closure algorithm is detailed separately in the next section. 5.3.3 Loop closure Some loop closure approaches have been proposed in the literature which are specially suited for omnidirectional images, like [Chapoulie et al., 2011] which is based on the well known bags of visual words, or [Oliva and Torralba, 2006] which is based on the registration of a global image descriptor. By employing PbMap registration also for loop closure, we reduce the computation burden with respect to the alternatives above, while we maintain the coherence in our SLAM approach which relies mainly on a geometric description and thus it can be applied also to range images. Note however that our loop closure strategy can be combined with those above to gain in robustness. 2www.openslam.org/g2o.html 5.4. Experimental validation 131 The loop closure search is carried out with every new keyframe by identifying the most representative keyframes of the local maps which are nearer to it (excluding the current local map and its first order neighbors which are constantly checked for SLAM). In order to estimate the most likely locations for a loop closure, we compute the relative pose between the current keyframe and each local map together with its covariance (this is done through pose composition among the different reference frames). Then, the ratio between the squared root of the maximum eigenvalue of the covariance of the relative translation and the norm of such translation (i.e. the Euclidean distance), provides a comparative measure of how likely is the current frame to be near the local map being evaluated. Arranging such measures in decreasing order provides the order in which the different local maps are checked for loop closure. This strategy results in sublinear loop closure computation with respect to the map size. Once the search order has been established, loop closure is tackled in a similar manner to the re-localization problem by registering the PbMap descriptor of the current frame with others from different local maps. If a match is found (the loop is closed), new keyframe connections are searched between the current local map and the one with which the loop has been closed. Then, the pose-graph containing the poses of the different local maps is optimized to include the new constraints of the loop closure in the global map. This optimization is carried out in a similar way as for the keyframes of a local map using g2o [Kummerle et al., 2011]. 5.4 Experimental validation This section presents some preliminary experiments to validate our SLAM system. These experiments are carried out with a wheeled robot with planar movement (see figure 5.8), though the SLAM approach is designed to work with 6 degrees of freedom. The robot has an on-board computer which performs all the computation with an Intel i7 processor with 8 cores at 3.1 GHz and 8Gb of memory. In our experiments we employ a reduced resolution of the omnidirectional RGB-D images with 960x160 pixels, since higher resolutions do not affect significantly the plane segmentation results and they have a higher computational cost. The depth images captured by the sensor are corrected as explained in the section 2.2.2, such correction takes around 2 ms per omnidirectional image. Several sequences are taken exploring different home and office environments, where the robot is remotely guided by a human at a maximum speed of 1 m/s. 5.4.1 Fast scene registration The main feature of our SLAM system is the fast registration of omnidirectional RGB-D images, which is used for camera tracking, re-localization, and keyframe selection. In this section we present experimental results comparing the performance of PbMab based registration with other registration approaches like ICP and dense intensity and depth alignment. Our registration approach requires building the PbMap 132 Chapter 5. A SLAM system for omnidirectional RGB-D sensors Figure 5.8: Robot with the omnidirectional RGB-D sensor. descriptors from the spherical RGB-D images, which implies the segmentation of planar surfaces from the images. Such segmentation is performed efficiently through region growing (see chapter 3), being the most demanding task for registration. This stage is also parallelized to exploit our multi-core processor to segment the planes of the spherical image in less than 20 ms. PbMap matching requires much less computation, in the order of microseconds. Furthermore, both ICP and dense alignment also need a previous preparation to compute the spherical point cloud and the spherical images, respectively, before computing the matching. Table 5.2 presents the average computation time of these three methods for spherical RGB-D image registration, calculated from 1000 consecutive registrations (odometry). For that, both ICP and dense alignment are performed using a pyramid of scales for robustness and efficiency. In this table, we can see how the registration based on PbMap is two orders of magnitude faster than the other two alternatives. Table 5.2: Average RGB-D sphere registration performance of different methods (in seconds). PbMap ICP Dense PbMap construction (s) 0.019 - - Sphere construction (s) - 0.010 0.093 Matching (s) 10−61.53 2.12 Total Registration (s) 0.019 1.54 2.22 5.4. Experimental validation 133 Besides the low computational burden, another important advantage of our registration technique with respect to classic approaches like ICP or dense alignment is that we do not require any initial estimation. Thus, we can register images taken further away, while ICP and dense alignment are limited to shorter distances without a good initial estimation (i.e. considering the identity as the initial transformation). This fact is also illustrated in table 5.3, which shows the average maximum Euclidean distance between the registered frames of the previous sequence. For that, each frame is registered with all the preceding frames until tracking is lost, selecting the last registered frame as the furthest one. Also, our method is better suited to dynamic environments where humans or other elements are constantly moving, since the large planar surfaces taken into account for registration are generally static. Home and office environments are a good example for that, where the humans change their pose, and also the poses of some objects like chairs, but where the scene structure remains unchanged. Table 5.3: Average of the maximum distance for registration with different methods. PbMap ICP Dense Registration dist. (m) 3.4 0.39 0.43 The registration of RGB-D images through PbMap permits to perform odometry estimation of the robot trajectory efficiently. This is done simply by registering the current frame to the previous one (see the video at www.youtube.com/watch?v= 8hzj6qhqpaA). Figure 5.9 shows the trajectory followed by our sensor in one of our exploration sequences in a home environment together with the point clouds from each spherical image superimposed. The consistency of the resulting map indicates that each sphere is registered correctly with respect to the previous one, though yet, we can appreciate the drift in the trajectory which comes as a consequence of the open loop approach. This qualitative experiment shows that despite the compact information extracted for fast registration of the spherical images, the accuracy of registration is still good for many applications. 5.4.2 Keyframe-based SLAM The above results for fast registration are exploited here to perform SLAM based on a multilayer metric-topological map of keyframes. The map is built concurrently while the robot explores different environments, including home and office environments. These experiments are basically a proof of concept for a new robust and efficient SLAM solution from omnidirectional RGB-D data. To our knowledge, this is the first SLAM system using such kind of data and thus, a comparative study cannot be provided here. The operation of our SLAM approach is shown with a video in www.sites. google.com/site/efernandezmoral/projects/rgbd360, where we can see how the map is built while the robot explores the scene, by adding new keyframes when 134 Chapter 5. A SLAM system for omnidirectional RGB-D sensors Figure 5.9: Trajectory of the sensor in a home environment composed of different rooms (the path is about 36 m). they provide new information of the scene. A snapshot from this video is shown in figure 5.10 showing the map as a set of superimposed point clouds extracted from keyframe locations, which are shown with sphere objects. Such keyframes are connected to other nearby keyframes with which they were registered, forming a network which is optimized as a pose-graph. The spheres are shown with different colours representing the different local maps. As we can see, the different local maps correspond to meaningful areas of the environment (i.e. different rooms), making this representation suitable for topological navigation. This representation is also well adapted to large scale SLAM operation since only a local portion of the whole map is managed as the robot moves around the scene. However, experiments on large scale are not shown here due to limitations in the environment where we had access during this thesis (i.e. small buildings). Such a work is left for some future research. From this proof of concept experiments we also see that the whole map is highly consistent thanks to the loop-closure mechanism. Another advantage of our approach that we corroborate in our experiments is the suitability of the maps for variable illumination. This is a direct consequence of using mainly geometric information extracted from depth images which do not depend on the available light (with the exception of direct sunlight). If the proposed representa- 5.5. Discussion 135 tion is to be used for more complex tasks which may require the intensity information (e.g. object recognition), the map can be easily adapted to take new keyframes when the lighting is considerably different to the previous time when that area was mapped, like during day and night. Figure 5.10: Keyframe-map of an office environment. The spheres represent the location where the keyframes were taken. The large spheres are the most representative keyframes of each local map, where different colours are used to represent such local maps. 5.5 Discussion A novel sensor set-up has been proposed here for online acquisition of spherical RGB-D images. This approach has advantages over other alternatives used today in terms of accuracy and real-time spherical image construction for indoor environments, which are specially interesting for mobile robotics. A calibration method for such device is presented, which takes into account the bias of each sensor independently. The proposed calibration method does not require any specific calibration pattern, taking into account the planar structure from the scene to cope with the fact that there is no overlapping between sensors. In order to demonstrate the potential of this device, we show how these images can be registered in real-time by extracting and matching planar surfaces. The proposed map structure has several advantages with respect to previous approaches in the literature: first, the map stores complete information about the scene in a compact fashion; second, it permits fast keyframe registration through the com- 136 Chapter 5. A SLAM system for omnidirectional RGB-D sensors pact PbMap descriptors for localization and loop closure, while dense registration is applied to refine the relative poses between keyframes; third, the map is maintained as a pose-graph which is optimized locally when new keyframes are added, and globally when loop closure is detected; and finally, the topological structure can also be used to define attributes of the scene like rooms, or to recognize places. The next step in our future research is focused to dynamic SLAM in scenarios that change constantly (e.g. presence of people moving, who also modify the objects present in the environment). For that, we plan to extract semantic cues in the scene that will be used for detecting changes in the scene, and so to update the existing map when necessary. Chapter 6 Conclusions This thesis has addressed different problems related to the topic of localization and mapping for mobile robotics. The research community has dedicated important effort to this topic and an extensive literature can be found around it. However, most approaches have still important limitations, mainly to cope with large scale and dynamic environments, and to work in a wider range of conditions and scenarios. In this context, several contributions have been presented in this thesis for calibrating sensor rigs, for efficient and compact map representations, and for fast and robust localization in such maps. Localization and mapping in mobile robotics is often addressed using a combination of sensors, in which case, these must be calibrated to refer all the data to a common frame of reference. The particular problem of calibrating a rig of range sensors has been previously solved only for very particular conditions. In this thesis, we have proposed a new methodology that permits to calibrate any combination of 2D and 3D range sensors in arbitrary configurations from the observation of common planar surfaces. This methodology is easy to apply, not requiring any special calibration pattern, and it is applicable to different configurations of mobile robots and autonomous cars. We have also presented a new mapping approach based on planar surfaces which can be easily segmented from range or RGB-D images. This plane-based map (PbMap) is particularly well suited for indoor scenarios, and has the advantage of being a very compact and still a descriptive representation which is useful to perform real-time place recognition and loop closure. A fast localization approach has been proposed to register contexts of planes by matching planar features taking into account their geometric relationships. This solution performs significantly faster than previous approaches. Also, a hybrid mapping strategy has been presented to deal with large scale SLAM and with navigation in complex environments. This approach organizes the map into local maps with highly related observations, permitting the abstraction of metric information unnecessary at the current robot location. For that, the map is dynamically organized in a metric-topological structure according to the sensor obser137