Incremental non-parametric (and parametric) solution to factor graphs

README.md

IncrementalInference.jl

Build Status codecov.io

IncrementalInference IncrementalInference IncrementalInference

Optimization routines for incremental non-parametric and parametric solutions based on factor graphs and the Bayes (Junction) tree implemented in the Julia language (and JuliaPro).

IMAGE ALT TEXT HERE

This package furthermore forms a cardinal piece of the Caesar.jl robotics toolkit, including 3D visualization and database interaction, which can serve as a base station for a robotic platform. A standalone Robot Motion Estimate package is also available.

Introduction

This package implements Multi-modal iSAM [1], a descendant of the iSAM2 [3] algorithm. The main algorithm is focused towards hybrid non-parametric and parametric inference over large factor graphs. Inference is performed via the Bayes tree (similar to Junction tree) where non-parametric and parametric solutions are based on belief propagation -- also known as the sum-product algorithm. Immediate benefits such as branch recycling is carried over as well. Also see related research work here [2].

The animation below shows 50% confidence lines of marginal beliefs relating to 6DOF robot poses. The approximate beliefs are being inferred through a process of successive approximation. The black trace shows the initial belief, and red the final output belief estimate. Notice the mode cycling during the process, brought about by information from elsewhere in the graph. This animation illustrates the sum-product (belief propagation) process, during the upward pass on Bayes tree from a real data.

successive approximation

Comments, questions and issues welcome.

Installation

Pre-install the following packages system wide packages[, and easily draw factor graph and Bayes tree]:

sudo apt-get install hdf5-tools
sudo apt-get install graphviz  # optional

Install the package from inside Julia

Pkg.add("IncrementalInference")

Basic example

This library is built as solver back-end which can be easily modified and extended. Specific emphasis is placed on allowing outside user defined constraint definitions to be used. The current major use case is through RoME.jl which introduces various sensor measurement and motion manifold functions for use in Robot Motion Estimate.

A few short examples, such as the multi-modal 4 door robot example, is available in the example folder:

examples/RobotfourDoor.jl

Here 4 simultaneous modes are considered producing multi-modal posterior beliefs in the continuous domain, final consensus output and ground truth belief are show below.

Four door final result

DataBase interaction layer

The data layer of the solver can be swapped away from the default Julia based Graphs.jl. For using the solver on a DataBase layer please see Caesar.jl and associated CloudGraphs project.

Contributors

D. Fourie, M. Kaess, J. Leonard

References

[1] Fourie, Dehann, et al. "A Nonparametric Belief Solution to the Bayes Tree" IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), (2016).

[2] Fourie, Dehann, "Multi-modal and Inertial Sensor Solutions for Navigation-type Factor Graphs", Joint Program with Massachusetts Institute of Technology and Woods Hole Oceanographic Institution, Cambridge, MA, USA, August 2017.

[3] Kaess, Michael, et al. "iSAM2: Incremental smoothing and mapping using the Bayes tree" The International Journal of Robotics Research (2011): 0278364911430419.