Merge pull request #10 from JuliaRobotics/feature/variablefactordefinition

Feature/variablefactordefinition

Author: GearsAD

REQUIRE

@@ -5,3 +5,6 @@ DocStringExtensions 0.4.1
 Requires 0.5.2
 JSON2
 Graphs
+# Caesar/IIF specific imports
+Distributions
+Dates

src/DistributedFactorGraphs.jl

@@ -3,6 +3,8 @@ module DistributedFactorGraphs
 using Base
 using DocStringExtensions
 using Requires
+using Dates
+using Distributions
 
 # Entities
 include("entities/AbstractTypes.jl")
@@ -13,6 +15,8 @@ export AbstractDFG
 export DFGNode
 export DFGFactor
 export DFGVariable
+export label, timestamp, tags, estimates, estimate, solverData, solverDataDict, id, smallData, bigData
+export label, data, id
 
 # Include the Graphs.jl API.
 include("services/GraphsDFG.jl")

src/entities/DFGFactor.jl

@@ -1,11 +1,33 @@
+"""
+$(TYPEDEF)
+"""
+mutable struct GenericFunctionNodeData{T, S}
+  fncargvID::Array{Int,1}
+  eliminated::Bool
+  potentialused::Bool
+  edgeIDs::Array{Int,1}
+  frommodule::S #Union{Symbol, AbstractString}
+  fnc::T
+  multihypo::String # likely to moved when GenericWrapParam is refactored
+  certainhypo::Vector{Int}
+  GenericFunctionNodeData{T, S}() where {T, S} = new{T,S}()
+  GenericFunctionNodeData{T, S}(x1, x2, x3, x4, x5::S, x6::T, x7::String="", x8::Vector{Int}=Int[]) where {T, S} = new{T,S}(x1, x2, x3, x4, x5, x6, x7, x8)
+  GenericFunctionNodeData(x1, x2, x3, x4, x5::S, x6::T, x7::String="", x8::Vector{Int}=Int[]) where {T, S} = new{T,S}(x1, x2, x3, x4, x5, x6, x7, x8)
+  # GenericFunctionNodeData(x1, x2, x3, x4, x5::S, x6::T, x7::String) where {T, S} = new{T,S}(x1, x2, x3, x4, x5, x6, x7)
+end
+
 """
     $(SIGNATURES)
 Fundamental structure for a DFG factor.
 """
-mutable struct DFGFactor <: DFGNode
+mutable struct DFGFactor{T, S} <: DFGNode
     label::Symbol
-    #TODO: Populate
+    data::GenericFunctionNodeData{T, S}
     _internalId::Int64
-    DFGFactor(label::Symbol) = new(label, 0)
-    DFGFactor(label::Symbol, _internalId::Int64) = new(label, _internalId)
+    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)
 end
+
+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

@@ -1,12 +1,74 @@
+"""
+$(TYPEDEF)
+"""
+mutable struct VariableNodeData
+  val::Array{Float64,2}
+  bw::Array{Float64,2}
+  BayesNetOutVertIDs::Array{Int,1}
+  dimIDs::Array{Int,1} # Likely deprecate
+  dims::Int
+  eliminated::Bool
+  BayesNetVertID::Int
+  separator::Array{Int,1}
+  groundtruth::Union{Nothing, Dict{ Tuple{Symbol, Vector{Float64}} } } # not packed yet
+  softtype
+  initialized::Bool
+  ismargin::Bool
+  dontmargin::Bool
+  VariableNodeData() = new()
+  # function VariableNodeData(x1,x2,x3,x4,x5,x6,x7,x8,x9)
+  #   @warn "Deprecated use of VariableNodeData(11 param), use 13 parameters instead"
+  #   new(x1,x2,x3,x4,x5,x6,x7,x8,x9, nothing, true, false, false) # TODO ensure this is initialized true is working for most cases
+  # end
+  VariableNodeData(x1::Array{Float64,2},
+                   x2::Array{Float64,2},
+                   x3::Vector{Int},
+                   x4::Vector{Int},
+                   x5::Int,
+                   x6::Bool,
+                   x7::Int,
+                   x8::Vector{Int},
+                   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)
+end
+
+struct VariableEstimate
+  estimate::Vector{Float64}
+  type::Symbol
+  key::Symbol
+end
 
 """
     $(SIGNATURES)
 Fundamental structure for a DFG variable.
 """
 mutable struct DFGVariable <: DFGNode
     label::Symbol
-    #TODO: Populate
+    timestamp::DateTime
+    tags::Vector{Symbol}
+    estimateDict::Dict{Symbol, VariableEstimate}
+    solverDataDict::Dict{Symbol, VariableNodeData}
+    smallData::Any
+    bigData::Any
     _internalId::Int64
-    DFGVariable(label::Symbol) = new(label, 0)
-    DFGVariable(label::Symbol, _internalId::Int64) = new(label, _internalId)
+    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)
 end
+
+# Accessors
+label(v::DFGVariable) = v.label
+timestamp(v::DFGVariable) = v.timestamp
+tags(v::DFGVariable) = v.tags
+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
+solverDataDict(v::DFGVariable) = v.solverDataDict
+id(v::DFGVariable) = v._internalId
+# Todo: Complete this.
+smallData(v::DFGVariable) = v.smallData
+bigData(v::DFGVariable) = v.bigData

src/services/GraphsDFG.jl

@@ -22,9 +22,9 @@ import Graphs: attributes, vertex_index
 function attributes(v::GraphsNode, g::T)::AttributeDict where T <:GenericIncidenceList
     AttributeDict(
         "label" => v.dfgNode.label,
-        "color" => typeof(v.dfgNode) == DFGVariable ? "red" : "blue",
-        "shape" => typeof(v.dfgNode) == DFGVariable ? "box" : "ellipse",
-        "fillcolor" => typeof(v.dfgNode) == DFGVariable ? "red" : "blue"
+        "color" => v.dfgNode isa DFGVariable ? "red" : "blue",
+        "shape" => v.dfgNode isa DFGVariable ? "box" : "ellipse",
+        "fillcolor" => v.dfgNode isa DFGVariable ? "red" : "blue"
         )
 end
 
@@ -226,7 +226,7 @@ List the DFGVariables in the DFG.
 Optionally specify a label regular expression to retrieves a subset of the variables.
 """
 function ls(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing)::Vector{DFGVariable}
-    variables = map(v -> v.dfgNode, filter(n -> typeof(n.dfgNode) == DFGVariable, collect(values(dfg.g.vertices))))
+    variables = map(v -> v.dfgNode, filter(n -> n.dfgNode isa DFGVariable, vertices(dfg.g)))
     if regexFilter != nothing
         variables = filter(v -> occursin(regexFilter, String(v.label)), variables)
     end
@@ -247,7 +247,7 @@ List the DFGFactors in the DFG.
 Optionally specify a label regular expression to retrieves a subset of the factors.
 """
 function lsf(dfg::GraphsDFG, regexFilter::Union{Nothing, Regex}=nothing)::Vector{DFGFactor}
-    factors = map(v -> v.dfgNode, filter(n -> typeof(n.dfgNode) == DFGFactor, collect(values(dfg.g.vertices))))
+    factors = map(v -> v.dfgNode, filter(n -> n.dfgNode isa DFGFactor, vertices(dfg.g)))
     if regexFilter != nothing
         factors = filter(f -> occursin(regexFilter, String(f.label)), factors)
     end
@@ -321,8 +321,8 @@ end
 function _copyIntoGraph!(sourceDFG::GraphsDFG, destDFG::GraphsDFG, variableFactorLabels::Vector{Symbol}, includeOrphanFactors::Bool=false)::Nothing
     # Split into variables and factors
     verts = map(id -> sourceDFG.g.vertices[sourceDFG.labelDict[id]], variableFactorLabels)
-    sourceVariables = filter(n -> typeof(n.dfgNode) == DFGVariable, verts)
-    sourceFactors = filter(n -> typeof(n.dfgNode) == DFGFactor, verts)
+    sourceVariables = filter(n -> n.dfgNode isa DFGVariable, verts)
+    sourceFactors = filter(n -> n.dfgNode isa DFGFactor, verts)
 
     # Now we have to add all variables first,
     for variable in sourceVariables

test/interfaceTests.jl

@@ -1,14 +1,14 @@
 dfg = testDFGAPI()
 v1 = DFGVariable(:a)
 v2 = DFGVariable(:b)
-f1 = DFGFactor(:f1)
+f1 = DFGFactor{Int, :Symbol}(:f1)
 @testset "Creating Graphs" begin
     global dfg,v1,v2,f1
     addVariable!(dfg, v1)
     @test_throws Exception addVariable!(dfg, v1)
     addVariable!(dfg, v2)
     addFactor!(dfg, [v1, v2], f1)
-    @test_throws Exception addFactor!(dfg, DFGFactor("f2"), [v1, DFGVariable("Nope")])
+    @test_throws Exception addFactor!(dfg, DFGFactor{Int, :Symbol}("f2"), [v1, DFGVariable("Nope")])
 end
 
 @testset "Listing Nodes" begin
@@ -21,7 +21,7 @@ end
 end
 
 # Gets
-@testset "Gets and Sets" begin
+@testset "Gets, Sets, and Accessors" begin
     global dfg,v1,v2,f1
     @test getVariable(dfg, v1.label) == v1
     @test getFactor(dfg, f1.label) == f1
@@ -35,6 +35,20 @@ end
     @test updateVariable!(dfg, v1Prime) != v1
     f1Prime = deepcopy(f1)
     @test updateFactor!(dfg, f1Prime) != f1
+
+    # Accessors
+    @test label(v1) == v1.label
+    @test timestamp(v1) == v1.timestamp
+    @test estimates(v1) == v1.estimateDict
+    @test estimate(v1, :notfound) == nothing
+    @test solverData(v1) == v1.solverDataDict[:default]
+    @test solverData(v1, :default) == v1.solverDataDict[:default]
+    @test solverDataDict(v1) == v1.solverDataDict
+    @test id(v1) == v1._internalId
+
+    @test label(f1) == f1.label
+    @test data(f1) == f1.data
+    @test id(f1) == f1._internalId
 end
 
 # Deletions
@@ -71,7 +85,7 @@ numNodes = 10
 dfg = testDFGAPI()
 verts = map(n -> DFGVariable(Symbol("x$n")), 1:numNodes)
 map(v -> addVariable!(dfg, v), verts)
-map(n -> addFactor!(dfg, [verts[n], verts[n+1]], DFGFactor(Symbol("x$(n)x$(n+1)f1"))), 1:(numNodes-1))
+map(n -> addFactor!(dfg, [verts[n], verts[n+1]], DFGFactor{Int, :Symbol}(Symbol("x$(n)x$(n+1)f1"))), 1:(numNodes-1))
 # map(n -> addFactor!(dfg, [verts[n], verts[n+2]], DFGFactor(Symbol("x$(n)x$(n+2)f2"))), 1:2:(numNodes-2))
 
 @testset "Getting Neighbors" begin