Variable Elimination in Hybrid Factor Graphs for Discrete-Continuous Inference & Estimation

TL;DR

This paper introduces a novel variable elimination algorithm for hybrid factor graphs that enables exact inference in problems involving both discrete and continuous variables, demonstrated on SLAM and pose graph tasks.

Contribution

It presents a new hybrid factor graph framework with an exact variable elimination method, improving inference accuracy and tractability in hybrid discrete-continuous estimation problems.

Findings

Exact hybrid Bayes network produced for inference

Framework applied successfully to SLAM and pose graph optimization

Demonstrated improved accuracy and tractability in hybrid inference

Abstract

Many problems in robotics involve both continuous and discrete components, and modeling them together for estimation tasks has been a long standing and difficult problem. Hybrid Factor Graphs give us a mathematical framework to model these types of problems, however existing approaches for solving them are based on approximations. In this work, we propose a new framework for hybrid factor graphs along with a novel variable elimination algorithm to produce a hybrid Bayes network, which can be used for exact Maximum A Posteriori estimation and marginalization over both sets of variables. Our approach first develops a novel hybrid Gaussian factor which can connect to both discrete and continuous variables, and a hybrid conditional which can represent multiple continuous hypotheses conditioned on the discrete variables. Using these representations, we derive the process of hybrid variable elimination under the Conditional Linear Gaussian scheme, giving us exact posteriors as a hybrid Bayes network. To bound the number of discrete hypotheses, we use a tree-structured representation of the factors coupled with a simple pruning and probabilistic assignment scheme, which allows for tractable inference. We demonstrate the applicability of our framework on a large scale SLAM dataset and a real world pose graph optimization problem, both with ambiguous measurements which require discrete choices to be made for the most likely measurements. Our demonstrated results showcase the accuracy, generality, and simplicity of our hybrid factor graph framework.