Merge pull request #351 from JuliaRobotics/feature/dfgcontinued

Required for CloudGraphsDFG
JuliaRobotics · Jul 19, 2019 · 7a8ced5 · 7a8ced5
2 parents d236e67 + 467fbe0
commit 7a8ced5
Show file tree

Hide file tree

Showing 7 changed files with 92 additions and 158 deletions.
diff --git a/src/DataLayerAPI.jl b/src/DataLayerAPI.jl
diff --git a/src/Deprecated.jl b/src/Deprecated.jl
@@ -91,7 +91,6 @@ function addNode!(fg::FactorGraph,
                   ready::Int=1,
                   dontmargin::Bool=false,
                   labels::Vector{<:AbstractString}=String[],
-                  api::DataLayerAPI=dlapi,
                   uid::Int=-1,
                   smalldata=""  ) where {T <:InferenceVariable}
   #
@@ -104,7 +103,6 @@ function addNode!(fg::FactorGraph,
                        ready=ready,
                        dontmargin=dontmargin,
                        labels=labels,
-                       api=api,
                        uid=uid,
                        smalldata=smalldata )
 end
@@ -116,7 +114,6 @@ function addNode!(fg::FactorGraph,
                   ready::Int=1,
                   dontmargin::Bool=false,
                   labels::Vector{<:AbstractString}=String[],
-                  api::DataLayerAPI=dlapi,
                   uid::Int=-1,
                   # dims::Int=-1,
                   smalldata=""  )
@@ -130,7 +127,6 @@ function addNode!(fg::FactorGraph,
                       ready=ready,
                       dontmargin=dontmargin,
                       labels=labels,
-                      api=api,
                       uid=uid,
                       smalldata=smalldata  )
 end
@@ -140,7 +136,7 @@ end
 
 Return all elements `ls(fg)` as tuples, or nodes connected to the a specific element, eg. `ls(fg, :x1)
 """
-function ls(fgl::FactorGraph, lbl::Symbol; api::DataLayerAPI=dlapi, ring::Int=1)
+function ls(fgl::FactorGraph, lbl::Symbol; ring::Int=1)
   @warn "Deprecated, please use DFG.ls"
   # TODO ring functionality must still be implemented
   lsa = Symbol[]
@@ -168,7 +164,6 @@ Experimental union of elements version of ls(::FactorGraph, ::Symbol).  Not mean
 """
 function ls(fgl::FactorGraph,
             lbls::Vector{Symbol};
-            api::DataLayerAPI=dlapi,
             ring::Int=1)
   @warn "Deprecated, please use DFG.ls"
   union(ls.(fgl, lbls, ring=ring, api=api)[:]...)
@@ -251,7 +246,7 @@ List factors in a factor graph.
 lsf(fg, :x1)
 ```
 """
-function lsf(fgl::FactorGraph, lbl::Symbol; api::DataLayerAPI=dlapi)
+function lsf(fgl::FactorGraph, lbl::Symbol)
   @warn "Deprecated, please use DFG.lsf"
   lsa = Symbol[]
   if haskey(fgl.fIDs, lbl)
@@ -280,8 +275,7 @@ lsf(fg)
 lsf(fgl::FactorGraph, lbl::T) where {T <: AbstractString} = lsf(fgl,Symbol(lbl))
 
 function lsf(fgl::FactorGraph,
-             mt::Type{T};
-             api::DataLayerAPI=dlapi  ) where {T <: FunctorInferenceType}
+             mt::Type{T}) where {T <: FunctorInferenceType}
   @warn "Deprecated, please use DFG.lsf"
   syms = Symbol[]
   for (fsym,fid) in fgl.fIDs

diff --git a/src/DispatchPackedConversions.jl b/src/DispatchPackedConversions.jl
@@ -82,36 +82,36 @@ function convert(::Type{T}, ::PT) where {T <: FunctorInferenceType, PT <: Packed
   getfield(PT.name.module, Symbol(string(PT.name.name)[7:end]))
 end
 
-function getmodule(t::T) where T
-  T.name.module
-end
-function getname(t::T) where T
-  T.name.name
-end
-function getpackedtype(typestring::AS) where {AS <: AbstractString}
-  # println("Caesar.getpackedtype($(typestring))")
-  eval(Meta.parse(typestring))() # TODO consider caching or better
-end
-function encodePackedType(topackdata::VariableNodeData)
-  @warn "'encodePackedType' Deprecated..."
-  # error("IncrementalInference.encodePackedType(::VariableNodeData): Unknown packed type encoding of $(topackdata)")
-  pack(nothing, topackdata)
-end
-function encodePackedType(topackdata::GenericFunctionNodeData{CommonConvWrapper{T}, Symbol}) where {T <: FunctorInferenceType}
-  # println("IncrementalInference.encodePackedType(::GenericFunctionNodeData{T,Symbol}): Unknown packed type encoding T=$(T) of $(topackdata)")
-  fnctype = getfnctype(topackdata)
-  fnc = getfield(getmodule(fnctype), Symbol("Packed$(getname(fnctype))"))
-  convert(PackedFunctionNodeData{fnc}, topackdata)
-end
-function encodePackedType(topackdata::GenericFunctionNodeData{T, <:AbstractString}) where {T <: PackedInferenceType}
-  error("IncrementalInference.encodePackedType(::FunctionNodeData{T, <:AbstractString}): Unknown packed type encoding T=$(T) of $(topackdata)")
-  # @show T, typeof(topackdata)
-  # @warn "Yes, its packed!"
-  # fnctype = getfnctype(topackdata)
-  # @show fnc = getfield(getmodule(fnctype), Symbol("Packed$(getname(fnctype))"))
-  # convert(PackedFunctionNodeData{T}, topackdata)
-  topackdata
-end
+# function getmodule(t::T) where T
+#   T.name.module
+# end
+# function getname(t::T) where T
+#   T.name.name
+# end
+# function getpackedtype(typestring::AS) where {AS <: AbstractString}
+#   # println("Caesar.getpackedtype($(typestring))")
+#   eval(Meta.parse(typestring))() # TODO consider caching or better
+# end
+# function encodePackedType(topackdata::VariableNodeData)
+#   @warn "'encodePackedType' Deprecated..."
+#   # error("IncrementalInference.encodePackedType(::VariableNodeData): Unknown packed type encoding of $(topackdata)")
+#   pack(nothing, topackdata)
+# end
+# function encodePackedType(topackdata::GenericFunctionNodeData{CommonConvWrapper{T}, Symbol}) where {T <: FunctorInferenceType}
+#   # println("IncrementalInference.encodePackedType(::GenericFunctionNodeData{T,Symbol}): Unknown packed type encoding T=$(T) of $(topackdata)")
+#   fnctype = getfnctype(topackdata)
+#   fnc = getfield(getmodule(fnctype), Symbol("Packed$(getname(fnctype))"))
+#   convert(PackedFunctionNodeData{fnc}, topackdata)
+# end
+# function encodePackedType(topackdata::GenericFunctionNodeData{T, <:AbstractString}) where {T <: PackedInferenceType}
+#   error("IncrementalInference.encodePackedType(::FunctionNodeData{T, <:AbstractString}): Unknown packed type encoding T=$(T) of $(topackdata)")
+#   # @show T, typeof(topackdata)
+#   # @warn "Yes, its packed!"
+#   # fnctype = getfnctype(topackdata)
+#   # @show fnc = getfield(getmodule(fnctype), Symbol("Packed$(getname(fnctype))"))
+#   # convert(PackedFunctionNodeData{T}, topackdata)
+#   topackdata
+# end
 
 """
     $(SIGNATURES)
@@ -131,19 +131,53 @@ end
 
 
 # Variables
-function decodePackedType(packeddata::PackedVariableNodeData,
-                          typestring::String )
-#TODO: typestring is unnecessary
+
+function decodePackedType(dfg::G, packeddata::PackedVariableNodeData) where G <: AbstractDFG
+  @warn "Deprecated?"
   convert(IncrementalInference.VariableNodeData, packeddata)
 end
 # Factors
-#TODO: typestring is unnecessary
-function decodePackedType(packeddata::GenericFunctionNodeData{PT,<:AbstractString}, notused::String) where {PT}
+function decodePackedType(dfg::G, packeddata::GenericFunctionNodeData{PT,<:AbstractString}) where {PT, G <: AbstractDFG}
   usrtyp = convert(FunctorInferenceType, packeddata.fnc)
   fulltype = FunctionNodeData{CommonConvWrapper{usrtyp}}
-  return convert(fulltype, packeddata)
+  factor = convert(fulltype, packeddata)
+  return factor
 end
 
+"""
+  $(SIGNATURES)
+After deserializing a factor using decodePackedType, use this to
+completely rebuild the factor's CCW and user data.
+"""
+function rebuildFactorMetadata!(dfg::G, factor::DFGFactor)::DFGFactor where G <: AbstractDFG
+  # Set up the neighbor data
+  neighbors = map(vId->getVariable(dfg, vId), getNeighbors(dfg, factor))
+  neighborUserData = map(v->getData(v).softtype, neighbors)
+
+  # Rebuilding the CCW
+  setDefaultFactorNode!(dfg, factor, neighbors, factor.data.fnc.usrfnc!)
+
+  #... Copying neighbor data into the factor?
+  ccw_new = getData(factor)
+  for i in 1:Threads.nthreads()
+    ccw_new.fnc.cpt[i].factormetadata.variableuserdata = deepcopy(neighborUserData)
+  end
+
+  # Copying all other fields in the factor
+  # TODO: Confirm whether we still need to do this?
+  ## Rebuild getData(fcnode).fncargvID, however, the list is order sensitive
+  # out_neighbors does not gaurantee ordering -- i.e. why is it not being saved
+  # for field in fieldnames(typeof(ccw_jld))
+  #   if field != :fnc
+  #     setfield!(ccw_new, field, getfield(ccw_jld, field))
+  #   end
+  # end
+
+  return factor
+end
+
+
+
 """
     $(SIGNATURES)
 
@@ -203,7 +237,7 @@ veeCategorical(val::Union{Nothing, Vector{Float64}}) = val
 
 Unpack PackedFunctionNodeData formats back to regular FunctonNodeData.
 """
-function decodefg(fgs::FactorGraph; api::DataLayerAPI=localapi)
+function decodefg(fgs::FactorGraph)
   fgu = deepcopy(fgs)
   fgu.cg = nothing # will be deprecated or replaced
   fgu.registeredModuleFunctions = nothing # TODO: obsolete

diff --git a/src/FGOSUtils.jl b/src/FGOSUtils.jl
@@ -112,6 +112,7 @@ function compareAll(Al::T,
   return TP
 end
 
+# TODO: KEEP
 function compareAll(Al::T1, Bl::T2; show::Bool=true, skip::Vector{Symbol}=Symbol[])::Bool where {T1 <: Union{SingleThreaded, MultiThreaded}, T2 <: Union{SingleThreaded, MultiThreaded}}
   return T1 == T2
 end
@@ -152,6 +153,7 @@ end
 #   return true
 # end
 
+# TODO: KEEP
 function compare(p1::BallTreeDensity, p2::BallTreeDensity)::Bool
   return compareAll(p1.bt,p2.bt, skip=[:calcStatsHandle; :data]) &&
          compareAll(p1,p2, skip=[:calcStatsHandle; :bt])
@@ -198,6 +200,7 @@ function compareAllSpecial(A::T1,
   end
 end
 
+# TODO: KEEP
 function compareAllSpecial(A::T1, B::T2;
                     skip=Symbol[], show::Bool=true) where {T1 <: CommonConvWrapper, T2 <: CommonConvWrapper}
   #
@@ -327,7 +330,7 @@ Related:
 
 `compareFactorGraphs`, `compareSimilarVariables`, `compareSimilarFactors`, `ls`.
 """
-function compareSubsetFactorGraph(fgS::FactorGraph, fgA::FactorGraph; api::DataLayerAPI=localapi)
+function compareSubsetFactorGraph(fgS::FactorGraph, fgA::FactorGraph)
   error("not implemented yet")
   return false
 end
@@ -629,7 +632,7 @@ end
 Return `::Bool` on whether this variable has been marginalized.
 """
 isMarginalized(vert::DFGVariable) = getData(vert).ismargin
-isMarginalized(dfg::G, sym::Symbol; api::DataLayerAPI=localapi) where G <: AbstractDFG = isMarginalized(DFG.getVariable(fg, sym))
+isMarginalized(dfg::G, sym::Symbol) where G <: AbstractDFG = isMarginalized(DFG.getVariable(fg, sym))
 
 
 

diff --git a/src/FactorGraph01.jl b/src/FactorGraph01.jl
@@ -864,12 +864,11 @@ end
 function addBayesNetVerts!(dfg::G, elimOrder::Array{Symbol,1}) where G <: AbstractDFG
   for pId in elimOrder
     vert = DFG.getVariable(dfg, pId)
-    # @show vert.label, getData(vert).BayesNetVertID
-    if getData(vert).BayesNetVertID == nothing
-      # fg.bnid+=1
+    if getData(vert).BayesNetVertID == nothing || getData(vert).BayesNetVertID == :_null # Special serialization case of nothing
+      @debug "[AddBayesNetVerts] Assigning $pId.data.BayesNetVertID = $pId"
       getData(vert).BayesNetVertID = pId
     else
-      @warn "addBayesNetVerts -- something is very wrong, should not have a Bayes net vertex"
+      @warn "addBayesNetVerts -- Something is wrong, variable '$pId' should not have an existing Bayes net reference to '$(getData(vert).BayesNetVertID)'"
     end
   end
 end