Solving Graphs

Non-parametric Batch Solve

When you have built the graph, you can call the solver to perform inference with the following:

# Perform inference
tree = solveTree!(fg) # or solveGraph!

The returned Bayes (Junction) tree object is described in more detail on a dedicated documentation page, while smt and hist return values most closely relate to development and debug outputs which can be ignored during general use. Should an error occur during, the exception information is easily accessible in the smt object (as well as file logs which default to /tmp/caesar/).

IncrementalInference.solveTree! — Function

solveTree!(dfgl; ...)
solveTree!(
    dfgl,
    oldtree;
    timeout,
    storeOld,
    verbose,
    verbosefid,
    delaycliqs,
    recordcliqs,
    limititercliqs,
    injectDelayBefore,
    skipcliqids,
    eliminationOrder,
    eliminationConstraints,
    smtasks,
    dotreedraw,
    runtaskmonitor,
    algorithm,
    solveKey,
    multithread
)

Perform inference over the Bayes tree according to opt::SolverParams and keyword arguments.

Notes

Aliased with solveGraph!
Variety of options, including fixed-lag solving – see getSolverParams(fg) for details.
- See online Documentation for more details: https://juliarobotics.org/Caesar.jl/latest/
Latest result always stored in solvekey=:default.
Experimental storeOld::Bool=true will duplicate the current result as supersolve :default_k.
- Based on solvable==1 assumption.
limititercliqs allows user to limit the number of iterations a specific CSM does.
keywords verbose and verbosefid::IOStream can be used together to to send output to file or default stdout.
keyword recordcliqs=[:x0; :x7...] identifies by frontals which cliques to record CSM steps.
- See repeatCSMStep!, printCSMHistoryLogical, printCSMHistorySequential

DevNotes

TODO Change keyword arguments to new @parameter SolverOptions type.

Example

# pass in old `tree` to enable compute recycling -- see online Documentation for more details
tree = solveTree!(fg [,tree])

solveGraph!, solveCliqUp!, solveCliqDown!, buildTreeReset!, repeatCSMStep, printCSMHistoryLogical

source

Automatic vs Manual Init

Currently the main automatic initialization technique used by IncrementalInference.jl by delayed propagation of belief on the factor graph. This can be globally or locally controlled via:

getSolverParams(fg).graphinit = false

# or locally at each addFactor
addFactor!(fg, [:x0;:x1], LinearRelative(Normal()); graphinit=false)

Use initVariable! if you'd like to force a particular numerical initialization of some or all the variables.

IncrementalInference.initVariable! — Function

initVariable!(
    variable::DFGVariable,
    ptsArr::ManifoldKernelDensity;
    ...
)
initVariable!(
    variable::DFGVariable,
    ptsArr::ManifoldKernelDensity,
    solveKey::Symbol;
    dontmargin,
    N
)

Method to manually initialize a variable using a set of points.

Notes

Disable automated graphinit on `addFactor!(fg, ...; graphinit=false)
- any un-initialized variables will automatically be initialized by solveTree!

Example:

# some variable is added to fg
addVariable!(fg, :somepoint3, ContinuousEuclid{2})

# data is organized as (row,col) == (dimension, samples)
pts = randn(2,100)
initVariable!(fg, :somepoint3, pts)

# manifold management should be done automatically.
# note upgrades are coming to consolidate with Manifolds.jl, see RoME #244

## it is also possible to initVariable! by using existing factors, e.g.
initVariable!(fg, :x3, [:x2x3f1])

DevNotes

TODO better document graphinit and treeinit.

source

All the variables can be initialized without solving with:

IncrementalInference.initAll! — Function

initAll!(dfg; ...)
initAll!(dfg, solveKey; _parametricInit, solvable, N)

Perform graphinit over all variables with solvable=1 (default).

See also: ensureSolvable!, (EXPERIMENTAL 'treeinit')

source

Using Incremental Updates (Clique Recycling I)

One of the major features of the MM-iSAMv2 algorithm (implemented by IncrementalInference.jl) is reducing computational load by recycling and marginalizing different (usually older) parts of the factor graph. In order to utilize the benefits of recycing, the previous Bayes (Junction) tree should also be provided as input (see fixed-lag examples for more details):

tree = solveTree!(fg, tree)

Using Clique out-marginalization (Clique Recycling II)

When building sysmtes with limited computation resources, the out-marginalization of cliques on the Bayes tree can be used. This approach limits the amount of variables that are inferred on each solution of the graph. This method is also a compliment to the above Incremental Recycling – these two methods can work in tandem. There is a default setting for a FIFO out-marginalization strategy (with some additional tricks):

defaultFixedLagOnTree!(fg, 50, limitfixeddown=true)

This call will keep the latest 50 variables fluid for inference during Bayes tree inference. The keyword limitfixeddown=true in this case will also prevent downward message passing on the Bayes tree from propagating into the out-marginalized branches on the tree. A later page in this documentation will discuss how the inference algorithm and Bayes tree aspects are put together.

Synchronizing Over a Factor Graph

When adding Variables and Factors, use solvable=0 to disable the new fragments until ready for inference, for example

addVariable!(fg, :x45, Pose2, solvable=0)
newfct = addFactor!(fg, [:x11,:x12], Pose2Pose2, solvable=0)

These parts of the factor graph can simply be activated for solving:

setSolvable!(fg, :x45, 1)
setSolvable!(fg, newfct.label, 1)