Merge pull request #14 from JuliaRobotics/feature/dfgintegration

Feature/dfgintegration for IncrementalInference

Author: GearsAD

Project.toml

@@ -1,6 +1,6 @@
 name = "DistributedFactorGraphs"
 uuid = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04"
-version = "0.0.0"
+version = "0.0.1"
 
 [deps]
 DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0"

src/DistributedFactorGraphs.jl

@@ -16,9 +16,18 @@ export DFGNode
 export DFGFactor
 export DFGVariable
 export label, timestamp, tags, estimates, estimate, solverData, solverDataDict, id, smallData, bigData
+export setSolverData
 export label, data, id
 
+# Solver (IIF) Exports
+export VariableNodeData, PackedVariableNodeData
+export GenericFunctionNodeData#, FunctionNodeData
+export getSerializationModule, setSerializationModule!
+export pack, unpack
+
 # Include the Graphs.jl API.
 include("services/GraphsDFG.jl")
+include("services/AbstractDFG.jl")
+include("services/DFGVariable.jl")
 
 end

src/entities/DFGFactor.jl

@@ -22,12 +22,18 @@ Fundamental structure for a DFG factor.
 """
 mutable struct DFGFactor{T, S} <: DFGNode
     label::Symbol
+    tags::Vector{Symbol}
     data::GenericFunctionNodeData{T, S}
+    ready::Int
+    backendset::Int
     _internalId::Int64
-    DFGFactor{T, S}(label::Symbol) where {T, S} = new{T, S}(label, GenericFunctionNodeData{T, S}(), 0)
-    DFGFactor{T, S}(label::Symbol, _internalId::Int64) where {T, S} = new{T, S}(label, GenericFunctionNodeData{T, S}(), _internalId)
+    DFGFactor{T, S}(label::Symbol) where {T, S} = new{T, S}(label, Symbol[], GenericFunctionNodeData{T, S}(), 0, 0, 0)
+    DFGFactor{T, S}(label::Symbol, _internalId::Int64) where {T, S} = new{T, S}(label, Symbol[], GenericFunctionNodeData{T, S}(), 0, 0, _internalId)
 end
 
+# const FunctionNodeData{T} = GenericFunctionNodeData{T, Symbol}
+# FunctionNodeData(x1, x2, x3, x4, x5::Symbol, x6::T, x7::String="", x8::Vector{Int}=Int[]) where {T <: Union{FunctorInferenceType, ConvolutionObject}}= GenericFunctionNodeData{T, Symbol}(x1, x2, x3, x4, x5, x6, x7, x8)
+
 label(f::F) where F <: DFGFactor = f.label
 data(f::F) where F <: DFGFactor = f.data
 id(f::F) where F <: DFGFactor = f._internalId

src/entities/DFGVariable.jl

@@ -4,15 +4,16 @@ $(TYPEDEF)
 mutable struct VariableNodeData
   val::Array{Float64,2}
   bw::Array{Float64,2}
-  BayesNetOutVertIDs::Array{Int,1}
+  BayesNetOutVertIDs::Array{Symbol,1}
   dimIDs::Array{Int,1} # Likely deprecate
   dims::Int
   eliminated::Bool
-  BayesNetVertID::Int
-  separator::Array{Int,1}
+  BayesNetVertID::Union{Nothing, Symbol}
+  separator::Array{Symbol,1}
   groundtruth::Union{Nothing, Dict{ Tuple{Symbol, Vector{Float64}} } } # not packed yet
   softtype
   initialized::Bool
+  partialinit::Bool
   ismargin::Bool
   dontmargin::Bool
   VariableNodeData() = new()
@@ -22,18 +23,57 @@ mutable struct VariableNodeData
   # end
   VariableNodeData(x1::Array{Float64,2},
                    x2::Array{Float64,2},
-                   x3::Vector{Int},
+                   x3::Vector{Symbol},
                    x4::Vector{Int},
                    x5::Int,
                    x6::Bool,
-                   x7::Int,
-                   x8::Vector{Int},
+                   x7::Union{Nothing, Symbol},
+                   x8::Vector{Symbol},
                    x9::Union{Nothing, Dict{ Tuple{Symbol, Vector{Float64}} } },
                    x10,
                    x11::Bool,
                    x12::Bool,
-                   x13::Bool) =
-    new(x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13)
+                   x13::Bool,
+                   x14::Bool) =
+    new(x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14)
+end
+
+"""
+$(TYPEDEF)
+"""
+mutable struct PackedVariableNodeData
+  vecval::Array{Float64,1}
+  dimval::Int
+  vecbw::Array{Float64,1}
+  dimbw::Int
+  BayesNetOutVertIDs::Array{Int,1}
+  dimIDs::Array{Int,1}
+  dims::Int
+  eliminated::Bool
+  BayesNetVertID::Int
+  separator::Array{Int,1}
+  # groundtruth::NothingUnion{ Dict{ Tuple{Symbol, Vector{Float64}} } }
+  softtype::String
+  initialized::Bool
+  partialinit::Bool
+  ismargin::Bool
+  dontmargin::Bool
+  PackedVariableNodeData() = new()
+  PackedVariableNodeData(x5::Vector{Float64},
+                         x6::Int,
+                         x7::Vector{Float64},
+                         x8::Int,
+                         x9::Vector{Int},
+                         x10::Vector{Int},
+                         x11::Int,
+                         x12::Bool,
+                         x13::Int,
+                         x14::Vector{Int},
+                         x15::String,
+                         x16::Bool,
+                         x17::Bool,
+                         x18::Bool,
+                         x19::Bool ) = new(x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19)
 end
 
 struct VariableEstimate
@@ -54,9 +94,11 @@ mutable struct DFGVariable <: DFGNode
     solverDataDict::Dict{Symbol, VariableNodeData}
     smallData::Any
     bigData::Any
+    ready::Int
+    backendset::Int
     _internalId::Int64
-    DFGVariable(label::Symbol, _internalId::Int64) = new(label, now(), Symbol[], Dict{Symbol, VariableEstimate}(), Dict{Symbol, VariableNodeData}(:default => VariableNodeData()), nothing, nothing, _internalId)
-    DFGVariable(label::Symbol) = new(label, now(), Symbol[], Dict{Symbol, VariableEstimate}(), Dict{Symbol, VariableNodeData}(:default => VariableNodeData()), nothing, nothing, 0)
+    DFGVariable(label::Symbol, _internalId::Int64) = new(label, now(), Symbol[], Dict{Symbol, VariableEstimate}(), Dict{Symbol, VariableNodeData}(:default => VariableNodeData()), nothing, nothing, 0, 0, _internalId)
+    DFGVariable(label::Symbol) = new(label, now(), Symbol[], Dict{Symbol, VariableEstimate}(), Dict{Symbol, VariableNodeData}(:default => VariableNodeData()), nothing, nothing, 0, 0, 0)
 end
 
 # Accessors
@@ -67,6 +109,7 @@ estimates(v::DFGVariable) = v.estimateDict
 estimate(v::DFGVariable, key::Symbol=:default) = haskey(v.estimateDict, key) ? v.estimateDict[key] : nothing
 #solverData(v::DFGVariable) = haskey(v.solverDataDict, :default) ? v.solverDataDict[:default] : nothing
 solverData(v::DFGVariable, key::Symbol=:default) = haskey(v.solverDataDict, key) ? v.solverDataDict[key] : nothing
+setSolverData(v::DFGVariable, data::VariableNodeData, key::Symbol=:default) = v.solverDataDict[key] = data
 solverDataDict(v::DFGVariable) = v.solverDataDict
 id(v::DFGVariable) = v._internalId
 # Todo: Complete this.

src/services/AbstractDFG.jl

@@ -0,0 +1,26 @@
+
+"""
+    $(SIGNATURES)
+
+De-serialization of IncrementalInference objects require discovery of foreign types.
+
+Example:
+
+Template to tunnel types from a user module:
+```julia
+# or more generic solution -- will always try Main if available
+IIF.setSerializationNamespace!(Main)
+
+# or a specific package such as RoME if you import all variable and factor types into a specific module.
+using RoME
+IIF.setSerializationNamespace!(RoME)
+```
+"""
+function setSerializationModule!(dfg::G, mod::Module)::Nothing where G <: AbstractDFG
+    @warn "Setting serialization module from AbstractDFG - override this in the '$(typeof(dfg)) structure! This is being ignored."
+end
+
+function getSerializationModule(dfg::G)::Module where G <: AbstractDFG
+    @warn "Retrieving serialization module from AbstractDFG - override this in the '$(typeof(dfg)) structure! This is returning Main"
+    return Main
+end

src/services/DFGFactor.jl

@@ -0,0 +1,65 @@
+using Base
+import Base.==
+
+function pack(dfg::Union{G, Nothing}, d::VariableNodeData)::PackedVariableNodeData where G <: AbstractDFG
+  @debug "Dispatching conversion variable -> packed variable for type $(string(d.softtype))"
+  return PackedVariableNodeData(d.val[:],size(d.val,1),
+                                d.bw[:], size(d.bw,1),
+                                d.BayesNetOutVertIDs,
+                                d.dimIDs, d.dims, d.eliminated,
+                                d.BayesNetVertID, d.separator,
+                                string(d.softtype), d.initialized, d.partialinit, d.ismargin, d.dontmargin)
+end
+
+function unpack(dfg::Union{G, Nothing}, d::PackedVariableNodeData)::VariableNodeData where G <: AbstractDFG
+  r3 = d.dimval
+  c3 = r3 > 0 ? floor(Int,length(d.vecval)/r3) : 0
+  M3 = reshape(d.vecval,r3,c3)
+
+  r4 = d.dimbw
+  c4 = r4 > 0 ? floor(Int,length(d.vecbw)/r4) : 0
+  M4 = reshape(d.vecbw,r4,c4)
+
+  # TODO -- allow out of module type allocation (future feature, not currently in use)
+  @debug "Dispatching conversion packed variable -> variable for type $(string(d.softtype))"
+  st = nothing #IncrementalInference.ContinuousMultivariate # eval(parse(d.softtype))
+  mainmod = getSerializationModule(dfg)
+  mainmod == nothing && error("Serialization module is null - please call setSerializationNamespace!(\"Main\" => Main) in your main program.")
+  try
+      unpackedTypeName = split(d.softtype, "(")[1]
+      unpackedTypeName = split(unpackedTypeName, '.')[end]
+      @debug "DECODING Softtype = $unpackedTypeName"
+      st = getfield(mainmod, Symbol(unpackedTypeName))()
+  catch ex
+      @error "Unable to deserialize soft type $(d.softtype)"
+      io = IOBuffer()
+      showerror(io, ex, catch_backtrace())
+      err = String(take!(io))
+      @error err
+  end
+  @debug "Net conversion result: $st"
+
+  return VariableNodeData(M3,M4, d.BayesNetOutVertIDs,
+    d.dimIDs, d.dims, d.eliminated, d.BayesNetVertID, d.separator,
+    nothing, st, d.initialized, d.partialinit, d.ismargin, d.dontmargin )
+end
+
+function compare(a::VariableNodeData,b::VariableNodeData)
+    TP = true
+    TP = TP && a.val == b.val
+    TP = TP && a.bw == b.bw
+    TP = TP && a.BayesNetOutVertIDs == b.BayesNetOutVertIDs
+    TP = TP && a.dimIDs == b.dimIDs
+    TP = TP && a.dims == b.dims
+    TP = TP && a.eliminated == b.eliminated
+    TP = TP && a.BayesNetVertID == b.BayesNetVertID
+    TP = TP && a.separator == b.separator
+    TP = TP && a.partialinit == b.partialinit
+    TP = TP && a.ismargin == b.ismargin
+    TP = TP && a.softtype == b.softtype
+    return TP
+end
+
+function ==(a::VariableNodeData,b::VariableNodeData, nt::Symbol=:var)
+  return DistributedFactorGraphs.compare(a,b)
+end