Merge pull request #510 from JuliaRobotics/master

fast forward release/v0.8

Project.toml

@@ -2,11 +2,12 @@ name = "IncrementalInference"
 uuid = "904591bb-b899-562f-9e6f-b8df64c7d480"
 keywords = ["mm-iSAM", "SLAM", "inference", "sum-product", "belief-propagation"]
 desc = "Implements the multimodal iSAM algorithm."
-version = "0.8.2"
+version = "0.8.3"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
 Dates = "ade2ca70-3891-5945-98fb-dc099432e06a"
+DelimitedFiles = "8bb1440f-4735-579b-a4ab-409b98df4dab"
 Distributed = "8ba89e20-285c-5b6f-9357-94700520ee1b"
 DistributedFactorGraphs = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04"
 Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f"
@@ -32,7 +33,7 @@ SuiteSparse = "4607b0f0-06f3-5cda-b6b1-a6196a1729e9"
 
 [compat]
 ApproxManifoldProducts = "0.1, 0.2"
-DistributedFactorGraphs = "0.5.1"
+DistributedFactorGraphs = "0.5.2"
 Distributions = "0.18, 0.19, 0.20, 0.21, 0.22, 0.23, 0.24, 0.25, 1"
 DocStringExtensions = "0.7, 0.8, 0.9, 0.10, 1"
 FileIO = "1.0.2, 1.1, 1.2"

src/AdditionalUtils.jl

@@ -11,7 +11,7 @@ Notes
 
 Related
 
-drawGraphCliq, spyCliqMat, drawTree, buildCliqSubgraphUp, buildSubgraphFromLabels
+drawGraphCliq, spyCliqMat, drawTree, buildCliqSubgraphUp, buildSubgraphFromLabels!
 """
 function drawCliqSubgraphUpMocking(fgl::G,
                                    treel::BayesTree,

src/CanonicalGraphExamples.jl

@@ -0,0 +1,106 @@
+
+export loadCanonicalFG_Kaess, loadCanonicalFG_TestSymbolic, loadCanonicalFG_CaesarRing1D
+
+
+"""
+    $SIGNATURES
+
+Canonical example from literature, Kaess, et al.: ISAM2, IJRR, 2011.
+
+Notes
+- Paper variable ordering: p = [:l1;:l2;:x1;:x2;:x3]
+"""
+function loadCanonicalFG_Kaess(;graphinit::Bool=false)
+  fg = initfg()
+
+  addVariable!(fg,:x1, ContinuousScalar)
+  addFactor!(fg, [:x1;], Prior(Normal()), autoinit=graphinit)
+
+
+  addVariable!(fg,:x2, ContinuousScalar)
+  addFactor!(fg,[:x1, :x2], LinearConditional(Normal()), autoinit=graphinit)
+
+  addVariable!(fg, :x3, ContinuousScalar)
+  addFactor!(fg,[:x2,:x3],LinearConditional(Normal()), autoinit=graphinit)
+
+  addVariable!(fg, :l1, ContinuousScalar)
+  addFactor!(fg, [:x1,:l1], LinearConditional(Normal()) , autoinit=graphinit)
+  addFactor!(fg, [:x2,:l1], LinearConditional(Normal()) , autoinit=graphinit)
+
+  addVariable!(fg, :l2, ContinuousScalar)
+  addFactor!(fg, [:x3,:l2], LinearConditional(Normal()), autoinit=graphinit)
+
+  return fg
+end
+
+
+"""
+    $SIGNATURES
+
+Canonical example introduced by Borglab.
+
+Notes
+- Known variable ordering: p = [:x1; :l3; :l1; :x5; :x2; :l2; :x4; :x3]
+"""
+function loadCanonicalFG_TestSymbolic(;graphinit::Bool=false)
+  fg = initfg()
+
+  addVariable!(fg, :x1, ContinuousScalar)
+  addVariable!(fg, :x2, ContinuousScalar)
+  addVariable!(fg, :x3, ContinuousScalar)
+  addVariable!(fg, :x4, ContinuousScalar)
+  addVariable!(fg, :x5, ContinuousScalar)
+  addVariable!(fg, :l1, ContinuousScalar)
+  addVariable!(fg, :l2, ContinuousScalar)
+  addVariable!(fg, :l3, ContinuousScalar)
+
+  addFactor!(fg, [:x1;:l1], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x1;:x2], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x2;:l1], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x2;:x3], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x3;:x4], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x4;:l2], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x4;:x5], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:l2;:x5], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x4;:l3], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x5;:l3], LinearConditional(Normal()), autoinit=graphinit)
+
+  return fg
+end
+
+
+
+"""
+    $SIGNATURES
+
+Canonical example introduced originally as Caesar Hex Example.
+
+Notes
+- Paper variable ordering: p = [:x0;:x2;:x4;:x6;:x1;:l1;:x5;:x3;]
+"""
+function loadCanonicalFG_CaesarRing1D(;graphinit::Bool=false)
+
+  fg = initfg()
+
+  addVariable!(fg, :x0, ContinuousScalar)
+  addVariable!(fg, :x1, ContinuousScalar)
+  addVariable!(fg, :x2, ContinuousScalar)
+  addVariable!(fg, :x3, ContinuousScalar)
+  addVariable!(fg, :x4, ContinuousScalar)
+  addVariable!(fg, :x5, ContinuousScalar)
+  addVariable!(fg, :x6, ContinuousScalar)
+
+  addFactor!(fg, [:x0], Prior(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x0;:x1], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x1;:x2], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x2;:x3], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x3;:x4], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x4;:x5], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x5;:x6], LinearConditional(Normal()), autoinit=graphinit)
+
+  addVariable!(fg, :l1, ContinuousScalar)
+  addFactor!(fg, [:x0;:l1], LinearConditional(Normal()), autoinit=graphinit)
+  addFactor!(fg, [:x6;:l1], LinearConditional(Normal()), autoinit=graphinit)
+
+  return fg
+end

src/CliqStateMachine.jl

@@ -499,6 +499,8 @@ Notes
 """
 function determineCliqNeedDownMsg_StateMachine(csmc::CliqStateMachineContainer)
 
+  infocsm(csmc, "7, start, forceproceed=$(csmc.forceproceed)")
+
   # fetch children status
   stdict = blockCliqUntilChildrenHaveUpStatus(csmc.tree, csmc.cliq, csmc.logger)
 
@@ -510,20 +512,32 @@ function determineCliqNeedDownMsg_StateMachine(csmc::CliqStateMachineContainer)
     # :needdownmsg # 'send' downward init msg direction
     !(clst in [:initialized;:upsolved;:marginalized;:downsolved;:uprecycled]) ? (proceed = false) : nothing
   end
-  infocsm(csmc, "7, proceed=$(proceed), forceproceed=$(csmc.forceproceed)")
+  infocsm(csmc, "7, proceed=$(proceed)")
 
 
   if proceed || csmc.forceproceed
+    # TODO, remove csmc.forceproceed
     csmc.forceproceed = false
     # return doCliqInferAttempt_StateMachine
     cliqst = getCliqStatus(csmc.cliq)
     infocsm(csmc, "7, status=$(cliqst), before attemptCliqInitDown_StateMachine")
     # d1,d2,cliqst = doCliqInitUpOrDown!(csmc.cliqSubFg, csmc.tree, csmc.cliq, isprntnddw)
     if cliqst == :needdownmsg && !isCliqParentNeedDownMsg(csmc.tree, csmc.cliq, csmc.logger)
+      # go to 8a
       return attemptCliqInitDown_StateMachine
+    # HALF DUPLICATED IN STEP 4
+    elseif cliqst == :marginalized
+      # go to 1
+      return isCliqUpSolved_StateMachine
+      ## NOTE -- what about notifyCliqUpInitStatus! ??
+      # go to 10
+      # return determineCliqIfDownSolve_StateMachine
     end
+
+    # go to 8b
     return attemptCliqInitUp_StateMachine
   else
+    # go to 7b
     return slowCliqIfChildrenNotUpsolved_StateMachine
   end
 end
@@ -539,6 +553,7 @@ function blockCliqSiblingsParentChildrenNeedDown_StateMachine(csmc::CliqStateMac
   infocsm(csmc, "6c, check/block sibl&prnt :needdownmsg")
   blockCliqSiblingsParentNeedDown(csmc.tree, csmc.cliq, logger=csmc.logger)
 
+  # go to 7
   return determineCliqNeedDownMsg_StateMachine
 end
 
@@ -572,18 +587,30 @@ end
     $SIGNATURES
 
 Notes
-- State machine function nr. 4
+- State machine function nr.4
 """
 function isCliqNull_StateMachine(csmc::CliqStateMachineContainer)
 
   prnt = getParent(csmc.tree, csmc.cliq)
-  infocsm(csmc, "4, isCliqNull_StateMachine, csmc.incremental=$(csmc.incremental), len(prnt)=$(length(prnt))")
+  cliqst = getCliqStatus(csmc.oldcliqdata)
+  infocsm(csmc, "4, isCliqNull_StateMachine, $cliqst, len(prnt)=$(length(prnt)), csmc.incremental=$(csmc.incremental)")
+
+  if cliqst in [:marginalized;]
+      # if cliqst == :marginalized || cliqst in [:downsolved; :uprecycled] && ( length(prnt) == 0 ) # ||
+      # 0 < length(prnt) && getCliqStatus(prnt) in [:downsolved; :uprecycled; :marginalized;] )
+      # go to 10 -- Add case for IIF issue #474
+      return determineCliqIfDownSolve_StateMachine
+  end
+
   #must happen before if :null
   stdict = blockCliqUntilChildrenHaveUpStatus(csmc.tree, csmc.cliq, csmc.logger)
   csmc.forceproceed = false
 
+  # if clique is marginalized, then no reason to continue here
+  # if no parent or parent will not update
+
   # for recycle computed clique values case
-  if csmc.incremental && getCliqStatus(csmc.oldcliqdata) == :downsolved
+  if csmc.incremental && cliqst == :downsolved
     csmc.incremental = false
     # might be able to recycle the previous clique solve, go to 0b
     return checkChildrenAllUpRecycled_StateMachine
@@ -614,6 +641,7 @@ function doesCliqNeeddownmsg_StateMachine(csmc::CliqStateMachineContainer)
 
   if cliqst != :null
     if cliqst != :needdownmsg
+      # go to 6c
       return blockCliqSiblingsParentChildrenNeedDown_StateMachine
     end
   else
@@ -638,9 +666,11 @@ function doesCliqNeeddownmsg_StateMachine(csmc::CliqStateMachineContainer)
     return blockCliqSiblingsParentChildrenNeedDown_StateMachine
   end # != :null
 
+  areChildDown = areCliqChildrenNeedDownMsg(csmc.tree, csmc.cliq)
+  infocsm(csmc, "4b, areCliqChildrenNeedDownMsg(csmc.tree, csmc.cliq)=$(areChildDown)")
   # if cliqst == :needdownmsg
-    if areCliqChildrenNeedDownMsg(csmc.tree, csmc.cliq)
-      infocsm(csmc, "4, must deal with child :needdownmsg")
+    if areChildDown
+      infocsm(csmc, "4b, must deal with child :needdownmsg")
       csmc.forceproceed = true
     else
       # go to 5
@@ -663,25 +693,11 @@ Notes
 """
 function buildCliqSubgraph_StateMachine(csmc::CliqStateMachineContainer)
   # build a local subgraph for inference operations
-  syms = getCliqAllVarIds(csmc.cliq)
-  # NOTE add all frontal factor neighbors DEV CASE -- use getData(cliq).dwnPotentials instead
-  # fnsyms = getCliqVarsWithFrontalNeighbors(csmc.dfg, csmc.cliq)
-
-  infocsm(csmc, "2, build subgraph syms=$(syms)")
-  # TODO review, are all updates atomic???
-  # if isa(csmc.dfg, InMemDFGType)
-  #   csmc.cliqSubFg = csmc.dfg
-  # else
-    csmc.cliqSubFg = buildSubgraphFromLabels(csmc.dfg, syms, solvable=1)
-  # end
+  infocsm(csmc, "2, build subgraph syms=$(getCliqAllVarIds(csmc.cliq))")
+  buildCliqSubgraph(csmc.dfg, csmc.tree, csmc.cliq, csmc.cliqSubFg)
 
-  # store the cliqSubFg for later debugging
-  opts = getSolverParams(csmc.dfg)
-  if opts.dbg
-    mkpath(joinpath(opts.logpath,"logs/cliq$(csmc.cliq.index)"))
-    DFG.saveDFG(csmc.cliqSubFg, joinpath(opts.logpath,"logs/cliq$(csmc.cliq.index)/fg_build"))
-    drawGraph(csmc.cliqSubFg, show=false, filepath=joinpath(opts.logpath,"logs/cliq$(csmc.cliq.index)/fg_build.pdf"))
-  end
+  # if dfg, store the cliqSubFg for later debugging
+  dbgSaveDFG(csmc.cliqSubFg, "cliq$(csmc.cliq.index)/fg_build")
 
   # go to 4
   return isCliqNull_StateMachine

src/CliqStateMachineUtils.jl

@@ -1,4 +1,29 @@
 
+"""
+    $SIGNATURES
+
+Internal helper function to save a dfg object to LogPath during clique state machine operations.
+
+Notes
+- will only save dfg object if `opts.dbg=true`
+
+Related
+
+saveDFG, loadDFG
+"""
+function dbgSaveDFG(dfg::AbstractDFG,
+                    filename::AbstractString="fg_temp",
+                    opts::AbstractParams=getSolverParams(dfg)  )::String
+  #
+  folder::String=joinpath(opts.logpath,"logs")
+  if opts.dbg
+    ispath(folder) && mkpath(folder)
+    DFG.saveDFG(dfg, joinpath(folder, "$filename"))
+    drawGraph(dfg, show=false, filepath=joinpath(folder, "$filename.pdf"))
+  end
+  folder*filename
+end
+
 
 """
     $SIGNATURES
@@ -117,9 +142,11 @@ function sandboxCliqResolveStep(tree::BayesTree,
                                 step::Int)
   #
   hist = getCliqSolveHistory(tree, frontal)
-    # clear Condition states to allow step solve
-    cond = getSolveCondition(hist[step][4].cliq)
-    cond.waitq = Any[]
+  # clear Condition states to allow step solve
+  getData(hist[step][4].cliq).solveCondition = Condition()
+
+  # cond = getSolveCondition(hist[step][4].cliq)
+  # cond = Any[]
   return sandboxStateMachineStep(hist, step)
 end
 
@@ -966,4 +993,57 @@ end
 
 
 
+"""
+    $SIGNATURES
+
+Build a new subgraph from `fgl<:AbstractDFG` containing all variables and factors
+associated with `cliq`.  Additionally add the upward message prior factors as
+needed for belief propagation (inference).
+
+Notes
+- `cliqsym::Symbol` defines the cliq where variable appears as a frontal variable.
+- `varsym::Symbol` defaults to the cliq frontal variable definition but can in case a
+  separator variable is required instead.
+
+DevNotes
+- TODO review, are all updates atomic?? Then perhaps in-memory only can be reduced to references back to csmc.dfg.
+"""
+function buildCliqSubgraph(dfg::AbstractDFG,
+                           treel::BayesTree,
+                           cliq::Graphs.ExVertex,
+                           subfg::InMemDFGType=GraphsDFG(params=getSolverParams(dfg)) )
+  #
+  # get cliq and variable labels
+  syms = getCliqAllVarIds(cliq)
+  # NOTE add all frontal factor neighbors DEV CASE -- use getData(cliq).dwnPotentials instead
+  # fnsyms = getCliqVarsWithFrontalNeighbors(dfg, cliq)
+
+  # frontals treated special
+  frontals = getCliqFrontalVarIds(cliq)
+  # get neighboring factor labels
+  factorFilter = union(map(s->getNeighbors(dfg, s), frontals)...)
+
+  # build the subgraph with subset of factors from frontals only
+  buildSubgraphFromLabels!(dfg, syms, subfg=subfg, solvable=1, allowedFactors=factorFilter )
+  # recent option but not consolidated
+  # buildSubgraphFromLabels!_SPECIAL(dfg, syms, subfg=subfg, solvable=1, allowedFactors=factorFilter ) # DFG v0.5.2
+
+  return subfg
+end
+# @warn "Obsolete, buildCliqSubGraph*() is no longer in use"
+# # build a subgraph copy of clique
+# subfg = buildSubgraphFromLabels(fgl,syms)
+#
+# # add upward messages to subgraph
+# msgs = getCliqChildMsgsUp(treel, cliq, BallTreeDensity)
+# addMsgFactors!(subfg, msgs)
+
+function buildCliqSubgraph(fgl::AbstractDFG,
+                           treel::BayesTree,
+                           cliqsym::Symbol,
+                           subfg::InMemDFGType=GraphsDFG(params=getSolverParams(fgl)) )
+  #
+  buildCliqSubgraph(fgl, treel, getCliq(treel, cliqsym), subfg)
+end
+
 #