getRangeKDEMax2D(fgl, vsym1, vsym2)

Calculate the cartesian distance between two vertices in the graph using their symbol name, and by maximum belief point.

initFactorGraph!(fg; P0, init, N, lbl, solvable, firstPoseType, labels)

Initialize a factor graph object as Pose2, Pose3, or neither and returns variable and factor symbols as array.

addOdoFG!(fg, n, DX, cov; N, solvable, labels)

Create a new variable node and insert odometry constraint factor between which will automatically increment latest pose symbol x<k+1> for new node new node and constraint factor are returned as a tuple.

addOdoFG!(fgl, odo; N, solvable, labels)

Create a new variable node and insert odometry constraint factor between which will automatically increment latest pose symbol x<k+1> for new node new node and constraint factor are returned as a tuple.

addVariable!(dfg, label, varTypeU; N, solvable, timestamp, nanosecondtime, dontmargin, labels, tags, smalldata, checkduplicates, initsolvekeys)

Add a variable node label::Symbol to dfg::AbstractDFG, as varType<:InferenceVariable.

Notes

• keyword nanosecondtime is experimental and intended as the whole subsection portion – i.e. accurateTime = (timestamp MOD second) + Nanosecond

Example

fg = initfg()
addVariable!(fg, :x0, Pose2)

addFactor!(dfg, Xi, usrfnc; multihypo, nullhypo, solvable, tags, timestamp, graphinit, threadmodel, maxparallel)

Add factor with user defined type <: FunctorInferenceType to the factor graph object. Define whether the automatic initialization of variables should be performed. Use order sensitive multihypo keyword argument to define if any variables are related to data association uncertainty.

approxCliqMarginalUp!(csmc)
approxCliqMarginalUp!(csmc, childmsgs; N, dbg, multiproc, logger, iters, drawpdf)

Approximate Chapman-Kolmogorov transit integral and return separator marginals as messages to pass up the Bayes (Junction) tree, along with additional clique operation values for debugging.

Notes

• onduplicate=true by default internally uses deepcopy of factor graph and Bayes tree, and does not update the given objects. Set false to update fgl and treel during compute.

Future

• TODO: internal function chain is too long and needs to be refactored for maintainability.

approxConv(dfg, fc, towards)
approxConv(dfg, fc, towards, measurement; N)

Draw samples from the approximate convolution of towards symbol using factor fct relative to the other variables. In addition the api can be adjusted to recover the data from elsewhere (likely to be replaced/removed in the future).

areCliqVariablesAllMarginalized(subfg, cliq)

Return true if all variables in clique are considered marginalized (and initialized).

asyncTreeInferUp!(dfg, treel; oldtree, verbose, drawtree, N, limititers, downsolve, incremental, limititercliqs, skipcliqids, delaycliqs, recordcliqs)

Perform tree based initialization of all variables not yet initialized in factor graph as non-blocking method.

Notes:

• To simplify debugging, this method does not include the usual @ sync around all the state machine async processes.
• Extract the error stack with a fetch on the failed process return by this function.

Related

initInferTreeUp!

attemptTreeSimilarClique(othertree, seeksSimilar)

Special internal function to try return the clique data if succesfully identified in othertree::AbstractBayesTree, based on contents of seeksSimilar::BayesTreeNodeData.

Notes

• Used to identify and skip similar cliques (i.e. recycle computations)

fetchChildrenStatusUp(tree, cliq)
fetchChildrenStatusUp(tree, cliq, logger)

Fetch (block) caller until child cliques of cliq::TreeClique have valid csm status.

Notes:

• Return ::Dict{Symbol} indicating whether next action that should be taken for each child clique.
• See status options at getCliqueStatus(..).
• Can be called multiple times

buildTreeFromOrdering!(dfg, p; drawbayesnet, maxparallel, solvable)

Build Bayes/Junction/Elimination tree from a given variable ordering.

buildTreeFromOrdering!(dfg, p; drawbayesnet, maxparallel)

Build Bayes/Junction/Elimination tree from a given variable ordering.

childCliqs(treel, cliq)

Return a vector of child cliques to cliq.

cliqGibbs(fg, cliq, vsym, inmsgs, N, dbg, manis)
cliqGibbs(fg, cliq, vsym, inmsgs, N, dbg, manis, logger)

Perform one step of the minibatch clique Gibbs operation for solving the Chapman-Kolmogov trasit integral – here involving separate approximate functional convolution and product operations.

cliqHistFilterTransitions(hist, nextfnc)

Return state machine transition steps from history such that the nextfnc::Function.

Related:

printCliqHistorySummary, filterHistAllToArray, sandboxCliqResolveStep

cliqInitSolveUpByStateMachine!(dfg, tree, cliq; N, verbose, oldcliqdata, drawtree, show, incremental, limititers, upsolve, downsolve, recordhistory, delay, logger)

Perform upward inference using a state machine solution approach.

Notes:

• will call on values from children or parent cliques
• can be called multiple times
• Assumes all cliques in tree are being solved simultaneously and in similar manner.
• State machine rev.1 – copied from first TreeBasedInitialization.jl.
• Doesn't do partial initialized state properly yet.

csmAnimate(tree, autohist; frames, interval, dpi, rmfirst, folderpath, fsmColors, defaultColor)

Animate multiple clique state machines on the same graphviz visualization. Renders according to linear time for all provided histories.

Example:

using Caesar

# build a factor graph
fg = initfg()
# addVariable!(...)
# addFactor!(...)
# ...

fsy = getTreeAllFrontalSyms(fg, tree) # for later use
# perform inference to find the factor graph marginal posterior estimates
tree, smt, hist = solveTree!(fg, recordcliqs=fsy)

# generate frames in standard location /tmp/caesar/csmCompound/
#  requires: sudo apt-get install graphviz
csmAnimate(fg, tree, fsy, frames=500)

# to render and show from default location (might require)
#  sudo apt-get install ffmpeg vlc

# .ogv [Totem Ubuntu default]
Base.rm("/tmp/caesar/csmCompound/out.ogv")
run(`ffmpeg -r 10 -i /tmp/caesar/csmCompound/csm_%d.png -c:v libtheora -vf fps=25 -pix_fmt yuv420p -vf "scale=trunc(iw/2)*2:trunc(ih/2)*2" -q 10 /tmp/caesar/csmCompound/out.ogv`)
run(`totem /tmp/caesar/csmCompound/out.ogv`)

# h.264 [VLC not default]
Base.rm("/tmp/caesar/csmCompound/out.mp4")
run(`ffmpeg -r 10 -i /tmp/caesar/csmCompound/csm_%d.png -c:v libx264 -vf fps=25 -pix_fmt yuv420p -vf "scale=trunc(iw/2)*2:trunc(ih/2)*2" /tmp/caesar/csmCompound/out.mp4`)
run(`vlc /tmp/caesar/csmCompound/out.mp4`)

cycleInitByVarOrder!(subfg, varorder; logger)

Cycle through var order and initialize variables as possible in subfg::AbstractDFG. Return true if something was updated.

Notes:

• assumed subfg is a subgraph containing only the factors that can be used.
  • including the required up or down messages
• intended for both up and down initialization operations.

Dev Notes

• Should monitor updates based on the number of inferred & solvable dimensions

doautoinit!(dfg, xi; singles, N, logger)

EXPERIMENTAL: initialize target variable xi based on connected factors in the factor graph fgl. Possibly called from addFactor!, or doCliqAutoInitUp! (?).

Notes:

• Special carve out for multihypo cases, see issue 427.

Development Notes:

Target factor is first (singletons) or second (dim 2 pairwise) variable vertex in xi.

• TODO use DFG properly with local operations and DB update at end.
• TODO get faster version of isInitialized for database version.
• TODO: Persist this back if we want to here.

downGibbsCliqueDensity(fg, cliq, dwnMsgs)
downGibbsCliqueDensity(fg, cliq, dwnMsgs, N)
downGibbsCliqueDensity(fg, cliq, dwnMsgs, N, MCMCIter)
downGibbsCliqueDensity(fg, cliq, dwnMsgs, N, MCMCIter, dbg)
downGibbsCliqueDensity(fg, cliq, dwnMsgs, N, MCMCIter, dbg, usemsgpriors)
downGibbsCliqueDensity(fg, cliq, dwnMsgs, N, MCMCIter, dbg, usemsgpriors, logger)

Perform Chapman-Kolmogorov transit integral approximation for cliq in downward pass direction.

Notes

• Only update frontal variables of the clique.

drawCliqSubgraphUpMocking(fgl, treel, frontalSym; show, filepath, engine, viewerapp)

Construct (new) subgraph and draw the subgraph associated with clique frontalSym::Symbol.

Notes

• See drawGraphCliq/writeGraphPdf for details on keyword options.

Related

drawGraphCliq, spyCliqMat, drawTree, buildCliqSubgraphUp, buildSubgraphFromLabels!

fifoFreeze!(dfg)

Freeze nodes that are older than the quasi fixed-lag length defined by fg.qfl, according to fg.fifo ordering.

Future:

• Allow different freezing strategies beyond fifo.

filterHistAllToArray(tree, hists, frontals, nextfnc)

Return state machine transition steps from all cliq histories with transition nextfnc::Function.

Related:

printCliqHistorySummary, cliqHistFilterTransitions, sandboxCliqResolveStep

findRelatedFromPotential(dfg, fct, varid, N)
findRelatedFromPotential(dfg, fct, varid, N, dbg; solveKey)

Compute proposal belief on vertid through fct representing some constraint in factor graph. Always full dimension variable node – partial constraints will only influence subset of variable dimensions. The remaining dimensions will keep pre-existing variable values.

Notes

• fulldim is true when "rank-deficient" – TODO swap to false (or even float)

fmcmc!(fgl, cliq, fmsgs, lbls, N, MCMCIter)
fmcmc!(fgl, cliq, fmsgs, lbls, N, MCMCIter, dbg)
fmcmc!(fgl, cliq, fmsgs, lbls, N, MCMCIter, dbg, logger)
fmcmc!(fgl, cliq, fmsgs, lbls, N, MCMCIter, dbg, logger, multithreaded)

Iterate successive approximations of clique marginal beliefs by means of the stipulated proposal convolutions and products of the functional objects for tree clique cliq.

getCliqAllVarIds(cliq)

Get all cliq variable ids::Symbol.

Related

getCliqVarIdsAll, getCliqAllFactIds, getCliqVarsWithFrontalNeighbors

Get all cliq variable labels as ::Symbol.

getCliqAssocMat(cliq)

Return boolean matrix of factor by variable (row by column) associations within clique, corresponds to order presented by getCliqFactorIds and getCliqAllVarIds.

getCliqDepth(tree, cliq)

Return depth in tree as ::Int, with root as depth=0.

Related

getCliq

getCliqDownMsgsAfterDownSolve(subdfg, cliq)

Return dictionary of down messages consisting of all frontal and separator beliefs of this clique.

Notes:

• Fetches numerical results from subdfg as dictated in cliq.
• return LikelihoodMessage

getCliqFrontalVarIds(cliqdata)

Get the frontal variable IDs ::Int for a given clique in a Bayes (Junction) tree.

getCliqVarInitOrderUp(tree, cliq)

Return the most likely ordering for initializing factor (assuming up solve sequence).

Notes:

• sorts id for increasing number of connected factors.

getCliqMat(cliq; showmsg)

Return boolean matrix of factor variable associations for a clique, optionally including (showmsg::Bool=true) the upward message singletons. Variable order corresponds to getCliqAllVarIds.

getCliqSeparatorVarIds(cliqdata)

Get cliq separator (a.k.a. conditional) variable ids::Symbol.

getCliqSiblings(treel, cliq)
getCliqSiblings(treel, cliq, inclusive)

Return a vector of all siblings to a clique, which defaults to not inclusive the calling cliq.

getCliqVarIdsPriors(cliq)
getCliqVarIdsPriors(cliq, allids)
getCliqVarIdsPriors(cliq, allids, partials)

Get variable ids::Int with prior factors associated with this cliq.

Notes:

• does not include any singleton messages from upward or downward message passing.

getCliqVarSingletons(cliq)
getCliqVarSingletons(cliq, allids)
getCliqVarSingletons(cliq, allids, partials)

Get cliq variable IDs with singleton factors – i.e. both in clique priors and up messages.

getBelief(vnd)

Get KernelDensityEstimate kde estimate stored in variable node.

getBelief(vnd)

Get KernelDensityEstimate kde estimate stored in variable node.

getLogPath(opt)

Get the folder location where debug and solver information is recorded for a particular factor graph.

getManifolds(vd)

Return the manifolds on which variable sym::Symbol is defined.

getParent(treel, afrontal)

Return cliq's parent clique.

getTreeAllFrontalSyms(fgl, tree)

Return one symbol (a frontal variable) from each clique in the ::BayesTree.

Notes

• Frontal variables only occur once in a clique per tree, therefore is a unique identifier.

Related:

whichCliq, printCliqHistorySummary

getVariableDim(vard)

Return the number of dimensions this variable vertex var contains.

Related

getVariableInferredDim, getVariableInferredDimFraction

getVariableInferredDim(vard)
getVariableInferredDim(vard, saturate)

Return the number of projected dimensions into a variable during inference.

Notes

• saturate clamps return value to no greater than variable dimension

Related

getVariableDim, getVariableInferredDimFraction, getVariableInferredDim, getVariableDim

hasClique(bt, frt)

Return boolean on whether the frontal variable frt::Symbol exists somewhere in the ::BayesTree.

initfg()
initfg(dfg; sessionname, robotname, username, cloudgraph)

Initialize an empty in-memory DistributedFactorGraph ::DistributedFactorGraph object.

initInferTreeUp!(dfg, treel; oldtree, verbose, drawtree, N, limititers, downsolve, incremental, limititercliqs, skipcliqids, recordcliqs, delaycliqs)

Perform tree based initialization of all variables not yet initialized in factor graph.

Related

asyncTreeInferUp!

isCliqMarginalizedFromVars(subfg, cliq)

Return ::Bool on whether all variables in this cliq are marginalzed.

isInitialized(vert)

Returns state of vertex data .initialized flag.

Notes:

• used by Bayes tree clique logic.
• similar method in DFG

isMarginalized(vert)

Return ::Bool on whether this variable has been marginalized.

isTreeSolved(treel; skipinitialized)

Return true or false depending on whether the tree has been fully initialized/solved/marginalized.

Return ::Bool on whether factor is a partial constraint.

localProduct(dfg, sym; solveKey, N, dbg, logger)

Using factor graph object dfg, project belief through connected factors (convolution with conditional) to variable sym followed by a approximate functional product.

Return: product belief, full proposals, partial dimension proposals, labels

makeCsmMovie(fg, tree)
makeCsmMovie(fg, tree, cliqs; assignhist, show, filename, frames)

Convenience function to assign and make video of CSM state machine for cliqs.

Notes

• Probably several teething issues still (lower priority).
• Use assignhist if solver params async was true, or errored.

Related

csmAnimate, printCliqHistorySummary

initManual!(dfg, vert, pX)

Workaround function when first-version (factor graph based) auto initialization fails. Usually occurs when using factors that have high connectivity to multiple variables.

parentCliq(treel, cliq)

Return cliq's parent clique.

packFromLocalPotentials!(dfg, dens, wfac, cliq, vsym, N)
packFromLocalPotentials!(dfg, dens, wfac, cliq, vsym, N, dbg)

Add all potentials associated with this clique and vertid to dens.

predictVariableByFactor(dfg, targetsym, fct, prevars)

Method to compare current and predicted estimate on a variable, developed for testing a new factor before adding to the factor graph.

Notes

• fct does not have to be in the factor graph – likely used to test beforehand.
• function is useful for detecting if multihypo should be used.
• approxConv will project the full belief estimate through some factor but must already be in factor graph.

Example

# fg already exists containing :x7 and :l3
pp = Pose2Point2BearingRange(Normal(0,0.1),Normal(10,1.0))
# possible new measurement from :x7 to :l3
curr, pred = predictVariableByFactor(fg, :l3, pp, [:x7; :l3])
# example of naive user defined test on fit score
fitscore = minkld(curr, pred)
# `multihypo` can be used as option between existing or new variables

Related

approxConv

prepBatchTree!(dfg; variableOrder, variableConstraints, ordering, drawpdf, show, filepath, viewerapp, imgs, drawbayesnet, maxparallel)

Build Bayes/Junction/Elimination tree.

Notes

• Default to free qr factorization for variable elimination order.

printCliqHistorySummary(fid, hist)
printCliqHistorySummary(fid, hist, cliqid)

Print a short summary of state machine history for a clique solve.

Related:

getTreeAllFrontalSyms, animateCliqStateMachines, printHistoryLine, printCliqHistorySequential

productpartials!(pGM, dummy, partials, manis)

Multiply different dimensions from partial constraints individually.

prodmultiplefullpartials(dens, partials, Ndims, N, manis)

Multiply various full and partial dimension constraints.

Multiply a single full and several partial dimension constraints.

resetBuildTreeFromOrder!(fgl, p)

Reset factor graph and build a new tree from the provided variable ordering p.

resetCliqSolve!(dfg, treel, cliq; solveKey)

Reset the state of all variables in a clique to not initialized.

Notes

• resets numberical values to zeros.

Dev Notes

• TODO not all kde manifolds will initialize to zero.
• FIXME channels need to be consolidated

resetData!(vdata)

Partial reset of basic data fields in ::VariableNodeData of ::FunctionNode structures.

resetTreeCliquesForUpSolve!(treel)

Reset the Bayes (Junction) tree so that a new upsolve can be performed.

Notes

• Will change previous clique status from :downsolved to :initialized only.
• Sets the color of tree clique to lightgreen.

resetVariable!(varid; solveKey)

Reset the solve state of a variable to uninitialized/unsolved state.

sandboxCliqResolveStep(tree, frontal, step)

Repeat a solver state machine step without changing history or primary values.

printCliqSummary, printCliqHistorySummary, cliqHistFilterTransitions

setCliqAsMarginalized!(cliq, status)

Set the marginalized status of a clique.

setfreeze!(dfg, sym)

Set variable(s) sym of factor graph to be marginalized – i.e. not be updated by inference computation.

setValKDE!(vd, pts, bws)
setValKDE!(vd, pts, bws, setinit)
setValKDE!(vd, pts, bws, setinit, inferdim)

Set the point centers and bandwidth parameters of a variable node, also set isInitialized=true if setinit::Bool=true (as per default).

Notes

• initialized is used for initial solve of factor graph where variables are not yet initialized.
• inferdim is used to identify if the initialized was only partial.

setVariableInitialized!(varid, status)

Set variable initialized status.

setVariableInferDim!(varid, val)

Set method for the inferred dimension value in a variable.

solveCliq!(dfgl, tree, cliqid; verbose, recordcliq, async)

Perform inference over one clique in the Bayes tree according to opt::SolverParams.

Example

tree = wipeBuildNewTree!(fg)
smt, hist = solveCliq!(fg, tree, :x1 [,cliqHistories=hist] )

Related

solveTree!, wipeBuildNewTree!

solveCliqWithStateMachine!(dfg, tree, frontal; iters, downsolve, recordhistory, verbose, nextfnc, prevcsmc)

Standalone state machine solution for a single clique.

Related:

initInferTreeUp!

solveTree!(dfgl)
solveTree!(dfgl, oldtree; storeOld, verbose, delaycliqs, recordcliqs, limititercliqs, skipcliqids, maxparallel, variableOrder, variableConstraints)

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

Notes

• Variety of options, including fixed-lag solving – see getSolverParams(fg) for details.
• 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.

Example

# without [or with] compute recycling
tree, smt, hist = solveTree!(fg [,tree])

Related

solveCliq!, wipeBuildNewTree!

transferUpdateSubGraph!(dest, src)
transferUpdateSubGraph!(dest, src, syms)
transferUpdateSubGraph!(dest, src, syms, logger; updatePPE, solveKey)

Transfer contents of src::AbstractDFG variables syms::Vector{Symbol} to dest::AbstractDFG. Notes

• Reads, dest := src, for all syms

treeProductDwn(fg, tree, cliq, sym; N, dbg)

Calculate a fresh–-single step–-approximation to the variable sym in clique cliq as though during the downward message passing. The full inference algorithm may repeatedly calculate successive apprimxations to the variable based on the structure of variables, factors, and incoming messages to this clique. Which clique to be used is defined by frontal variable symbols (cliq in this case) – see getClique(...) for more details. The sym symbol indicates which symbol of this clique to be calculated. Note that the sym variable must appear in the clique where cliq is a frontal variable.

treeProductUp(fg, tree, cliq, sym; N, dbg)

Calculate a fresh (single step) approximation to the variable sym in clique cliq as though during the upward message passing. The full inference algorithm may repeatedly calculate successive apprimxations to the variables based on the structure of the clique, factors, and incoming messages. Which clique to be used is defined by frontal variable symbols (cliq in this case) – see getClique(...) for more details. The sym symbol indicates which symbol of this clique to be calculated. Note that the sym variable must appear in the clique where cliq is a frontal variable.

unfreezeVariablesAll!(fgl)

Free all variables from marginalization.

Related

dontMarginalizeVariablesAll!

dontMarginalizeVariablesAll!(fgl)

Free all variables from marginalization.

updateFGBT!(fg, bt, cliqID, drt; dbg, fillcolor, logger)

Update cliq cliqID in Bayes (Juction) tree bt according to contents of ddt – intended use is to update main clique after a downward belief propagation computation has been completed per clique.

updateFGBT!(fg, cliq, IDvals; dbg, fillcolor, logger)

Update cliq cliqID in Bayes (Juction) tree bt according to contents of urt – intended use is to update main clique after a upward belief propagation computation has been completed per clique.

updateTreeCliquesAsMarginalizedFromVars!(fgl, tree)

Run through entire tree and set cliques as marginalized if all clique variables are marginalized.

Notes:

• TODO can be made fully parallel, consider converting for use with @threads for.

upGibbsCliqueDensity(inp)
upGibbsCliqueDensity(inp, N)
upGibbsCliqueDensity(inp, N, dbg)
upGibbsCliqueDensity(inp, N, dbg, iters)
upGibbsCliqueDensity(inp, N, dbg, iters, logger)

Perform computations required for the upward message passing during belief propation on the Bayes (Junction) tree. This function is usually called as via remote_call for multiprocess dispatch.

Example

inp = ExploreTreeType(fg,tree,cliq,parent,childmsgs)
urt = upGibbsCliqueDensity(inp)

• fg factor graph,
• tree Bayes tree,
• cliq which cliq to perform the computation on,
• parent the parent clique to where the upward message will be sent,
• childmsgs is for any incoming messages from child cliques.

resetVariableAllInitializations!(fgl)

Reset initialization flag on all variables in ::AbstractDFG.

Notes

• Numerical values remain, but inference will overwrite since init flags are now false.