article

Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing

Authors:

Frank Dellaert,

Michael KaessAuthors Info & Claims

International Journal of Robotics Research, Volume 25, Issue 12

Pages 1181 - 1203

https://doi.org/10.1177/0278364906072768

Published: 01 December 2006 Publication History

Abstract

Solving the SLAM (simultaneous localization and mapping) problem is one way to enable a robot to explore, map, and navigate in a previously unknown environment. Smoothing approaches have been investigated as a viable alternative to extended Kalman filter (EKF)-based solutions to the problem. In particular, approaches have been looked at that factorize either the associated information matrix or the measurement Jacobian into square root form. Such techniques have several significant advantages over the EKF: they are faster yet exact; they can be used in either batch or incremental mode; are better equipped to deal with non-linear process and measurement models; and yield the entire robot trajectory, at lower cost for a large class of SLAM problems. In addition, in an indirect but dramatic way, column ordering heuristics automatically exploit the locality inherent in the geographic nature of the SLAM problem. This paper presents the theory underlying these methods, along with an interpretation of factorization in terms of the graphical model associated with the SLAM problem. Both simulation results and actual SLAM experiments in large-scale environments are presented that underscore the potential of these methods as an alternative to EKF-based approaches.

References

[1]

Amestoy, P. R., Davis, T., and Duff, I. S. 1996. An approximate minimum degree ordering algorithm . SIAM Journal on Matrix Analysis and Applications 17(4): 886-905 .

Digital Library

[2]

Ayache, N. and Faugeras, O. D. 1989. Maintaining representations of the environment of a mobile robot . IEEE Transactions on Robotics and Automation 5(6): 804-819 .

[3]

Bierman, G. J. 1977. Factorization Methods for Discrete Sequential Estimation, volume 128 of Mathematics in Science and Engineering. Academic Press, New York .

[4]

Blair, J. R. S. and Peyton, B. W. 1993. An introduction to chordal graphs and clique trees. In Graph Theory and Sparse Matrix Computations, volume 56 of IMA Volumes in Mathematics and its Applications (eds J. A. George, J. R. Gilbert, and J. W-H. Liu ) pp. 1-27, Springer-Verlag .

[5]

Bosse, M., Newman, P., Leonard, J., Soika, M., Feiten, W., and Teller, S. 2003. An Atlas framework for scalable mapping . IEEE International Conference on Robotics and Automation (ICRA).

[6]

Brooks, R. A. 1985. Visual map making for a mobile robot . IEEE International Conference on Robotics and Automation (ICRA), March, volume 2, pp. 824-829 .

[7]

Brown, D. C. 1976. The bundle adjustment--progress and prospects . International Archives Photogrammetry 21(3).

[8]

Castellanos, J. A., Montiel, J. M. M., Neira, J., and Tardos, J. D. 1999. The SPmap: A probabilistic framework for simultaneous localization and map building . IEEE Transactions on Robotics and Automation 15(5): 948-953 .

[9]

Chatila, R. and Laumond, J.-P. 1985. Position referencing and consistent world modeling for mobile robots . IEEE International Conference on Robotics and Automation (ICRA), pp. 138-145 .

[10]

Cooper, M. A. R. and Robson, S. 1996. Theory of close range photogrammetry. In Close Range Photogrammetry and Machine Vision (ed. K. B. Atkinson ), pp. 9-51, Whittles Publishing .

[11]

Cowell, R. G., Dawid, A. P., Lauritzen, S. L., and Spiegel-halter, D. J. 1999. Probabilistic Networks and Expert Systems. Statistics for Engineering and Information Science. Springer-Verlag .

[12]

Davis, T. and Hager, W. 1996. Modifying a sparse Cholesky factorization . SIAM Journal on Matrix Analysis and Applications 20(3): 606-627 .

Digital Library

[13]

Davis, T. A. 2004 Algorithm 8xx: a concise sparse Cholesky factorization package. Technical Report TR-04-001, University of Florida, January . Submitted to ACM Transactions in Mathematics and Software.

[14]

Davis, T. A., Gilbert, J. R., Larimore, S. I., and Ng, E. G. 2004. A column approximate minimum degree ordering algorithm . ACM Transactions in Mathematics and Software 30(3): 353-376 .

Digital Library

[15]

Davis, T. A. and Stanley, K. 2004. KLU: a "Clark Kent" sparse LU factorization algorithm for circuit matrices . 2004 SIAM Conference on Parallel Processing for Scientific Computing (PP04).

[16]

Deans, M. and Hebert, M. 2000. Experimental comparison of techniques for localization and mapping using a bearings only sensor . Proceedings of the ISER '00 Seventh International Symposium on Experimental Robotics, December.

[17]

Deans, M. and Hebert, M. 2000. Invariant filtering for simultaneous localization and mapping . IEEE International Conference on Robotics and Automation (ICRA), April, pp. 1042-1047 .

[18]

Dellaert, F. 2005. Square Root SAM: Simultaneous location and mapping via square root information smoothing . Robotics: Science and Systems (RSS).

[19]

Dellaert, F., Kipp, A., and Krauthausen, P. 2005. A multi-frontal QR factorization approach to distributed inference applied to multi-robot localization and mapping . AAAI National Conference on Artificial Intelligence.

[20]

Dennis, J. E. and Schnabel, R. B. 1983. Numerical Methods for Unconstrained Optimization and Nonlinear Equations. Prentice-Hall .

[21]

Dissanayake, M. W. M. G., Newman, P. M., Durrant-Whyte, H. F., Clark, S., and Csorba, M. 2001. A solution to the simultaneous localization and map building (SLAM) problem . IEEE Transactions on Robotics and Automation 17(3): 229-241 .

[22]

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

[23]

Duckett, T., Marsland, S., and Shapiro, J. 2002. Fast, on-line learning of globally consistent maps . Autonomous Robots 12(3): 287-300 .

Digital Library

[24]

Eustice, R., Singh, H., and Leonard, J. 2005. Exactly sparse delayed-state filters . IEEE International Conference on Robotics and Automation (ICRA), Barcelona, Spain, April, pp. 2428-2435 .

[25]

Faugeras, O. D. 1993. Three-dimensional Computer Vision: A Geometric Viewpoint. The MIT Press, Cambridge, MA .

[26]

Folkesson, J. and Christensen, H. I. 2004. Graphical SLAM--a self-correcting map . IEEE International Conference on Robotics and Automation (ICRA), volume 1, pp. 383-390 .

[27]

Folkesson, J., Jensfelt, P., and Christensen, H. I. 2005. Graphical SLAM using vision and the measurement subspace . IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[28]

Frese, U. 2004. <it>An O(</it>log <it>n) Algorithm for Simultaneous Localization and Mapping of Mobile Robots in Indoor Environments</it>. PhD thesis, University of Erlangen-Nürnberg.

[29]

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

[30]

Frese, U. and Duckett, T. 2003. A multigrid approach for accelerating relaxation-based SLAM . Proceedings of the IJCAI-03 on Reasoning with Uncertainty in Robotics.

[31]

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

Digital Library

[32]

George, J. A., Gilbert, J. R., and Liu, J. W-H. eds. 1993. Graph Theory and Sparse Matrix Computations, volume 56 of IMA Volumes in Mathematics and its Applications. Springer-Verlag .

[33]

Golfarelli, M., Maio, D., and Rizzi, S. 1998. Elastic correction of dead-reckoning errors in map building . IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 905-911 .

[34]

Golfarelli, M., Maio, D., and Rizzi, S. 2001. Correction of dead-reckoning errors in map building . IEEE Transactions on Robotics and Automation 17(1).

[35]

Golub, G. H. and Van Loan, C. F. 1996. Matrix Computations, 3rd edn. Johns Hopkins University Press, Baltimore .

[36]

Golub, G. H. and Plemmons, R. J. 1980. Large-scale geodetic least-squares adjustment by dissection and orthogonal decomposition . Linear Algebra and Its Applications 34: 3-28 .

[37]

Granshaw, S. 1980. Bundle adjustment methods in engineering photogrammetry . Photogrammetric Record 10(56): 181-207 .

[38]

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 .

[39]

Guivant, J., Nebot, E., Nieto, J., and Masson, F. 2004. Navigation and mapping in large unstructured environments . International Journal of Robotics Research 23: 449-472 .

[40]

Gutmann, J.-S. and Konolige, K. 2000. Incremental mapping of large cyclic environments . Proceedings of the IEEE Internatioal Symposium on Computational Intelligence in Roboticsand Automation(CIRA), November, pp. 318-325 .

[41]

Hartley, R. and Zisserman, A. 2000. Multiple View Geometry in Computer Vision. Cambridge University Press .

[42]

Heggernes, P. and Matstoms, P. 1996. Finding good column orderings for sparse QR factorization . In Second SIAM Conference on Sparse Matrices.

[43]

Howard, A., Matarić, M. J., and Sukhatme, G. S. Oct. 2001. Relaxation on a mesh: a formalism for generalized localization . In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1055-1060, Wailea, Hawaii.

[44]

Julier, S. J. and Uhlmann, J. K. 2001. A counter example to the theory of simultaneous localization and map building . In IEEE International Conference on Robotics and Automation (ICRA), volume 4, pp. 4238-4243 .

[45]

Julier, S. J. and Uhlmann, J. K. 2001. Simultaneous localisation and map building using split covariance intersection . In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), volume 3, pp. 1257-1262 .

[46]

Konolige, K. Large-scale map-making . In AAAI National Conference on Artificial Intelligence, San Jose, 2004.

[47]

Krauthausen, P., Dellaert, F., and Kipp, A. 2006. Exploiting locality by nested dissection for square root smoothing and mapping . In Robotics: Science and Systems (RSS).

[48]

Kschischang, F. R., Frey, B. J., and Loeliger, H.-A. 2001. Factor graphs and the sum-product algorithm . IEEE Transactions on Information Theory 47(2).

Digital Library

[49]

Leonard, J. J. and Feder, H. J. S. 2001. Decoupled stochastic mapping . IEEE Journal of Oceanic Engineering 10: 561-571 .

[50]

Leonard, J. J. and Newman, P. M. 2003. Consistent, convergent, and constant-time SLAM . International Joint Conference on Artificial Intelligence (IJCAI).

[51]

Leonard, J. J., Cox, I. J., and Durrant-Whyte, H. F. 1992. Dynamic map building for an autonomous mobile robot . International Journal of Robotics Research 11(4): 286-289 .

Digital Library

[52]

Leonard, J. J. and Durrant-Whyte, H. F. 1991. Simultaneous map building and localization for an autonomous mobile robot . IEEE International Workshop on Intelligent Robots and Systems, pp. 1442-1447 .

[53]

Leonard, J. J. and Durrant-Whyte, H. F. 1992. Directed Sonar Sensing for Mobile Robot Navigation. Kluwer Academic, Boston, MA .

[54]

Lipton, R. J. and Tarjan, R. E. 1979a. Generalized nested dissection . SIAM Journal of Applied Mathematics 16(2): 346-358 .

[55]

Lipton, R. J. and Tarjan, R. E. 1979b. A separator theorem for planar graphs . SIAM Journal of Applied Mathematics 36(2): 177-189 .

[56]

Lu, F. and Milios, E. 1997. Globally consistent range scan alignment for environment mapping . Autonomous Robots April: 333-349 .

[57]

Lu, F. and Milios, E. 1997. Robot pose estimation in unknown environments by matching 2D range scans . Journal of Intelligent and Robotic Systems 4: 249-275 .

[58]

Matstoms, P. 1994. Sparse QR factorization in MAT LAB . ACM Transactions on Mathematics and Software 20(1): 136-159 .

Digital Library

[59]

Maybeck, P. 1979. Stochastic Models, Estimation and Control, volume 1. Academic Press, New York .

[60]

Montemerlo, M., Thrun, S., Koller, D., and Wegbreit, B. 2002. FastSLAM: A factored solution to the simultaneous localization and mapping problem . AAAI National Conference on Artificial Intelligence.

Digital Library

[61]

Moutarlier, P. and Chatila, R. 1989. An experimental system for incremental environment modelling by an autonomous mobile robot . Experimental Robotics I, The First International Symposium, Montréal, Canada, pp. 327-346 .

[62]

Moutarlier, P. and Chatila, R. 1989. Stochastic multisensor data fusion for mobile robot location and environment modelling . 5th International Symposium on Robotics Research.

[63]

Murphy, K. 1999. Bayesian map learning in dynamic environments . Advances in Neural Information Processing Systems (NIPS).

[64]

Newman, P. 1999. <it>On the Structure and Solution of the Simultaneous Localisation and Map Building Problem</it>. PhD thesis, The University of Sydney.

[65]

Paskin, M. A. 2003. Thin junction tree filters for simultaneous localization and mapping . International Joint Conference on Artificial Intelligence (IJCAI).

[66]

Pothen, A. and Sun, C. 1992. Distributed multifrontal factorization using clique trees . In Proceedings of the Fifth SIAM Conference on Parallel Processing for Scientific Computing, pp. 34-40 . Society for Industrial and Applied Mathematics.

[67]

Rodriguez-Losada, D., Matia, F., and Jimenez, A. 2004. Local maps fusion for real time multirobot indoor simultaneous localization and mapping . IEEE International Conference on Robotics and Automation (ICRA), volume 2, pp. 1308-1313 .

[68]

Slama, C. C. ed. 1980. Manual of Photogrammetry. American Society of Photogrammetry and Remote Sensing, Falls Church, VA .

[69]

Smith, R. and Cheeseman, P. 1987. On the representation and estimation of spatial uncertainty . International Journal of Robotics Research 5(4): 56-68 .

Digital Library

[70]

Smith, R., Self, M., and Cheeseman, P. 1987. A stochastic map for uncertain spatial relationships . International Symposium on Robotics Research.

[71]

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 .

[72]

Szeliski, R. and Kang, S. B. 1993. Recovering 3D shape and motion from image streams using non-linear least squares. Technical Report CRL 93/3, DEC Cambridge Research Lab .

[73]

Szeliski, R. and Kang, S. B. 1994. Recovering 3D shape and motion from image streams using nonlinear least squares . Journal of Visual Communication and Image Representation 5(1).

[74]

Tang, Y. and Lee, C. G. 1992. A geometric feature relation graph formulation for consistent sensor fusion . IEEE Transactions on Systems, Man and Cybernetics, 22: 115-129 .

[75]

Tardós, J. D., Neira, J., Newman, P. M., and Leonard, J. J. 2002. Robust mapping and localization in indoor environments using sonar data . International Journal of Robotics Research 21(4): 311-330 .

[76]

Thrun, S. 2005. Robotic mapping: a survey. In Exploring Artificial Intelligence in the New Millennium, pp. 1-35, Morgan Kaufmann .

[77]

Thrun, S., Burgard, W., and Fox, D. 2005. Probabilistic Robotics. The MIT Press, Cambridge, MA .

[78]

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 .

[79]

Triggs, B., McLauchlan, P., Hartley, R., and Fitzgibbon, A. 2000. Bundle adjustment--a modern synthesis. In Vision Algorithms: Theory and Practice (eds B. Triggs, A. Zisserman, and R. Szeliski ), LNCS, pp. 298-375, Springer Verlag .

[80]

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

[81]

Winkler, G. 1995. Image Analysis, Random Fields and Dynamic Monte Carlo Methods. Springer Verlag .

[82]

Yedidia, J. S., Freeman, W. T., and Weiss, Y. 2000. Generalized belief propagation . Advances in Neural Information Processing Systems (NIPS), pp. 689-695 .

Cited By

Zhang DTan WZelek JMa LLi J(2025)SLAM-TSM: Enhanced Indoor LiDAR SLAM With Total Station Measurements for Accurate Trajectory EstimationIEEE Transactions on Intelligent Transportation Systems10.1109/TITS.2024.350448726:2(1743-1753)Online publication date: 1-Feb-2025
https://dl.acm.org/doi/10.1109/TITS.2024.3504487
Ferrer GIarosh DKornilova A(2025)Eigen-factors a bilevel optimization for plane SLAM of 3D point cloudsAutonomous Robots10.1007/s10514-025-10189-549:1Online publication date: 1-Mar-2025
https://dl.acm.org/doi/10.1007/s10514-025-10189-5
Hao YGan YYu BLiu QHan YWan ZLiu STsafrir DMusuvathi MGupta RAbu-Ghazaleh N(2024)ORIANNA: An Accelerator Generation Framework for Optimization-based Robotic ApplicationsProceedings of the 29th ACM International Conference on Architectural Support for Programming Languages and Operating Systems, Volume 210.1145/3620665.3640379(813-829)Online publication date: 27-Apr-2024
https://dl.acm.org/doi/10.1145/3620665.3640379
Show More Cited By

Recommendations

Dual FastSLAM: Dual Factorization of the Particle Filter Based Solution of the Simultaneous Localization and Mapping Problem

The process of building a map with a mobile robot is known as the Simultaneous Localization and Mapping (SLAM) problem, and is considered essential for achieving true autonomy. The best existing solutions to the SLAM problem are based on probabilistic ...
Mobile robot visual control and navigation using strategies of moving object localization and mapping
CSECS'09: Proceedings of the 8th WSEAS International Conference on Circuits, systems, electronics, control & signal processing

Mobile robots are navigated with a set of sensors including the visual sensors (for example PTZ or stereo cameras). The 2D or 3D images from these video sensors contain considerable information of the stationary and moving objects in the observation ...
Cloud-based Parallel Implementation of SLAM for Mobile Robots
ICC '16: Proceedings of the International Conference on Internet of things and Cloud Computing

Simultaneous Localization and Mapping (SLAM) for mobile robots is a computationally expensive task. A robot capable of SLAM needs a powerful onboard computer, but this can limit the robot's mobility because of weight and power demands. We consider ...

Comments

Please enable JavaScript to view thecomments powered by Disqus.

Information & Contributors

Information

Published In

cover image International Journal of Robotics Research

International Journal of Robotics Research Volume 25, Issue 12

December 2006

145 pages

ISSN:0278-3649

Issue’s Table of Contents

Publisher

Sage Publications, Inc.

United States

Publication History

Published: 01 December 2006

Author Tags

Qualifiers

Article

Contributors

Other Metrics

View Article Metrics

Bibliometrics & Citations

Bibliometrics

Article Metrics

134
Total Citations
View Citations
0
Total Downloads

Downloads (Last 12 months)0
Downloads (Last 6 weeks)0

Reflects downloads up to 28 Feb 2025

Other Metrics

View Author Metrics

Citations

Cited By

Zhang DTan WZelek JMa LLi J(2025)SLAM-TSM: Enhanced Indoor LiDAR SLAM With Total Station Measurements for Accurate Trajectory EstimationIEEE Transactions on Intelligent Transportation Systems10.1109/TITS.2024.350448726:2(1743-1753)Online publication date: 1-Feb-2025
https://dl.acm.org/doi/10.1109/TITS.2024.3504487
Ferrer GIarosh DKornilova A(2025)Eigen-factors a bilevel optimization for plane SLAM of 3D point cloudsAutonomous Robots10.1007/s10514-025-10189-549:1Online publication date: 1-Mar-2025
https://dl.acm.org/doi/10.1007/s10514-025-10189-5
Hao YGan YYu BLiu QHan YWan ZLiu STsafrir DMusuvathi MGupta RAbu-Ghazaleh N(2024)ORIANNA: An Accelerator Generation Framework for Optimization-based Robotic ApplicationsProceedings of the 29th ACM International Conference on Architectural Support for Programming Languages and Operating Systems, Volume 210.1145/3620665.3640379(813-829)Online publication date: 27-Apr-2024
https://dl.acm.org/doi/10.1145/3620665.3640379
Papalia AFishberg AO'Neill BHow JRosen DLeonard J(2024)Certifiably Correct Range-Aided SLAMIEEE Transactions on Robotics10.1109/TRO.2024.345443040(4265-4283)Online publication date: 1-Jan-2024
https://dl.acm.org/doi/10.1109/TRO.2024.3454430
Kaltiokallio OHostettler RGe YKim HTalvitie JWymeersch HValkama M(2024)A Multihypotheses Importance Density for SLAM in Cluttered ScenariosIEEE Transactions on Robotics10.1109/TRO.2023.333897540(1019-1035)Online publication date: 1-Jan-2024
https://dl.acm.org/doi/10.1109/TRO.2023.3338975
Yoon ZKim JPark H(2024)Invariant Smoother for Legged Robot State Estimation With Dynamic Contact Event InformationIEEE Transactions on Robotics10.1109/TRO.2023.332820240(193-212)Online publication date: 1-Jan-2024
https://dl.acm.org/doi/10.1109/TRO.2023.3328202
Shen ZLi XWang XWu ZLi XZhou YLi S(2024)A Novel Factor Graph Framework for Tightly Coupled GNSS/INS Integration With Carrier-Phase Ambiguity ResolutionIEEE Transactions on Intelligent Transportation Systems10.1109/TITS.2024.338746625:10(13091-13105)Online publication date: 1-Oct-2024
https://dl.acm.org/doi/10.1109/TITS.2024.3387466
Zheng JHe YKondo M(2023)Exploiting Data Parallelism in Graph-Based Simultaneous Localization and Mapping: A Case Study with GPU AccelerationsProceedings of the International Conference on High Performance Computing in Asia-Pacific Region10.1145/3578178.3578237(126-139)Online publication date: 27-Feb-2023
https://dl.acm.org/doi/10.1145/3578178.3578237
Matsuka KChung S(2023)Localized and Incremental Probabilistic Inference for Large-Scale Networked Dynamical SystemsIEEE Transactions on Robotics10.1109/TRO.2023.329701039:5(3516-3535)Online publication date: 1-Oct-2023
https://dl.acm.org/doi/10.1109/TRO.2023.3297010
Huang QPu CKhosoussi KRosen DFourie DHow JLeonard J(2023)Incremental Non-Gaussian Inference for SLAM Using Normalizing FlowsIEEE Transactions on Robotics10.1109/TRO.2022.321649839:2(1458-1475)Online publication date: 1-Apr-2023
https://dl.acm.org/doi/10.1109/TRO.2022.3216498
Show More Cited By

View Options

View options

Figures

Tables

Media

View Issue’s Table of Contents