legacy solveTree(=:parametric) functional, intro CSMOptions

IncrementalInference/Project.toml

@@ -76,7 +76,7 @@ CliqueTrees = "0.5, 1"
 Combinatorics = "1.0"
 DataStructures = "0.18, 0.19"
 DelimitedFiles = "1"
-DifferentialEquations = "7"
+DifferentialEquations = "7, 8"
 DistributedFactorGraphs = "0.29"
 Distributions = "0.24, 0.25"
 DocStringExtensions = "0.8, 0.9"
@@ -102,7 +102,7 @@ Optim = "1"
 OrderedCollections = "1"
 PrecompileTools = "1"
 ProgressMeter = "1"
-RecursiveArrayTools = "3, 4"
+RecursiveArrayTools = "3"
 Reexport = "1"
 SparseDiffTools = "2"
 StaticArrays = "1"

IncrementalInference/src/CliqueStateMachine/services/CliqueStateMachine.jl

@@ -12,57 +12,43 @@ function initStartCliqStateMachine!(
   tree::AbstractBayesTree,
   cliq::TreeClique,
   timeout::Union{Nothing, <:Real} = nothing;
-  solverparams::SolverParams,
   oldcliqdata::BayesTreeNodeData = BayesTreeNodeData(),
-  verbose::Bool = false,
-  verbosefid = stdout,
-  drawtree::Bool = false,
-  show::Bool = false,
-  incremental::Bool = true,
-  limititers::Int = 20,
-  upsolve::Bool = true,
-  downsolve::Bool = true,
-  recordhistory::Bool = false,
-  delay::Bool = false,
   logger::SimpleLogger = SimpleLogger(Base.stdout),
-  solve_progressbar = nothing,
-  algorithm::Symbol = :default,
-  solveKey::Symbol = algorithm,
+  csmoptions::CSMOptions = CSMOptions(;
+    solverparams = getSolverParams(dfg)
+  ),
 )
 
   # NOTE use tree and messages for operations involving children and parents
   # TODO deprecate children and prnt clique copies
   # children = TreeClique[]
   # prnt = TreeClique[]
 
-  destType = dfg isa InMemoryDFGTypes ? typeof(dfg) : LocalDFG
+  destType = dfg isa InMemoryDFGTypes ? typeof(dfg) : LocalDFG # TODO, is this type-stable?
 
-  csmc = CliqStateMachineContainer(
+  csmc = CliqStateMachineContainer(;
     dfg,
-    initfg(destType; solverParams = solverparams),
+    cliqSubFg = initfg(destType; solverParams = csmoptions.solverparams),
     tree,
     cliq,
-    incremental,
-    drawtree,
-    downsolve,
-    delay,
-    solverparams,
-    Dict{Symbol, String}(),
+    incremental = csmoptions.incremental,
+    drawtree = csmoptions.drawtree,
+    dodownsolve = csmoptions.downsolve,
+    delay = csmoptions.delay,
+    opts = csmoptions.solverparams,
+    refactoring = Dict{Symbol, String}(),
     oldcliqdata,
     logger,
-    cliq.id,
-    algorithm,
-    0,
-    true,
-    solveKey,
-    0,
+    cliqId = cliq.id,
+    algorithm = csmoptions.algorithm,
+    solveKey = csmoptions.solveKey,
   )
 
-  !upsolve && !downsolve && error("must attempt either up or down solve")
+  !csmoptions.upsolve && !csmoptions.downsolve && error("must attempt either up or down solve")
   # nxt = buildCliqSubgraph_StateMachine
   nxt = setCliqueRecycling_StateMachine
 
-  csmiter_cb = if solverparams.drawCSMIters
+  csmiter_cb = if csmoptions.solverparams.drawCSMIters
     ((st::StateMachine) -> (cliq.attributes["xlabel"] = st.iter; csmc._csm_iter = st.iter))
   else
     ((st) -> (csmc._csm_iter = st.iter))
@@ -72,7 +58,7 @@ function initStartCliqStateMachine!(
     StateMachine{CliqStateMachineContainer}(; next = nxt, name = "cliq$(getId(cliq))")
 
   # store statemachine and csmc in task
-  if solverparams.dbg || recordhistory
+  if csmoptions.solverparams.dbg || csmoptions.recordhistory
     task_local_storage(:statemachine, statemachine)
     task_local_storage(:csmc, csmc)
   end
@@ -84,17 +70,20 @@ function initStartCliqStateMachine!(
   # verbosefid=verbosefid
   # injectDelayBefore=injectDelayBefore
 
+  counter = 0
+
   while statemachine(
     csmc,
     timeout;
-    verbose = verbose,
-    verbosefid = verbosefid,
+    verbose = csmoptions.verbose,
+    verbosefid = csmoptions.verbosefid,
     verboseXtra = getCliqueStatus(csmc.cliq),
-    iterlimit = limititers,
-    recordhistory = recordhistory,
+    iterlimit = csmoptions.limititers,
+    recordhistory = csmoptions.recordhistory,
     housekeeping_cb = csmiter_cb,
   )
-    !isnothing(solve_progressbar) && next!(solve_progressbar)
+    counter += 1
+    !isnothing(csmoptions.solve_progressbar) && next!(csmoptions.solve_progressbar)
   end
 
   return CSMHistoryTuple.(statemachine.history)

IncrementalInference/src/Deprecated.jl

@@ -1,5 +1,53 @@
 
 
+
+
+# function CliqStateMachineContainer(
+#   dfg::G,
+#   cliqSubFg::M,
+#   tree::T,
+#   cliq::TreeClique,
+#   incremental::Bool,
+#   drawtree::Bool,
+#   dodownsolve::Bool,
+#   delay::Bool,
+#   opts::SolverParams,
+#   refactoring::Dict{Symbol, String} = Dict{Symbol, String}(),
+#   oldcliqdata::BTND = BayesTreeNodeData(),
+#   logger::SimpleLogger = SimpleLogger(Base.stdout);
+#   cliqId::CliqueId = cliq.id,
+#   algorithm::Symbol = :default,
+#   init_iter::Int = 0,
+#   enableLogging::Bool = true,
+#   solveKey::Symbol = :default,
+#   _csm_iter::Int = 0,
+# ) where {BTND, G <: AbstractDFG, M <: InMemoryDFGTypes, T <: AbstractBayesTree}
+#   #
+#   return CliqStateMachineContainer{BTND, G, M, T}(
+#     dfg,
+#     cliqSubFg,
+#     tree,
+#     cliq,
+#     incremental,
+#     drawtree,
+#     dodownsolve,
+#     delay,
+#     opts,
+#     refactoring,
+#     oldcliqdata,
+#     logger,
+#     cliqId,
+#     algorithm,
+#     init_iter,
+#     enableLogging,
+#     solveKey,
+#     _csm_iter,
+#   )
+#   #
+# end
+
+
+
 """
     $SIGNATURES
 

IncrementalInference/src/entities/BeliefTypes.jl

@@ -92,7 +92,13 @@ end
 
 function TreeBelief(state::State, solvDim::Real = 0)
   pts = getPoints(state.belief; permute=false) # TODO likely want to go back to sorted order here, DX debugging with permute=false
-  cv = getBW(state.belief)[1]
+  cv = if length(getBW(state.belief)) <= 0 || !isassigned(getBW(state.belief), 1)
+    @info "WHY" string(getBW(state.belief)) Npts(state.belief)
+    @warn "TreeBelief constructor: belief has no bandwidth, defaulting to identity, incorrect -- must refactor, see #1929" maxlog = 20
+    cov(state.belief)
+  else
+    getBW(state.belief)[1] # FIXME, bw for nonparametric, cov for parametric -- can reuse bw for parametric during AMP 15 refactor
+  end
   obsv = DFG.refObservability(state)
   statekind = getStateKind(state)
 

IncrementalInference/src/entities/JunctionTreeTypes.jl

@@ -21,6 +21,24 @@ end
 const BayesTree = MetaBayesTree
 
 
+@kwdef mutable struct CSMOptions
+  solverparams::Any #FIXME deprecation step
+  verbose::Bool = false
+  verbosefid::Any = stdout
+  drawtree::Bool = false
+  limititers::Int = -1
+  downsolve::Bool = false
+  upsolve::Bool = true # TODO unchecked consolidation, consolidate from solverparams
+  incremental::Bool = false
+  delaycliqs::Vector{Symbol} = Symbol[]
+  recordcliqs::Vector{Symbol} = Symbol[]
+  solve_progressbar::Any = nothing
+  algorithm::Symbol = :default
+  solveKey::Symbol = algorithm
+  recordhistory::Bool = false
+  delay::Bool = false
+end
+
 """
     $TYPEDEF
 
@@ -29,7 +47,7 @@ Container for upward tree solve / initialization.
 DevNotes
 - TODO more direct clique access (cliq, parent, children), for multi-process solves
 """
-mutable struct CliqStateMachineContainer{
+@kwdef mutable struct CliqStateMachineContainer{
   BTND,
   G <: AbstractDFG,
   InMemG <: InMemoryDFGTypes,
@@ -39,22 +57,23 @@ mutable struct CliqStateMachineContainer{
   cliqSubFg::InMemG
   tree::BT
   cliq::TreeClique
-  incremental::Bool
-  drawtree::Bool
+    incremental::Bool
+    drawtree::Bool
   dodownsolve::Bool
-  delay::Bool
+    delay::Bool
   opts::SolverParams
-  refactoring::Dict{Symbol, String}
-  oldcliqdata::BTND
-  logger::SimpleLogger
-  cliqId::CliqueId
-  algorithm::Symbol
-  init_iter::Int
-  enableLogging::Bool
-  solveKey::Symbol
-  _csm_iter::Int
+  refactoring::Dict{Symbol, String} = Dict{Symbol, String}()
+  oldcliqdata::BTND = BayesTreeNodeData()
+  logger::SimpleLogger = SimpleLogger(Base.stdout)
+  cliqId::CliqueId = cliq.id # obsolete?
+    algorithm::Symbol = :default
+  init_iter::Int = 0
+  enableLogging::Bool = true
+    solveKey::Symbol = :default
+  _csm_iter::Int = 0
 end
 
+
 #TODO use @NamedTuple if julia compat > 1.5
 
 const CSMHistoryTuple = NamedTuple{

IncrementalInference/src/parametric/services/ParametricCSMFunctions.jl

@@ -38,10 +38,8 @@ function solveUp_ParametricStateMachine(csmc::CliqStateMachineContainer)
       vnd = getState(getVariable(csmc.cliqSubFg, v), :parametric)
       # fill in the variable node data value
       logCSM(csmc, "$(csmc.cliq.id) up: updating $v : $val")
-      DFG.refMeans(vnd)[1] = val.val
-      #calculate and fill in covariance
-      #TODO rather broadcast than make new memory
-      DFG.refCovariances(vnd)[1] = val.cov
+      # #calculate and fill in covariance
+      setBelief!(vnd, HomotopyDensity_legacy(getStateKind(vnd), [val.val,]; bw=val.cov, newbw=false))
     end
     # elseif length(lsfPriors(csmc.cliqSubFg)) == 0 #FIXME
     #   @error "Par-3, clique $(csmc.cliq.id) failed to converge in upsolve, but ignoring since no priors" result
@@ -118,8 +116,10 @@ function solveDown_ParametricStateMachine(csmc::CliqStateMachineContainer)
         #TODO maybe combine variable and factor in new prior?
         vnd = getState(getVariable(csmc.cliqSubFg, msym), :parametric)
         logCSM(csmc, "$(csmc.cliq.id): Updating separator $msym from message $(belief.val)")
-        DFG.refMeans(vnd)[1] = belief.val[1] #FIXME 🦨 shares data structure in belief
-        DFG.refCovariances(vnd)[1] = belief.bw
+        bel = HomotopyDensity_legacy(belief) #TODO maybe need to deepcopy here, not sure if its shared data structure
+        setBelief!(vnd, bel)
+        # DFG.refMeans(vnd)[1] = belief.val[1] #FIXME 🦨 shares data structure in belief
+        # DFG.refCovariances(vnd)[1] = belief.bw
       end
     end
   end
@@ -146,8 +146,8 @@ function solveDown_ParametricStateMachine(csmc::CliqStateMachineContainer)
         logCSM(csmc, "$(csmc.cliq.id) down: updating $v : $val"; loglevel = Logging.Info)
         vnd = getState(getVariable(csmc.cliqSubFg, v), :parametric)
         #Update subfg variables
-        DFG.refMeans(vnd)[1] = val.val
-        DFG.refCovariances(vnd)[1] = val.cov
+        hode = HomotopyDensity_legacy(getStateKind(vnd), [val.val,]; bw=val.cov, newbw=false)
+        setBelief!(vnd, hode)
       end
     else
       @error "Par-5, clique $(csmc.cliq.id) failed to converge in down solve" result

IncrementalInference/src/parametric/services/ParametricManopt.jl

@@ -450,7 +450,7 @@ function solve_RLM(
 
   #Can use varIntLabel (because its an OrderedDict), but varLabelsAP makes the ArrayPartition.
   p0 = map(varlabelsAP) do label
-    DFG.refMeans(getState(fg, label, solveKey))[1]
+    mean(getBelief(getState(fg, label, solveKey)))
   end
 
   # create an ArrayPartition{CalcFactorResidual} for faclabels
@@ -609,7 +609,7 @@ function solve_RLM_conditional(
   all_varlabelsAP = ArrayPartition((frontal_varlabelsAP.x..., separator_varlabelsAP.x...))
 
   all_points = map(all_varlabelsAP) do label
-    DFG.refMeans(getState(fg, label, solveKey))[1]
+    mean(getBelief(getState(fg, label, solveKey)))
   end
 
   p0 = ArrayPartition(all_points.x[1:length(frontal_varlabelsAP.x)])
@@ -780,34 +780,46 @@ function autoinitParametric!(
         getStateKind(getVariable(dfg, vl)) === my_kind
       end
       if !isempty(same_kind)
-        DFG.refMeans(vnd)[1] = DFG.refMeans(getState(dfg, same_kind[1], solveKey))[1]
+        mn = mean(getBelief(getState(dfg, same_kind[1], solveKey)))
+        _bw = cov(getBelief(vnd))
+        # BW = getBW(getBelief(vnd))
+        # _bw = (0<length(BW)) && isassigned(BW,1) ? BW[1] : nothing
+        _hode = HomotopyDensity_legacy(getStateKind(vnd),[mn,]; bw=_bw, newbw=false)
+        setBelief!(vnd, _hode)
       end
     end
   end
 
   # Solve
   M, varlabelsAP, lm_r, Λ, _ = solve_RLM_conditional(dfg, to_init, active_separators; solveKey, linear_subsolver!, kwargs...)
 
+  _Σ = (I)(size(Λ, 1))
+  _Σ_ = sparse(1.0*I, size(Λ, 1), size(Λ, 1)) # create a sparse identity matrix
+  # _Σ_ = (1.0*I)(size(Λ, 1)) # legacy was Bool, weird refactor forced premature Float64
+
+  offset = 0
+
+  invertonce = true
   # Update each frontal variable with result
   for (i, v) in enumerate(varlabelsAP)
-    vnd = getState(dfg, v, solveKey)
-    DFG.refMeans(vnd)[1] = lm_r[i]
-    vnd.initialized = true
-  end
-
-  # Update covariances from joint precision if positive definite
-  if !isnothing(Λ)
-    F = cholesky!(Λ; check = false)
-    if issuccess(F)
-      Σ = F \ I(size(Λ, 1))
-      offset = 0
-      for (i, v) in enumerate(varlabelsAP)
-        dim = manifold_dimension(getManifold(getVariable(dfg, v)))
-        r = (offset + 1):(offset + dim)
-        DFG.refCovariances(getState(dfg, v, solveKey))[1] .= Σ[r, r]
-        offset += dim
+    vrb = getVariable(dfg, v)
+    state = getState(vrb, solveKey)
+
+    # Update covariances from joint precision if positive definite
+    if invertonce && !isnothing(Λ)
+      F = cholesky!(Λ; check = false)
+      if issuccess(F)
+        invertonce = false
+        _Σ_ .= (F \ _Σ) 
       end
     end
+    dim = getDimension(vrb)
+    r = (offset + 1):(offset + dim)
+    offset += dim
+    bw = ApproxManifoldProducts._forcestatic(_Σ_[r,r])
+    # @info "WHAT" string(r) string(lm_r[i]) string(bw)
+    hode = HomotopyDensity_legacy(getStateKind(state),[lm_r[i],]; bw, newbw=false)
+    setBelief!(state, hode, true)
   end
 
   return true