Updates for DFG factor refactoring (#1870)

* Updates for DFG factor refactoring.
* fix tests for removal of GFND
* rather use getState -> getFactorState
* bump to v0.36 and use copyto
* getSolverData -> getVariableState

Author: Affie

Project.toml

@@ -2,7 +2,7 @@ name = "IncrementalInference"
 uuid = "904591bb-b899-562f-9e6f-b8df64c7d480"
 keywords = ["MM-iSAMv2", "Bayes tree", "junction tree", "Bayes network", "variable elimination", "graphical models", "SLAM", "inference", "sum-product", "belief-propagation"]
 desc = "Implements the Multimodal-iSAMv2 algorithm."
-version = "0.35.6"
+version = "0.36.0"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -73,7 +73,7 @@ Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"
 DelimitedFiles = "1"
 DifferentialEquations = "7"
-DistributedFactorGraphs = "0.25, 0.26"
+DistributedFactorGraphs = "0.26, 0.27"
 Distributions = "0.24, 0.25"
 DocStringExtensions = "0.8, 0.9"
 FileIO = "1"

ext/FluxModelsSerialization.jl

@@ -33,7 +33,7 @@ function _deserializeFluxDataBase64(sdata::AbstractString)
   return data
 end
 
-function packDistribution(obj::FluxModelsDistribution)
+function DFG.packDistribution(obj::FluxModelsDistribution)
   #
 
   # and the specialSampler function -- likely to be deprecated
@@ -75,7 +75,7 @@ function packDistribution(obj::FluxModelsDistribution)
   #
 end
 
-function unpackDistribution(obj::PackedFluxModelsDistribution)
+function DFG.unpackDistribution(obj::PackedFluxModelsDistribution)
   #
   obj.serializeHollow && @warn(
     "Deserialization of FluxModelsDistribution.serializationHollow=true is not yet well developed, please open issues at IncrementalInference.jl accordingly."
@@ -104,15 +104,15 @@ function Base.convert(
 )
   #
   # convert to packed type first
-  return packDistribution(obj)
+  return DFG.packDistribution(obj)
 end
 
 function convert(
   ::Union{Type{<:SamplableBelief}, Type{<:FluxModelsDistribution}},
   obj::PackedFluxModelsDistribution,
 )
   #
-  return unpackDistribution(obj)
+  return DFG.unpackDistribution(obj)
 end
 
 #

src/CliqueStateMachine/services/CliqueStateMachine.jl

@@ -362,7 +362,7 @@ function initUp_StateMachine(csmc::CliqStateMachineContainer)
   if init_for_differential
     frontal_vars = getVariable.(csmc.cliqSubFg, getCliqFrontalVarIds(csmc.cliq))
     filter!(!isInitialized, frontal_vars)
-    foreach(fvar -> getSolverData(fvar, csmc.solveKey).initialized = true, frontal_vars)
+    foreach(fvar -> getVariableState(fvar, csmc.solveKey).initialized = true, frontal_vars)
     logCSM(
       csmc,
       "CSM-2b init_for_differential: ";
@@ -404,7 +404,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 -> getSolverData(fvar, csmc.solveKey).initialized = false, frontal_vars)
+    foreach(fvar -> getVariableState(fvar, csmc.solveKey).initialized = false, frontal_vars)
     if someInit
       solveStatus = UPSOLVED
     end

src/ExportAPI.jl

@@ -277,7 +277,7 @@ export CSMHistory,
   saveDFG,
   loadDFG!,
   loadDFG,
-  rebuildFactorMetadata!,
+  rebuildFactorCache!,
   getCliqVarSolveOrderUp,
   getFactorsAmongVariablesOnly,
   setfreeze!,
@@ -287,7 +287,6 @@ export CSMHistory,
 
   # some utils
   compare,
-  compareAllSpecial,
   getMeasurements,
   findFactorsBetweenFrom,
   addDownVariableFactors!,

src/Factors/Circular.jl

@@ -51,13 +51,15 @@ Related
 
 [`Circular`](@ref), [`Prior`](@ref), [`PartialPrior`](@ref)
 """
-mutable struct PriorCircular{T <: SamplableBelief} <: AbstractPrior
-  Z::T
-end
+DFG.@defFactorType PriorCircular AbstractPrior Manifolds.RealCircleGroup()
+
+# mutable struct PriorCircular{T <: SamplableBelief} <: AbstractPrior
+#   Z::T
+# end
 
 PriorCircular(::UniformScaling) = PriorCircular(Normal())
 
-DFG.getManifold(::PriorCircular) = RealCircleGroup()
+# DFG.getManifold(::PriorCircular) = RealCircleGroup()
 
 function getSample(cf::CalcFactor{<:PriorCircular})
   # FIXME workaround for issue #TBD with manifolds CircularGroup, 
@@ -77,21 +79,22 @@ function Base.convert(::Type{<:MB.AbstractManifold}, ::InstanceType{PriorCircula
   return Manifolds.RealCircleGroup()
 end
 
-"""
-$(TYPEDEF)
-
-Serialized object for storing PriorCircular.
-"""
-Base.@kwdef struct PackedPriorCircular <: AbstractPackedFactor
-  Z::PackedSamplableBelief
-end
-function convert(::Type{PackedPriorCircular}, d::PriorCircular)
-  return PackedPriorCircular(convert(PackedSamplableBelief, d.Z))
-end
-function convert(::Type{PriorCircular}, d::PackedPriorCircular)
-  distr = convert(SamplableBelief, d.Z)
-  return PriorCircular{typeof(distr)}(distr)
-end
+# """
+# $(TYPEDEF)
+
+# Serialized object for storing PriorCircular.
+# """
+# Base.@kwdef struct PackedPriorCircular <: AbstractPackedFactor
+#   Z::PackedSamplableBelief
+# end
+
+# function convert(::Type{PackedPriorCircular}, d::PriorCircular)
+#   return PackedPriorCircular(convert(PackedSamplableBelief, d.Z))
+# end
+# function convert(::Type{PriorCircular}, d::PackedPriorCircular)
+#   distr = convert(SamplableBelief, d.Z)
+#   return PriorCircular{typeof(distr)}(distr)
+# end
 
 # --------------------------------------------
 

src/Factors/DefaultPrior.jl

@@ -33,4 +33,11 @@ function convert(::Type{Prior}, d::PackedPrior)
   return Prior(convert(SamplableBelief, d.Z))
 end
 
+function DFG.pack(d::Prior)
+  return PackedPrior(DFG.packDistribution(d.Z))
+end
+function DFG.unpack(d::PackedPrior)
+  return Prior(DFG.unpackDistribution(d.Z))
+end
+
 #

src/Factors/LinearRelative.jl

@@ -68,5 +68,11 @@ end
 function convert(::Type{LinearRelative}, d::PackedLinearRelative)
   return LinearRelative(convert(SamplableBelief, d.Z))
 end
+function DFG.pack(d::LinearRelative)
+  return PackedLinearRelative(DFG.packDistribution(d.Z))
+end
+function DFG.unpack(d::PackedLinearRelative)
+  return LinearRelative(DFG.unpackDistribution(d.Z))
+end
 
 #

src/Factors/Mixture.jl

@@ -179,9 +179,13 @@ function convert(::Type{<:PackedMixture}, obj::Mixture{N, F, S, T}) where {N, F,
     # FIXME ON FIRE, likely to be difficult for non-standard "Samplable" types -- e.g. Flux models in RoME
     push!(allcomp, dtr_)
   end
-  pm = DFG.convertPackedType(obj.mechanics)
-  pm_ = convert(pm, obj.mechanics)
-  sT = string(typeof(pm_))
+  if hasmethod(pack, (typeof(obj.mechanics),))
+    pm = pack(obj.mechanics)
+  else
+    @warn("No pack method for mechanics type $(typeof(obj.mechanics)), using deprecated convert instead.")
+    pm = convert(DFG.convertPackedType(obj.mechanics), obj.mechanics)
+  end
+  sT = string(typeof(pm))
   dvst = convert(PackedSamplableBelief, obj.diversity)
   return PackedMixture(N, sT, string.(collect(S)), allcomp, dvst)
 end

src/IncrementalInference.jl

@@ -68,10 +68,9 @@ import ApproxManifoldProducts: getBW
 import ApproxManifoldProducts: mmd
 import ApproxManifoldProducts: isPartial
 import ApproxManifoldProducts: _update!
-import DistributedFactorGraphs: reconstFactorData
 import DistributedFactorGraphs: addVariable!, addFactor!, ls, lsf, isInitialized
-import DistributedFactorGraphs: compare, compareAllSpecial
-import DistributedFactorGraphs: rebuildFactorMetadata!
+import DistributedFactorGraphs: compare
+import DistributedFactorGraphs: rebuildFactorCache!
 import DistributedFactorGraphs: getDimension, getManifold, getPointType, getPointIdentity
 import DistributedFactorGraphs: getPPE, getPPEDict
 import DistributedFactorGraphs: getFactorOperationalMemoryType
@@ -80,6 +79,7 @@ import DistributedFactorGraphs: getVariableType
 import DistributedFactorGraphs: AbstractPointParametricEst, loadDFG
 import DistributedFactorGraphs: getFactorType
 import DistributedFactorGraphs: solveGraph!, solveGraphParametric!
+import DistributedFactorGraphs: packDistribution, unpackDistribution
 
 # will be deprecated in IIF
 import DistributedFactorGraphs: isSolvable

src/Serialization/services/DispatchPackedConversions.jl

@@ -5,7 +5,8 @@
 function convert(
   ::Type{PackedFunctionNodeData{P}},
   d::FunctionNodeData{T},
-) where {P <: AbstractPackedFactor, T <: FactorOperationalMemory}
+) where {P <: AbstractPackedFactor, T <: FactorSolverCache}
+  error("TODO remove. PackedFunctionNodeData is obsolete")
   return PackedFunctionNodeData(
     d.eliminated,
     d.potentialused,
@@ -22,12 +23,15 @@ end
 ## unpack converters------------------------------------------------------------
 
 # see #1424
-function reconstFactorData(
+#TODO Consolidate: this looks alot like `getDefaultFactorData`
+function DFG.reconstFactorData(
   dfg::AbstractDFG,
   varOrder::AbstractVector{Symbol},
   ::Type{<:GenericFunctionNodeData{<:CommonConvWrapper{F}}},
   packed::GenericFunctionNodeData{<:AbstractPackedFactor},
 ) where {F <: AbstractFactor}
+
+  error("TODO remove. Obsolete: use `DFG.rebuildFactorCache!` and getDefaultFactorData instead.")
   #
   # TODO store threadmodel=MutliThreaded,SingleThreaded in persistence layer
   usrfnc = convert(F, packed.fnc)
@@ -89,7 +93,7 @@ Dev Notes:
 - TODO: We should only really do this in-memory if we can by without it (review this).
 - TODO: needs testing
 """
-function rebuildFactorMetadata!(
+function DFG.rebuildFactorCache!(
   dfg::AbstractDFG{SolverParams},
   factor::DFGFactor,
   neighbors = map(vId -> getVariable(dfg, vId), listNeighbors(dfg, factor));
@@ -99,50 +103,54 @@ function rebuildFactorMetadata!(
   # Set up the neighbor data
 
   # Rebuilding the CCW
-  fsd = getSolverData(factor)
-  fnd_new = getDefaultFactorData(
+  state = DFG.getFactorState(factor)
+  state, solvercache = getDefaultFactorData(
     dfg,
     neighbors,
-    getFactorType(factor);
-    multihypo = fsd.multihypo,
-    nullhypo = fsd.nullhypo,
-    # special inflation override 
-    inflation = fsd.inflation,
-    eliminated = fsd.eliminated,
-    potentialused = fsd.potentialused,
-    edgeIDs = fsd.edgeIDs,
-    solveInProgress = fsd.solveInProgress,
+    DFG.getObservation(factor);
+    multihypo = state.multihypo,
+    nullhypo = state.nullhypo,
+    # special inflation override
+    inflation = state.inflation,
+    eliminated = state.eliminated,
+    potentialused = state.potentialused,
+    solveInProgress = state.solveInProgress,
     _blockRecursion=_blockRecursionGradients
   )
   #
-
-  factor_ = if typeof(fnd_new) != typeof(getSolverData(factor))
-    # must change the type of factor solver data FND{CCW{...}}
-    # create a new factor
-    factor__ = DFGFactor(
-      getLabel(factor),
-      getTimestamp(factor),
-      factor.nstime,
-      getTags(factor),
-      fnd_new,
-      getSolvable(factor),
-      Tuple(getVariableOrder(factor)),
-    )
-    #
-
-    # replace old factor in dfg with a new one
-    deleteFactor!(dfg, factor; suppressGetFactor = true)
-    addFactor!(dfg, factor__)
-
-    factor__
-  else
-    setSolverData!(factor, fnd_new)
-    # We're not updating here because we don't want
-    # to solve cloud in loop, we want to make sure this flow works:
-    # Pull big cloud graph into local -> solve local -> push back into cloud.
-    # updateFactor!(dfg, factor)
-    factor
-  end
+  DFG.setCache!(factor, solvercache)
+  return factor
+
+  # factor_ = if typeof(solvercache) != typeof(DFG.getCache(factor)) 
+  #   # must change the type of factor solver data FND{CCW{...}}
+  #   # create a new factor
+  #   factor__ = FactorCompute(
+  #     getLabel(factor),
+  #     Tuple(getVariableOrder(factor)),
+  #     DFG.getObservation(factor),
+  #     state,
+  #     solvercache;
+  #     timestamp = getTimestamp(factor),
+  #     nstime = factor.nstime,
+  #     tags = getTags(factor),
+  #     solvable = getSolvable(factor),
+  #   )
+  #   #
+
+  #   # replace old factor in dfg with a new one
+  #   deleteFactor!(dfg, factor; suppressGetFactor = true)
+  #   addFactor!(dfg, factor__)
+
+  #   factor__
+  # else
+  #   setSolverData!(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:
+  #   # Pull big cloud graph into local -> solve local -> push back into cloud.
+  #   # updateFactor!(dfg, factor)
+  #   factor
+  # end
 
   #... Copying neighbor data into the factor?
   # JT TODO it looks like this is already updated in getDefaultFactorData -> _createCCW
@@ -151,7 +159,7 @@ function rebuildFactorMetadata!(
   #   ccw_new.fnc.cpt[i].factormetadata.variableuserdata = deepcopy(neighborUserData)
   # end
 
-  return factor_
+  # return factor_
 end
 
 ## =================================================================

src/Serialization/services/SerializingDistributions.jl

@@ -69,11 +69,6 @@ function convert(::Type{<:PackedSamplableBelief}, obj::StringThemSamplableBelief
 end
 convert(::Type{<:SamplableBelief}, obj::PackedSamplableBelief) = unpackDistribution(obj)
 
-function convert(::Type{<:PackedSamplableBelief}, nt::Union{NamedTuple, JSON3.Object})
-  distrType = DFG.getTypeFromSerializationModule(nt._type)
-  return distrType(; nt...)
-end
-
 ##===================================================================================
 
 # FIXME ON FIRE, must deprecate nested JSON written fields in all serialization

src/entities/BeliefTypes.jl

@@ -16,9 +16,7 @@ abstract type MessageType end
 struct NonparametricMessage <: MessageType end
 struct ParametricMessage <: MessageType end
 
-abstract type PackedSamplableBelief end
-
-StructTypes.StructType(::Type{<:PackedSamplableBelief}) = StructTypes.UnorderedStruct()
+using DistributedFactorGraphs: PackedSamplableBelief
 
 const SamplableBelief = Union{
   <:Distributions.Distribution,
@@ -89,7 +87,7 @@ function TreeBelief(vnd::VariableNodeData{T}, solvDim::Real = 0) where {T}
 end
 
 function TreeBelief(vari::DFGVariable, solveKey::Symbol = :default; solvableDim::Real = 0)
-  return TreeBelief(getSolverData(vari, solveKey), solvableDim)
+  return TreeBelief(getVariableState(vari, solveKey), solvableDim)
 end
 #
 

src/entities/FactorGradients.jl

@@ -59,7 +59,7 @@ Related
 [`calcFactorResidualTemporary`](@ref), [`_buildGraphByFactorAndTypes`](@ref)
 """
 mutable struct FactorGradientsCached!{F <: AbstractRelative, S, M, P, G, L}
-  dfgfct::DFGFactor{<:CommonConvWrapper{F}}
+  dfgfct::DFGFactor{F}
   # cached jacobian matrix of gradients
   cached_gradients::Matrix{Float64}
   # likely <:AbstractVector, while CalcFactor residuals are vectors in Rn but could change to Tangent vectors

src/entities/FactorOperationalMemory.jl

@@ -27,7 +27,7 @@ Base.@kwdef struct CommonConvWrapper{
   HR <: HypoRecipeCompute,
   MT, 
   G
-} <: FactorOperationalMemory
+} <: FactorSolverCache
   # Basic factor topological info
   """ Values consistent across all threads during approx convolution """
   usrfnc!::T # user factor / function