Merge pull request #176 from JuliaRobotics/enhancement/drawtreesubgraph

helper functions to monitor tree during solve

src/FGOSUtils.jl

@@ -92,6 +92,18 @@ function ls(fgl::FactorGraph, lbl::Symbol; api::DataLayerAPI=dlapi, ring::Int=1)
 end
 ls(fgl::FactorGraph, lbl::T) where {T <: AbstractString} = ls(fgl, Symbol(lbl))
 
+"""
+    $(SIGNATURES)
+
+Experimental union of elements version of ls(::FactorGraph, ::Symbol).  Not mean't to replace broadcasting `ls.(fg, [:x1;:x2])`
+"""
+function ls(fgl::FactorGraph,
+            lbls::Vector{Symbol};
+            api::DataLayerAPI=dlapi,
+            ring::Int=1)
+  union(ls.(fgl, lbls, ring=ring, api=api)[:]...)
+end
+
 """
     $(SIGNATURES)
 
@@ -206,6 +218,27 @@ function ls2(fgl::FactorGraph, vsym::Symbol)
   return xlxl
 end
 
+
+"""
+    $(SIGNATURES)
+
+Return array of all variable nodes connected to the last `n` many poses (`:x*`).
+
+Example:
+
+```julia
+# Shallow copy the tail end of poses from a factor graph `fg1`
+vars = lsRear(fg1, 5)
+fg1_r5 = subgraphFromVerts(fg1, vars)
+```
+"""
+function lsRear(fgl::FactorGraph, n::Int=1)
+  lasts = ls(fgl)[1][(end-n):end]
+  syms = ls(fgl, lasts)
+  union(lsf.(fgl, syms)[:]...)
+end
+
+
 hasOrphans(fg) = sum(length.(ls.(fg, [ls(fg)[1];ls(fg)[2]])) .== 0) > 0
 
 """

src/FactorGraph01.jl

@@ -723,10 +723,10 @@ function addChainRuleMarginal!(fg::FactorGraph, Si)
   for s in Si
     push!(Xi, getVert(fg, s, api=localapi))
   end
-  @info "adding marginal to"
-  for x in Xi
-    @info "x.index=",x.index
-  end
+  # @info "adding marginal to"
+  # for x in Xi
+  #   @info "x.index=",x.index
+  # end
   addFactor!(fg, Xi, genmarg, api=localapi, autoinit=false)
   nothing
 end
@@ -1050,22 +1050,30 @@ function subgraphFromVerts(fgl::FactorGraph,
   return subgraphFromVerts(fgl,vertdict,neighbors=neighbors)
 end
 
-# explore all shortest paths combinations in verts, add neighbors and reference subgraph
-# Using unique index into graph data structure
+"""
+    $(SIGNATURES)
+
+Explore all shortest paths combinations in verts, add neighbors and reference
+subgraph using unique index into graph data structure.
+"""
 function subgraphFromVerts(fgl::FactorGraph,
-    verts::Array{String,1};
-    neighbors::Int=0  )
+                           verts::Union{Vector{String},Vector{Symbol}};
+                           neighbors::Int=0  )
 
   vertdict = Dict{Int,Graphs.ExVertex}()
   for vert in verts
     id = -1
-    if haskey(fgl.IDs, vert)
-      id = fgl.IDs[Symbol(vert)]
+    vsym = Symbol(vert)
+    if haskey(fgl.IDs, vsym)
+      id = fgl.IDs[vsym]
     else
       error("FactorGraph01 only supports variable node subgraph search at this point")
     end
-    vertdict[id] = fgl.g.vertices[id]
+    vertdict[id] = getVert(fgl, vsym) # fgl.g.vertices[id]
   end
 
   return subgraphFromVerts(fgl,vertdict,neighbors=neighbors)
 end
+
+
+#

src/IncrementalInference.jl

@@ -2,7 +2,7 @@ module IncrementalInference
 
 @info "Multithreaded  convolutions possible, Threads.nthreads()=$(Threads.nthreads()).  See `addFactor!(.;threadmodel=MultiThreaded)`."
 
-
+using Distributed
 using Reexport
 
 @reexport using Distributions
@@ -11,7 +11,6 @@ using Reexport
 @reexport using LinearAlgebra
 
 using
-  Distributed,
   Statistics,
   Random,
   NLsolve,
@@ -38,6 +37,7 @@ export
   showcurrentdlapi,
   setdatalayerAPI!,
   DataLayerAPI,
+  check_procs,
 
   # general types for softtyping of variable nodes
   InferenceVariable,
@@ -116,6 +116,7 @@ export
   ls,
   lsf,
   ls2,
+  lsRear,
   hasOrphans,
   printgraphmax,
   allnums,

src/JunctionTree.jl

@@ -160,6 +160,42 @@ function buildTree!(tree::BayesTree, fg::FactorGraph, p::Array{Int,1})
   end
 end
 
+function showTree(;filepath::String="/tmp/bt.pdf",
+                   viewerapp::String="evince"  )
+  #
+  try
+    @async run(`$(viewerapp) $(filepath)`)
+  catch ex
+    @warn "not able to show via $(viewerapp) $(filepath)"
+    @show ex
+    @show stacktrace()
+  end
+end
+
+function drawTree(treel::BayesTree;
+                  show::Bool=false,                  # must remain false for stability and automated use in solver
+                  filepath::String="/tmp/bt.pdf",
+                  viewerapp::String="evince"  )
+  #
+  fext = split(filepath, '.')[end]
+  fpwoext = split(filepath, '.')[end-1]
+  fid = IOStream("")
+  try
+    fid = open("$(fpwoext).dot","w+")
+    write(fid,to_dot(treel.bt))
+    close(fid)
+    run(`dot $(fpwoext).dot -T$(fext) -o $(filepath)`)
+  catch ex
+    @warn ex
+    @show stacktrace()
+  finally
+    close(fid)
+  end
+
+  show ? showTree(viewerapp=viewerapp, filepath=filepath) : nothing
+end
+
+
 
 ## Find batch belief propagation solution
 function prepBatchTree!(fg::FactorGraph;
@@ -187,13 +223,7 @@ function prepBatchTree!(fg::FactorGraph;
   # Michael reference -- x2->x1, x2->x3, x2->x4, x2->l1, x4->x3, l1->x3, l1->x4
   println("Bayes Tree")
   if drawpdf
-    fext = split(filepath, '.')[end]
-    fpwoext = split(filepath, '.')[end-1]
-    fid = open("$(fpwoext).dot","w+")
-    write(fid,to_dot(tree.bt))
-    close(fid)
-    run(`dot $(fpwoext).dot -T$(fext) -o $(filepath)`)
-    show ? (@async run(`$(viewerapp) $(filepath)`)) : nothing
+    drawTree(tree, show=show, filepath=filepath, viewerapp=viewerapp)
   end
 
   # GraphViz.Graph(to_dot(tree.bt))
@@ -234,9 +264,15 @@ function resetFactorGraphNewTree!(fg::FactorGraph)
   nothing
 end
 
-function wipeBuildNewTree!(fg::FactorGraph; ordering=:qr,drawpdf=false)
+function wipeBuildNewTree!(fg::FactorGraph;
+                           ordering=:qr,
+                           drawpdf=false,
+                           show::Bool=false,
+                           filepath::String="/tmp/bt.pdf",
+                           viewerapp::String="evince"  )
+  #
   resetFactorGraphNewTree!(fg);
-  return prepBatchTree!(fg, ordering=ordering, drawpdf=drawpdf);
+  return prepBatchTree!(fg, ordering=ordering, drawpdf=drawpdf, show=show, filepath=filepath, viewerapp=viewerapp);
 end
 
 function whichCliq(bt::BayesTree, frt::T) where {T <: AbstractString}