updateState -> copytoState

Author: Affie

IncrementalInferenceTypes/src/IncrementalInferenceTypes.jl

@@ -56,7 +56,7 @@ include("variables/DefaultVariableTypes.jl")
 # Factor Definitions
 include("factors/DefaultPrior.jl")
 #FIXME maybe upgrade linear relative to this
-# include("factors/LinearRelative.jl")
+include("factors/LinearRelative.jl")
 include("factors/Circular.jl")
 
 # Distribution Serialization

Project.toml

@@ -21,6 +21,7 @@ FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
 FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41"
 FiniteDifferences = "26cc04aa-876d-5657-8c51-4c34ba976000"
 FunctionalStateMachine = "3e9e306e-7e3c-11e9-12d2-8f8f67a2f951"
+IncrementalInferenceTypes = "9808408f-4dbc-47e4-913c-6068b950e289"
 JSON3 = "0f8b85d8-7281-11e9-16c2-39a750bddbf1"
 KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"

src/Factors/DefaultPrior.jl

@@ -1,43 +1,7 @@
 
-"""
-$(TYPEDEF)
+IIFTypes.Prior(::UniformScaling) = Prior(Normal())
 
-Default prior on all dimensions of a variable node in the factor graph.  `Prior` is
-not recommended when non-Euclidean dimensions are used in variables.
-"""
-struct Prior{T <: SamplableBelief} <: AbstractPrior
-  Z::T
-end
-Prior(::UniformScaling) = Prior(Normal())
-
-getManifold(pr::Prior) = TranslationGroup(getDimension(pr.Z))
 # getSample(cf::CalcFactor{<:Prior}) = rand(cf.factor.Z)
 
 # basic default
 (s::CalcFactor{<:Prior})(z, x1) = z .- x1
-
-## packed types are still developed by hand.  Future versions would likely use a @packable macro to write Protobuf safe versions of factors
-
-"""
-$(TYPEDEF)
-
-Serialization type for Prior.
-"""
-Base.@kwdef mutable struct PackedPrior <: AbstractPackedFactor
-  Z::PackedSamplableBelief
-end
-function convert(::Type{PackedPrior}, d::Prior)
-  return PackedPrior(convert(PackedSamplableBelief, d.Z))
-end
-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

@@ -1,40 +1,6 @@
 
 export LinearRelative, PackedLinearRelative
 
-"""
-$(TYPEDEF)
-
-Default linear offset between two scalar variables.
-
-```math
-X_2 = X_1 + η_Z
-```
-"""
-struct LinearRelative{N, T <: SamplableBelief} <: AbstractManifoldMinimize # AbstractRelativeMinimize
-  Z::T
-end
-
-# need several helper constructors since the dimension over which LinearRelative will be used is unknown at this point
-function LinearRelative{N}(
-  z0::T = MvNormal(zeros(N), diagm(ones(N))),
-) where {N, T <: SamplableBelief}
-  #
-  return LinearRelative{N, T}(z0)
-end
-
-function LinearRelative(::UniformScaling = LinearAlgebra.I)
-  return LinearRelative{1}(MvNormal(zeros(1), diagm(ones(1))))
-end
-function LinearRelative(nm::Distributions.ContinuousUnivariateDistribution)
-  return LinearRelative{1, typeof(nm)}(nm)
-end
-LinearRelative(nm::MvNormal) = LinearRelative{length(nm.μ), typeof(nm)}(nm)
-function LinearRelative(nm::Union{<:BallTreeDensity, <:ManifoldKernelDensity})
-  return LinearRelative{Ndim(nm), typeof(nm)}(nm)
-end
-
-getManifold(::InstanceType{LinearRelative{N}}) where {N} = getManifold(ContinuousEuclid{N})
-
 # TODO standardize
 getDimension(::InstanceType{LinearRelative{N}}) where {N} = N
 
@@ -55,24 +21,3 @@ function Base.convert(
   return Manifolds.TranslationGroup(N)
 end
 
-"""
-$(TYPEDEF)
-Serialization type for `LinearRelative` binary factor.
-"""
-Base.@kwdef mutable struct PackedLinearRelative <: AbstractPackedFactor
-  Z::PackedSamplableBelief
-end
-function convert(::Type{PackedLinearRelative}, d::LinearRelative)
-  return PackedLinearRelative(convert(PackedSamplableBelief, d.Z))
-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/IncrementalInference.jl

@@ -102,8 +102,8 @@ const BeliefArray{T} = Union{<:AbstractMatrix{<:T}, <:Adjoint{<:T, AbstractMatri
 # FIXME, remove this and let the user do either import or const definitions
 export KDE, AMP, DFG, FSM, IIF
 
-include("../IncrementalInferenceTypes/src/IncrementalInferenceTypes.jl")
-using ..IncrementalInferenceTypes
+# include("../IncrementalInferenceTypes/src/IncrementalInferenceTypes.jl")
+@reexport using IncrementalInferenceTypes
 
 # TODO temporary for initial version of on-manifold products
 KDE.setForceEvalDirect!(true)
@@ -175,7 +175,6 @@ include("services/TreeBasedInitialization.jl")
 
 # included variables of IIF, easy to extend in user's context
 include("Variables/DefaultVariables.jl")
-include("Variables/Circular.jl")
 
 # included factors, see RoME.jl for more examples
 include("Factors/GenericFunctions.jl")

src/Serialization/services/SerializationMKD.jl

@@ -43,7 +43,7 @@ function parchDistribution(mkd::ManifoldKernelDensity)
 end
 
 # Data converters for MKD
-function packDistribution(mkd::ManifoldKernelDensity)
+function DFG.packDistribution(mkd::ManifoldKernelDensity)
   #
   pts = getPoints(mkd)
 
@@ -58,7 +58,7 @@ function packDistribution(mkd::ManifoldKernelDensity)
   )
 end
 
-function unpackDistribution(dtr::PackedManifoldKernelDensity)
+function DFG.unpackDistribution(dtr::PackedManifoldKernelDensity)
   # find InferenceVariable type from string (anything Manifolds.jl?)
   M = DFG.getTypeFromSerializationModule(dtr.varType) |> getManifold
   vecP = [AMP.makePointFromCoords(M, pt) for pt in dtr.pts]

src/Variables/Circular.jl

@@ -448,7 +448,13 @@ function resetInitialValues!(
   for vs in varList
     vnd = getVariableState(getVariable(src, vs), initKey)
     # guess we definitely want to use copy to preserve the initKey memory
-    updateVariableSolverData!(dest, vs, vnd, solveKey, true; warn_if_absent = false)
+    # updateVariableSolverData!(dest, vs, vnd, solveKey, true; warn_if_absent = false)
+    DFG.copytoVariableState!(
+      dest,
+      vs,
+      solveKey,
+      vnd,
+    )    
   end
   return dest
 end

src/services/GraphInit.jl

@@ -142,16 +142,17 @@ function transferUpdateSubGraph!(
   end
 
   # transfer specific fields into dest from src
-  for var in (x -> getVariable(src, x)).(syms)
-    # copy not required since a broadcast is used internally
-    updateVariableSolverData!(
+  @time for var in (x -> getVariable(src, x)).(syms)
+    # NOTE compared copytoVariableState! vs surgical updateVariableSolverData!
+    # updateVariableSolverData! 0.000626 seconds (1.11 k allocations: 114.289 KiB)
+    # copytoVariableState! 0.000099 seconds (315 allocations: 27.758 KiB)
+    DFG.copytoVariableState!(
       dest,
-      var,
+      getLabel(var),
       solveKey,
-      false,
-      [:val; :bw; :infoPerCoord; :solvedCount; :initialized];
-      warn_if_absent = false,
+      getVariableState(var, solveKey),
     )
+
     if updatePPE
       # create ppe on new key using defaults, TODO improve
       if haskey(getPPEDict(var), solveKey)