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.
Article PDF
Similar content being viewed by others
Explore related subjects
Discover the latest articles, news and stories from top researchers in related subjects.Avoid common mistakes on your manuscript.
References
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)
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)
Agarwal, P., Olson, E.: Variable Reordering Strategies for SLAM IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3844–3850 (2012)
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)
Bailey, T., Durrant-Whyte, H.: Simultaneous localization and mapping (SLAM): Part II. IEEE Robot. Autom. Mag. 13(3), 108–117 (2006)
Buluç, A., Meyerhenke, H., Safro, I., Sanders, P., Schulz, C.: Recent advances in graph partitioning Preprint (2013)
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)
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)
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)
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)
Davis, T.A.: Direct methods for sparse linear systems, vol. 2. Siam (2006)
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)
Davis, T.A., Hager, W.W.: Modifying a sparse Cholesky factorization. SIAM J. Matrix Anal. Appl. 20(3), 606–627 (1999)
Davis, T.A., Hager, W.W.: Multiple-rank modifications of a sparse Cholesky factorization. SIAM J. Matrix Anal. Appl. 22(4), 997–1013 (2001)
Davis, T.A., Hager, W.W.: Row modifications of a sparse Cholesky factorization. SIAM J. Matrix Anal. Appl. 26(3), 621–639 (2005)
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)
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)
Durrant-Whyte, H., Bailey, T.: Simultaneous localization and mapping: Part I. Robot. Autom. Mag. 13(2), 99–110 (2006)
Folkesson, J., Christensen, H.: Graphical SLAM - a Self-Correcting Map. In: IEEE International Conference on Robotics and Automation, Vol. 1, pp 383–390 (2004)
George, A.: Nested dissection of a regular finite element mesh. SIAM J. Numer. Anal. 10(2), 345–363 (1973)
George, A., Liu, J.W.: An optimal agorithm for symbolic factorization of symmetric matrices. SIAM J. Comput. 9(3), 583–593 (1980)
Gill, P.E., Golub, G.H., Murray, W., Saunders, M.A.: Methods for modifying matrix factorizations. Math. Comput. 28(126), 505–535 (1974)
Golub, G.H., Van Loan, C.F.: Matrix computations, vol. 3. JHU Press (2012)
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)
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)
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)
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)
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)
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)
Kaess, M., Ranganathan, A., Dellaert, F.: iSAM: Incremental smoothing and mapping. IEEE Trans. Robot. 24(6), 1365–1378 (2008)
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)
Karypis, G., Kumar, V.: Metis-unstructured graph partitioning and sparse matrix ordering system, version 2.0
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)
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)
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
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)
LaSalle, D., Karypis, G.: Multi-Threaded Graph Partitioning. In: IEEE International Symposium on Parallel Andamp; Distributed Processing, Pp. 225–236 (2013)
Liu, J.W.: The role of elimination trees in sparse factorization. SIAM J. Matrix Anal. Appl. 11(1), 134–172 (1990)
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)
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)
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)
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)
Polok, L., Viorela, I.: Slam++ (2015). http://sourceforge.net/projects/slam-plusplus/
Reid, J.K., Scott, J.A.: An out-of-core sparse Cholesky solver. ACM Trans. Math. Softw. 36(2), 9 (2009)
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)
Sherman, A.H.: On the efficient solution of sparse systems of linear and nonlinear equations. Ph.D. thesis, Yale (1975)
Tinney, W.F., Walker, J.: Direct solutions of sparse network equations by optimally ordered triangular factorization. IEEE Proc. 55(11), 1801–1809 (1967)
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)
Vaquero, L., Cuadrado, F., Logothetis, D., Martella, C.: xDGP: A dynamic graph processing system with adaptive partitioning. ACM Computing Research Repository (2013)
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)
Wilson, J.: A new era for airships. Aerosp. Am. 42(5), 27–31 (2004)
Computing the mininimum fill-in is NP-complete, vol. 2 (1981)
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)
Author information
Authors and Affiliations
Corresponding author
Rights and permissions
About this article
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
Received:
Accepted:
Published:
Issue Date:
DOI: https://doi.org/10.1007/s10846-016-0367-7