In this paper we defend the superior scalability of the Relative Bundle Adjustment (RBA) framework for tackling with the SLAM problem. Although such a statement was already done with the introduction of the sliding window (SW) solution to RBA [16], we claim that the map extension that can be maintained locally consistent for some fixed computational cost critically depends on the specific pattern in which new keyframes are connected to previous ones. By rethinking from scratch what we call loop closures in relative coordinates we will show the unexploited flexibility of the RBA framework, which allows us a continuum of strategies from pure relative BA to hybrid submapping with local maps. In this work we derive a systematic way of constructing the problem graph which lies close to submapping and which generates graphs that can be solved more efficiently than those built as previously reported in the literature. As a necessary tool we also present an algorithm for incrementally updating all the spanning-trees demanded by any efficient solution to RBA. Under weak assumptions on the map, and implemented on carefully designed data structures, it is demonstrated to run in bounded time, no matter how large the map becomes. We also present experiments with a synthetic dataset of 55K keyframes in a world of 4.3M landmarks. Our C++ implementation has been released as open source.

This paper presents ERODE, an efficient outlier detector with a quality similar to that of standard RANSAC but at a fraction of its computational cost. In contrast to RANSAC-based methods which follow a hypothesis-and-verify approach, ERODE employs instead the whole set of observations together with a robust kernel to perform robustified least-squares minimization. Our proposal has important practical applications among computer vision problems, which we demonstrate with stereovisual odometry experiments with both simulated and real data.

The most common criteria to determine data association rely on minimizing the squared Mahalanobis distance (SMD) between observations and predictions. We hold that the SMD is just a heuristic, while the alternative matching likelihood is the optimal statistic to be maximized. Thorough experiments undoubtedly confirm this idea, with false positive reductions of up to 16%.

Gas distribution modelling constitutes an ideal application area for mobile robots, which - as intelligent mobile gas sensors - offer several advantages compared to stationary sensor networks. In this paper we propose the Kernel DM+V algorithm to learn a statistical 2-d gas distribution model from a sequence of localized gas sensor measurements. The algorithm does not make strong assumptions about the sensing locations and can thus be applied on a mobile robot that is not primarily used for gas distribution monitoring, and also in the case of stationary measurements. Kernel DM+V treats distribution modelling as a density estimation problem. In contrast to most previous approaches, it models the variance in addition to the distribution mean. Estimating the predictive variance entails a significant improvement for gas distribution modelling since it allows to evaluate the model quality in terms of the data likelihood. This offers a solution to the problem of ground truth evaluation, which has always been a critical issue for gas distribution modelling. Estimating the predictive variance also provides the means to learn meta parameters and to suggest new measurement locations based on the current model. We derive the Kernel DM+V algorithm and present a method for learning the hyper-parameters. Based on real world data collected with a mobile robot we demonstrate the consistency of the obtained maps and present a quantitative comparison, in terms of the data likelihood of unseen samples, with an alternative approach that estimates the predictive variance.

The lack of a parameterized observation model in robot localization using occupancy grids requires the application of sampling-based methods, or particle filters. This work addresses the problem of optimal Bayesian filtering for dynamic systems with observation models that cannot be approximated properly as any parameterized distribution, which includes localization and SLAM with occupancy grids. By integrating ideas from previous works on adaptive sample size, auxiliary particle filters, and rejection sampling, we derive a new particle filter algorithm that enables the usage of the optimal proposal distribution to estimate the true posterior density of a non-parametric dynamic system. Our solution avoids approximations adopted in previous approaches at the cost of a higher computational burden. We present simulations and experimental results for a real robot showing the suitability of the method for localization.

Range-only SLAM (RO-SLAM) represents a difficult problem due to the inherent ambiguity of localizing either the robot or the beacons from distance measurements only. Most previous approaches to this problem employ non-probabilistic batch optimizations or delay the initialization of new beacons within a probabilistic filter until a good estimate is available. The contribution of this work is the formulation of RO-SLAM as an online Bayesian estimation process based on a Rao-Blackwellized particle filter. The conditional distribution for each beacon is initialized using an additional particle filter which, eventually, is transformed into an extended Kalman filter when the uncertainty becomes sufficiently small. This approach allows the introduction of new beacons without either delay or any special non-probabilistic processing. We validate our proposal with experiments for both simulated and real datasets.

This work addresses range-only SLAM (RO-SLAM) as the Bayesian inference problem of sequentially tracking a vehicle while estimating the location of a set of beacons without any prior information. The only assumptions are the availability of odometry and a range sensor able of identifying the different beacons. We propose exploiting the conditional independence between the position distributions of each beacon within a Rao-Blackwellized Particle Filter (RBPF) for maintaining independent Sum of Gaussians (SOGs) for each beacon. Unlike other approaches, it is shown then that a proper probabilistic observation model can be derived for online operation with no need for delayed initializations. We provide a rigorous statistical comparison of this proposal with previous work of the authors where a Monte-Carlo approximation was employed instead for the conditional densities. As verified experimentally, this new proposal represents a significant improvement in accuracy, computation time, and robustness against outliers.

This paper introduces a new approach to simultaneous localization and mapping (SLAM) that pursues robustness and accuracy in large-scale environments. Like most successful works on SLAM, we use Bayesian filtering to provide a probabilistic estimation that can cope with uncertainty in the measurements, the robot pose, and the map. Our approach is based on the reconstruction of the robot path in a hybrid discrete-continuous state space, which naturally combines metric and topological maps. There are two fundamental characteristics that set this paper apart from previous ones: 1) the use of a unified Bayesian inference approach both for the metrical and the topological parts of the problem and 2) the analytical formulation of belief distributions over hybrid maps, which allows us to maintain the spatial uncertainty in large spaces more accurately and efficiently than in previous works. We also describe a practical implementation that aims for real-time operation. Our ideas have been validated by promising experimental results in large environments (up to 30 000 m$^2$, a 2 km robot path) with multiple nested loops, which could hardly be managed appropriately by other approaches.

Most successful works in simultaneous localization and mapping (SLAM) aim to build a metric map under a probabilistic viewpoint employing Bayesian filtering techniques. This work introduces a new hybrid metric-topological approach, where the aim is to reconstruct the path of the robot in a hybrid continuous-discrete state space which naturally combines metric and topological maps. Our fundamental contributions are: (i) the estimation of the topological path, an improvement similar to that of Rao-Blackwellized particle filters (RBPF) and FastSLAM in the field of metric map building; and (ii) the application of grounded methods to the abstraction of topology (including loop closure) from raw sensor readings. It is remarkable that our approach could be still represented as a Bayesian inference problem, becoming an extension of purely metric SLAM. Besides providing the formal definitions and the basics for our approach, we also describe a practical implementation aimed to real-time operation. Promising experimental results mapping large environments with multiple nested loops (~30.000 m2, ~2Km robot path) validate our work.

One of the main elements of probabilistic localization and SLAM is the probabilistic sensor model (also known as the observation likelihood function). However, when dealing with very accurate sensors like laser range scanners, most approaches artificially inflate the uncertainty in the range measurements and assume conditional independence between the individual ranges of the scan to compute this likelihood function. In this paper we propose an alternative method where each sample in the scan can contribute an accurate estimation according to both its real uncertainty and its compatible correspondences with a given map. These likelihood values of individual measurements are fused via a linear opinion pool (LOP), a method from consensus theory. Our approach results in a more precise likelihood function than others and excels in robustness in dynamic environments. To validate our research we provide systematic comparisons with other proposals in the context of localization with particle filters.

This work aims at improving real-time motion control and dead-reckoning of wheeled skid-steer vehicles by considering the effects of slippage, but without introducing the complexity of dynamics computations in the loop. This traction scheme is found both in many off-the-shelf mobile robots due to its mechanical simplicity and in outdoor applications due to its maneuverability. In previous works, we reported a method to experimentally obtain an optimized kinematic model for skid-steer tracked vehicles based on the boundedness of the instantaneous centers of rotation (ICRs) of treads on the motion plane. This paper provides further insight on this method, which is now proposed for wheeled skid-steer vehicles. It has been successfully applied to a popular research robotic platform, pioneer P3-AT, with different kinds of tires and terrain types.

Recently, hybrid maps that combine metric and topological world information have been proposed as a powerful representation of mobile robot environments. Among others, these maps are of special interest for efficiently managing large-scale environments, and for accurate localization. For achieving that, local geometric maps are stored in the nodes of a graph-based global map. In this paper we present a novel approach for automatically obtaining those local maps from observations. The method considers the space sensed in each observation as a node of a graph with arcs representing the space overlap between observations. The recursive partition (cut) of this graph produces groups of strongly connected nodes from which consistent local maps for accurate localization are derived. The proposed partition technique is well-grounded in the spectral graph theory of, and it is formulated for any type of sensor observation. We depict an implementation for grouping 2D laser scans, and show experimental results with real data that demonstrate the performance of the method

In Bayesian based approaches to mobile robot simultaneous localization and mapping, Rao-Blackwellized particle filters (RBPF) enable the efficient estimation of the posterior belief over robot poses and the map. These particle filters have been recently adopted by many exploration approaches, to whom a central issue is measuring the certainty inherent to a given estimation in order to be able to select robot actions that increase it. In this paper we propose a new certainty measurement grounded in information theory that unifies the two kinds of uncertainty which are intrinsic to SLAM: in the robot pose and in the map content. Most previous works have considered only one of them or a weighted average. Our method combines them more appropriately by first building an expected map (EM) which condenses all the current map hypotheses and then computing its mean information (MI) - an entropy derived measurement that quantifies the inconsistencies in the EM. Experimental results comparing our method (EMMI) with others verify its correctness and its better behavior for detecting the decrease in certainty when the robot enters unexplored areas and its increase after closing a loop

The reactive navigation of a non-holonomic mobile robot implies selecting at each instant of time a motion command satisfying two conditions: to avoid collisions and to comply with the robot non-holonomic constraints. Most proposed reactive navigation approaches deal with these requirements simultaneously in an indivisible way. This paper proposes a clear separation of these problems by introducing a representation space where a robot losses its kinematics restrictions and can be dealt as a "free-flying-point." The collision avoidance can therefore be solved by existing holonomic methods, which are able to steer non-holonomic, any-shaped robots when applied in this space, named the trajectory parameter space (TP-space). We also formalize the transformation between this space and the robot physical space introducing the parameterized trajectory generator (PTG), a translation between both spaces by means of a family of parameterized trajectories. This formalization is addressed in a generalized form to allow us deriving any number of different transformations. Unlike previous non-holonomic approaches that use just one single transformation, the proposed method considers a variety of them simultaneously which becomes an obvious improvement to reactive approaches: each one can detect a collision-free path that the others can not. We present some experimental results to show the suitability of our method and its advantages compared with traditional approaches