Update and fix VariableState and solveKey (#1876)

Author: Affie

IncrementalInference/src/Deprecated.jl

@@ -120,7 +120,7 @@ function solveGraphParametric2(
   flatvar = FlatVariables(fg, varIds)
 
   for vId in varIds
-    p = getVariableSolverData(fg, vId, solvekey).val[1]
+    p = getVariableState(fg, vId, solvekey).val[1]
     flatvar[vId] = getCoordinates(getVariableType(fg, vId), p)
   end
 

IncrementalInference/src/Serialization/services/DispatchPackedConversions.jl

@@ -143,7 +143,7 @@ function DFG.rebuildFactorCache!(
 
   #   factor__
   # else
-  #   setSolverData!(factor, new_solverData)
+  #   mergeVariableState!(factor, new_solverData)
   #   DFG.setCache!(factor, solvercache)
   #   # We're not updating here because we don't want
   #   # to solve cloud in loop, we want to make sure this flow works:

IncrementalInference/src/parametric/services/ParametricUtils.jl

@@ -489,7 +489,7 @@ function initPoints!(p, gsc, fg::AbstractDFG, solveKey = :parametric)
   for (i, vartype) in enumerate(gsc.varTypes)
     varIds = gsc.varTypesIds[vartype]
     for (j, vId) in enumerate(varIds)
-      p[gsc.M, i][j] = getVariableSolverData(fg, vId, solveKey).val[1]
+      p[gsc.M, i][j] = getVariableState(fg, vId, solveKey).val[1]
     end
   end
 end
@@ -674,7 +674,7 @@ function solveConditionalsParametric(
   flatvar = FlatVariables(fg, varIds)
 
   for vId in varIds
-    p = getVariableSolverData(fg, vId, solvekey).val[1]
+    p = getVariableState(fg, vId, solvekey).val[1]
     flatvar[vId] = getCoordinates(getVariableType(fg, vId), p)
   end
   initValues = flatvar.X

IncrementalInference/src/services/BayesNet.jl

@@ -76,20 +76,20 @@ function addBayesNetVerts!(dfg::AbstractDFG, elimOrder::Array{Symbol, 1})
   #
   for pId in elimOrder
     vert = DFG.getVariable(dfg, pId)
-    if  getVariableState(vert).BayesNetVertID === nothing ||
-        getVariableState(vert).BayesNetVertID == :_null # Special serialization case of nothing
+    if  getVariableState(vert, :default).BayesNetVertID === nothing ||
+        getVariableState(vert, :default).BayesNetVertID == :_null # Special serialization case of nothing
       @debug "[AddBayesNetVerts] Assigning $pId.data.BayesNetVertID = $pId"
-      getVariableState(vert).BayesNetVertID = pId
+      getVariableState(vert, :default).BayesNetVertID = pId
     else
-      @warn "addBayesNetVerts -- Something is wrong, variable '$pId' should not have an existing Bayes net reference to '$(getVariableState(vert).BayesNetVertID)'"
+      @warn "addBayesNetVerts -- Something is wrong, variable '$pId' should not have an existing Bayes net reference to '$(getVariableState(vert, :default).BayesNetVertID)'"
     end
   end
 end
 
 function addConditional!(dfg::AbstractDFG, vertId::Symbol, Si::Vector{Symbol})
   #
   bnv = DFG.getVariable(dfg, vertId)
-  bnvd = getVariableState(bnv)
+  bnvd = getVariableState(bnv, :default)
   bnvd.separator = Si
   for s in Si
     push!(bnvd.BayesNetOutVertIDs, s)
@@ -188,7 +188,7 @@ function buildBayesNet!(dfg::AbstractDFG, elimorder::Vector{Symbol}; solvable::I
     end
 
     # mark variable
-    getVariableState(vert).eliminated = true
+    getVariableState(vert, :default).eliminated = true
 
     # TODO -- remove links from current vertex to any marginals
     rmVarFromMarg(dfg, vert, gm)

IncrementalInference/src/services/FGOSUtils.jl

@@ -168,19 +168,19 @@ joinLogPath(dfg::AbstractDFG, str...) = joinLogPath(getSolverParams(dfg), str...
 
 Set variable(s) `sym` of factor graph to be marginalized -- i.e. not be updated by inference computation.
 """
-function setfreeze!(dfg::AbstractDFG, sym::Symbol)
+function setfreeze!(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol = :default)
   if !isInitialized(dfg, sym)
     @warn "Vertex $(sym) is not initialized, and won't be frozen at this time."
     return nothing
   end
   vert = DFG.getVariable(dfg, sym)
-  data = getVariableState(vert)
+  data = getVariableState(vert, solveKey)
   data.ismargin = true
   return nothing
 end
-function setfreeze!(dfg::AbstractDFG, syms::Vector{Symbol})
+function setfreeze!(dfg::AbstractDFG, syms::Vector{Symbol}, solveKey::Symbol = :default)
   for sym in syms
-    setfreeze!(dfg, sym)
+    setfreeze!(dfg, sym, solveKey)
   end
 end
 
@@ -357,10 +357,10 @@ Reset initialization flag on all variables in `::AbstractDFG`.
 Notes
 - Numerical values remain, but inference will overwrite since init flags are now `false`.
 """
-function resetVariableAllInitializations!(fgl::AbstractDFG)
+function resetVariableAllInitializations!(fgl::AbstractDFG, solveKey::Symbol = :default)
   vsyms = ls(fgl)
   for sym in vsyms
-    setVariableInitialized!(getVariable(fgl, sym), :false)
+    setVariableInitialized!(getVariable(fgl, sym), solveKey, false)
   end
   return nothing
 end

IncrementalInference/src/services/FactorGraph.jl

@@ -302,12 +302,11 @@ end
 Set variable initialized status.
 """
 function setVariableInitialized!(varid::VariableNodeData, status::Bool)
-  #
   return varid.initialized = status
 end
-#TODO why no solveKey
-function setVariableInitialized!(vari::DFGVariable, status::Bool)
-  return setVariableInitialized!(getVariableState(vari), status)
+
+function setVariableInitialized!(vari::DFGVariable, solveKey::Symbol, status::Bool)
+  return setVariableInitialized!(getVariableState(vari, solveKey), status)
 end
 
 """
@@ -348,7 +347,7 @@ end
 
 Reset the solve state of a variable to uninitialized/unsolved state.
 """
-function resetVariable!(varid::VariableNodeData; solveKey::Symbol = :default)::Nothing
+function resetVariable!(varid::VariableNodeData)
   #
   val = getBelief(varid)
   pts = AMP.getPoints(val)
@@ -363,17 +362,12 @@ function resetVariable!(varid::VariableNodeData; solveKey::Symbol = :default)::N
   return nothing
 end
 
-function resetVariable!(vari::DFGVariable; solveKey::Symbol = :default)
-  return resetVariable!(getVariableState(vari); solveKey = solveKey)
+function resetVariable!(vari::DFGVariable, solveKey::Symbol = :default)
+  return resetVariable!(getVariableState(vari, solveKey))
 end
 
-function resetVariable!(
-  dfg::G,
-  sym::Symbol;
-  solveKey::Symbol = :default,
-)::Nothing where {G <: AbstractDFG}
-  #
-  return resetVariable!(getVariable(dfg, sym); solveKey = solveKey)
+function resetVariable!(dfg::AbstractDFG, sym::Symbol, solveKey::Symbol = :default)
+  return resetVariable!(getVariableState(dfg, sym, solveKey))
 end
 
 # return VariableNodeData
@@ -438,7 +432,7 @@ function setDefaultNodeDataParametric!(
   kwargs...,
 )
   vnd = DefaultNodeDataParametric(0, variableType |> getDimension, variableType; solveKey, kwargs...)
-  setSolverData!(v, vnd, solveKey)
+  mergeVariableState!(v, vnd)
   nothing
 end
 
@@ -485,7 +479,7 @@ function setDefaultNodeData!(
     (val, bw)
   end
   # make and set the new solverData
-  setSolverData!(
+  mergeVariableState!(
     v,
     VariableNodeData(varType;
       id=nothing,
@@ -504,8 +498,7 @@ function setDefaultNodeData!(
       # 0,
       # 0,
       solveKey,
-    ),
-    solveKey,
+    )
   )
   return nothing
 end
@@ -556,7 +549,7 @@ function setVariableRefence!(
   )
   #
   # set the value in the DFGVariable
-  return setSolverData!(var, vnd, refKey)
+  return mergeVariableState!(var, vnd)
 end
 
 # get instance from variableType

IncrementalInference/src/services/GraphInit.jl

@@ -179,8 +179,8 @@ function doautoinit!(
         # Update the estimates (longer DFG function used so cloud is also updated)
         setVariablePosteriorEstimates!(dfg, xi.label, solveKey)
         # Update the data in the event that it's not local
-        # TODO perhaps usecopy=false
-        updateVariableSolverData!(dfg, xi, solveKey, true; warn_if_absent = false)
+        # TODO perhaps use merge, but keeping to deepcopy as update variant used was set to copy.
+        DFG.copytoVariableState!(dfg, xi.label, solveKey, getVariableState(xi, solveKey))
         # deepcopy graphinit value, see IIF #612
         DFG.copytoVariableState!(
           dfg,

IncrementalInference/src/services/JunctionTreeUtils.jl

@@ -403,7 +403,7 @@ function identifyFirstEliminatedSeparator(
   dfg::AbstractDFG,
   elimorder::Vector{Symbol},
   firvert::DFGVariable,
-  Sj = getVariableState(firvert).separator,
+  Sj = getVariableState(firvert, :default).separator,
 )::DFGVariable
   #
   firstelim = (2^(Sys.WORD_SIZE - 1) - 1)
@@ -440,7 +440,7 @@ function newPotential(
 ) where {G <: AbstractDFG}
   firvert = DFG.getVariable(dfg, var)
   # no parent
-  if (length(getVariableState(firvert).separator) == 0)
+  if (length(getVariableState(firvert, :default).separator) == 0)
     # if (length(getCliques(tree)) == 0)
     # create new root
     addClique!(tree, dfg, var)
@@ -451,7 +451,7 @@ function newPotential(
     # end
   else
     # find parent clique Cp that containts the first eliminated variable of Sj as frontal
-    Sj = getVariableState(firvert).separator
+    Sj = getVariableState(firvert, :default).separator
     felbl = identifyFirstEliminatedSeparator(dfg, elimorder, firvert, Sj).label
     # get clique id of first eliminated frontal
     CpID = tree.frontals[felbl]
@@ -888,7 +888,7 @@ can be constructed.
 """
 function resetFactorGraphNewTree!(dfg::AbstractDFG)
   for v in DFG.getVariables(dfg)
-    resetData!(getVariableState(v))
+    resetData!(getVariableState(v, :default))
   end
   for f in DFG.getFactors(dfg)
     resetData!(DFG.getFactorState(f))

IncrementalInference/src/services/TreeMessageUtils.jl

@@ -36,7 +36,7 @@ function resetCliqSolve!(
   cda = getCliqueData(cliq)
   vars = getCliqVarIdsAll(cliq)
   for varis in vars
-    resetVariable!(dfg, varis; solveKey = solveKey)
+    resetVariable!(dfg, varis, solveKey)
   end
   # TODO remove once consolidation with upMsgs is done
   putCliqueMsgUp!(cda, LikelihoodMessage())

IncrementalInference/test/testSpecialEuclidean2Mani.jl

@@ -48,7 +48,7 @@ p = addFactor!(fg, [:x0], mp)
 doautoinit!(fg, :x0)
 
 ##
-vnd = getVariableSolverData(fg, :x0)
+vnd = getVariableState(fg, :x0, :default)
 @test all(isapprox.(mean(vnd.val), ArrayPartition(SA[0.0,0.0], SA[1.0 0.0; 0.0 1.0]), atol=0.1))
 @test all(is_point.(Ref(M), vnd.val))
 
@@ -59,7 +59,7 @@ f = addFactor!(fg, [:x0, :x1], mf)
 
 doautoinit!(fg, :x1)
 
-vnd = getVariableSolverData(fg, :x1)
+vnd = getVariableState(fg, :x1, :default)
 @test all(isapprox(M, mean(M,vnd.val), ArrayPartition(SA[1.0,2.0], SA[0.7071 -0.7071; 0.7071 0.7071]), atol=0.1))
 @test all(is_point.(Ref(M), vnd.val))
 
@@ -68,11 +68,11 @@ smtasks = Task[]
 solveTree!(fg; smtasks, verbose=true) #, recordcliqs=ls(fg))
 # hists = fetchCliqHistoryAll!(smtasks);
 
-vnd = getVariableSolverData(fg, :x0)
+vnd = getVariableState(fg, :x0, :default)
 @test all(isapprox.(mean(vnd.val), ArrayPartition(SA[0.0,0.0], SA[1.0 0.0; 0.0 1.0]), atol=0.1))
 @test all(is_point.(Ref(M), vnd.val))
 
-vnd = getVariableSolverData(fg, :x1)
+vnd = getVariableState(fg, :x1, :default)
 @test all(isapprox.(mean(vnd.val), ArrayPartition(SA[1.0,2.0], SA[0.7071 -0.7071; 0.7071 0.7071]), atol=0.1))
 @test all(is_point.(Ref(M), vnd.val))
 
@@ -194,13 +194,13 @@ addFactor!(fg, [:x6; :l1], mf)
 smtasks = Task[]
 solveTree!(fg; smtasks);
 
-vnd = getVariableSolverData(fg, :x0)
+vnd = getVariableState(fg, :x0, :default)
 @test isapprox(M, mean(M, vnd.val), ArrayPartition([10.0,10.0], [-1.0 0.0; 0.0 -1.0]), atol=0.2)
 
-vnd = getVariableSolverData(fg, :x1)
+vnd = getVariableState(fg, :x1, :default)
 @test isapprox(M, mean(M, vnd.val), ArrayPartition([0.0,10.0], [-0.5 0.866; -0.866 -0.5]), atol=0.4)
 
-vnd = getVariableSolverData(fg, :x6)
+vnd = getVariableState(fg, :x6, :default)
 @test isapprox(M, mean(M, vnd.val), ArrayPartition([10.0,10.0], [-1.0 0.0; 0.0 -1.0]), atol=0.5)
 
 ## Special test for manifold based messages
@@ -310,18 +310,18 @@ f = addFactor!(fg, [:x0, :x1], mf)
 
 doautoinit!(fg, :x1)
 
-vnd = getVariableSolverData(fg, :x1)
+vnd = getVariableState(fg, :x1, :default)
 @test all(isapprox.(mean(vnd.val), [1.0,2.0], atol=0.1))
 
 ##
 smtasks = Task[]
 solveTree!(fg; smtasks, verbose=true, recordcliqs=ls(fg))
 # # hists = fetchCliqHistoryAll!(smtasks);
 
-vnd = getVariableSolverData(fg, :x0)
+vnd = getVariableState(fg, :x0, :default)
 @test isapprox(mean(getManifold(fg,:x0),vnd.val), ArrayPartition([0.0,0.0], [1.0 0.0; 0.0 1.0]), atol=0.1)
 
-vnd = getVariableSolverData(fg, :x1)
+vnd = getVariableState(fg, :x1, :default)
 @test all(isapprox.(mean(vnd.val), [1.0,2.0], atol=0.1))
 
 ##
@@ -551,7 +551,7 @@ f = addFactor!(fg, [:x0, :x1a, :x1b], mf; multihypo=[1,0.5,0.5])
 
 solveTree!(fg)
 
-vnd = getVariableSolverData(fg, :x0)
+vnd = getVariableState(fg, :x0, :default)
 @test isapprox(SpecialEuclidean(2; vectors=HybridTangentRepresentation()), mean(SpecialEuclidean(2; vectors=HybridTangentRepresentation()), vnd.val), ArrayPartition([0.0,0.0], [1.0 0; 0 1]), atol=0.1)
 
 #FIXME I would expect close to 50% of particles to land on the correct place
@@ -622,7 +622,7 @@ f = addFactor!(fg, [:x0, :x1a, :x1b], mf; multihypo=[1,0.5,0.5])
 
 solveTree!(fg)
 
-vnd = getVariableSolverData(fg, :x0)
+vnd = getVariableState(fg, :x0, :default)
 @test isapprox(SpecialEuclidean(2; vectors=HybridTangentRepresentation()), mean(SpecialEuclidean(2; vectors=HybridTangentRepresentation()), vnd.val), ArrayPartition([0.0,0.0], [1.0 0; 0 1]), atol=0.1)
 
 #FIXME I would expect close to 50% of particles to land on the correct place