Exactly Sparse Extended Information Filters for Feature-based SLAM

Abstract

Recent research concerning the Gaussian canonical form for Simultaneous Localization and Mapping (SLAM) has given rise to a handful of algorithms that attempt to solve the SLAM scalability problem for arbitrarily large environments. One such estimator that has received due attention is the Sparse Extended Information Filter (SEIF) proposed by Thrun et al., which is reported to be nearly constant time, irrespective of the size of the map. The key to the SEIF's scalability is to prune weak links in what is a dense information (inverse covariance) matrix to achieve a sparse approximation that allows for efficient, scalable SLAM. We demonstrate that the SEIF sparsification strategy yields error estimates that are overconfident when expressed in the global reference frame, while empirical results show that relative map consistency is maintained.

In this paper, we propose an alternative scalable estimator based on an information form that maintains sparsity while preserving consistency. The paper describes a method for controlling the population of the information matrix, whereby we track a modified version of the SLAM posterior, essentially by ignoring a small fraction of temporal measurements. In this manner, the Exactly Sparse Extended Information Filter (ESEIF) performs inference over a model that is conservative relative to the standard Gaussian distribution. We compare our algorithm to the SEIF and standard EKF both in simulation as well as on two nonlinear datasets. The results convincingly show that our method yields conservative estimates for the robot pose and map that are nearly identical to those of the EKF.

Keywords

mobile robotics SLAM Kalman filters information filters robotic mapping robotic navigation

Get full access to this article

View all access options for this article.

References

Barrett, R. , Berry, M. , Chan, T.F. , Demmel, J. , Donato, J. , Dongarra, J. et al. (1994). Templates for the Solution of Linear Systems: Building Blocks for Iterative Methods, 2nd edn. SIAM, Philadelphia, PA.

Bar-Shalom, Y. , Rong Li, X. , and Kirubarajan, T. , (2001). Estimation with Applications to Tracking and Navigation . John Wiley & Sons, Inc., New York.

Bosse, M. , Newman, P. , Leonard, J. , and Teller, S. (2004). Simultaneous localization and map building in large-scale cyclic environments using the atlas framework. International Journal of Robotics Research, 23(12): 1113— 1139.

Dellaert, F. (2005) Square root SAM. Proceedings of Robotics: Science and Systems (RSS), Cambridge, MA, June, pp.177— 184.

Duckett, T. , Marsland, S. , and Shapiro, J. (2000). Learning globally consistent maps by relaxation . Proceeding of the IEEE International Conference on Robotics and Automation (ICRA), San Francisco, CA, pp.3841—3846.

Eustice, R. , Singh, H. , and Leonard, J. (2005a). Exactly sparse delayed-state filters. Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Barcelona, Spain, April, pp.2417—2424.

Eustice, R. , Walter, M. , and Leonard, J. (2005b) Sparse extended information filters: Insights into sparsification. Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Edmonton, Alberta, Canada, August, pp.3281—3288.

Eustice, R. , Singh, H. , Leonard, J. , Walter, M. , and Ballard, R. (2005c). Visually navigating the RMS Titanic with SLAM information filters. Proceedings of Robotics: Science and Systems (RSS), Cambridge, MA, June, pp.57—64. Folkesson, J. and Christensen, H. (2004). Graphical SLAM — a self-correcting map. Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), New Or-leans, LA, April, pp.383—390.

Frese, U. (2005a). Treemap: An O(log n) algorithm for simultaneous localization and mapping. Spatial Cognition IV (ed. C. Freksa ), pp.455—476. Springer Verlag.

10.

Frese, U. (2005b). A proof for the approximate sparsity of SLAM information matrices. Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Barcelona , April, pp.331—337.

11.

Frese, U. and Hirzinger, G. (2001). Simultaneous localization and mapping — a discussion. Proceedings of the IJCAI Workshop on Reasoning with Uncertainty in Robotics, pp.17—26.

12.

Frese, U. , Larsson, P. , and Duckett, T. (2005). A multilevel relaxation algorithm for simultaneous localization and mapping. IEEE Transactions on Robotics, 21(2): 196—207.

13.

Guivant, J. and Nebot, E. (2001). Optimization of the simultaneous localization and map-building algorithm for real-time implementation. IEEE Transactions on Robotics and Automation, 17(3): 242—257.

14.

Julier, S. and Uhlmann, J. (1997). A non-divergent estimation algorithm in the presence of unknown correlations. Proceedings of the American Control Conference, Albuquerque, NM, June, pp.2369—2373.

15.

Leonard, J. and Newman, P. (2003). Consistent, convergent, and constant-time SLAM . Proceedings of the International Joint Conference on Artif icial Intelligence (IJCAI), Acapulco, Mexico, August, pp.1143—1150.

16.

Liu, Y. and Thrun, S. (2003). Results for outdoor-SLAM using sparse extended information filters. Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Taipei, Taiwan, May, pp.1227—1233.

17.

Maybeck, P.S. (1979). Stochastic Models, Estimation, and Control, Vol. 1. Academic Press, New York.

18.

Montemerlo, M. , Thrun, S. , Koller, D. , and Wegbreit, B. (2002) FastSLAM: A factored solution to the simultaneous localization and mapping problem. Proceedings of the AAAI National Conference on Artif icial Intelligence, Edmonton, Canada, pp.593—598.

19.

Montemerlo, M. , Thrun, S. , Koller, D. , and Wegbreit, B. (2003). FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges. Proceedings of the International Joint Conference on Artif icial Intelligence (IJCAI), Acapulco, Mexico, August, pp.1151—1156.

20.

Moutarlier, P. and Chatila, R. (1989). An experimental system for incremental environment modelling by an autonomous mobile robot. Proceedings of the 1st International Symposium on Experimental Robotics, Montreal, Canada, June.

21.

Paskin, M. (2002). Thin junction tree filters for simultaneous localization and mapping. University of California, Berkeley, Technical Report UCB/CSD-02-1198, September.

22.

Shewchuck, J. (1994). An introduction to the conjugate gradient method without the agonizing pain. Carnegie Mellon University , Technical Report CMU-CS-94-125, August.

23.

Smith, R. , Self, M. , and Cheeseman, P. (1990). Estimating uncertain spatial relationships in robotics. In Autonomous Robot Vehicles (eds I. Cox and G. Wilfong) , pp. 167—193, Springer-Verlag.

24.

Speed, T.P. and Kiiveri, H.T. (1986). Gaussian Markov distributions over finite graphs . Annals of Statistics, 14(1): 138—150.

25.

Strang, G. (1980). Linear Algebra and Its Applications, 2nd edn. Academic Press, New York.

26.

Thrun, S. , Burgard, W. , and Fox, D. (2005). Probabilistic Robotics. MIT Press.

27.

Thrun, S. , Liu, Y. , Koller, D. , Ng, A.Y. , Ghahramani, Z. , and Durrant-Whyte, H. (2004). Simultaneous localization and mapping with sparse extended information filters. International Journal of Robotics Research, 23(7—8): 693—716.

28.

van der Merwe, R. , Doucet, A. , de Freitas, N. , and Wan, E. (2000). The unscented particle filter. Cambridge University Engineering Department, Technical Report CUED/F-INFENG/TR380, August.

29.

Wang, Z. , Huang, S. , and Dissanayake, G. (2005). D-SLAM: Decoupled localization and mapping for autonomous robots. Proceedings of the 12th International Symposium of Robotics Research, San Francisco, CA, October.

30.

Whitcomb, L. , Yoerger, D. , Singh, H. , and Mindell, D. (1998). Towards precision robotic maneuvering, survey, and manipulation in unstructured undersea environments. in Robotics Research — The Eighth International Symposium (eds Y. Shirai and S. Hirose), pp. 45—54, Springer-Verlag, London.

31.

Williams, S. , Dissanayake, G. , and Durrant-Whyte, H. (2002). An efficient approach to the simultaneous localisation and mapping problem. Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Washington, DC, May, pp.406—411.