SLAM++-A highly efficient and temporally scalable incremental SLAM framework

Abstract

The most common way to deal with the uncertainty present in noisy sensorial perception and action is to model the problem with a probabilistic framework. Maximum likelihood estimation is a well-known estimation method used in many robotic and computer vision applications. Under Gaussian assumption, the maximum likelihood estimation converts to a nonlinear least squares problem. Efficient solutions to nonlinear least squares exist and they are based on iteratively solving sparse linear systems until convergence. In general, the existing solutions provide only an estimation of the mean state vector, the resulting covariance being computationally too expensive to recover. Nevertheless, in many simultaneous localization and mapping (SLAM) applications, knowing only the mean vector is not enough. Data association, obtaining reduced state representations, active decisions and next best view are only a few of the applications that require fast state covariance recovery. Furthermore, computer vision and robotic applications are in general performed online. In this case, the state is updated and recomputed every step and its size is continuously growing, therefore, the estimation process may become highly computationally demanding. This paper introduces a general framework for incremental maximum likelihood estimation called SLAM++, which fully benefits from the incremental nature of the online applications, and provides efficient estimation of both the mean and the covariance of the estimate. Based on that, we propose a strategy for maintaining a sparse and scalable state representation for large scale mapping, which uses information theory measures to integrate only informative and non-redundant contributions to the state representation. SLAM++ differs from existing implementations by performing all the matrix operations by blocks. This led to extremely fast matrix manipulation and arithmetic operations used in nonlinear least squares. Even though this paper tests SLAM++ efficiency on SLAM problems, its applicability remains general.

Graph SLAM Sparsification With Populated Topologies Using Factor Descent Optimization

“…Until convergence, the optimal step ∆x * is used to update the expectations h k (x) and the Jacobians J k and (2) is solved again. Current methods use Cholesky [2,15] or QR [1,16] matrix factorizations to solve for ∆x * . Incremental methods [1,2], update the problem directly on the factorized matrix obtaining important speedups.…”

“…Current methods use Cholesky [2,15] or QR [1,16] matrix factorizations to solve for ∆x * . Incremental methods [1,2], update the problem directly on the factorized matrix obtaining important speedups. However, linearization errors are accumulated and the problem should be often rebuilt partially or completely.…”

“…2 (a). Optionally, this cropped problem can be solved using (2). Then, the new solution can be used henceforth, yielding slightly better results especially in on-line cases [12].…”

“…To tackle such growing resource demands, efforts have been invested mainly in two directions: by improving the efficiency of the algorithms, and by reducing the problem size. Despite recent improvements in algorithm efficiency [1,2], the later is still of concern, as the complexity of the solution is always linked to the size of the problem. Therefore, methods for reducing the problem size while keeping as much information as possible are essential, especially for large SLAM experiments.…”

Non-Parametric Bayesian State Space Estimator for Negative Information

“…As a result, multiple Graph-SLAM solvers have been released such as Large ScaleDirected monocular SLAM (LSD-SLAM) (Engel et al, 2014), which can handle environments with different scales and is able to build large indoor and outdoor density maps, and ORB-SLAM (Mur-Artal et al, 2015;Mur-Artal and Tardos, 2016), which uses ORB (Rublee et al, 2011) features extracted from keyframes. Recent developments in the context of vision-SLAM efforts have focused on efficiency and extraction of the pose uncertainty (Ila et al, 2017) as Graph-SLAM methods generally only find the mean trajectory of the robot and not its variance. Graph-SLAM approaches mostly differ in the type of feature extraction methods (known as the front-end) while retain the same non-linear solver (bundle adjustment) such as g2o (Kuemmerle et al, 2011).…”