Skip to main content
SpringerLink
Log in

Efficient Cholesky Factor Recovery for Column Reordering in Simultaneous Localisation and Mapping

  • Published:
Journal of Intelligent & Robotic Systems Aims and scope Submit manuscript

Abstract

Simultaneous Localisation And Mapping problems are inherently dynamic and the structure of the graph representing them changes significantly over time. To obtain the least square solution of such systems efficiently, it is desired to maintain a good column ordering such that fill-ins are reduced. This comes at a cost since general ordering changes require the complete re-computation of the Cholesky factor. While some methods have obtained good results with reordering at loop closing, the changes are not guaranteed to be limited to the scope of the loop, leading to suboptimal performance. In this article, it is shown that the Cholesky factorisation of an updated matrix can be efficiently recovered from the previous factorisation if the permutations are localised. This is experimentally demonstrated on 2D SLAM datasets. A method is then provided to identify when such recovery is advantageous over the complete re-computation of the Cholesky factor. Furthermore, a hybrid algorithm combining factorisation recovery and re-computation of the Cholesky factor is proposed for dynamically evolving problems and tested on SLAM datasets. Steps where reordering occurs can be executed up to 67 % faster with the proposed method.

This is a preview of subscription content, log in via an institution to check access.

Access this article

Log in via an institution

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Institutional subscriptions

Similar content being viewed by others

References

  1. Adams, J.A., Cooper, J.L., Goodrich, M.A., Humphrey, C., Quigley, M., Buss, B.G., Morse, B.S.: Camera-equipped mini UAVs for wilderness search support: Task analysis and lessons from field trials. J. Field Robot. 25(1–2) (2007)

  2. Adams, S.M., Friedland, C.J.: A Survey of Unmanned Aerial Vehicle (UAV) Usage for Imagery Collection in Disaster Research and Management. In: 9Th International Workshop on Remote Sensing for Disaster Response (2011)

  3. Agarwal, P., Olson, E.: Variable Reordering Strategies for SLAM IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3844–3850 (2012)

  4. Amestoy, P.R., Davis, T.A., Duff, I.S.: An approximate minimum degree ordering algorithm. SIAM J. Matrix Anal. Appl. 17(4), 886–905 (1996)

    Article  MathSciNet  MATH  Google Scholar 

  5. Bailey, T., Durrant-Whyte, H.: Simultaneous localization and mapping (SLAM): Part II. IEEE Robot. Autom. Mag. 13(3), 108–117 (2006)

    Article  Google Scholar 

  6. Buluç, A., Meyerhenke, H., Safro, I., Sanders, P., Schulz, C.: Recent advances in graph partitioning Preprint (2013)

  7. Chen, Y., Davis, T.A., Hager, W.W., Rajamanickam, S.: Algorithm 887: CHOLMOD, supernodal sparse Cholesky factorization and update/downdate. ACM Trans. Math. Softw. 35(3), 22 (2008)

    Article  MathSciNet  Google Scholar 

  8. Cunningham, A., Indelman, V., Dellaert, F.: DDF-SAM 2.0: Consistent Distributed Smoothing and Mapping. In: IEEE International Conference on Robotics and Automation, pp 5220–5227 (2013)

  9. Cunningham, A., Paluri, M., Dellaert, F.: DDF-SAM: Fully Distributed SLAM Using Constrained Factor Graphs. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3025–3030 (2010)

  10. Cunningham, A., Wurm, K.M., Burgard, W., Dellaert, F.: Fully Distributed Scalable Smoothing and Mapping with Robust Multi-Robot Data Association. In: IEEE International Conference on Robotics and Automation, pp. 1093–1100 (2012)

  11. Davis, T.A.: Direct methods for sparse linear systems, vol. 2. Siam (2006)

  12. Davis, T.A., Gilbert, J.R., Larimore, S.I., Ng, E.G.: Algorithm 836: COLAMD, a column approximate minimum degree ordering algorithm. ACM Trans. Math. Softw. 30(3), 377–380 (2004)

    Article  MathSciNet  MATH  Google Scholar 

  13. Davis, T.A., Hager, W.W.: Modifying a sparse Cholesky factorization. SIAM J. Matrix Anal. Appl. 20(3), 606–627 (1999)

    Article  MathSciNet  MATH  Google Scholar 

  14. Davis, T.A., Hager, W.W.: Multiple-rank modifications of a sparse Cholesky factorization. SIAM J. Matrix Anal. Appl. 22(4), 997–1013 (2001)

    Article  MathSciNet  MATH  Google Scholar 

  15. Davis, T.A., Hager, W.W.: Row modifications of a sparse Cholesky factorization. SIAM J. Matrix Anal. Appl. 26(3), 621–639 (2005)

    Article  MathSciNet  MATH  Google Scholar 

  16. Dellaert, F., Carlson, J., Ila, V., Ni, K., Thorpe, C.E.: Subgraph-Preconditioned Conjugate Gradients for Large Scale SLAM. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2566–2571 (2010)

  17. Dellaert, F., Kaess, M.: Square root SAM: Simultaneous localization and mapping via square root information smoothing. The Int. J. Robot. Res. 25(12), 1181–1203 (2006)

    Article  MATH  Google Scholar 

  18. Durrant-Whyte, H., Bailey, T.: Simultaneous localization and mapping: Part I. Robot. Autom. Mag. 13(2), 99–110 (2006)

    Article  Google Scholar 

  19. Folkesson, J., Christensen, H.: Graphical SLAM - a Self-Correcting Map. In: IEEE International Conference on Robotics and Automation, Vol. 1, pp 383–390 (2004)

  20. George, A.: Nested dissection of a regular finite element mesh. SIAM J. Numer. Anal. 10(2), 345–363 (1973)

    Article  MathSciNet  MATH  Google Scholar 

  21. George, A., Liu, J.W.: An optimal agorithm for symbolic factorization of symmetric matrices. SIAM J. Comput. 9(3), 583–593 (1980)

    Article  MathSciNet  MATH  Google Scholar 

  22. Gill, P.E., Golub, G.H., Murray, W., Saunders, M.A.: Methods for modifying matrix factorizations. Math. Comput. 28(126), 505–535 (1974)

    Article  MathSciNet  MATH  Google Scholar 

  23. Golub, G.H., Van Loan, C.F.: Matrix computations, vol. 3. JHU Press (2012)

  24. Grigori, L., Boman, E.G., Donfack, S., Davis, T.A.: Hypergraph-based unsymmetric nested dissection ordering for sparse LU factorization. SIAM J. Sci. Comput. 32(6), 3426–3446 (2010)

    Article  MathSciNet  MATH  Google Scholar 

  25. Hogg, J.D., Reid, J.K., Scott, J.A.: Design of a multicore sparse Cholesky factorization using DAGs. SIAM J. Sci. Comput. 32(6), 3627–3649 (2010)

    Article  MathSciNet  MATH  Google Scholar 

  26. Huang, G., Truax, R., Kaess, M., Leonard, J.J.: Unscented ISAM: A Consistent Incremental Solution to Cooperative Localization and Target Tracking. In: IEEE European Conference on Mobile Robots, pp. 248–254 (2013)

  27. Indelman, V., Williams, S., Kaess, M., Dellaert, F.: Information fusion in navigation systems via factor graph based incremental smoothing. Robot. Auton. Syst. 61(8), 721–738 (2013)

    Article  Google Scholar 

  28. Kaess, M., Ila, V., Roberts, R., Dellaert, F.: The Bayes Tree: Enabling Incremental Reordering and Fluid Relinearization for Online Mapping. Technical Report., DTIC Document (2010)

  29. Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J.J., Dellaert, F.: isam2: Incremental smoothing and mapping using the bayes tree. Int. J. Robot. Res. 31(2), 216–235 (2012)

    Article  Google Scholar 

  30. Kaess, M., Ranganathan, A., Dellaert, F.: iSAM: Incremental smoothing and mapping. IEEE Trans. Robot. 24(6), 1365–1378 (2008)

    Article  Google Scholar 

  31. Kang, S., Lee, W., Nam, M., Tsubouchi, T., Yuta, S.: Wheeled Blimp: Hybrid Structured Airship with Passive Wheel Mechanism for Tele-Guidance Applications. In: IEEE International Conference on Intelligent Robots and Systems, Vol. 4, pp. 3552–3557 (2003)

  32. Karypis, G., Kumar, V.: Metis-unstructured graph partitioning and sparse matrix ordering system, version 2.0

  33. Kim, B., Kaess, M., Fletcher, L., Leonard, J., Bachrach, A., Roy, N., Teller, S.: Multiple Relative Pose Graphs for Robust Cooperative Mapping. In: IEEE International Conference on Robotics and Automation, pp. 3185–3192 (2010)

  34. Konolige, K., Grisetti, G., Kummerle, R., Burgard, W., Limketkai, B., Vincent, R.: Efficient Sparse Pose Adjustment for 2D Mapping. In: IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 22–29 (2010)

  35. Kümmerle, R., Steder, B., Dornhege, C., Ruhnke, M., Grisetti, G., Stachniss, C., Kleiner, A.: Slam benchmarking (2015). http://kaspar.informatik.uni-freiburg.de/slamEvaluation/index.php

  36. Küummerle, R., Grisetti, G., Strasdat, H., Konolige, K., Burgard, W.: G 2 O: a General Framework for Graph Optimization. In: IEEE International Conference on Robotics and Automation, pp. 3607–3613 (2011)

  37. LaSalle, D., Karypis, G.: Multi-Threaded Graph Partitioning. In: IEEE International Symposium on Parallel Andamp; Distributed Processing, Pp. 225–236 (2013)

  38. Liu, J.W.: The role of elimination trees in sparse factorization. SIAM J. Matrix Anal. Appl. 11(1), 134–172 (1990)

    Article  MathSciNet  MATH  Google Scholar 

  39. Ni, K., Dellaert, F.: Multi-Level Submap Based SLAM Using Nested Dissection. In: IEEE International Conference on Intelligent Robots and Systems, Pp. 2558–2565 (2010)

  40. Ni, K., Steedly, D., Dellaert, F.: Tectonic SAM: Exact, out-of-core, submap-based SLAM. In: IEEE International Conference on Robotics and Automation, pp. 1678–1685 (2007)

  41. Polok, L., Solony, M., Ila, V., Smrz, P., Zemcik, P.: Efficient Implementation for Block Matrix Operations for Nonlinear Least Squares Problems in Robotic Applications. In: IEEE International Conference on Robotics and Automation, pp. 2263–2269 (2013)

  42. Polok, L., Solony, M., Ila, V., Smrz, P., Zemcik, P.: Incremental Cholesky Factorization for Least Squares Problems in Robotics. In: Intelligent Autonomous Vehicles, Vol. 8, pp. 172–178 (2013)

  43. Polok, L., Viorela, I.: Slam++ (2015). http://sourceforge.net/projects/slam-plusplus/

  44. Reid, J.K., Scott, J.A.: An out-of-core sparse Cholesky solver. ACM Trans. Math. Softw. 36(2), 9 (2009)

    Article  MathSciNet  Google Scholar 

  45. Rennich, S.C., Stosic, D., Davis, T.A.: Accelerating Sparse Cholesky Factorization on GPUs. In: Proceedings of the Fourth Workshop on Irregular Applications: Architectures and Algorithms, pp. 9–16 (2014)

  46. Sherman, A.H.: On the efficient solution of sparse systems of linear and nonlinear equations. Ph.D. thesis, Yale (1975)

  47. Tinney, W.F., Walker, J.: Direct solutions of sparse network equations by optimally ordered triangular factorization. IEEE Proc. 55(11), 1801–1809 (1967)

    Article  Google Scholar 

  48. Tsourakakis, C., Gkantsidis, C., Radunovic, B., Vojnovic, M.: FENNEL: Streaming Graph Partitioning for Massive Scale Graphs. In: Proceedings of the 7Th ACM International Conference on Web Search and Data Mining, pp. 333–342. ACM (2014)

  49. Vaquero, L., Cuadrado, F., Logothetis, D., Martella, C.: xDGP: A dynamic graph processing system with adaptive partitioning. ACM Computing Research Repository (2013)

  50. Williams, S., Indelman, V., Kaess, M., Roberts, R., Leonard, J.J., Dellaert, F.: Concurrent Filtering and Smoothing. In: IEEE International Conference on Information Fusion, pp. 1300–1307 (2012)

  51. Wilson, J.: A new era for airships. Aerosp. Am. 42(5), 27–31 (2004)

    Google Scholar 

  52. Computing the mininimum fill-in is NP-complete, vol. 2 (1981)

  53. Zou, D., Dou, Y.: Implementation of Parallel Sparse Cholesky Factorization on GPU. In: International Conference on Computer Science and Network Technology, pp. 2228–2232 (2012)

Download references

Author information

Authors and Affiliations

  1. School of Electrical Engineering and Computer Science, University of Ottawa, Ottawa, ON, Canada

    S. Touchette & W. Gueaieb

  2. Department of Mechanical Engineering, University of Ottawa, Ottawa, ON, Canada

    E. Lanteigne

Authors
  1. S. Touchette
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. W. Gueaieb
    View author publications

    You can also search for this author in PubMed Google Scholar

  3. E. Lanteigne
    View author publications

    You can also search for this author in PubMed Google Scholar

Corresponding author

Correspondence to S. Touchette.

Rights and permissions

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Touchette, S., Gueaieb, W. & Lanteigne, E. Efficient Cholesky Factor Recovery for Column Reordering in Simultaneous Localisation and Mapping. J Intell Robot Syst 84, 859–875 (2016). https://doi.org/10.1007/s10846-016-0367-7

Download citation

  • Received:

  • Accepted:

  • Published:

  • Issue Date:

  • DOI: https://doi.org/10.1007/s10846-016-0367-7

Keywords

Search

Navigation