Maximum likelihood estimation (MLE) is a well-known estimation method used in many robotic and computer vision applications. Under Gaussian assumption, the MLE converts to a nonlinear least squares (NLS) problem. Efficient solutions to NLS 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 localisation 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 MLE 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. Even though this paper tests SLAM++ efficiency on SLAM problems, its applicability remains general.
Several approximations for marginal covariance recovery have been proposed in the literature. @cite_5 suggested using conditional covariances, which are inversions of sub-blocks of @math called the Markov blankets. The result is an overconfident approximation of the marginal covariances. Online, conservative approximations were proposed in @cite_33 , where at every step, the covariances corresponding to the new variables are computed by solving the augmented system with a set of basis vectors. The recovered covariance column is passed to a Kalman filter bank, which updates the rest of the covariance matrix. The filtering is reported to run in constant time, and the recovery speed is bounded by the linear solving. In the context of MLE, belief propagation over a spanning tree or loopy intersection propagation can be used to obtain conservative approximations suitable for data association, @cite_18 .
@cite_5 @cite_18 @cite_33
1977464512 2061372028
In this paper we describe a scalable algorithm for the simultaneous mapping and localization (SLAM) problem. SLAM is the problem of acquiring a map of a static environment with a mobile robot. The vast majority of SLAM algorithms are based on the extended Kalman filter (EKF). In this paper we advocate an algorithm that relies on the dual of the EKF, the extended information filter (EIF). We show that when represented in the information form, map posteriors are dominated by a small number of links that tie together nearby features in the map. This insight is developed into a sparse variant of the EIF, called the sparse extended information filter (SEIF). SEIFs represent maps by graphical networks of features that are locally interconnected, where links represent relative information between pairs of nearby features, as well as information about the robot’s pose relative to the map. We show that all essential update equations in SEIFs can be executed in constant time, irrespective of the size of the map. We... Smoothing and optimization approaches are an effective means for solving the simultaneous localization and mapping (SLAM) problem. Most of the existing techniques focus mainly on determining the most likely map and leave open how to efficiently compute the marginal covariances. These marginal covariances, however, are essential for solving the data association problem. In this paper we present a novel algorithm for computing an approximation of the marginal. In experiments we demonstrate that our approach outperforms two commonly used techniques, namely loopy belief propagation and belief propagation on a spanning tree. Compared to these approaches, our algorithm yields better estimates while preserving the same time complexity.