Research Abstracts - 2006

A Provably Consistent Information Filter for Large-scale SLAM

Matthew Walter & John Leonard

Introduction

An integral capability for autonomous robotics is a means of localizing in a priori unknown environments. This realization has lead to extensive research in the field of Simultaneous Localization and Mapping (SLAM), which addresses the problem of mapping an area while concurrently using the map for localization. Within the last decade, a number of successful SLAM algorithms have been developed [1] but most suffer from a computational cost that limits their application to small environments.

One promising solution utilizes a variation on the information filter, the dual of the conventional Kalman Filter, to achieve near constant-time cost and, in turn, scalability [2]. The problem is that this particular method requires additional approximations which have been shown to yield inconsistent estimates for the robot pose and map [3]. Alternatively, we propose a SLAM filter that achieves the computational advantages of a sparse information parametrization while preserving consistency.

Exactly Sparse Extended Information Filters

In fashion with a majority of approaches to the SLAM problem, we represent the probability distribution over the robot pose and map as a Gaussian. Traditionally, the distribution is represented by the mean vector, , and covariance matrix, , which are then tracked over time with an Extended Kalman Filter (EKF). The scalability of the EKF is limited by the fact that the covariance matrix is fully populated and results in storage and computational requirements that are quadratic in the size of the map.

Alternatively, an equivalent parametrization of the Gaussian posterior is in the form of the information matrix, , and information vector, . In the context of SLAM, the information matrix has the characteristic of being nearly sparse as most elements are very small relative to a few large terms (see Figure 1). In [2], the authors make the key insight that, if these small terms were actually zero, the computational cost of SLAM is nearly independent of the size of the map while the storage requirement is only linear in the number of features. In order to achieve this sparsity, they take weak information shared between the robot and landmarks and approximate it as being zero.

The problem is that forcing these terms to be exactly zero is not trivial since it imposes conditional independence among variables that, in reality, are dependent. In [3], we show that this leads to overconfidence in the accuracy of the state estimates, which has a number of consequences, particularly when it comes to data association.

Rather than approximate these weak terms as being zero, the Exactly Sparse Extended Information Filter (ESEIF) [4] takes a preemptive approach by controlling the formation of links. Shared information (a link) is created between the robot and a landmark when the feature is first observed. With time, the feature becomes linked to other features that share information with the robot pose, fully populating the information matrix. This implies that we can control the creation of non-zero terms in the matrix by actively regulating the formation of links between the robot and the map. A naive approach would be simply to ignore observations of some features but, of course, this means that we would be throwing out a great deal of useful information. The ESEIF, on the other hand, takes a different strategy to achieve sparsity with only an occasional loss of very little information.

The essence of the algorithm is quite straightforward and involves a modified version of the Extended Information Filter update step. When the number of links between the robot and the map becomes too large, we perform sparsification with a partitioning of the current landmark observations into two disjoint sets, and . In the first step, depicted in the left-hand side of Figure 2, we condition the posterior distribution to include the measurement data through the standard EIF update process. Subsequently, the robot pose is marginalized from the posterior, leaving a distribution over only the map. The remaining measurements are then used to relocate the vehicle back into the map. The result is that the robot is connected only to the features associated with the observations as we indicate in the right-hand side of Figure 2. Note that the ESEIF performs sparsification only when there are too many links between the robot and the map and, otherwise, proceeds with the standard EIF update step.

The ESEIF does require any approximations in order to achieve zero shared information between the robot and the map. By "kidnapping" the robot we avoid the need to artificially remove links to the map by, effectively, moving all shared information over to links between features. We then use relocalization to choose which landmarks are linked to the robot. This has the important consequence that the ESEIF maintains uncertainty estimates that are conservative rather than overconfident while also providing the scalability benefits of a sparse information matrix. It should be noted that some temporal information is lost as a result of marginalizing over the pose. In general, though, we sacrifice only a small amount of information and do so only when there are too many features linked to the robot pose.

Results

As an application of the ESEIF algorithm, we consider the Victoria Park dataset for which a truck equipped with a laser scanner was driven through Victoria Park in Sydney, Australia. Trees within the park serve as point features in the SLAM map. Figure 3 compares the ESEIF map and estimated robot trajectory with the results obtained using the SEIF algorithm [2]. For each feature we include the three-sigma uncertainty ellipse as well as the location estimated by the "gold-standard" EKF. Notice that the SEIF uncertainty intervals are much smaller and, for many features, do not capture the locations as estimated by the EKF. This is an indication of overconfidence and is a consequence of the SEIF sparsification strategy. The ESEIF, on the other hand, yields conservative bounds that are consistent with the EKF map.

Future Work

The biggest limitation of the ESEIF and any other algorithm based upon the information filter is that it does not provide direct access to the mean vector or covariance matrix. We can recover both by inverting the information matrix but that comes at a significant computational cost. There are a number of alternative solutions that are more efficient, but they generally produce only approximate estimates. Our current work is focused on looking at ways to exploit the sparsity of the information matrix in order to accurately solve for the mean and covariance in an efficient manner.

Secondly, there are a number of advantages to the information form when it comes to cooperative, multiple-vehicle SLAM. The information filter and, in particular, a sparse matrix, is well suited for merging maps built by different robots as well as for bandwidth-limited communication between platforms. We would like to extend the ESEIF algorithm as an efficient framework for large-scale cooperative SLAM. One specific application that we have in mind is seafloor mapping with a team of underwater vehicles equipped with either multibeam sonar or downward-looking cameras.

References

[1] S.Thrun. Robotic mapping: a survey. In G. Lakemeyer and B. Nebel, editors, Exploring Artificial Intelligence in the New Millennium, Morgan Kaufmann, 2002.

[2] S. Thrun, Y. Liu, D. Koller, A.Y. Ng, Z. Ghahramani, and H. Durrant-Whyte. Simultaneous localization and mapping with sparse extended information filters. In International Journal of Robotics Research, Volume 23, Number 7-8, pp. 693-716, July-August 2004.

[3] R. Eustice, M. Walter, and J. Leonard. Sparse extended information filters: insights into sparsification. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Edmonton, Alberta, Canada, August 2005.

[4] M. Walter, R. Eustice, and J. Leonard. A provably consistent method for imposing exact sparsity in feature-based SLAM information filters. In Proceedings of the 12th International Symposium of Robotics Research (ISRR), San Francisco, CA, USA, October 2005.