updated solveTree_X closer to solveTree API (#968)

Author: Affie

src/AdditionalUtils.jl

@@ -58,7 +58,7 @@ function drawGraph(fgl::AbstractDFG;
   #
   mkpath(joinpath( "/", (split(filepath, '/')[1:(end-1)])...) )
 
-  @info "Writing factor graph file"
+  @debug "Writing factor graph file"
   fext = split(filepath, '.')[end]
   fpwoext = filepath[1:(end-length(fext)-1)] # split(filepath, '.')[end-1]
   dotfile = fpwoext*".dot"

src/CliqStateMachineUtils.jl

@@ -2,6 +2,7 @@
 export setVariablePosteriorEstimates!
 export updateCliqSolvableDims!, fetchCliqSolvableDims
 
+# using Serialization
 
 """
     $SIGNATURES
@@ -24,6 +25,8 @@ function _dbgCSMSaveSubFG(csmc::CliqStateMachineContainer, filename::String)
     if !ispath(folder)
       mkpath(folder)
     end
+    # NOTE there was a bug using saveDFG, so used serialize, left for future use  
+    # serialize(joinpath(folder, filename), csmc.cliqSubFg)
     DFG.saveDFG(csmc.cliqSubFg, joinpath(folder, filename))
     drawGraph(csmc.cliqSubFg, show=false, filepath=joinpath(folder, "$(filename).pdf"))
   end

src/experimental/CliqStateMachine_X-stroke.jl

@@ -13,12 +13,13 @@ function initStartCliqStateMachine_X!(dfg::AbstractDFG,
                                        drawtree::Bool=false,
                                        show::Bool=false,
                                        incremental::Bool=true,
-                                       limititers::Int=100,
+                                       limititers::Int=20,
                                        upsolve::Bool=true,
                                        downsolve::Bool=true,
                                        recordhistory::Bool=false,
                                        delay::Bool=false,
-                                       logger::SimpleLogger=SimpleLogger(Base.stdout))
+                                       logger::SimpleLogger=SimpleLogger(Base.stdout),
+                                       solve_progressbar=nothing)
 
   # NOTE use tree and messages for operations involving children and parents
   children = TreeClique[]
@@ -34,7 +35,7 @@ function initStartCliqStateMachine_X!(dfg::AbstractDFG,
 
   nxt = buildCliqSubgraph_X_StateMachine
 
-  statemachine = StateMachine{CliqStateMachineContainer}(next=nxt)
+  statemachine = StateMachine{CliqStateMachineContainer}(next=nxt, name="cliq$(cliq.index)")
 
 
   # store statemachine and csmc in task
@@ -45,7 +46,9 @@ function initStartCliqStateMachine_X!(dfg::AbstractDFG,
 
   logCSM(csmc, "Clique $(csmc.cliq.index) starting", loglevel=Logging.Info)
 
-  while statemachine(csmc, verbose=verbose, iterlimit=limititers, recordhistory=recordhistory); end
+  while statemachine(csmc, verbose=verbose, iterlimit=limititers, recordhistory=recordhistory)
+    !isnothing(solve_progressbar) && next!(solve_progressbar)
+  end
 
   return statemachine.history
 
@@ -100,7 +103,7 @@ function waitForUp_X_StateMachine(csmc::CliqStateMachineContainer)
       # Blocks until data is available
       beliefMsg = takeBeliefMessageUp!(csmc.tree, e)
       beliefMessages[thisEdge] = beliefMsg
-      logCSM(csmc, "$(csmc.cliq.index): Belief message received with status $(beliefMsg.status)")
+      logCSM(csmc, "$(csmc.cliq.index): Belief message received with status $(beliefMsg.status)"; msgvars = keys(beliefMsg.belief))
     end
   end
 
@@ -148,8 +151,8 @@ function doCliqUpSolveInitialized!(csmc::CliqStateMachineContainer)
   logCSM(csmc, "8g, doCliqUpSolveInitialized_StateMachine -- clique status = $(status)")
   setCliqDrawColor(csmc.cliq, "red")
   # get Dict{Symbol, TreeBelief} of all updated variables in csmc.cliqSubFg
-  retdict = approxCliqMarginalUp!(csmc; iters=4, logger=csmc.logger)
-  # retdict = approxCliqMarginalUp!(csmc, LikelihoodMessage[]; iters=4)#, logger=csmc.logger)
+  # retdict = approxCliqMarginalUp!(csmc; iters=4, logger=csmc.logger)
+  retdict = approxCliqMarginalUp!(csmc, LikelihoodMessage[]; iters=4, logger=csmc.logger)
 
   logCSM(csmc, "aproxCliqMarginalUp!"; retdict=retdict)
 
@@ -159,7 +162,6 @@ function doCliqUpSolveInitialized!(csmc::CliqStateMachineContainer)
 
   # notify of results (part of #459 consolidation effort)
   getCliqueData(csmc.cliq).upsolved = true
-  status = :upsolved
 
   # go to 8h
   return nothing
@@ -180,15 +182,21 @@ function solveUp_X_StateMachine(csmc::CliqStateMachineContainer)
   opts = getSolverParams(csmc.dfg)
 
   # add message factors from upRx: cached messages taken from children saved in this clique
-  upmsg = addMsgFactors!(csmc.cliqSubFg, messages(csmc.cliq).upRx, UpwardPass)
-  logCSM(csmc, "adding messages for up"; upmsg=[m.label for m in upmsg])
+  addMsgFactors!(csmc.cliqSubFg, messages(csmc.cliq).upRx, UpwardPass)
+  logCSM(csmc, "messages for up"; upmsg=lsf(csmc.cliqSubFg, tags=[:LIKELIHOODMESSAGE]))
 
   # store the cliqSubFg for later debugging
   _dbgCSMSaveSubFG(csmc, "fg_beforeupsolve")
 
   #UPSOLVE here
   all_child_status = map(msg -> msg.status, values(messages(csmc.cliq).upRx))
 
+  if all(all_child_status .== :UPSOLVED) && !areCliqVariablesAllInitialized(csmc.cliqSubFg, csmc.cliq) 
+    logCSM(csmc, "All children upsolved, not init, try init then upsolve"; c=csmc.cliqKey)
+    varorder = getCliqVarInitOrderUp(csmc.tree, csmc.cliq)
+    someInit = cycleInitByVarOrder!(csmc.cliqSubFg, varorder, logger=csmc.logger)
+  end
+
   if all(all_child_status .== :UPSOLVED) && areCliqVariablesAllInitialized(csmc.cliqSubFg, csmc.cliq) 
     logCSM(csmc, "X-3 doing upSolve -- all initialized")
 
@@ -199,6 +207,27 @@ function solveUp_X_StateMachine(csmc::CliqStateMachineContainer)
     converged_and_happy = true
 
   elseif !areCliqVariablesAllInitialized(csmc.cliqSubFg, csmc.cliq)
+    
+        # FIXME experimental init to whatever is in frontals
+        # should work if linear manifold
+        # hardcoded off 
+        linear_on_manifold = false
+        init_for_differential = begin
+          allvars = getVariables(csmc.cliqSubFg)
+          any_init = any(isInitialized.(allvars))
+          is_root = isempty(getEdgesParent(csmc.tree, csmc.cliq)) 
+          logCSM(csmc, "init_for_differential: "; c=csmc.cliqKey, is_root=is_root, any_init=any_init)
+          linear_on_manifold && !is_root && !any_init
+        end
+        
+        if init_for_differential
+          frontal_vars = getVariable.(csmc.cliqSubFg,  getCliqFrontalVarIds(csmc.cliq))
+          filter!(!isInitialized, frontal_vars)
+          foreach(fvar->getSolverData(fvar).initialized = true, frontal_vars)
+          logCSM(csmc, "init_for_differential: "; c=csmc.cliqKey,lbl=getLabel.(frontal_vars))
+        end
+        ## END experimental
+
     setCliqDrawColor(csmc.cliq, "green")
 
     logCSM(csmc, "X-3, Trying up init -- all not initialized"; c=csmc.cliqKey)
@@ -209,11 +238,23 @@ function solveUp_X_StateMachine(csmc::CliqStateMachineContainer)
     # is clique fully upsolved or only partially?
     # print out the partial init status of all vars in clique
     printCliqInitPartialInfo(csmc.cliqSubFg, csmc.cliq, csmc.logger)
-    logCSM(csmc, "X-3, tryInitCliq_StateMachine -- someInit=$someInit, varorder=$varorder"; c=csmc.cliqKey)
+    logCSM(csmc, "X-3, solveUp_X try init -- someInit=$someInit, varorder=$varorder"; c=csmc.cliqKey)
+  
+    someInit ? setCliqDrawColor(csmc.cliq, "darkgreen") :  setCliqDrawColor(csmc.cliq, "lightgreen")
 
     solveStatus = someInit ? :INIT : :NO_INIT
     converged_and_happy = true
-    
+
+        ## FIXME init to whatever is in frontals
+        # set frontals init back to false
+        if init_for_differential #experimental_sommer_init_to_whatever_is_in_frontals
+          foreach(fvar->getSolverData(fvar).initialized = false, frontal_vars)
+          if someInit 
+            solveStatus = :UPSOLVED
+          end
+        end
+        ## END EXPERIMENTAL
+
   else
     setCliqDrawColor(csmc.cliq, "brown")
     logCSM(csmc, "X-3, we are initialized but children need to init, don't do anything")
@@ -247,7 +288,9 @@ function solveUp_X_StateMachine(csmc::CliqStateMachineContainer)
   end
 
   #fill in belief
-  beliefMsg = prepCliqInitMsgsUp(csmc.cliqSubFg, csmc.cliq, solveStatus, logger=csmc.logger)
+  beliefMsg = prepCliqueMsgUpConsolidated(csmc.cliqSubFg, csmc.cliq, solveStatus, logger=csmc.logger)
+
+  logCSM(csmc, "X-3, prepCliqueMsgUpConsolidated", msgon=keys(beliefMsg.belief), beliefMsg=beliefMsg)
 
   setCliqueStatus!(csmc.cliq, solveStatus)
 
@@ -365,6 +408,7 @@ function tryDownInit_X_StateMachine(csmc::CliqStateMachineContainer)
     deleteMsgFactors!(csmc.cliqSubFg, msgfcts) # msgfcts # TODO, use tags=[:LIKELIHOODMESSAGE], see #760
     logCSM(csmc, "tryDownInit_X_StateMachine - removing factors, length=$(length(msgfcts))")
 
+    someInit ? setCliqDrawColor(csmc.cliq, "seagreen") :  setCliqDrawColor(csmc.cliq, "khaki")
 
   else
     solveStatus = getCliqueStatus(csmc.cliq)

src/experimental/SolveTree_X.jl

@@ -1,20 +1,24 @@
 
 function solveTree_X!(dfgl::AbstractDFG,
                     oldtree::AbstractBayesTree=emptyBayesTree();
+                    timeout::Union{Nothing, <:Real}=nothing,
                     storeOld::Bool=false,
                     verbose::Bool=false,
+                    verbosefid=stdout,
                     delaycliqs::Vector{Symbol}=Symbol[],
                     recordcliqs::Vector{Symbol}=Symbol[],
                     limititercliqs::Vector{Pair{Symbol, Int}}=Pair{Symbol, Int}[],
+                    injectDelayBefore::Union{Nothing,Vector{<:Pair{Int,<:Pair{<:Function, <:Real}}}}=nothing,
                     skipcliqids::Vector{Symbol}=Symbol[],
                     maxparallel::Union{Nothing, Int}=nothing,
                     variableOrder::Union{Nothing, Vector{Symbol}}=nothing,
                     variableConstraints::Vector{Symbol}=Symbol[],  
                     smtasks::Vector{Task}=Task[],
+                    dotreedraw = Int[1;],
                     runtaskmonitor::Bool=true,
+                    algorithm::Symbol=:default,
                     multithread::Bool=false)
   #
-  global dotreedraw
   # workaround in case isolated variables occur
   ensureSolvable!(dfgl)
   opt = getSolverParams(dfgl)
@@ -24,25 +28,28 @@ function solveTree_X!(dfgl::AbstractDFG,
 
   # depcrecation
   if maxparallel !== nothing
-    @warn "maxparallel keyword is deprecated, use getSolverParams(fg).maxincidence instead."
+    @warn "`maxparallel` keyword is deprecated, use `getSolverParams(fg).maxincidence` instead."
     opt.maxincidence = maxparallel
   end
 
   # update worker pool incase there are more or less
   setWorkerPool!()
   if opt.multiproc && nprocs() == 1
-    @warn "Cannot use multiproc with only one process, setting `.multiproc=false`."
+    @info "Setting `.multiproc=false` since `Distributed.nprocs() == 1`"
     opt.multiproc = false
   end
 
   if opt.graphinit
-    @info "ensure all initialized (using graphinit)"
+    @info "Ensure variables are all initialized (graphinit)"
     ensureAllInitialized!(dfgl)
+    if algorithm==:parametric
+      @warn "Parametric is using default graphinit"
+      initParametricFrom(dfgl)
+    end
   end
-
   # construct tree
   @info "Solving over the Bayes (Junction) tree."
- 
+  
   hist = Dict{Int, Vector{Tuple{DateTime, Int, Function, CliqStateMachineContainer}}}()
 
   if opt.isfixedlag
@@ -66,25 +73,45 @@ function solveTree_X!(dfgl::AbstractDFG,
   orderMethod = 0 < length(variableConstraints) ? :ccolamd : :qr
 
   # current incremental solver builds a new tree and matches against old tree for recycling.
-  tree = wipeBuildNewTree!(dfgl, variableOrder=variableOrder, drawpdf=opt.drawtree, show=opt.showtree,ensureSolvable=false,filepath=joinpath(opt.logpath,"bt.pdf"), variableConstraints=variableConstraints, ordering=orderMethod)
+  tree = resetBuildTree!(dfgl, variableOrder=variableOrder, drawpdf=opt.drawtree, show=opt.showtree,ensureSolvable=false,filepath=joinpath(opt.logpath,"bt.pdf"), variableConstraints=variableConstraints, ordering=orderMethod)
   # setAllSolveFlags!(tree, false)
 
   initTreeMessageChannels!(tree)
 
   # if desired, drawtree in a loop
-  treetask, dotreedraw = drawTreeAsyncLoop(tree, opt)
+  treetask, _dotreedraw = drawTreeAsyncLoop(tree, opt; dotreedraw = dotreedraw)
 
   @info "Do tree based init-inference on tree"
-  if opt.async
-    error("not implemented")
-    smtasks = asyncTreeInferUp!(dfgl, tree, oldtree=oldtree, N=opt.N, verbose=verbose, drawtree=opt.drawtree, recordcliqs=recordcliqs, limititers=opt.limititers, downsolve=opt.downsolve, incremental=opt.incremental, skipcliqids=skipcliqids, delaycliqs=delaycliqs, limititercliqs=limititercliqs )
-    @info "Tree based init-inference progressing asynchronously, check all CSM clique tasks for completion."
-  else
-    # smtasks, hist = taskSolveTree_X!(dfgl, tree; alltasks=smtasks, oldtree=oldtree, N=opt.N, verbose=verbose,  drawtree=opt.drawtree, recordcliqs=recordcliqs, limititers=opt.limititers, downsolve=opt.downsolve, incremental=opt.incremental, skipcliqids=skipcliqids, delaycliqs=delaycliqs, limititercliqs=limititercliqs, runtaskmonitor=runtaskmonitor )
-    smtasks, hist = taskSolveTree_X!(dfgl, tree; multithread=multithread, alltasks=smtasks, oldtree=oldtree, verbose=verbose,  drawtree=opt.drawtree, recordcliqs=recordcliqs, limititers=opt.limititers, downsolve=opt.downsolve, incremental=opt.incremental, skipcliqids=skipcliqids, delaycliqs=delaycliqs )
-    @info "Finished tree based init-inference"
+
+  # choose algorithm 
+  if algorithm == :parametric
+    @error "Under development, do not use, see #539"
+    storeOld && @error("parametric storeOld keyword not wired up yet.") 
+    initTreeMessageChannels!(tree)
+    alltasks, hist = taskSolveTreeParametric!(dfgl, tree; smtasks=smtasks, oldtree=tree, verbose=verbose, drawtree=opt.drawtree, recordcliqs=recordcliqs, limititers=opt.limititers, incremental=opt.incremental, skipcliqids=skipcliqids, delaycliqs=delaycliqs, multithread=multithread )
+    @info "Finished tree based Parametric inference"
+  elseif true # algorithm == :experimental
+    if opt.async
+      error("not implemented")
+    else
+      # smtasks, hist = taskSolveTree_X!(dfgl, tree; alltasks=smtasks, oldtree=oldtree, N=opt.N, verbose=verbose,  drawtree=opt.drawtree, recordcliqs=recordcliqs, limititers=opt.limititers, downsolve=opt.downsolve, incremental=opt.incremental, skipcliqids=skipcliqids, delaycliqs=delaycliqs, limititercliqs=limititercliqs, runtaskmonitor=runtaskmonitor )
+      smtasks, hist = taskSolveTree_X!(dfgl, tree         ; multithread=multithread, alltasks=smtasks, oldtree=oldtree,          verbose=verbose,                        drawtree=opt.drawtree, recordcliqs=recordcliqs, limititers=opt.limititers, downsolve=opt.downsolve, incremental=opt.incremental, skipcliqids=skipcliqids, delaycliqs=delaycliqs)
+    # smtasks, hist = initInferTreeUp!(dfgl, tree, timeout;                          alltasks=smtasks, oldtree=oldtree, N=opt.N, verbose=verbose, verbosefid=verbosefid, drawtree=opt.drawtree, recordcliqs=recordcliqs, limititers=opt.limititers, downsolve=opt.downsolve, incremental=opt.incremental, skipcliqids=skipcliqids, delaycliqs=delaycliqs, limititercliqs=limititercliqs, injectDelayBefore=injectDelayBefore, runtaskmonitor=runtaskmonitor)
+      @info "Finished tree based init-inference"
+    end
+  else #fall back is :default 
+    if opt.async
+      smtasks = asyncTreeInferUp!(dfgl, tree, timeout, oldtree=oldtree, N=opt.N, verbose=verbose, verbosefid=verbosefid, drawtree=opt.drawtree, recordcliqs=recordcliqs, limititers=opt.limititers, downsolve=opt.downsolve, incremental=opt.incremental, skipcliqids=skipcliqids, delaycliqs=delaycliqs, limititercliqs=limititercliqs, injectDelayBefore=injectDelayBefore )
+      @info "Tree based init-inference progressing asynchronously, check all CSM clique tasks for completion."
+    else
+
+      smtasks, hist = initInferTreeUp!(dfgl, tree, timeout; alltasks=smtasks, oldtree=oldtree, N=opt.N, verbose=verbose, verbosefid=verbosefid, drawtree=opt.drawtree, recordcliqs=recordcliqs, limititers=opt.limititers, downsolve=opt.downsolve, incremental=opt.incremental, skipcliqids=skipcliqids, delaycliqs=delaycliqs, limititercliqs=limititercliqs, injectDelayBefore=injectDelayBefore, runtaskmonitor=runtaskmonitor)
+
+      @info "Finished tree based init-inference"
+    end
   end
-  
+
+
   # NOTE copy of data from new tree in to replace outisde oldtree
   oldtree.bt = tree.bt
   oldtree.btid = tree.btid
@@ -104,8 +131,7 @@ function solveTree_X!(dfgl::AbstractDFG,
 
   # if debugging and not async then also print the CSMHistory
   if opt.dbg && !opt.async
-    #FIXME
-    # printCliqHistorySequential(hist, joinLogPath(dfgl,"HistoryCSMAll.txt") )
+    printCliqHistorySequential(hist, nothing, joinLogPath(dfgl,"HistoryCSMAll.txt") )
   end
 
   return oldtree, smtasks, hist
@@ -127,10 +153,7 @@ function taskSolveTree_X!(dfg::AbstractDFG,
                           alltasks = Task[])
   #
   # revert :downsolved status to :initialized in preparation for new upsolve
-  #TODO JT die ene lyk reg
   resetTreeCliquesForUpSolve!(treel)
-  #TODO JT needs to be updated
-  # setTreeCliquesMarginalized!(dfg, treel)
 
   drawtree ? drawTree(treel, show=true, filepath=joinpath(getSolverParams(dfg).logpath,"bt.pdf")) : nothing
 
@@ -140,6 +163,9 @@ function taskSolveTree_X!(dfg::AbstractDFG,
 
   resize!(alltasks, getNumCliqs(treel))
 
+  approx_iters = getNumCliqs(treel)*20
+  solve_progressbar = ProgressUnknown("Solve Progress: approx max $approx_iters, at iter")
+
   if !isTreeSolved(treel, skipinitialized=true)
     @sync begin
       monitortask = IIF.monitorCSMs(treel, alltasks)
@@ -148,9 +174,9 @@ function taskSolveTree_X!(dfg::AbstractDFG,
         scsym = getCliqFrontalVarIds(getClique(treel, i))
         if length(intersect(scsym, skipcliqids)) == 0
           if multithread
-            alltasks[i] = Threads.@spawn tryCliqStateMachineSolve_X!(dfg, treel, i, oldtree=oldtree, verbose=verbose, drawtree=drawtree, limititers=limititers, downsolve=downsolve, incremental=incremental, delaycliqs=delaycliqs, recordcliqs=recordcliqs)
+            alltasks[i] = Threads.@spawn tryCliqStateMachineSolve_X!(dfg, treel, i, oldtree=oldtree, verbose=verbose, drawtree=drawtree, limititers=limititers, downsolve=downsolve, incremental=incremental, delaycliqs=delaycliqs, recordcliqs=recordcliqs, solve_progressbar=solve_progressbar)
           else
-            alltasks[i] = @async tryCliqStateMachineSolve_X!(dfg, treel, i, oldtree=oldtree, verbose=verbose, drawtree=drawtree, limititers=limititers, downsolve=downsolve, incremental=incremental, delaycliqs=delaycliqs, recordcliqs=recordcliqs)
+            alltasks[i] = @async tryCliqStateMachineSolve_X!(dfg, treel, i, oldtree=oldtree, verbose=verbose, drawtree=drawtree, limititers=limititers, downsolve=downsolve, incremental=incremental, delaycliqs=delaycliqs, recordcliqs=recordcliqs, solve_progressbar=solve_progressbar)
           end
         end # if
       end # for
@@ -159,6 +185,8 @@ function taskSolveTree_X!(dfg::AbstractDFG,
 
   # if record cliques is in use, else skip computational delay
   0 == length(recordcliqs) ? nothing : fetchCliqHistoryAll!(alltasks, cliqHistories)
+  
+  finish!(solve_progressbar)
 
   return alltasks, cliqHistories
 end
@@ -174,7 +202,8 @@ function tryCliqStateMachineSolve_X!(dfg::G,
                                              downsolve::Bool=false,
                                              incremental::Bool=false,
                                              delaycliqs::Vector{Symbol}=Symbol[],
-                                             recordcliqs::Vector{Symbol}=Symbol[]) where G <: AbstractDFG
+                                             recordcliqs::Vector{Symbol}=Symbol[],
+                                             solve_progressbar=nothing) where G <: AbstractDFG
   #
   clst = :na
   cliq = getClique(treel, cliqKey) #treel.cliques[cliqKey]
@@ -195,7 +224,7 @@ function tryCliqStateMachineSolve_X!(dfg::G,
                                                     drawtree=drawtree, verbose=verbose,
                                                     limititers=limititers, downsolve=downsolve,
                                                     recordhistory=recordthiscliq, incremental=incremental,
-                                                    delay=delaythiscliq, logger=logger )
+                                                    delay=delaythiscliq, logger=logger, solve_progressbar=solve_progressbar )
     #
     # cliqHistories[cliqKey] = history
     if length(history) >= limititers && limititers != -1