Graph SLAM sparsification with populated topologies using factor descent optimization

© 20xx IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works.Current solutions to the simultaneous localization and mapping (SLAM) problem approach it as the optimization of a graph of geometric constraints. Scalability is achieved by reducing the size of the graph, usually in two phases. First, some selected nodes in the graph are marginalized and then, the dense and non-relinearizable result is sparsified. The sparsified network has a new set of relinearizable factors and is an approximation to the original dense one. Sparsification is typically approached as a Kullback-Liebler divergence (KLD) minimization between the dense marginalization result and the new set of factors. For a simple topology of the new factors, such as a tree, there is a closed form optimal solution. However, more populated topologies can achieve a much better approximation because more information can be encoded, although in that case iterative optimization is needed to solve the KLD minimization. Iterative optimization methods proposed by the state-of-art sparsification require parameter tuning which strongly affect their convergence. In this paper, we propose factor descent and non-cyclic factor descent, two simple algorithms for SLAM sparsification that match the state-of-art methods without any parameters to be tuned. The proposed methods are compared against the state of the art with regards to accuracy and CPU time, in both synthetic and real world datasets.Peer ReviewedPostprint (author's final draft

Pose-graph SLAM sparsification using factor descent

Since state of the art simultaneous localization and mapping (SLAM) algorithms are not constant time, it is often necessary to reduce the problem size while keeping as much of the original graph’s information content. In graph SLAM, the problem is reduced by removing nodes and rearranging factors. This is normally faced locally: after selecting a node to be removed, its Markov blanket sub-graph is isolated, the node is marginalized and its dense result is sparsified. The aim of sparsification is to compute an approximation of the dense and non-relinearizable result of node marginalization with a new set of factors. Sparsification consists on two processes: building the topology of new factors, and finding the optimal parameters that best approximate the original dense distribution. This best approximation can be obtained through minimization of the Kullback-Liebler divergence between the two distributions. Using simple topologies such as Chow-Liu trees, there is a closed form for the optimal solution. However, a tree is oftentimes too sparse and produces bad distribution approximations. On the contrary, more populated topologies require nonlinear iterative optimization. In the present paper, the particularities of pose-graph SLAM are exploited for designing new informative topologies and for applying the novel factor descent iterative optimization method for sparsification. Several experiments are provided comparing the proposed topology methods and factor descent optimization with state-of-the-art methods in synthetic and real datasets with regards to approximation accuracy and computational cost.Peer ReviewedPostprint (author's final draft

Information metrics for localization and mapping

Decades of research have made possible the existence of several autonomous systems that successfully and efficiently navigate within a variety of environments under certain conditions. One core technology that has allowed this is simultaneous localization and mapping (SLAM), the process of building a representation of the environment while localizing the robot in it. State-of-the-art solutions to the SLAM problem still rely, however, on heuristic decisions and options set by the user. In this thesis we search for principled solutions to various aspects of the localization and mapping problem with the help of information metrics. One such aspect is the issue of scalability. In SLAM, the problem size grows indefinitely as the experiment goes by, increasing computational resource demands. To maintain the problem tractable, we develop methods to build an approximation to the original network of constraints of the SLAM problem by reducing its size while maintaining its sparsity. In this thesis we propose three methods to build the topology of such approximated network, and two methods to perform the approximation itself. In addition, SLAM is a passive application. It means, it does not drive the robot. The problem of driving the robot with the aim of both accurately localizing the robot and mapping the environment is called active SLAM. In this problem two normally opposite forces drive the robot, one to new places discovering unknown regions and another to revisit previous configurations to improve localization. As opposed to heuristics, in this thesis we pose the problem as the joint minimization of both map and trajectory estimation uncertainties, and present four different active SLAM approaches based on entropy-reduction formulation. All methods presented in this thesis have been rigorously validated in both synthetic and real datasets.Dècades de recerca han fet possible l’existència de nombrosos sistemes autònoms que naveguen eficaçment i eficient per varietat d’entorns sota certes condicions. Una de les principals tecnologies que ho han fet possible és la localització i mapeig simultanis (SLAM), el procés de crear una representació de l’entorn mentre es localitza el robot en aquesta. De tota manera, els algoritmes d’SLAM de l’estat de l’art encara basen moltes decisions en heurístiques i opcions a escollir per l’usuari final. Aquesta tesi persegueix solucions fonamentades per a varietat d’aspectes del problema de localització i mappeig amb l’ajuda de mesures d’informació. Un d’aquests aspectes és l’escalabilitat. En SLAM, el problema creix indefinidament a mesura que l’experiment avança fent créixer la demanda de recursos computacionals. Per mantenir el problema tractable, desenvolupem mètodes per construir una aproximació de la xarxa de restriccions original del problema d’SLAM, reduint així el seu tamany a l’hora que es manté la seva naturalesa dispersa. En aquesta tesi, proposem tres métodes per confeccionar la topologia de l’approximació i dos mètodes per calcular l’aproximació pròpiament. A més, l’SLAM és una aplicació passiva. És a dir que no dirigeix el robot. El problema de guiar el robot amb els objectius de localitzar el robot i mapejar l’entorn amb precisió es diu SLAM actiu. En aquest problema, dues forces normalment oposades guien el robot, una cap a llocs nous descobrint regions desconegudes i l’altra a revisitar prèvies configuracions per millorar la localització. En contraposició amb mètodes heurístics, en aquesta tesi plantegem el problema com una minimització de l’incertesa tant en el mapa com en l’estimació de la trajectòria feta i presentem quatre mètodes d’SLAM actiu basats en la reducció de l’entropia. Tots els mètodes presentats en aquesta tesi han estat rigurosament validats tant en sèries de dades sintètiques com en reals

Information metrics for localization and mapping

Aplicat embargament des de la defensa de la tesi fins al 12/2019Decades of research have made possible the existence of several autonomous systems that successfully and efficiently navigate within a variety of environments under certain conditions. One core technology that has allowed this is simultaneous localization and mapping (SLAM), the process of building a representation of the environment while localizing the robot in it. State-of-the-art solutions to the SLAM problem still rely, however, on heuristic decisions and options set by the user. In this thesis we search for principled solutions to various aspects of the localization and mapping problem with the help of information metrics. One such aspect is the issue of scalability. In SLAM, the problem size grows indefinitely as the experiment goes by, increasing computational resource demands. To maintain the problem tractable, we develop methods to build an approximation to the original network of constraints of the SLAM problem by reducing its size while maintaining its sparsity. In this thesis we propose three methods to build the topology of such approximated network, and two methods to perform the approximation itself. In addition, SLAM is a passive application. It means, it does not drive the robot. The problem of driving the robot with the aim of both accurately localizing the robot and mapping the environment is called active SLAM. In this problem two normally opposite forces drive the robot, one to new places discovering unknown regions and another to revisit previous configurations to improve localization. As opposed to heuristics, in this thesis we pose the problem as the joint minimization of both map and trajectory estimation uncertainties, and present four different active SLAM approaches based on entropy-reduction formulation. All methods presented in this thesis have been rigorously validated in both synthetic and real datasets.Dècades de recerca han fet possible l’existència de nombrosos sistemes autònoms que naveguen eficaçment i eficient per varietat d’entorns sota certes condicions. Una de les principals tecnologies que ho han fet possible és la localització i mapeig simultanis (SLAM), el procés de crear una representació de l’entorn mentre es localitza el robot en aquesta. De tota manera, els algoritmes d’SLAM de l’estat de l’art encara basen moltes decisions en heurístiques i opcions a escollir per l’usuari final. Aquesta tesi persegueix solucions fonamentades per a varietat d’aspectes del problema de localització i mappeig amb l’ajuda de mesures d’informació. Un d’aquests aspectes és l’escalabilitat. En SLAM, el problema creix indefinidament a mesura que l’experiment avança fent créixer la demanda de recursos computacionals. Per mantenir el problema tractable, desenvolupem mètodes per construir una aproximació de la xarxa de restriccions original del problema d’SLAM, reduint així el seu tamany a l’hora que es manté la seva naturalesa dispersa. En aquesta tesi, proposem tres métodes per confeccionar la topologia de l’approximació i dos mètodes per calcular l’aproximació pròpiament. A més, l’SLAM és una aplicació passiva. És a dir que no dirigeix el robot. El problema de guiar el robot amb els objectius de localitzar el robot i mapejar l’entorn amb precisió es diu SLAM actiu. En aquest problema, dues forces normalment oposades guien el robot, una cap a llocs nous descobrint regions desconegudes i l’altra a revisitar prèvies configuracions per millorar la localització. En contraposició amb mètodes heurístics, en aquesta tesi plantegem el problema com una minimització de l’incertesa tant en el mapa com en l’estimació de la trajectòria feta i presentem quatre mètodes d’SLAM actiu basats en la reducció de l’entropia. Tots els mètodes presentats en aquesta tesi han estat rigurosament validats tant en sèries de dades sintètiques com en reals.Postprint (published version

Information metrics for localization and mapping

Decades of research have made possible the existence of several autonomous systems that successfully and efficiently navigate within a variety of environments under certain conditions. One core technology that has allowed this is simultaneous localization and mapping (SLAM), the process of building a representation of the environment while localizing the robot in it. State-of-the-art solutions to the SLAM problem still rely, however, on heuristic decisions and options set by the user. In this thesis we search for principled solutions to various aspects of the localization and mapping problem with the help of information metrics. One such aspect is the issue of scalability. In SLAM, the problem size grows indefinitely as the experiment goes by, increasing computational resource demands. To maintain the problem tractable, we develop methods to build an approximation to the original network of constraints of the SLAM problem by reducing its size while maintaining its sparsity. In this thesis we propose three methods to build the topology of such approximated network, and two methods to perform the approximation itself. In addition, SLAM is a passive application. It means, it does not drive the robot. The problem of driving the robot with the aim of both accurately localizing the robot and mapping the environment is called active SLAM. In this problem two normally opposite forces drive the robot, one to new places discovering unknown regions and another to revisit previous configurations to improve localization. As opposed to heuristics, in this thesis we pose the problem as the joint minimization of both map and trajectory estimation uncertainties, and present four different active SLAM approaches based on entropy-reduction formulation. All methods presented in this thesis have been rigorously validated in both synthetic and real datasets.Dècades de recerca han fet possible l’existència de nombrosos sistemes autònoms que naveguen eficaçment i eficient per varietat d’entorns sota certes condicions. Una de les principals tecnologies que ho han fet possible és la localització i mapeig simultanis (SLAM), el procés de crear una representació de l’entorn mentre es localitza el robot en aquesta. De tota manera, els algoritmes d’SLAM de l’estat de l’art encara basen moltes decisions en heurístiques i opcions a escollir per l’usuari final. Aquesta tesi persegueix solucions fonamentades per a varietat d’aspectes del problema de localització i mappeig amb l’ajuda de mesures d’informació. Un d’aquests aspectes és l’escalabilitat. En SLAM, el problema creix indefinidament a mesura que l’experiment avança fent créixer la demanda de recursos computacionals. Per mantenir el problema tractable, desenvolupem mètodes per construir una aproximació de la xarxa de restriccions original del problema d’SLAM, reduint així el seu tamany a l’hora que es manté la seva naturalesa dispersa. En aquesta tesi, proposem tres métodes per confeccionar la topologia de l’approximació i dos mètodes per calcular l’aproximació pròpiament. A més, l’SLAM és una aplicació passiva. És a dir que no dirigeix el robot. El problema de guiar el robot amb els objectius de localitzar el robot i mapejar l’entorn amb precisió es diu SLAM actiu. En aquest problema, dues forces normalment oposades guien el robot, una cap a llocs nous descobrint regions desconegudes i l’altra a revisitar prèvies configuracions per millorar la localització. En contraposició amb mètodes heurístics, en aquesta tesi plantegem el problema com una minimització de l’incertesa tant en el mapa com en l’estimació de la trajectòria feta i presentem quatre mètodes d’SLAM actiu basats en la reducció de l’entropia. Tots els mètodes presentats en aquesta tesi han estat rigurosament validats tant en sèries de dades sintètiques com en reals

Factor descent optimization for sparsification in graph SLAM

© 20xx IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works.In the context of graph-based simultaneous localization and mapping, node pruning consists in removing a subset of nodes from the graph, while keeping the graph’s information content as close as possible to the original. One often tackles this problem locally by isolating the Markov blanket sub-graph of a node, marginalizing this node and sparsifying the dense result. It means computing an approximation with a new set of factors. For a given approximation topology, the factors’ mean and covariance that best approximate the original distribution can be obtained through minimization of the Kullback-Liebler divergence. For simple topologies such as Chow-Liu trees, there is a closed form for the optimal solution. However, a tree is oftentimes too sparse to explain some graphs. More complex topologies require nonlinear iterative optimization. In the present paper we propose Factor Descent, a new iterative optimization method to sparsify the dense result of node marginalization, which works by iterating factor by factor. We also provide a thorough comparison of our approach with state-of-the-art methods in real world datasets with regards to the obtained solution and convergence rates.Peer ReviewedPostprint (author's final draft

ObVi-SLAM: Long-Term Object-Visual SLAM

Robots responsible for tasks over long time scales must be able to localize consistently and scalably amid geometric, viewpoint, and appearance changes. Existing visual SLAM approaches rely on low-level feature descriptors that are not robust to such environmental changes and result in large map sizes that scale poorly over long-term deployments. In contrast, object detections are robust to environmental variations and lead to more compact representations, but most object-based SLAM systems target short-term indoor deployments with close objects. In this paper, we introduce ObVi-SLAM to overcome these challenges by leveraging the best of both approaches. ObVi-SLAM uses low-level visual features for high-quality short-term visual odometry; and to ensure global, long-term consistency, ObVi-SLAM builds an uncertainty-aware long-term map of persistent objects and updates it after every deployment. By evaluating ObVi-SLAM on data from 16 deployment sessions spanning different weather and lighting conditions, we empirically show that ObVi-SLAM generates accurate localization estimates consistent over long-time scales in spite of varying appearance conditions.Comment: 8 pages, 7 figures, 1 table plus appendix with 4 figures and 1 tabl

WOLF: A modular estimation framework for robotics based on factor graphs

© 2022 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes,creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works.This paper introduces WOLF, a C++ estimation framework based on factor graphs and targeted at mobile robotics. WOLF can be used beyond SLAM to handle self-calibration, model identification, or the observation of dynamic quantities other than localization. The architecture of WOLF allows for a modular yet tightly-coupled estimator. Modularity is enhanced via reusable plugins that are loaded at runtime depending on application setup. This setup is achieved conveniently through YAML files, allowing users to configure a wide range of applications without the need of writing or compiling code. Most procedures are coded as abstract algorithms in base classes with varying levels of specialization. Overall, all these assets allow for coherent processing and favor code re-usability and scalability. WOLF can be used with ROS, and is made publicly available and open to collaboration.Peer ReviewedPostprint (author's final draft

Multi-SLAM Systems for Fault-Tolerant Simultaneous Localization and Mapping

Mobile robots need accurate, high fidelity models of their operating environments in order to complete their tasks safely and efficiently. Generating these models is most often done via Simultaneous Localization and Mapping (SLAM), a paradigm where the robot alternatively estimates the most up-to-date model of the environment and its position relative to this model as it acquires new information from its sensors over time. Because robots operate in many different environments with different compute, memory, sensing, and form constraints, the nature and quality of information available to individual instances of different SLAM systems varies substantially. `One-size-fits-all\u27 solutions are thus exceedingly difficult to engineer, and highly specialized systems, which represent the state-of-the-art for most types of deployments, are not robust to operating conditions in which their assumptions are not met. This thesis seeks to investigate an alternative approach to these robustness and universality problems by incorporating existing SLAM solutions within a larger framework supported by planning and learning. The central idea is to combine learned models that estimate SLAM algorithm performance under a variety of sensory conditions, in this case neural networks, with planners designed for planning under uncertainty and partial observability, in this case partially observable Markov decision problems (POMDPs). Models of existing SLAM algorithms can be learned, and these models can then be used online to estimate the performance of a range of solutions to the SLAM problem at hand. The POMDP policy then selects the appropriate algorithm, given the estimated performance, cost of switching methods, and other information. This general approach may also be applicable to many other robotics problems that rely on data-fusion, such as grasp planning, motion planning, or object identification

Towards new sensing capabilities for legged locomotion using real-time state estimation with low-cost IMUs

L'estimation en robotique est un sujet important affecté par les compromis entre certains critères majeurs parmi lesquels nous pouvons citer le temps de calcul et la précision. L'importance de ces deux critères dépend de l'application. Si le temps de calcul n'est pas important pour les méthodes hors ligne, il devient critique lorsque l'application doit s'exécuter en temps réel. De même, les exigences de précision dépendent des applications. Les estimateurs EKF sont largement utilisés pour satisfaire les contraintes en temps réel tout en obtenant une estimation avec des précisions acceptables. Les centrales inertielles (Inertial Measurement Unit - IMU) demeurent des capteurs répandus dnas les problèmes d'estimation de trajectoire. Ces capteurs ont par ailleurs la particularité de fournir des données à une fréquence élevée. La principale contribution de cette thèses est une présentation claire de la méthode de préintégration donnant lieu à une meilleure utilisation des centrales inertielles. Nous appliquons cette méthode aux problèmes d'estimation dans les cas de la navigation piétonne et celle des robots humanoïdes. Nous souhaitons par ailleurs montrer que l'estimation en temps réel à l'aide d'une centrale inertielle à faible coût est possible avec des méthodes d'optimisation tout en formulant les problèmes à l'aide d'un modèle graphique bien que ces méthodes soient réputées pour leurs coûts élevés en terme de calculs. Nous étudions également la calibration des centrales inertielles, une étape qui demeure critique pour leurs utilisations. Les travaux réalisés au cours de cette thèse ont été pensés en gardant comme perspective à moyen terme le SLAM visuel-inertiel. De plus, ce travail aborde une autre question concernant les robots à jambes. Contrairement à leur architecture habituelle, pourrions-nous utiliser plusieurs centrales inertielles à faible coût sur le robot pour obtenir des informations précieuses sur le mouvement en cours d'exécution ?Estimation in robotics is an important subject affected by trade-offs between some major critera from which we can cite the computation time and the accuracy. The importance of these two criteria are application-dependent. If the computation time is not important for off-line methods, it becomes critical when the application has to run on real-time. Similarly, accuracy requirements are dependant on the applications. EKF estimators are widely used to satisfy real-time constraints while achieving acceptable accuracies. One sensor widely used in trajectory estimation problems remains the inertial measurement units (IMUs) providing data at a high rate. The main contribution of this thesis is a clear presentation of the preintegration theory yielding in a better use IMUs. We apply this method for estimation problems in both pedestrian and humanoid robots navigation to show that real-time estimation using a low- cost IMU is possible with smoothing methods while formulating the problems with a factor graph. We also investigate the calibration of the IMUs as it is a critical part of those sensors. All the development made during this thesis was thought with a visual-inertial SLAM background as a mid-term perspective. Firthermore, this work tries to rise another question when it comes to legged robots. In opposition to their usual architecture, could we use multiple low- cost IMUs on the robot to get valuable information about the motion being executed