Generic factor-based node marginalization and edge sparsification for pose-graph SLAM

Abstract

Abstract-This paper reports on a factor-based method for node marginalization in simultaneous localization and mapping (SLAM) pose-graphs. Node marginalization in a pose-graph induces fill-in and leads to computational challenges in performing inference. The proposed method is able to produce a new set of constraints over the elimination clique that can represent either the true marginalization, or a sparse approximation of the true marginalization using a Chow-Liu tree. The proposed algorithm improves upon existing methods in two key ways: First, it is not limited to strictly full-state relative-pose constraints and works equally well with other low-rank constraints such as those produced by monocular vision. Second, the new factors are produced in a way that accounts for measurement correlation, a problem ignored in other methods that rely upon measurement composition. We evaluate the proposed method over several realworld SLAM graphs and show that it outperforms other stateof-the-art methods in terms of Kullback-Leibler divergence.

Long-term simultaneous localization and mapping with generic linear constraint node removal

“…Unfortunately, as shown in [18], pairwise measurement composition has two key drawbacks when used for node removal. First, it is not uncommon for a graph to be composed of many different types of "low-rank" constraints, such as bearing-only, range-only and other partial-state constraints.…”

“…GLC node removal shares many of the properties that make measurement composition appealing, while addressing heterogeneous graphs with non-fullstate constraints and avoiding double counting measurement information. Our previous work, [18], demonstrated improvement in accuracy and consistency over pairwise measurement composition when performing large batch node removal operations. Here, we explore complexity management schemes that repeatedly apply GLC to remove nodes as the map is built.…”

“…In this paper, we promote the use of GLC node removal [18] for long-term SLAM. GLC node removal shares many of the properties that make measurement composition appealing, while addressing heterogeneous graphs with non-fullstate constraints and avoiding double counting measurement information.…”

“…In [19], these linearized potentials are refereed to as "star nodes." The dense formulation of our proposed generic linear constraint (GLC) [18] is essentially equivalent to "star nodes" while the sparse approximate GLC replaces the dense n-nary connectivity with an sparse tree structure. The method recently proposed in [20] again starts with a dense linear potential similar to star-nodes and dense-GLC but then approximates the potential through a L 1 -regularized guaranteed-consistent optimization to produce a sparse nnary linear factor over the elimination clique.…”

“…Methods that remove nodes without measurement composition have been proposed in [18]- [20]. These methods are based on replacing the factors in the marginalization clique with a linearized potential or a set of linearized potentials.…”