From 41b3d2f65b3060cb967b645e40491bc657e866a8 Mon Sep 17 00:00:00 2001 From: dehann Date: Sat, 3 Nov 2018 14:16:11 -0400 Subject: [PATCH 01/11] wip --- src/FGOSUtils.jl | 12 +++++++ src/FactorGraph01.jl | 24 +++++++++----- src/IncrementalInference.jl | 4 +-- src/JunctionTree.jl | 32 +++++++++++++------ src/SolveTree01.jl | 62 ++++++++++++++++++++++++------------- src/SolverUtilities.jl | 4 +-- 6 files changed, 96 insertions(+), 42 deletions(-) diff --git a/src/FGOSUtils.jl b/src/FGOSUtils.jl index f11f9c6c2..a49eaed2d 100644 --- a/src/FGOSUtils.jl +++ b/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) diff --git a/src/FactorGraph01.jl b/src/FactorGraph01.jl index d776a4970..d7a840d4a 100644 --- a/src/FactorGraph01.jl +++ b/src/FactorGraph01.jl @@ -994,6 +994,7 @@ function getShortestPathNeighbors(fgl::FactorGraph; to::Graphs.ExVertex=nothing, neighbors::Int=0 ) + @show num_edges(fgl.g), from, to edgelist = shortest_path(fgl.g, ones(num_edges(fgl.g)), from, to) vertdict = Dict{Int,Graphs.ExVertex}() edgedict = edgelist2edgedict(edgelist) @@ -1026,6 +1027,7 @@ function subgraphFromVerts(fgl::FactorGraph, for i in 1:len, j in (i+1):len from = verts[allkeys[i]] to = verts[allkeys[j]] + @show from, to, neighbors vertdict = getShortestPathNeighbors(fgl, from=from, to=to, neighbors=neighbors) for vert in vertdict if !haskey(allverts, vert[1]) @@ -1051,22 +1053,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 + + +# diff --git a/src/IncrementalInference.jl b/src/IncrementalInference.jl index 8308a8dc2..aa1f80b0d 100644 --- a/src/IncrementalInference.jl +++ b/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, diff --git a/src/JunctionTree.jl b/src/JunctionTree.jl index e3230ef65..28db3085b 100644 --- a/src/JunctionTree.jl +++ b/src/JunctionTree.jl @@ -160,6 +160,20 @@ function buildTree!(tree::BayesTree, fg::FactorGraph, p::Array{Int,1}) 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 = open("$(fpwoext).dot","w+") + write(fid,to_dot(treel.bt)) + close(fid) + run(`dot $(fpwoext).dot -T$(fext) -o $(filepath)`) + show ? (@async run(`$(viewerapp) $(filepath)`)) : nothing +end + ## Find batch belief propagation solution function prepBatchTree!(fg::FactorGraph; @@ -187,13 +201,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 +242,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} diff --git a/src/SolveTree01.jl b/src/SolveTree01.jl index b643f8bc2..f7ebe1e1b 100644 --- a/src/SolveTree01.jl +++ b/src/SolveTree01.jl @@ -67,6 +67,15 @@ end # end +""" +Ensure the desired number of julia processes are present. +""" +function check_procs(nprocs::Int) + if nprocs > 1 + nprocs() < nprocs ? addprocs(nprocs-nprocs()) : nothing + end +end + #global pidx pidx = 1 pidl = 1 @@ -603,7 +612,7 @@ function downGibbsCliqueDensity(fg::FactorGraph, cliq::Graphs.ExVertex, dwnMsgs: end -function updateFGBT!(fg::FactorGraph, bt::BayesTree, cliqID::Int, ddt::DownReturnBPType; dbg::Bool=false) +function updateFGBT!(fg::FactorGraph, bt::BayesTree, cliqID::Int, ddt::DownReturnBPType; dbg::Bool=false, fillcolor::String="") # if dlapi.cgEnabled # return nothing # end @@ -611,6 +620,10 @@ function updateFGBT!(fg::FactorGraph, bt::BayesTree, cliqID::Int, ddt::DownRetur if dbg cliq.attributes["debugDwn"] = deepcopy(ddt.dbgDwn) end + if fillcolor != "" + cliq.attributes["fillcolor"] = fillcolor + cliq.attributes["style"] = filled + end for dat in ddt.IDvals #TODO -- should become an update call updvert = dlapi.getvertex(fg,dat[1]) @@ -738,12 +751,13 @@ function partialExploreTreeType(pfg::FactorGraph, pbt::BayesTree, cliqCursor::Gr end function dispatchNewDwnProc!(fg::FactorGraph, - bt::BayesTree, - parentStack::Array{Graphs.ExVertex,1}, - stkcnt::Int, - refdict::Dict{Int,Future}; - N::Int=200, - dbg::Bool=false ) + bt::BayesTree, + parentStack::Array{Graphs.ExVertex,1}, + stkcnt::Int, + refdict::Dict{Int,Future}; + N::Int=200, + dbg::Bool=false, + drawpdf::Bool=false ) # cliq = parentStack[stkcnt] while !haskey(refdict, cliq.index) # nodedata.cliq @@ -754,7 +768,8 @@ function dispatchNewDwnProc!(fg::FactorGraph, delete!(refdict,cliq.index) # nodedata if rDDT != Union{} - updateFGBT!(fg, bt, cliq.index, rDDT, dbg=dbg) + updateFGBT!(fg, bt, cliq.index, rDDT, dbg=dbg, fillcolor="lightblue") + drawpdf ? drawTree(bt) : nothing end emptr = BayesTree(Union{}, 0, Dict{Int,Graphs.ExVertex}(), Dict{String,Int}()); @@ -768,11 +783,12 @@ function dispatchNewDwnProc!(fg::FactorGraph, end function processPreOrderStack!(fg::FactorGraph, - bt::BayesTree, - parentStack::Array{Graphs.ExVertex,1}, - refdict::Dict{Int,Future}; - N::Int=200, - dbg::Bool=false ) + bt::BayesTree, + parentStack::Array{Graphs.ExVertex,1}, + refdict::Dict{Int,Future}; + N::Int=200, + dbg::Bool=false, + drawpdf::Bool=false ) # # dwn message passing function for iterative tree exploration stkcnt = 0 @@ -780,7 +796,7 @@ function processPreOrderStack!(fg::FactorGraph, @sync begin sendcnt = 1:length(parentStack) # separate memory for remote calls for i in 1:sendcnt[end] - @async dispatchNewDwnProc!(fg, bt, parentStack, sendcnt[i], refdict, N=N, dbg=dbg) # stkcnt ##pidxI,nodedata + @async dispatchNewDwnProc!(fg, bt, parentStack, sendcnt[i], refdict, N=N, dbg=dbg, drawpdf=drawpdf) # stkcnt ##pidxI,nodedata end end nothing @@ -890,10 +906,11 @@ end # upward belief propagation message passing function function processPostOrderStacks!(fg::FactorGraph, - bt::BayesTree, - childStack::Array{Graphs.ExVertex,1}; - N::Int=200, - dbg::Bool=false ) + bt::BayesTree, + childStack::Array{Graphs.ExVertex,1}; + N::Int=200, + dbg::Bool=false, + drawpdf::Bool=false ) # refdict = Dict{Int,Future}() @@ -917,7 +934,8 @@ function processPostOrderStacks!(fg::FactorGraph, @info "upward leftovers, $(keys(refdict))" - updateFGBT!(fg, bt, childStack[1].index, ur, dbg=dbg ) # nodedata + updateFGBT!(fg, bt, childStack[1].index, ur, dbg=dbg, fillcolor="pink" ) # nodedata + drawpdf ? drawTree(bt) : nothing nothing end @@ -935,14 +953,14 @@ function upMsgPassingIterative!(startett::ExploreTreeType; N::Int=200, dbg::Bool end -function inferOverTree!(fgl::FactorGraph, bt::BayesTree; N::Int=200, dbg::Bool=false) +function inferOverTree!(fgl::FactorGraph, bt::BayesTree; N::Int=200, dbg::Bool=false, drawpdf::Bool=false) @info "Ensure all nodes are initialized" ensureAllInitialized!(fgl) @info "Do multi-process inference over tree" cliq = bt.cliques[1] - upMsgPassingIterative!(ExploreTreeType(fgl, bt, cliq, Union{}, NBPMessage[]),N=N, dbg=dbg); + upMsgPassingIterative!(ExploreTreeType(fgl, bt, cliq, Union{}, NBPMessage[]),N=N, dbg=dbg, drawpdf=drawpdf); cliq = bt.cliques[1] - downMsgPassingIterative!(ExploreTreeType(fgl, bt, cliq, Union{}, NBPMessage[]),N=N, dbg=dbg); + downMsgPassingIterative!(ExploreTreeType(fgl, bt, cliq, Union{}, NBPMessage[]),N=N, dbg=dbg, drawpdf=drawpdf); nothing end diff --git a/src/SolverUtilities.jl b/src/SolverUtilities.jl index f6447a0d3..c4e45b998 100644 --- a/src/SolverUtilities.jl +++ b/src/SolverUtilities.jl @@ -153,13 +153,13 @@ end Perform multimodal incremental smoothing and mapping (mm-iSAM) computations over given factor graph `fgl::FactorGraph` on the local computer. A pdf of the Bayes (Junction) tree will be generated in the working folder with `drawpdf=true` """ -function batchSolve!(fgl::FactorGraph; drawpdf::Bool=false, N::Int=100) +function batchSolve!(fgl::FactorGraph; drawpdf::Bool=false, show::Bool=false, N::Int=100) if fgl.isfixedlag @info "Quasi fixed-lag is enabled (a feature currently in testing)!" fifoFreeze!(fgl) end tree = wipeBuildNewTree!(fgl, drawpdf=drawpdf) - inferOverTree!(fgl, tree, N=N) + inferOverTree!(fgl, tree, N=N, drawpdf=drawpdf) tree end From 775c3632e7a61f4d3a2a88edce03551506db18a6 Mon Sep 17 00:00:00 2001 From: dehann Date: Sat, 3 Nov 2018 16:59:57 -0400 Subject: [PATCH 02/11] successfully color fill on Bayes Tree during solve --- src/SolveTree01.jl | 58 ++++++++++++++++++++++++++++------------------ 1 file changed, 36 insertions(+), 22 deletions(-) diff --git a/src/SolveTree01.jl b/src/SolveTree01.jl index f7ebe1e1b..0d6af288a 100644 --- a/src/SolveTree01.jl +++ b/src/SolveTree01.jl @@ -622,7 +622,7 @@ function updateFGBT!(fg::FactorGraph, bt::BayesTree, cliqID::Int, ddt::DownRetur end if fillcolor != "" cliq.attributes["fillcolor"] = fillcolor - cliq.attributes["style"] = filled + cliq.attributes["style"] = "filled" end for dat in ddt.IDvals #TODO -- should become an update call @@ -635,7 +635,7 @@ function updateFGBT!(fg::FactorGraph, bt::BayesTree, cliqID::Int, ddt::DownRetur end # TODO -- use Union{} for two types, rather than separate functions -function updateFGBT!(fg::FactorGraph, bt::BayesTree, cliqID::Int, urt::UpReturnBPType; dbg::Bool=false) +function updateFGBT!(fg::FactorGraph, bt::BayesTree, cliqID::Int, urt::UpReturnBPType; dbg::Bool=false, fillcolor::String="") # if dlapi.cgEnabled # return nothing # end @@ -644,6 +644,10 @@ function updateFGBT!(fg::FactorGraph, bt::BayesTree, cliqID::Int, urt::UpReturnB if dbg cliq.attributes["debug"] = deepcopy(urt.dbgUp) end + if fillcolor != "" + cliq.attributes["fillcolor"] = fillcolor + cliq.attributes["style"] = "filled" + end for dat in urt.IDvals updvert = dlapi.getvertex(fg,dat[1]) setValKDE!(updvert, deepcopy(dat[2])) # (fg.v[dat[1]], ## TODO -- not sure if deepcopy is required @@ -654,12 +658,13 @@ function updateFGBT!(fg::FactorGraph, bt::BayesTree, cliqID::Int, urt::UpReturnB end # pass NBPMessages back down the tree -- pre order tree traversal -function downMsgPassingRecursive(inp::ExploreTreeType; N::Int=200, dbg::Bool=false) +function downMsgPassingRecursive(inp::ExploreTreeType; N::Int=200, dbg::Bool=false, drawpdf::Bool=false) @info "====================== Clique $(inp.cliq.attributes["label"]) =============================" mcmciter = inp.prnt != Union{} ? 3 : 0; # skip mcmc in root on dwn pass rDDT = downGibbsCliqueDensity(inp.fg, inp.cliq, inp.sendmsgs, N, mcmciter, dbg) #dwnMsg - updateFGBT!(inp.fg, inp.bt, inp.cliq.index, rDDT, dbg=dbg) + updateFGBT!(inp.fg, inp.bt, inp.cliq.index, rDDT, dbg=dbg, fillcolor="pink") + drawpdf ? drawTree(bt) : nothing # rr = Array{Future,1}() pcs = procs() @@ -675,7 +680,7 @@ function downMsgPassingRecursive(inp::ExploreTreeType; N::Int=200, dbg::Bool=fal end # post order tree traversal and build potential functions -function upMsgPassingRecursive(inp::ExploreTreeType; N::Int=200, dbg::Bool=false) #upmsgdict = Dict{Int, Array{Float64,2}}() +function upMsgPassingRecursive(inp::ExploreTreeType; N::Int=200, dbg::Bool=false, drawpdf::Bool=false) #upmsgdict = Dict{Int, Array{Float64,2}}() @info "Start Clique $(inp.cliq.attributes["label"]) =============================" childMsgs = Array{NBPMessage,1}() @@ -692,7 +697,8 @@ function upMsgPassingRecursive(inp::ExploreTreeType; N::Int=200, dbg::Bool=false ett = ExploreTreeType(inp.fg, inp.bt, inp.cliq, Union{}, childMsgs) urt = upGibbsCliqueDensity(ett, N, dbg) # upmsgdict - updateFGBT!(inp.fg, inp.bt, inp.cliq.index, urt, dbg=dbg) + updateFGBT!(inp.fg, inp.bt, inp.cliq.index, urt, dbg=dbg, fillcolor="lightblue") + drawpdf ? drawTree(bt) : nothing @info "End Clique $(inp.cliq.attributes["label"]) =============================" urt.upMsgs end @@ -802,7 +808,11 @@ function processPreOrderStack!(fg::FactorGraph, nothing end -function downMsgPassingIterative!(startett::ExploreTreeType; N::Int=200, dbg::Bool=false) +function downMsgPassingIterative!(startett::ExploreTreeType; + N::Int=200, + dbg::Bool=false, + drawpdf::Bool=false ) + # # this is where we launch the downward iteration process from parentStack = Array{Graphs.ExVertex,1}() refdict = Dict{Int,Future}() @@ -815,13 +825,15 @@ function downMsgPassingIterative!(startett::ExploreTreeType; N::Int=200, dbg::Bo push!(parentStack, startett.cliq ) # r prepDwnPreOrderStack!(startett.bt, parentStack) - processPreOrderStack!(startett.fg, startett.bt, parentStack, refdict, N=N, dbg=dbg) + processPreOrderStack!(startett.fg, startett.bt, parentStack, refdict, N=N, dbg=dbg, drawpdf=drawpdf ) @info "dwnward leftovers, $(keys(refdict))" nothing end -function prepPostOrderUpPassStacks!(bt::BayesTree, parentStack::Array{Graphs.ExVertex,1}, childStack::Array{Graphs.ExVertex,1}) +function prepPostOrderUpPassStacks!(bt::BayesTree, + parentStack::Array{Graphs.ExVertex,1}, + childStack::Array{Graphs.ExVertex,1} ) # upward message passing preparation while ( length(parentStack) != 0 ) #2.1 Pop a node from first stack and push it to second stack @@ -842,12 +854,13 @@ end # for up message passing function asyncProcessPostStacks!(fgl::FactorGraph, - bt::BayesTree, - chldstk::Vector{Graphs.ExVertex}, - stkcnt::Int, - refdict::Dict{Int,Future}; - N::Int=200, - dbg::Bool=false ) + bt::BayesTree, + chldstk::Vector{Graphs.ExVertex}, + stkcnt::Int, + refdict::Dict{Int,Future}; + N::Int=200, + dbg::Bool=false, + drawpdf::Bool=false ) # if stkcnt == 0 @info "asyncProcessPostStacks! ERROR stkcnt=0" @@ -871,7 +884,8 @@ function asyncProcessPostStacks!(fgl::FactorGraph, else ur = child.attributes["remoteref"] end - updateFGBT!( fgl, bt, child.index, ur, dbg=dbg ) # deep copies happen in the update function + updateFGBT!( fgl, bt, child.index, ur, dbg=dbg, fillcolor="pink" ) # deep copies happen in the update function + drawpdf ? drawTree(bt) : nothing #delete!(child.attributes, "remoteref") push!(childMsgs, ur.upMsgs) @@ -919,7 +933,7 @@ function processPostOrderStacks!(fg::FactorGraph, @sync begin sendcnt = stkcnt:-1:1 # separate stable memory for i in 1:stkcnt - @async asyncProcessPostStacks!(fg, bt, childStack, sendcnt[i], refdict, N=N, dbg=dbg) # deepcopy(stkcnt) + @async asyncProcessPostStacks!(fg, bt, childStack, sendcnt[i], refdict, N=N, dbg=dbg, drawpdf=drawpdf ) # deepcopy(stkcnt) end end @info "processPostOrderStacks! -- THIS ONLY HAPPENS AFTER SYNC" @@ -939,7 +953,7 @@ function processPostOrderStacks!(fg::FactorGraph, nothing end -function upMsgPassingIterative!(startett::ExploreTreeType; N::Int=200, dbg::Bool=false) +function upMsgPassingIterative!(startett::ExploreTreeType; N::Int=200, dbg::Bool=false, drawpdf::Bool=false) #http://www.geeksforgeeks.org/iterative-postorder-traversal/ # this is where we launch the downward iteration process from parentStack = Array{Graphs.ExVertex,1}() @@ -948,7 +962,7 @@ function upMsgPassingIterative!(startett::ExploreTreeType; N::Int=200, dbg::Bool push!(parentStack, startett.cliq ) # Starting at the root means we have a top down view of the tree prepPostOrderUpPassStacks!(startett.bt, parentStack, childStack) - processPostOrderStacks!(startett.fg, startett.bt, childStack, N=N, dbg=dbg) + processPostOrderStacks!(startett.fg, startett.bt, childStack, N=N, dbg=dbg, drawpdf=drawpdf) nothing end @@ -964,13 +978,13 @@ function inferOverTree!(fgl::FactorGraph, bt::BayesTree; N::Int=200, dbg::Bool=f nothing end -function inferOverTreeR!(fgl::FactorGraph, bt::BayesTree; N::Int=200, dbg::Bool=false) +function inferOverTreeR!(fgl::FactorGraph, bt::BayesTree; N::Int=200, dbg::Bool=false, drawpdf::Bool=false) @info "Ensure all nodes are initialized" ensureAllInitialized!(fgl) @info "Do recursive inference over tree" cliq = bt.cliques[1] - upMsgPassingRecursive(ExploreTreeType(fgl, bt, cliq, Union{}, NBPMessage[]), N=N, dbg=dbg); + upMsgPassingRecursive(ExploreTreeType(fgl, bt, cliq, Union{}, NBPMessage[]), N=N, dbg=dbg, drawpdf=drawpdf); cliq = bt.cliques[1] - downMsgPassingRecursive(ExploreTreeType(fgl, bt, cliq, Union{}, NBPMessage[]), N=N, dbg=dbg); + downMsgPassingRecursive(ExploreTreeType(fgl, bt, cliq, Union{}, NBPMessage[]), N=N, dbg=dbg, drawpdf=drawpdf); nothing end From 4356e6a41665996fbe6f6080c35e9c02196b3b0a Mon Sep 17 00:00:00 2001 From: dehann Date: Sat, 3 Nov 2018 21:22:15 -0400 Subject: [PATCH 03/11] drawTree uses try catch for in compute error tolerance --- src/JunctionTree.jl | 17 +++++++++++++---- src/SolveTree01.jl | 10 +++++----- 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/src/JunctionTree.jl b/src/JunctionTree.jl index 28db3085b..4990965c1 100644 --- a/src/JunctionTree.jl +++ b/src/JunctionTree.jl @@ -167,10 +167,19 @@ function drawTree(treel::BayesTree; # fext = split(filepath, '.')[end] fpwoext = split(filepath, '.')[end-1] - fid = open("$(fpwoext).dot","w+") - write(fid,to_dot(treel.bt)) - close(fid) - run(`dot $(fpwoext).dot -T$(fext) -o $(filepath)`) + 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 ? (@async run(`$(viewerapp) $(filepath)`)) : nothing end diff --git a/src/SolveTree01.jl b/src/SolveTree01.jl index 0d6af288a..5a73d2407 100644 --- a/src/SolveTree01.jl +++ b/src/SolveTree01.jl @@ -432,7 +432,7 @@ function cliqGibbs(fg::FactorGraph, pGM = productbelief(fg, vertid, dens, partials, N, dbg=dbg ) if dbg potprod.product = pGM end - @info " " + # @info " " return pGM, potprod end @@ -474,7 +474,7 @@ function fmcmc!(fgl::FactorGraph, end end !dbg ? nothing : push!(mcmcdbg, dbgvals) - @info "" + # @info "" end # populate dictionary for return NBPMessage in multiple dispatch @@ -588,7 +588,7 @@ function dwnPrepOutMsg(fg::FactorGraph, cliq::Graphs.ExVertex, dwnMsgs::Array{NB i+=1 # TODO -- convert to points only since kde replace by rkhs in future # outDens[i] = cdwndict[cvid] - @info "" + # @info "" # info("Looking for cvid=$(cvid)") m.p[cvid] = deepcopy(dwnMsgs[1].p[cvid]) # TODO -- maybe this can just be a union(,) end @@ -664,7 +664,7 @@ function downMsgPassingRecursive(inp::ExploreTreeType; N::Int=200, dbg::Bool=fal mcmciter = inp.prnt != Union{} ? 3 : 0; # skip mcmc in root on dwn pass rDDT = downGibbsCliqueDensity(inp.fg, inp.cliq, inp.sendmsgs, N, mcmciter, dbg) #dwnMsg updateFGBT!(inp.fg, inp.bt, inp.cliq.index, rDDT, dbg=dbg, fillcolor="pink") - drawpdf ? drawTree(bt) : nothing + drawpdf ? drawTree(inp.bt) : nothing # rr = Array{Future,1}() pcs = procs() @@ -698,7 +698,7 @@ function upMsgPassingRecursive(inp::ExploreTreeType; N::Int=200, dbg::Bool=false urt = upGibbsCliqueDensity(ett, N, dbg) # upmsgdict updateFGBT!(inp.fg, inp.bt, inp.cliq.index, urt, dbg=dbg, fillcolor="lightblue") - drawpdf ? drawTree(bt) : nothing + drawpdf ? drawTree(inp.bt) : nothing @info "End Clique $(inp.cliq.attributes["label"]) =============================" urt.upMsgs end From 4b48ca272a131576cecc7d1811cdda8acc6d643c Mon Sep 17 00:00:00 2001 From: dehann Date: Sat, 3 Nov 2018 22:25:41 -0400 Subject: [PATCH 04/11] basic cleanup --- src/SolveTree01.jl | 52 +++++++++++++++++++++++++++++++++------------- 1 file changed, 38 insertions(+), 14 deletions(-) diff --git a/src/SolveTree01.jl b/src/SolveTree01.jl index 5a73d2407..e32b101a8 100644 --- a/src/SolveTree01.jl +++ b/src/SolveTree01.jl @@ -237,11 +237,11 @@ function prodmultipleonefullpartials( dens::Vector{BallTreeDensity}, end function productbelief(fg::FactorGraph, - vertid::Int, - dens::Vector{BallTreeDensity}, - partials::Dict{Int, Vector{BallTreeDensity}}, - N::Int; - dbg::Bool=false ) + vertid::Int, + dens::Vector{BallTreeDensity}, + partials::Dict{Int, Vector{BallTreeDensity}}, + N::Int; + dbg::Bool=false ) # pGM = Array{Float64,2}(undef, 0,0) @@ -258,7 +258,7 @@ function productbelief(fg::FactorGraph, denspts = getVal(fg,vertid,api=localapi) Ndims = size(denspts,1) @info "[$(lennonp)x$(lenpart)p,d$(Ndims),N$(N)]," - dummy = kde!(rand(Ndims,N),[1.0]) # TODO -- reuse memory rather than rand here + dummy = kde!(rand(Ndims,N), ones(Ndims)) # [1.0] # TODO -- reuse memory rather than rand here pGM = deepcopy(denspts) productpartials!(pGM, dummy, partials) # elseif lennonp == 0 && lenpart == 1 @@ -268,7 +268,7 @@ function productbelief(fg::FactorGraph, # pGM[dimnum,:] = getPoints(pp) # end elseif lennonp == 1 && lenpart == 0 - @info "[drct]" + # @info "[drct]" pGM = getPoints(dens[1]) else @warn "Unknown density product on vertid=$(vertid), lennonp=$(lennonp), lenpart=$(lenpart)" @@ -357,6 +357,14 @@ function predictbelief(fgl::FactorGraph, predictbelief(fgl, destvertsym, ls(fgl, destvertsym, api=api), N=N, api=api, dbg=dbg ) end +""" + $(SIGNATURES) + +Using factor graph object `fg`, 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 +""" function localProduct(fgl::FactorGraph, sym::Symbol; N::Int=100, @@ -385,11 +393,20 @@ function localProduct(fgl::FactorGraph, return pp, dens, partials, lb end + +""" + $(SIGNATURES) + +Using factor graph object `fg`, 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 +""" localProduct(fgl::FactorGraph, lbl::T; N::Int=100, dbg::Bool=false) where {T <: AbstractString} = localProduct(fgl, Symbol(lbl), N=N, dbg=dbg) """ - initializeNode!(::FactorGraph, ::Symbol; N::Int=100, api::DataLayerAPI=dlapi) + $(SIGNATURES) Initialize the belief of a variable node in the factor graph struct. """ @@ -436,13 +453,20 @@ function cliqGibbs(fg::FactorGraph, return pGM, potprod end +""" + $(SIGNATURES) + +Iterate successive approximations of clique marginal beliefs by means +of the stipulated proposal convolutions and products of the functional objects +for tree clique `cliq`. +""" function fmcmc!(fgl::FactorGraph, - cliq::Graphs.ExVertex, - fmsgs::Vector{NBPMessage}, - IDs::Vector{Int}, - N::Int, - MCMCIter::Int, - dbg::Bool=false ) + cliq::Graphs.ExVertex, + fmsgs::Vector{NBPMessage}, + IDs::Vector{Int}, + N::Int, + MCMCIter::Int, + dbg::Bool=false ) # @info "---------- successive fnc approx ------------$(cliq.attributes["label"])" # repeat several iterations of functional Gibbs sampling for fixed point convergence From 8f34e9da2a383d8b3053d464061c18567056031a Mon Sep 17 00:00:00 2001 From: dehann Date: Sun, 4 Nov 2018 15:29:05 -0500 Subject: [PATCH 05/11] add showTree and to batchSolve! --- src/JunctionTree.jl | 15 ++++++++++++++- src/SolverUtilities.jl | 1 + 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/src/JunctionTree.jl b/src/JunctionTree.jl index 4990965c1..ff0b8c267 100644 --- a/src/JunctionTree.jl +++ b/src/JunctionTree.jl @@ -160,6 +160,18 @@ 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", @@ -180,10 +192,11 @@ function drawTree(treel::BayesTree; close(fid) end - show ? (@async run(`$(viewerapp) $(filepath)`)) : nothing + show ? showTree(viewerapp=viewerapp, filepath=filepath) : nothing end + ## Find batch belief propagation solution function prepBatchTree!(fg::FactorGraph; ordering::Symbol=:qr, diff --git a/src/SolverUtilities.jl b/src/SolverUtilities.jl index c4e45b998..f06a4991a 100644 --- a/src/SolverUtilities.jl +++ b/src/SolverUtilities.jl @@ -159,6 +159,7 @@ function batchSolve!(fgl::FactorGraph; drawpdf::Bool=false, show::Bool=false, N: fifoFreeze!(fgl) end tree = wipeBuildNewTree!(fgl, drawpdf=drawpdf) + show ? showTree() : nothing inferOverTree!(fgl, tree, N=N, drawpdf=drawpdf) tree end From c9a8d0a72485ec608d5f8bd3d19901cc9d0fc5b8 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 5 Nov 2018 07:31:30 -0500 Subject: [PATCH 06/11] add lsRear function --- src/FGOSUtils.jl | 21 +++++++++++++++++++++ src/IncrementalInference.jl | 1 + 2 files changed, 22 insertions(+) diff --git a/src/FGOSUtils.jl b/src/FGOSUtils.jl index a49eaed2d..edea28776 100644 --- a/src/FGOSUtils.jl +++ b/src/FGOSUtils.jl @@ -218,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 """ diff --git a/src/IncrementalInference.jl b/src/IncrementalInference.jl index aa1f80b0d..76a56d9ce 100644 --- a/src/IncrementalInference.jl +++ b/src/IncrementalInference.jl @@ -114,6 +114,7 @@ export ls, lsf, ls2, + lsRear, hasOrphans, allnums, isnestednum, From 1813ae7ba6b25e77c0f687a350c0e422728fe28c Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 5 Nov 2018 23:32:43 -0500 Subject: [PATCH 07/11] remove obsolete file --- src/tmpimgs/readme | 1 - 1 file changed, 1 deletion(-) delete mode 100644 src/tmpimgs/readme diff --git a/src/tmpimgs/readme b/src/tmpimgs/readme deleted file mode 100644 index f655d496d..000000000 --- a/src/tmpimgs/readme +++ /dev/null @@ -1 +0,0 @@ -Images for MCMC gif illustration temporarily stored here From ea7faee81432ac14d8a431593bd27a463f86572b Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 6 Nov 2018 00:26:24 -0500 Subject: [PATCH 08/11] remove show statements --- src/FactorGraph01.jl | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/FactorGraph01.jl b/src/FactorGraph01.jl index d7a840d4a..9d72bf596 100644 --- a/src/FactorGraph01.jl +++ b/src/FactorGraph01.jl @@ -994,7 +994,6 @@ function getShortestPathNeighbors(fgl::FactorGraph; to::Graphs.ExVertex=nothing, neighbors::Int=0 ) - @show num_edges(fgl.g), from, to edgelist = shortest_path(fgl.g, ones(num_edges(fgl.g)), from, to) vertdict = Dict{Int,Graphs.ExVertex}() edgedict = edgelist2edgedict(edgelist) @@ -1027,7 +1026,6 @@ function subgraphFromVerts(fgl::FactorGraph, for i in 1:len, j in (i+1):len from = verts[allkeys[i]] to = verts[allkeys[j]] - @show from, to, neighbors vertdict = getShortestPathNeighbors(fgl, from=from, to=to, neighbors=neighbors) for vert in vertdict if !haskey(allverts, vert[1]) From 9af092c406e1ec4e39abcbf04c055938a45ca4b3 Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 6 Nov 2018 01:29:31 -0500 Subject: [PATCH 09/11] suppress some printing during bayes tree construction --- src/FactorGraph01.jl | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/FactorGraph01.jl b/src/FactorGraph01.jl index d7a840d4a..208869fbc 100644 --- a/src/FactorGraph01.jl +++ b/src/FactorGraph01.jl @@ -724,10 +724,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 From dfc001b06763cf9d7f4ac4c3f918524bba7b3c1b Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 6 Nov 2018 12:54:41 -0500 Subject: [PATCH 10/11] add recursive to batch --- src/SolverUtilities.jl | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/SolverUtilities.jl b/src/SolverUtilities.jl index f06a4991a..20cd782ab 100644 --- a/src/SolverUtilities.jl +++ b/src/SolverUtilities.jl @@ -153,14 +153,24 @@ end Perform multimodal incremental smoothing and mapping (mm-iSAM) computations over given factor graph `fgl::FactorGraph` on the local computer. A pdf of the Bayes (Junction) tree will be generated in the working folder with `drawpdf=true` """ -function batchSolve!(fgl::FactorGraph; drawpdf::Bool=false, show::Bool=false, N::Int=100) +function batchSolve!(fgl::FactorGraph; + drawpdf::Bool=false, + show::Bool=false, + N::Int=100, + recursive::Bool=false ) if fgl.isfixedlag @info "Quasi fixed-lag is enabled (a feature currently in testing)!" fifoFreeze!(fgl) end tree = wipeBuildNewTree!(fgl, drawpdf=drawpdf) show ? showTree() : nothing - inferOverTree!(fgl, tree, N=N, drawpdf=drawpdf) + + if recursive + # recursive is a single core method that is slower but occasionally helpful for better stack traces during debugging + inferOverTreeR!(fgl, tree, N=N, drawpdf=drawpdf) + else + inferOverTree!(fgl, tree, N=N, drawpdf=drawpdf) + end tree end From 5f7f2e07236b56351341fb44d03c988d57e5d8d6 Mon Sep 17 00:00:00 2001 From: dehann Date: Tue, 6 Nov 2018 16:43:59 -0500 Subject: [PATCH 11/11] modify test tolerance --- test/testmultihypothesisapi.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/testmultihypothesisapi.jl b/test/testmultihypothesisapi.jl index 04fe517db..db3c8aa25 100644 --- a/test/testmultihypothesisapi.jl +++ b/test/testmultihypothesisapi.jl @@ -229,7 +229,7 @@ global pts = approxConv(fg, :x2x3x4x5f1, :x3, N=N) @test 0.1*N < sum(pts .== 3.0) < 0.5*N @test 0.1*N < sum(pts .== 4.0) < 0.5*N -@test sum(80 .< pts .< 100.0) + sum(pts .== 3.0) + sum(pts .== 4.0) == N +@test sum(70 .< pts .< 110.0) + sum(pts .== 3.0) + sum(pts .== 4.0) == N # solve for one of uncertain variables