Generic Node Removal for Factor-Graph SLAM

Abstract

Abstract-This paper reports on a generic factor-based method for node removal in factor-graph simultaneous localization and mapping (SLAM), which we call generic linear constraints (GLCs). The need for a generic node removal tool is motivated by long-term SLAM applications whereby nodes are removed in order to control the computational cost of graph optimization. GLC is able to produce a new set of linearized factors over the elimination clique that can represent either the true marginalization (i.e., dense GLC), or a sparse approximation of the true marginalization using a Chow-Liu tree (i.e., sparse GLC). The proposed algorithm improves upon commonly used methods in two key ways: First, it is not limited to graphs with strictly full-state relative-pose factors and works equally well with other low-rank factors such as those produced by monocular vision. Second, the new factors are produced in a way that accounts for measurement correlation, a problem encountered in other methods that rely strictly upon pairwise measurement composition. We evaluate the proposed method over multiple real-world SLAM graphs and show that it outperforms other recently-proposed methods in terms of Kullback-Leibler divergence. Additionally, we experimentally demonstrate that the proposed GLC method provides a principled and flexible tool to control the computational complexity of long-term graph SLAM, with results shown for 34.9 h of real-world indoor-outdoor data covering 147.4 km collected over 27 mapping sessions spanning a period of 15 months.

Graph SLAM Sparsification With Populated Topologies Using Factor Descent Optimization

“…To obtain the projection, one can re-parametrize the problem to relative poses w.r.t an arbitrarily chosen node [10,11] or use a rank-revealing eigen decomposition [12].…”

“…The state-of-the-art literature [10]- [12] proposes two different optimization methods for the factor recovery problem: Interior Point (IP) and Limited-memory Projected Quasi-Newton (PQN) [17].…”

“…Different approaches pose sparsification as a KLD minimization problem [10]- [12]. The best sparse approximation is the one with a minimum KLD with respect to the dense distribution resulting from marginalization.…”

“…State of the art methods [10]- [12] propose the use of interior point and projected quasi-Newton optimization methods to achieve sparsification. Both methods require the tuning of a set of parameters that strongly affects their convergence and robustness.…”

Building 3D mosaics from an Autonomous Underwater Vehicle, Doppler velocity log, and 2D imaging sonar

“…In particular, by estimating surface normals from Doppler velocity log (DVL) range returns, we can constrain the normals of nearby planar patches and produce more self-consistent maps. This approach also has tremendous benefits for performing long-term SLAM since it can be effectively combined with recent developments in graph sparsification techniques [16,17].…”