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

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.