consolidate CSMOptions, drop TreeBelief

IncrementalInference/src/CliqueStateMachine/services/CliqStateMachineUtils.jl

@@ -350,7 +350,7 @@ Future
 """
 function approxCliqMarginalUp!(
   csmc::CliqStateMachineContainer,
-  childmsgs = LikelihoodMessage[];#fetchMsgsUpChildren(csmc, TreeBelief);
+  childmsgs = LikelihoodMessage[];
   N::Int = getCliqueSolverParams(csmc).N,
   dbg::Bool = getCliqueSolverParams(csmc).dbg,
   multiproc::Bool = getCliqueSolverParams(csmc).multiproc,
@@ -379,7 +379,7 @@ function approxCliqMarginalUp!(
         getWorkerPool(),
         fg_,
         cliqc,
-        csmc.solveKey,
+        csmc.csmoptions.solveKey,
         childmsgs,
         N,
         dbg,
@@ -401,10 +401,10 @@ function approxCliqMarginalUp!(
       @info "Single process upsolve clique=$(cliq.id)"
     end
     retdict =
-      upGibbsCliqueDensity(fg_, cliq, csmc.solveKey, childmsgs, N, dbg, iters, logger)
+      upGibbsCliqueDensity(fg_, cliq, csmc.csmoptions.solveKey, childmsgs, N, dbg, iters, logger)
 
     # DEBUG ON THE FLY
-    Bs = [HomotopyDensity_legacy(b) for (l,b) in retdict]
+    # Bs = [HomotopyDensity_legacy(b) for (l,b) in retdict]
     # @info "DX bw" string.(getBandwidth.(Bs))
 
   end

IncrementalInference/src/CliqueStateMachine/services/CliqueStateMachine.jl

@@ -31,17 +31,10 @@ function initStartCliqStateMachine!(
     cliqSubFg = initfg(destType; solverParams = csmoptions.solverparams),
     tree,
     cliq,
-    incremental = csmoptions.incremental,
-    drawtree = csmoptions.drawtree,
-    dodownsolve = csmoptions.downsolve,
-    delay = csmoptions.delay,
-    opts = csmoptions.solverparams,
-    refactoring = Dict{Symbol, String}(),
     oldcliqdata,
     logger,
     cliqId = cliq.id,
-    algorithm = csmoptions.algorithm,
-    solveKey = csmoptions.solveKey,
+    csmoptions,
   )
 
   !csmoptions.upsolve && !csmoptions.downsolve && error("must attempt either up or down solve")
@@ -107,8 +100,8 @@ function setCliqueRecycling_StateMachine(csmc::CliqStateMachineContainer)
 
     # canCliqIncrRecycle
     # check if should be trying and can recycle clique computations
-  elseif csmc.incremental && oldstatus == DOWNSOLVED
-    csmc.cliq.data.isCliqReused = true
+  elseif csmc.csmoptions.incremental && oldstatus == DOWNSOLVED
+    csmc.csmoptions.dodownsolve && (csmc.cliq.data.isCliqReused = true)
     setCliqueStatus!(csmc.cliq, UPRECYCLED)
   end
   logCSM(
@@ -138,7 +131,7 @@ function buildCliqSubgraph_StateMachine(csmc::CliqStateMachineContainer)
   frontsyms = getCliqFrontalVarIds(csmc.cliq)
   sepsyms = getCliqSeparatorVarIds(csmc.cliq)
 
-  # TODO optimize by only fetching csmc.solveKey -- upgrades required
+  # TODO optimize by only fetching csmc.csmoptions.solveKey -- upgrades required
   buildCliqSubgraph!(csmc.cliqSubFg, csmc.dfg, frontsyms, sepsyms)
 
   # store the cliqSubFg for later debugging
@@ -160,12 +153,12 @@ function presolveChecklist_StateMachine(csmc::CliqStateMachineContainer)
 
   # check if solveKey is available in all variables?
   for var in getVariable.(csmc.cliqSubFg, ls(csmc.cliqSubFg))
-    if prepareState!(var, NPBPSolver(), csmc.solveKey; num_kernels=getCliqueSolverParams(csmc).N) == 1
+    if prepareState!(var, NPBPSolver(), csmc.csmoptions.solveKey; num_kernels=getCliqueSolverParams(csmc).N) == 1
       logCSM(
         csmc,
-        "CSM-0b create empty data for $(getLabel(var)) on solveKey=$(csmc.solveKey)",
+        "CSM-0b create empty data for $(getLabel(var)) on solveKey=$(csmc.csmoptions.solveKey)",
       )
-      @info "create vnd solveKey" csmc.solveKey getCliqueSolverParams(csmc).N
+      @info "create vnd solveKey" csmc.csmoptions.solveKey getCliqueSolverParams(csmc).N
       @info "also" listStates(var)
     end
   end
@@ -227,7 +220,7 @@ function waitForUp_StateMachine(csmc::CliqStateMachineContainer)
 
     return waitForDown_StateMachine
 
-  elseif csmc.algorithm == :parametric
+  elseif csmc.csmoptions.algorithm == :parametric
     !all(all_child_status .== UPSOLVED) && error("#FIXME")
     return solveUp_ParametricStateMachine
 
@@ -296,7 +289,7 @@ function preUpSolve_StateMachine(csmc::CliqStateMachineContainer)
   # if all(all_child_status .== UPSOLVED) 
   if all_child_finished_up
     return solveUp_StateMachine
-  elseif !areCliqVariablesAllInitialized(csmc.cliqSubFg, csmc.cliq, csmc.solveKey; N = getCliqueSolverParams(csmc).N)
+  elseif !areCliqVariablesAllInitialized(csmc.cliqSubFg, csmc.cliq, csmc.csmoptions.solveKey; N = getCliqueSolverParams(csmc).N)
     return initUp_StateMachine
   else
     setCliqueDrawColor!(csmc.cliq, "brown")
@@ -324,7 +317,7 @@ function initUp_StateMachine(csmc::CliqStateMachineContainer)
   linear_on_manifold = false
   init_for_differential = begin
     allvars = getVariables(csmc.cliqSubFg)
-    any_init = any(isInitialized.(allvars, csmc.solveKey))
+    any_init = any(isInitialized.(allvars, csmc.csmoptions.solveKey))
     is_root = isempty(getEdgesParent(csmc.tree, csmc.cliq))
     logCSM(
       csmc,
@@ -339,7 +332,7 @@ function initUp_StateMachine(csmc::CliqStateMachineContainer)
   if init_for_differential
     frontal_vars = getVariable.(csmc.cliqSubFg, getCliqFrontalVarIds(csmc.cliq))
     filter!(!isInitialized, frontal_vars)
-    foreach(fvar -> getState(fvar, csmc.solveKey).initialized = true, frontal_vars)
+    foreach(fvar -> getState(fvar, csmc.csmoptions.solveKey).initialized = true, frontal_vars)
     logCSM(
       csmc,
       "CSM-2b init_for_differential: ";
@@ -358,13 +351,13 @@ function initUp_StateMachine(csmc::CliqStateMachineContainer)
   someInit = cycleInitByVarOrder!(
     csmc.cliqSubFg,
     varorder;
-    solveKey = csmc.solveKey,
+    solveKey = csmc.csmoptions.solveKey,
     logger = csmc.logger,
     N = getCliqueSolverParams(csmc).N,
   )
   # is clique fully upsolved or only partially?
   # print out the partial init status of all vars in clique
-  printCliqInitPartialInfo(csmc.cliqSubFg, csmc.cliq, csmc.solveKey, csmc.logger)
+  printCliqInitPartialInfo(csmc.cliqSubFg, csmc.cliq, csmc.csmoptions.solveKey, csmc.logger)
   logCSM(
     csmc,
     "CSM-2b solveUp try init -- someInit=$someInit, varorder=$varorder";
@@ -382,7 +375,7 @@ function initUp_StateMachine(csmc::CliqStateMachineContainer)
   ## 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 -> getState(fvar, csmc.solveKey).initialized = false, frontal_vars)
+    foreach(fvar -> getState(fvar, csmc.csmoptions.solveKey).initialized = false, frontal_vars)
     if someInit
       solveStatus = UPSOLVED
     end
@@ -406,7 +399,7 @@ function solveUp_StateMachine(csmc::CliqStateMachineContainer)
   setCliqueDrawColor!(csmc.cliq, "red")
 
   #Make sure all are initialized
-  if !areCliqVariablesAllInitialized(csmc.cliqSubFg, csmc.cliq, csmc.solveKey; N = getCliqueSolverParams(csmc).N)
+  if !areCliqVariablesAllInitialized(csmc.cliqSubFg, csmc.cliq, csmc.csmoptions.solveKey; N = getCliqueSolverParams(csmc).N)
     logCSM(
       csmc,
       "CSM-2c All children upsolved, not init, try init then upsolve";
@@ -416,13 +409,13 @@ function solveUp_StateMachine(csmc::CliqStateMachineContainer)
     someInit = cycleInitByVarOrder!(
       csmc.cliqSubFg,
       varorder;
-      solveKey = csmc.solveKey,
+      solveKey = csmc.csmoptions.solveKey,
       logger = csmc.logger,
       N = getCliqueSolverParams(csmc).N,
     )
   end
 
-  isinit = areCliqVariablesAllInitialized(csmc.cliqSubFg, csmc.cliq, csmc.solveKey; N = getCliqueSolverParams(csmc).N)
+  isinit = areCliqVariablesAllInitialized(csmc.cliqSubFg, csmc.cliq, csmc.csmoptions.solveKey; N = getCliqueSolverParams(csmc).N)
   logCSM(csmc, "CSM-2c midway, isinit=$isinit")
   # Check again  
   if isinit
@@ -518,7 +511,7 @@ function postUpSolve_StateMachine(csmc::CliqStateMachineContainer)
   beliefMsg = prepCliqueMsgUp(
     csmc.cliqSubFg,
     csmc.cliq,
-    csmc.solveKey,
+    csmc.csmoptions.solveKey,
     solveStatus;
     logger = csmc.logger,
     sender = (; id = csmc.cliq.id.value, step = csmc._csm_iter),
@@ -613,7 +606,7 @@ function waitForDown_StateMachine(csmc::CliqStateMachineContainer)
       putErrorDown(csmc)
       return IncrementalInference.exitStateMachine
 
-    elseif csmc.algorithm == :parametric
+    elseif csmc.csmoptions.algorithm == :parametric
       beliefMsg.status != DOWNSOLVED && error("#FIXME")
       return solveDown_ParametricStateMachine
     elseif beliefMsg.status in [MARGINALIZED, DOWNSOLVED, INITIALIZED, NO_INIT]
@@ -636,7 +629,7 @@ function waitForDown_StateMachine(csmc::CliqStateMachineContainer)
 
   # The clique is a root
   # root clique down branching happens here
-  if csmc.algorithm == :parametric
+  if csmc.csmoptions.algorithm == :parametric
     return solveDown_ParametricStateMachine
   else
     return preDownSolve_StateMachine
@@ -659,8 +652,8 @@ function CliqDownMessage(csmc::CliqStateMachineContainer, status = DOWNSOLVED)
   # create all messages from subfg
   for mk in getCliqFrontalVarIds(csmc.cliq)
     v = getVariable(csmc.cliqSubFg, mk)
-    if isInitialized(v, csmc.solveKey)
-      newDwnMsgs.belief[mk] = TreeBelief(v, csmc.solveKey)
+    if isInitialized(v, csmc.csmoptions.solveKey)
+      newDwnMsgs.belief[mk] = TreeBelief(v, csmc.csmoptions.solveKey)
     end
   end
 
@@ -747,13 +740,13 @@ function tryDownInit_StateMachine(csmc::CliqStateMachineContainer)
   someInit = cycleInitByVarOrder!(
     csmc.cliqSubFg,
     initorder;
-    solveKey = csmc.solveKey,
+    solveKey = csmc.csmoptions.solveKey,
     logger = csmc.logger,
     N = getCliqueSolverParams(csmc).N,
   )
   # is clique fully upsolved or only partially?
   # print out the partial init status of all vars in clique
-  printCliqInitPartialInfo(csmc.cliqSubFg, csmc.cliq, csmc.solveKey, csmc.logger)
+  printCliqInitPartialInfo(csmc.cliqSubFg, csmc.cliq, csmc.csmoptions.solveKey, csmc.logger)
   logCSM(csmc, "CSM-4b tryInitCliq_StateMachine -- someInit=$someInit, varorder=$initorder")
 
   msgfcts = deleteMsgFactors!(csmc.cliqSubFg, [:__DOWNWARD_COMMON__;]) # msgfcts # TODO, use tags=[:__LIKELIHOODMESSAGE__], see #760
@@ -904,37 +897,37 @@ Notes
 - CSM function 5
 """
 function updateFromSubgraph_StateMachine(csmc::CliqStateMachineContainer)
-  isParametricSolve = csmc.algorithm == :parametric
+  isParametricSolve = csmc.csmoptions.algorithm == :parametric
 
   # set solved for all frontals
   if !isParametricSolve
     for sym in getCliqFrontalVarIds(csmc.cliq)
       # set solved flag
       vari = getVariable(csmc.cliqSubFg, sym)
-      setSolvedCount!(vari, getSolvedCount(vari, csmc.solveKey) + 1, csmc.solveKey)
+      setSolvedCount!(vari, getSolvedCount(vari, csmc.csmoptions.solveKey) + 1, csmc.csmoptions.solveKey)
     end
   end
 
   # transfer results to main factor graph
   frsyms = getCliqFrontalVarIds(csmc.cliq)
   logCSM(
     csmc,
-    "CSM-5 finishingCliq -- transferUpdateSubGraph! with solveKey=$(csmc.solveKey) on $frsyms",
+    "CSM-5 finishingCliq -- transferUpdateSubGraph! with solveKey=$(csmc.csmoptions.solveKey) on $frsyms",
   )
   transferUpdateSubGraph!(
     csmc.dfg,
     csmc.cliqSubFg,
     frsyms,
     csmc.logger;
-    solveKey = csmc.solveKey,
+    solveKey = csmc.csmoptions.solveKey,
   )
 
   #solve finished change color
   setCliqueDrawColor!(csmc.cliq, "lightblue")
 
   logCSM(
     csmc,
-    "CSM-5 Clique $(csmc.cliq.id) finished, solveKey=$(csmc.solveKey)";
+    "CSM-5 Clique $(csmc.cliq.id) finished, solveKey=$(csmc.csmoptions.solveKey)";
     loglevel = Logging.Info,
   )
   return IncrementalInference.exitStateMachine

IncrementalInference/src/Deprecated.jl

@@ -1,6 +1,97 @@
 
 
+# convert(
+#   ::Type{<:ApproxManifoldProducts.HomotopyDensity}, 
+#   src::TreeBelief,
+# ) = HomotpyDensity_legacy(src)
+
+# function setValKDE!(
+#   v::VariableCompute,
+#   em::TreeBelief,
+#   setinit::Bool = true;
+#   # inferdim::Union{Float32, Float64, Int32, Int64}=0;
+#   solveKey::Symbol = :default,
+# )
+#   #
+#   setValKDE!(v, em.val, em.bw, setinit, em.infoPerCoord; solveKey = solveKey)
+#   return nothing
+# end
+
+# function convert(::Type{Tuple{ApproxManifoldProducts.HomotopyDensity, Float64}}, p::TreeBelief)
+#   # 
+#   return (convert(ApproxManifoldProducts.HomotopyDensity, p), p.infoPerCoord)
+# end
+
+# DFG.getStateKind(tb::TreeBelief) = tb.variableType
+
+# DFG.getManifold(treeb::TreeBelief) = getManifold(treeb.variableType)
+
+# function compare(t1::TreeBelief, t2::TreeBelief)
+#   TP = true
+#   TP = TP && norm(t1.val - t2.val) < 1e-5
+#   TP = TP && norm(t1.bw - t2.bw) < 1e-5
+#   TP = TP && isapprox(t1.infoPerCoord, t2.infoPerCoord; atol = 1e-4)
+#   TP = TP && t1.variableType == t2.variableType
+#   TP = TP && abs(t1.solvableDim - t2.solvableDim) < 1e-5
+#   return TP
+# end
+
+# struct TreeBelief{T <: StateType, P, M <: MB.AbstractManifold}
+#   val::Vector{P}
+#   bw::Array{Float64, 2}
+#   infoPerCoord::Vector{Float64}
+#   # see DFG #603, variableType defines the domain and manifold as well as group operations for a variable in the factor graph
+#   variableType::T
+#   # TODO -- DEPRECATE
+#   manifold::M # Tuple{Vararg{Symbol}} # NOTE added during #459 effort
+#   # only populated during up as solvableDims for each variable in clique, #910
+#   solvableDim::Float64
+# end
+
 
+# function HomotopyDensity_legacy(
+#   treeb::TreeBelief,
+# )
+#   # FIXME, partials still need to be dealt with here
+#   return ApproxManifoldProducts.HomotopyDensity_legacy(
+#     treeb.variableType,
+#     treeb.val;
+#     bw = treeb.bw,
+#     newbw = false,
+#     observability = treeb.infoPerCoord,
+#   )
+# end
+
+
+# """
+#     solveGrapn!
+
+# Just an alias, see documentation for `solveTree!`.
+# """
+# DFG.solveGraph!(dfg::AbstractDFG, w...;kw...) = solveTree!(dfg, w...;kw...)
+
+
+@deprecate solveTree!(
+  dfg::AbstractDFG,
+  oldtree::AbstractBayesTree = BayesTree();
+  # tree options
+  eliminationOrder::Union{Nothing, Vector{Symbol}} = nothing,
+  eliminationConstraints::Vector{Symbol} = Symbol[],
+  smtasks = Task[],
+  # execution options
+  solverparams = getSolverParams(dfg),
+  kwargs...
+) solveGraph!(
+  dfg, 
+  oldtree; 
+  eliminationOrder, 
+  eliminationConstraints, 
+  smtasks,
+  csmoptions = CSMOptions(;
+    solverparams = solverparams,
+    kwargs...,
+  )
+)
 
 # function CliqStateMachineContainer(
 #   dfg::G,
@@ -100,7 +191,7 @@ function calcMeanMaxSuggested(
 end
 
 
-@deprecate manikde!(tb::TreeBelief) HomotopyDensity_legacy(tb)
+@deprecate manikde!(hode::HomotopyDensity) hode
 
 @deprecate manikde!(
   varT::InstanceType{<:StateType},

IncrementalInference/src/Serialization/services/DispatchPackedConversions.jl

@@ -21,13 +21,6 @@ function DFG.rebuildFactorCache!(
   return nothing
 end
 
-## =================================================================
-## TODO Can code below be deprecated?
-## =================================================================
 
-function convert(::Type{Tuple{ApproxManifoldProducts.HomotopyDensity, Float64}}, p::TreeBelief)
-  # 
-  return (convert(ApproxManifoldProducts.HomotopyDensity, p), p.infoPerCoord)
-end
 
 #