Non-Gaussian Factor Graph Inference for Robotic Navigation

Abstract

This thesis addresses non-Gaussian factor graph inference problems that arise in simultaneous localization and mapping (SLAM). We present a general framework to draw samples from the joint posterior distributions of a SLAM problem via ancestral sampling on the Bayes tree. This conditional sampling framework works by traversing all cliques of the Bayes tree from leaves to the root, to learn the local conditional distributions, then sampling the conditional distributions from the root to leaves. By leveraging the Bayes tree, the conditional sampling framework is able to exploit the sparsity structure of the factor graph, thus enabling efficient incremental updates similar to iSAM2, albeit in the more challenging non-Gaussian setting. With this conditional sampling framework, we use normalizing flows to learn local conditional distributions on cliques of the Bayes tree. The normalizing flows exploit the expressive power of neural networks, and train a coupling function that connects a low-dimensional non-Gaussian distribution to a standard Gaussian distribution. Together with our conditional sampling framework, normalizing flows make a novel non-Gaussian inference algorithm, Normalizing Flow iSAM (NF-iSAM), for solving high-dimensional SLAM problems with non-Gaussian factors. We demonstrate the performance of NF-iSAM and compare it against the state-of-the-art algorithms such as iSAM2 (Gaussian) and mm-iSAM (non-Gaussian) in synthetic and real range-only SLAM datasets. NF-iSAM shows better accuracy and efficiency than mm-iSAM, and is able to capture the non-Gaussian posterior distributions that iSAM2 cannot tackle.