We present incremental smoothing and mapping (iSAM), a novel approach to the simultaneous localization and mapping problem that is based on fast incremental matrix factorization. iSAM provides an efficient and exact solution by updating a QR factorization of the naturally sparse smoothing information matrix, therefore recalculating only the matrix entries that actually change. iSAM is efficient even for robot trajectories with many loops as it avoids unnecessary fill-in in the factor matrix by periodic variable reordering. Also, to enable data association in real-time, we provide efficient algorithms to access the estimation uncertainties of interest based on the factored information matrix. We systematically evaluate the different components of iSAM as well as the overall algorithm using various simulated and real-world datasets for both landmark and pose-only settings.

Abstract — Learning models of the environment is one of the fundamental tasks of mobile robots since maps are needed for a wide range of robotic applications, such as navigation and transportation tasks, service robotic applications, and several others. In the past, numerous efficient approaches to map learning have been proposed. Most of them, however, assume that the robot lives on a plane. In this paper, we present a highly efficient maximum likelihood approach that is able to solve 3D as well as 2D problems. Our approach addresses the so-called graph-based formulation of the simultaneous localization and mapping (SLAM) and can be seen as an extension of Olson’s algorithm [27] towards non-flat environments. It applies a novel parameterization of the nodes of the graph that significantly improves the performance of the algorithm and can cope with arbitrary network topologies. The latter allows us to bound the complexity of the algorithm to the size of the mapped area and not to the length of the trajectory. Furthermore, our approach is able to appropriately distribute the roll, pitch and yaw error over a sequence of poses in 3D mapping problems. We implemented our technique and compared it to multiple other graph-based SLAM solutions. As we demonstrate in simulated and in real world experiments, our method converges faster than the other approaches and yields accurate maps of the environment. I.

Data association is one of the core problems of simultaneous localization and mapping (SLAM), and it requires knowledge about the uncertainties of the estimation problem in the form of marginal covariances. However, it is often difficult to access these quantities without calculating the full and dense covariance matrix, which is prohibitively expensive. We present a dynamic programming algorithm for efficient recovery of the marginal covariances needed for data association. As input we use a square root information matrix as maintained by our incremental smoothing and mapping (iSAM) algorithm. The contributions beyond our previous work are an improved algorithm for recovering the marginal covariances and a more thorough treatment of data association now including the joint compatibility branch and bound (JCBB) algorithm. We further show how to make information theoretic decisions about measurements before actually taking the measurement, therefore allowing a reduction in estimation complexity by omitting uninformative measurements. We evaluate our work on simulated and real-world data. Key words: data association, smoothing, simultaneous localization and mapping 1.

Abstract—In this paper we describe a system that can carry out SLAM in large indoor and outdoor environments using a stereo pair moving with 6DOF as the only sensor. Unlike current visual SLAM systems that use either bearing-only monocular information or 3D stereo information, our system accommodates both monocular and stereo. Textured point features are extracted from the images and stored as 3D points if seen in both images with sufficient disparity, or stored as inverse depth points otherwise. This allows the system to map both near and far features: the first provide distance and orientation, and the second orientation information. Unlike other vision only SLAM systems, stereo does not suffer from ’scale drift ’ because of unobservability problems, and thus no other information such as gyroscopes or accelerometers is required in our system. Our SLAM algorithm generates sequences of conditionally independent local maps that can share information related to the camera motion and common features being tracked. The system computes the full map using the novel Conditionally Independent Divide and Conquer algorithm, which allows constant time operation most of the time, with linear time updates to compute the full map. To demonstrate the robustness and scalability of our system, we show experimental results in indoor and outdoor urban environments of 210m and 140m loop trajectories, with the stereo camera being carried in hand by a person walking at normal walking speeds of 4 − 5km/hour.

Abstract — We present an algorithm for pose estimation using fixed-lag smoothing. We show that fixed-lag smoothing enables inclusion of measurements from multiple asynchronous measurement sources in an optimal manner. Since robots usually have a plurality of uncoordinated sensors, our algorithm has an advantage over filtering-based estimation algorithms, which cannot incorporate delayed measurements optimally. We provide an implementation of the general fixed-lag smoothing algorithm using square root smoothing, a technique that has recently become prominent. Square root smoothing uses fast sparse matrix factorization and enables our fixed-lag pose estimation algorithm to run at upwards of 20 Hz. Our algorithm has been extensively tested over hundreds of hours of operation on a robot operating in outdoor environments. We present results based on these tests that verify our claims using wheel encoders, visual odometry, and GPS as sensors.

Abstract — This paper introduces a Square Root Unscented Kalman Filter (SRUKF) solution to the problem of performing visual Simultaneous Localization and Mapping (SLAM) using a single camera. Several authors have proposed the conventional UKF for SLAM to improve the handling of non-linearities compared with the more widely used EKF, but at the expense increasing computational complexity from O(N 2) to O(N 3) in the map size, making it unattractive for video-rate application. Van der Merwe and Wan’s general SRUKF delivers identical results to a general UKF along with computational savings, but remains O(N 3) overall. This paper shows how the SRUKF for the SLAM problem can be re-posed with O(N 2) complexity, matching that of the EKF. The paper also shows how the method of inverse depth feature initialization developed by Montiel et al. for the EKF can be reformulated to work with the SRUKF. Experimental results confirm that the SRUKF and the UKF produce identical estimates, and that the SRUKF is more consistent than the EKF. Although the complexity is the same, the SRUKF remains more expensive to compute. I.

In this paper, we address the problem of creating an objective benchmark for evaluating SLAM approaches. We propose a framework for analyzing the results of a SLAM approach based on a metric for measuring the error of the corrected trajectory. This metric uses only relative relations between poses and does not rely on a global reference frame. This overcomes serious shortcomings of approaches using a global reference frame to compute the error. Our method furthermore allows us to compare SLAM approaches that use different estimation techniques or different sensor modalities since all computations are made based on the corrected trajectory of the robot. We provide sets of relative relations needed to compute our metric for an extensive set of datasets frequently used in the robotics community. The relations have been obtained by manually matching laser-range observations to avoid the errors caused by matching algorithms. Our benchmark framework allows the user to easily analyze and objectively compare different SLAM approaches.

This paper presents a new local submap joining algorithm for building large-scale feature based maps. The algorithm is based on the recently developed Sparse Local Submap Joining Filter (SLSJF) and uses multiple iterations to improve the estimate and hence is called Iterated SLSJF (I-SLSJF). The input to the I-SLSJF algorithm is a sequence of local submaps. The output of the algorithm is a global map containing the global positions of all the features as well as all the robot start/end poses of the local submaps. In the submap joining step of I-SLSJF, whenever the change of state estimate computed by an Extended Information Filter (EIF) is larger than a predefined threshold, the information vector and the information matrix is recomputed as a sum of all the local map contributions. This improves the accuracy of the estimate as well as avoids the possibility that the Jacobian with respect to the same feature gets evaluated at different estimate values, which is one of the major causes of inconsistency for EIF/EKF algorithms. Although the computational cost of I-SLSJF is higher than that of SLSJF, the algorithm can still be implemented efficiently due to the exactly sparseness of the information matrix. The new algorithm is compared with EKF SLAM and SLSJF using both computer simulation and experimental examples. 1

Linköping 2011This is a Swedish Licentiate’s Thesis. Swedish postgraduate education leads to a Doctor’s degree and/or a Licentiate’s degree. A Doctor’s Degree comprises 240 ECTS credits (4 years of full-time studies). A Licentiate’s degree comprises 120 ECTS credits, of which at least 60 ECTS credits constitute a Licentiate’s thesis.

Quite a number of approaches for solving the simultaneous localization and mapping (SLAM) problem exist by now. Some of them have recently been extended to mapping environments with six-degree-of-freedom poses, yielding 6D SLAM approaches. To demonstrate the capabilities of the respective algorithms, it is common practice to present generated maps and successful loop closings in large outdoor environments. Unfortunately, it is nontrivial to compare different 6D SLAM approaches objectively, because ground truth data about the outdoor environments used for demonstration are typically unavailable. We present a novel benchmarking method for generating the ground truth data based on reference maps. The method is then demonstrated by comparing the absolute performance of some previously existing 6D SLAM algorithms that build a large urban outdoor