Merge pull request #461 from JuliaRobotics/test/priorusetest

simple prior use test

Project.toml

@@ -33,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/CliqStateMachine.jl

@@ -693,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
+
 #

src/Deprecated.jl

@@ -1,4 +1,57 @@
 
+### DONT DELETE YET -- see more likely list below
+
+
+function getShortestPathNeighbors(fgl::FactorGraph;
+    from::Graphs.ExVertex=nothing,
+    to::Graphs.ExVertex=nothing,
+    neighbors::Int=0 )
+
+  edgelist = shortest_path(fgl.g, ones(num_edges(fgl.g)), from, to)
+  vertdict = Dict{Int,Graphs.ExVertex}()
+  edgedict = edgelist2edgedict(edgelist)
+  expandVertexList!(fgl, edgedict, vertdict) # grow verts
+  for i in 1:neighbors
+    expandEdgeListNeigh!(fgl, vertdict, edgedict) # grow edges
+    expandVertexList!(fgl, edgedict, vertdict) # grow verts
+  end
+  return vertdict
+end
+
+function subgraphShortestPath(fgl::FactorGraph;
+                              from::Graphs.ExVertex=nothing,
+                              to::Graphs.ExVertex=nothing,
+                              neighbors::Int=0  )
+  #
+  vertdict = getShortestPathNeighbors(fgl, from=from, to=to, neighbors=neighbors)
+  return genSubgraph(fgl, vertdict)
+end
+
+# explore all shortest paths combinations in verts, add neighbors and reference subgraph
+function subGraphFromVerts(fgl::FactorGraph,
+                           verts::Dict{Int,Graphs.ExVertex};
+                           neighbors::Int=0  )
+  #
+  allverts = Dict{Int,Graphs.ExVertex}()
+  allkeys = collect(keys(verts))
+  len = length(allkeys)
+  # union all shortest path combinations in a vertdict
+  for i in 1:len, j in (i+1):len
+    from = verts[allkeys[i]]
+    to = verts[allkeys[j]]
+    vertdict = getShortestPathNeighbors(fgl, from=from, to=to, neighbors=neighbors)
+    for vert in vertdict
+      if !haskey(allverts, vert[1])
+        allverts[vert[1]] = vert[2]
+      end
+    end
+  end
+
+  return genSubgraph(fgl, allverts)
+end
+
+
+
 """
     $SIGNATURES
 
@@ -48,15 +101,6 @@ end
 
 
 
-
-
-
-
-
-
-
-
-
 # explore all shortest paths combinations in verts, add neighbors and reference subgraph
 # Using unique index into graph data structure
 function subgraphFromVerts(fgl::FactorGraph,
@@ -108,7 +152,50 @@ end
 
 
 
+### MORE LIKELY TO BE DELETED BELOW
+
+
 
+# TODO -- convert to use add_vertex! instead, since edges type must be made also
+function addVerticesSubgraph(fgl::FactorGraph,
+    fgseg::FactorGraph,
+    vertdict::Dict{Int,Graphs.ExVertex})
+
+    for vert in vertdict
+      fgseg.g.vertices[vert[1]] = vert[2]
+      if haskey(fgl.v,vert[1])
+        fgseg.g.vertices[vert[1]] = vert[2]
+        fgseg.IDs[Symbol(vert[2].label)] = vert[1]
+
+        # add edges going in opposite direction
+        elr = Graphs.out_edges(vert[2], fgl.g)
+        len = length(elr)
+        keeprm = trues(len)
+        j = 0
+        for i in 1:len
+          if !haskey(vertdict, elr[i].target.index) # a function node in set, so keep ref
+            keeprm[i] = false
+            j+=1
+          end
+        end
+        if j < len
+          elridx = elr[1].source.index
+          fgseg.g.inclist[elridx] = elr[keeprm]
+        end
+      elseif haskey(fgl.f, vert[1])
+        fgseg.f[vert[1]] = vert[2] # adding element to subgraph
+        fgseg.fIDs[Symbol(vert[2].label)] = vert[1]
+        # get edges associated with function nodes and push edges onto incidence list
+        el = Graphs.out_edges(vert[2], fgl.g)
+        elidx = el[1].source.index
+        fgseg.g.inclist[elidx] = el # okay because treating function nodes only
+        fgseg.g.nedges += length(el)
+      else
+        error("Unknown type factor graph vertex type, something is wrong")
+      end
+    end
+    nothing
+end
 
 """
     $SIGNATURES

src/FGOSUtils.jl

@@ -268,26 +268,6 @@ function getIdx(pp::V, sym::Symbol, i::Int=0)::Tuple{Int, Int} where {V <: Infer
 end
 
 
-"""
-   $SIGNATURES
-
-Display the content of `VariableNodeData` to console for a given factor graph and variable tag`::Symbol`.
-
-Dev Notes
-- TODO split as two show macros between AMP and DFG
-"""
-function showVariable(fgl::G, vsym::Symbol) where G <: AbstractDFG
-  vert = DFG.getVariable(fg, vsym)
-  vnd = solverData(vert)
-  println("label: $(vert.label), exVertexId: $(vert.index)")
-  println("tags: $( haskey(vert.attributes, string(:tags)) ? vert.attributes[string(:tags)] : string(:none))")
-  println("size marginal samples $(size(getVal(vnd)))")
-  println("kde bandwidths: $(getBW(vnd)[:,1])")
-  println("kde mean: $(round.(getKDEMean(getKDE(vnd)),digits=4))")
-  println("kde max: $(round.(getKDEMax(getKDE(vnd)),digits=4))")
-  println()
-  vnd
-end
 
 """
     $SIGNATURES

src/IncrementalInference.jl

@@ -213,8 +213,9 @@ export
   getBWVal,
   setBW!,
   setValKDE!,
-  buildCliqSubgraphUp,
-  buildCliqSubgraphDown,
+  buildCliqSubgraph,
+  # buildCliqSubgraphUp,
+  # buildCliqSubgraphDown,
   setCliqUpInitMsgs!,
   cliqInitSolveUpByStateMachine!,
 
@@ -475,8 +476,6 @@ export
   compare,
   compareAllSpecial,
   getIdx,
-  showFactor,
-  showVariable,
   getMeasurements,
   findFactorsBetweenFrom,
   addDownVariableFactors!,
@@ -523,13 +522,13 @@ include("BeliefTypes.jl")
 include("AliasScalarSampling.jl")
 include("DefaultNodeTypes.jl")
 include("FactorGraph01.jl")
-include("SubGraphFunctions.jl")
 include("SerializingDistributions.jl")
 include("DispatchPackedConversions.jl")
 include("FGOSUtils.jl")
 include("CompareUtils.jl")
 
 include("JunctionTreeTypes.jl")
+include("SubGraphFunctions.jl")
 include("JunctionTree.jl")
 include("TreeBasedInitialization.jl")
 include("GraphConstraintTypes.jl")