This tutorial illustrates how
IncrementalInference enables algebraic relations (residual functions) between multiple stochastic variables, and how a final posterior belief estimate is calculated from several pieces of information. The application of this tutorial is presented in abstract from which the user is free to imagine any system of relationships: For example, a robot driving in a one dimensional world; or a time traveler making uncertain jumps forwards and backwards in time. The tutorial implicitly shows a multi-modal uncertainty introduced and transmitted. The tutorial also illustrates consensus through an additional piece of information, which reduces all stochastic variable marginal beliefs to unimodal only beliefs. The example will also illustrate the use of non-Gaussian beliefs and global inference. Lastly, the tutorial demonstrates how automatic initialization of variables works.
This tutorial requires
IncrementalInference v0.3.0+, RoME v0.1.0, RoMEPlotting packages be installed. In addition, the optional
GraphViz package will allow easy visualization of the
FactorGraph object structure.
To start, the two major mathematical packages are brought into scope.
using IncrementalInference # using Distributions # automatically reexported by IncrementalInference
Guidelines for developing your own functions are discussed here in Adding Variables and Factors, and we note that mechanizations and manifolds required for robotic simultaneous localization and mapping (SLAM) has been tightly integrated with the expansion package RoME.jl.
The next step is to describe the inference problem with a graphical model of type
IncrementalInference.FactorGraph. The first step is to create an empty factor graph object and start populating it with variable nodes. The variable nodes are identified by
:x0, :x1, :x2, :x3.
# Start with an empty factor graph fg = emptyFactorGraph() # add the first node addVariable!(fg, :x0, ContinuousScalar) # this is unary (prior) factor and does not immediately trigger autoinit of :x0. addFactor!(fg, [:x0], Prior(Normal(0,1)))
Factor graphs are bipartite graphs with
factors that act as mathematical structure between interacting
variables. After adding node
:x0, a singleton factor of type
Prior (which was defined by the user earlier) is 'connected to' variable node
:x0. This unary factor is taken as a
Distributions.Normal distribution with zero mean and a standard devitation of
Graphviz can be used to visualize the factor graph structure, although the package is not installed by default –
$ sudo apt-get install graphviz. Furthermore, the
writeGraphPdf member definition is given at the end of this tutorial, which allows the user to store the graph image in graphviz supported image types.
writeGraphPdf(fg) # writeGraphPdf(fg, file="fgx01.pdf") # file="fgx01.png"
The two node factor graph is shown in the image below.
Automatic initialization of variables depend on how the factor graph model is constructed. This tutorial demonstrates this behavior by first showing that
:x0 is not initialized:
@show isInitialized(fg, :x0) # false
:x0 not initialized? Since no other variable nodes have been 'connected to' (or depend) on
:x0 and future intentions of the user are unknown, the initialization of
:x0 is deferred until the latest possible moment.
IncrementalInference.jl assumes that the user will generally populate new variable nodes with most of the associated factors before moving to the next variable. By delaying initialization of a new variable (say
:x0) until a second newer uninitialized variable (say
:x1) depends on
IncrementalInference algorithms hope to then initialize
:x0 with the more information from previous and surrounding variables and factors. Also note that initialization of variables is a local operation based only on the neighboring nodes – global inference will over the entire graph is shows later in this tutorial.
:x1 and connecting it through the
Normal distributed factor, the automatic initialization of
:x0 is triggered.
addVariable!(fg, :x1, ContinuousScalar) # P(Z | :x1 - :x0 ) where Z ~ Normal(10,1) addFactor!(fg, [:x0, :x1], LinearConditional(Normal(10.0,1))) @show isInitialized(fg, :x0) # true
Note that the automatic initialization of
:x0 is aware that
:x1 is not initialized and therefore only used the
Prior(Normal(0,1)) unary factor to initialize the marginal belief estimate for
:x0. The structure of the graph has now been updated to two variable nodes and two factors.
Global inference requires that the entire factor graph be initialized before the numerical belief computation algorithms can be performed. Notice how the new
:x1 variable is not yet initialized:
@show isInitialized(fg, :x1) # false
RoMEPlotting.jl package allows visualization (plotting) of the belief state over any of the variable nodes. Remember the first time executions are slow given required code compilation, and that future versions of these package will use more precompilation to reduce first execution running cost.
using RoMEPlotting plotKDE(fg, :x0)
By forcing the initialization of
:x1 and plotting its belief estimate,
ensureAllInitialized!(fg) plotKDE(fg, [:x0, :x1])
the predicted influence of the
P(Z| X1 - X0) = LinearConditional(Normal(10, 1)) is shown by the red trace.
The red trace (predicted belief of
:x1) is noting more than the approximated convolution of the current marginal belief of
:x0 with the conditional belief described by
P(Z | X1 - X0).
:x2 is 'connected' to
:x1 through a more complicated
MixtureLinearConditional likelihood function.
addVariable!(fg, :x2, ContinuousScalar) mmo = MixtureLinearConditional([Rayleigh(3); Uniform(30,55)], Categorical([0.4; 0.6])) addFactor!(fg, [:x1, :x2], mmo)
mmo variable illustrates how a near arbitrary mixture probability distribution can be used as a conditional relationship between variable nodes in the factor graph. In this case, a 40%/60% balance of a Rayleigh and truncated Uniform distribution which acts as a multi-modal conditional belief. Interpret carefully what a conditional belief of this nature actually means.
Following the tutorial's practical example frameworks (robot navigation or time travel), this multi-modal belief implies that moving from one of the probable locations in
:x1 to a location in
:x2 by some processes defined by
mmo=P(Z | X2, X1) is uncertain to the same 40%/60% ratio. In practical terms, collapsing (through observation of an event) the probabilistic likelihoods of the transition from
:x2 may result in the
:x2 location being at either 15-20, or 40-65-ish units. The predicted belief over
:x2 is illustrated by plotting the predicted belief (green trace), after forcing initialization.
ensureAllInitialized!(fg) plotKDE(fg, [:x0, :x1, :x2])
Adding one more variable
:x3 through another
addVariable!(fg, :x3, ContinuousScalar) addFactor!(fg, [:x2, :x3], LinearConditional(Normal(-50, 1)))
expands the factor graph to to four variables and four factors.
This part of the tutorial shows how a unimodal likelihood (conditional belief) can transmit the bimodal belief currently contained in
ensureAllInitialized!(fg) plotKDE(fg, [:x0, :x1, :x2, :x3])
Notice the blue trace (
:x3) is a shifted and slightly spread out version of the initialized belief on
:x2, through the convolution with the conditional belief
P(Z | X2, X3).
Global inference over the entire factor graph has still not occurred, and will at this stage produce roughly similar results to the predicted beliefs shown above. Only by introducing more information into the factor graph can inference extract more precise marginal belief estimates for each of the variables. A final piece of information added to this graph is a factor directly relating
addFactor!(fg, [:x3, :x0], LinearConditional(Normal(40, 1)))
Pay close attention to what this last factor means in terms of the probability density traces shown in the previous figure. The blue trace for
:x3 has two major modes, one that overlaps with
:x0, :x1 near 0 and a second mode further to the left at -40. The last factor introduces a shift
LinearConditional(Normal(40,1)) which essentially aligns the left most mode of
:x3 back onto
This last factor forces a mode selection through consensus. By doing global inference, the new information obtained in
:x3 will be equally propagated to
:x2 where only one of the two modes will remain.
Global inference is achieved with local computation using two function calls, as follows.
tree = batchSolve!(fg) # and visualization plotKDE(fg, [:x0, :x1, :x2, :x3])
The resulting posterior marginal beliefs over all the system variables are:
It is import to note that although this tutorial ends with all marginal beliefs having near Gaussian shape and are unimodal, that the package supports multi-modal belief estimates during both the prediction and global inference processes. In fact, many of the same underlying inference functions are involved with the automatic initialization process and the global multi-modal iSAM inference procedure. This concludes the ContinuousScalar tutorial particular to the