Merge pull request #796 from JuliaRobotics/21Q3/ser/factorgrads

new features for deserializing factor gradients

Author: dehann

src/FileDFG/services/FileDFG.jl

@@ -152,6 +152,7 @@ function loadDFG!(dfgLoadInto::AbstractDFG, dst::AbstractString)
     map(f->addFactor!(dfgLoadInto, f), factors)
 
     # Finally, rebuild the CCW's for the factors to completely reinflate them
+    # NOTE CREATES A NEW DFGFactor IF  CCW TYPE CHANGES
     @info "Rebuilding CCW's for the factors..."
     for factor in factors
         rebuildFactorMetadata!(dfgLoadInto, factor)

src/LightDFG/services/LightDFG.jl

@@ -79,13 +79,13 @@ end
 # end
 
 
-function addFactor!(dfg::LightDFG{<:AbstractParams, <:AbstractDFGVariable, F}, factor::F)::F where F <: AbstractDFGFactor
+function addFactor!(dfg::LightDFG{<:AbstractParams, <:AbstractDFGVariable, F}, factor::F) where F <: AbstractDFGFactor
     if haskey(dfg.g.factors, factor.label)
         error("Factor '$(factor.label)' already exists in the factor graph")
     end
     # TODO
     # @assert FactorGraphs.addFactor!(dfg.g, getVariableOrder(factor), factor)
-    @assert FactorGraphs.addFactor!(dfg.g, factor._variableOrderSymbols, factor)
+    @assert FactorGraphs.addFactor!(dfg.g, Symbol[factor._variableOrderSymbols...], factor)
     return factor
 end
 

src/entities/DFGFactor.jl

@@ -38,7 +38,7 @@ Designing (WIP)
 - in DFG.AbstractRelativeMinimize <: FunctorInferenceType
 - in Main.SomeFactor <: AbstractRelativeMinimize
 """
-mutable struct GenericFunctionNodeData{T<:Union{PackedInferenceType, FunctorInferenceType, FactorOperationalMemory}}
+mutable struct GenericFunctionNodeData{T<:Union{<:PackedInferenceType, <:AbstractFactor, <:FactorOperationalMemory}}
     eliminated::Bool
     potentialused::Bool
     edgeIDs::Vector{Int}
@@ -80,7 +80,7 @@ end
 const PackedFunctionNodeData{T} = GenericFunctionNodeData{T} where T <: AbstractPackedFactor
 PackedFunctionNodeData(args...) = PackedFunctionNodeData{typeof(args[4])}(args...)
 
-const FunctionNodeData{T} = GenericFunctionNodeData{T} where T <: Union{AbstractFactor, FactorOperationalMemory}
+const FunctionNodeData{T} = GenericFunctionNodeData{T} where T <: Union{<:AbstractFactor, <:FactorOperationalMemory}
 FunctionNodeData(args...) = FunctionNodeData{typeof(args[4])}(args...)
 
 

src/services/AbstractDFG.jl

@@ -76,7 +76,7 @@ getFactorOperationalMemoryType(dfg::AbstractDFG) = getFactorOperationalMemoryTyp
 
 Method must be overloaded by the user for Serialization to work.
 """
-rebuildFactorMetadata!(dfg::AbstractDFG{<:AbstractParams}, factor::AbstractDFGFactor) = error("rebuildFactorMetadata! is not implemented for $(typeof(dfg))")
+rebuildFactorMetadata!(dfg::AbstractDFG{<:AbstractParams}, factor::AbstractDFGFactor, neighbors=[]) = error("rebuildFactorMetadata! is not implemented for $(typeof(dfg))")
 
 ##------------------------------------------------------------------------------
 ## Setters

src/services/Serialization.jl

@@ -373,14 +373,14 @@ function unpackFactor(dfg::G, packedProps::Dict{String, Any})::DFGFactor where G
 
     # Rebuild DFGFactor
     #TODO use constuctor to create factor
-    factor = DFGFactor(Symbol(label),
-                       timestamp,
-                       nstime,
-                       Set(tags),
-                       fullFactorData,
-                       solvable,
-                       Tuple(_variableOrderSymbols))
-
+    factor = DFGFactor( Symbol(label),
+                        timestamp,
+                        nstime,
+                        Set(tags),
+                        fullFactorData,
+                        solvable,
+                        Tuple(_variableOrderSymbols))
+    #
 
     # Note, once inserted, you still need to call rebuildFactorMetadata!
     return factor